[Robot] Lob short in wing

SouthwestRoboticsProgramming · Mar 23, 2024 · 062b66e · 062b66e
1 parent 90e46f1
commit 062b66e
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 11 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java b/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java
@@ -1,7 +1,5 @@
 package com.swrobotics.robot.commands;
 
-import org.littletonrobotics.junction.AutoLogOutput;
-
 import com.swrobotics.lib.net.NTUtil;
 import com.swrobotics.mathlib.MathUtil;
 import com.swrobotics.robot.config.NTData;
@@ -12,10 +10,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
-import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public final class AimTowardsLobCommand extends Command {

diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java
@@ -117,7 +117,8 @@ public final class NTData {
     public static final NTEntry<Double> SHOOTER_PIVOT_REVERSE_ANGLE = new NTDouble("Shooter/Pivot/Reverse Angle (deg)", 35).setPersistent();
 
     public static final NTEntry<Double> SHOOTER_LOB_POWER_COEFFICIENT = new NTDouble("Shooter/Lob/Power Coefficient", 1.25).setPersistent(); // To go from real velocity to flywheel velocity
-    public static final NTEntry<Double> SHOOTER_LOB_HEIGHT_METERS = new NTDouble("Shooter/Lob/Lob Height (m)", 3).setPersistent();
+    public static final NTEntry<Double> SHOOTER_LOB_TALL_HEIGHT_METERS = new NTDouble("Shooter/Lob/Tall Lob Height (m)", 4).setPersistent();
+    public static final NTEntry<Double> SHOOTER_LOB_SHORT_HEIGHT_METERS = new NTDouble("Shooter/Lob/Short Lob Height (m)", 1).setPersistent();
     public static final NTEntry<Double> SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25);
 
     private NTData() {

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java
@@ -10,15 +10,26 @@ public class LobCalculator implements AimCalculator {
     public static final LobCalculator INSTANCE = new LobCalculator();
 
     private static final double twoG = 9.8 * 2;
-    private double sqrt2gh;
+    private double tallSqrt2gh;
+    private double shortSqrt2gh;
 
     public LobCalculator() {
-        NTData.SHOOTER_LOB_HEIGHT_METERS.onChange((a) -> this.updateHeight());
-        sqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_HEIGHT_METERS.get());
+        NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.onChange((a) -> this.updateHeight());
+        NTData.SHOOTER_LOB_SHORT_HEIGHT_METERS.onChange((a) -> this.updateHeight());
+        tallSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get());
+        shortSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_SHORT_HEIGHT_METERS.get());
     }
 
     public Aim calculateAim(double distanceToSpeaker, double velocityTowardsGoal) {
-        double angleRad = Math.atan2(4 * NTData.SHOOTER_LOB_HEIGHT_METERS.get(), distanceToSpeaker);
+        boolean inWing = distanceToSpeaker > 5;
+        double angleRad = 0;
+
+        if (inWing) {
+            angleRad = Math.atan2(4 * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get(), distanceToSpeaker);
+        }
+
+        double sqrt2gh = (inWing) ? tallSqrt2gh : shortSqrt2gh;
+
         double velocity = sqrt2gh / Math.sin(angleRad);
 
         Translation2d velocityVector = new Translation2d(velocity, new Rotation2d(angleRad));
@@ -37,7 +48,8 @@ public Aim calculateAim(double distanceToSpeaker) {
     }
 
     private void updateHeight() {
-        sqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_HEIGHT_METERS.get());
+        tallSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get());
+        shortSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_SHORT_HEIGHT_METERS.get());
     }
 
 }