[Robot] Added angle correction when moving towards or away from the s…

…peaker
SouthwestRoboticsProgramming · Mar 27, 2024 · 2ab7d0f · 2ab7d0f
1 parent 4e6a53a
commit 2ab7d0f
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 7 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java
@@ -122,6 +122,7 @@ public final class NTData {
     public static final NTEntry<Double> SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_BLUE = new NTDouble("Shooter/Lob/Drive Angle Correction Blue (deg)", -25).setPersistent();
     public static final NTEntry<Double> SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_RED = new NTDouble("Shooter/Lob/Drive Angle Correction Red (deg)", -25).setPersistent();
     public static final NTEntry<Double> SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25);
+    public static final NTEntry<Double> SHOOTER_FADEAWAY_COEFFICIENT = new NTDouble("Shooter/Fadeaway Coefficient", 0.2);
 
     private NTData() {
         throw new AssertionError();

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java
@@ -89,15 +89,21 @@ private double sample(TreeMap<Double, Double> map, double distanceToSpeaker) {
 
     @Override
     public Aim calculateAim(double distanceToSpeaker) {
-       logDistance.set(distanceToSpeaker);
+        return calculateAim(distanceToSpeaker, 0);
+    }
+
+    public Aim calculateAim(double distanceToSpeaker, double velocityTowardsGoal) {
+        logDistance.set(distanceToSpeaker);
+
+        distanceToSpeaker *= NTData.SHOOTER_DISTANCE_SCALE.get();
+
+        double flywheelVelocity = sample(flywheelVelocityMap, distanceToSpeaker);
+        double pivotAngle = sample(pivotAngleMap, distanceToSpeaker);
 
-       distanceToSpeaker *= NTData.SHOOTER_DISTANCE_SCALE.get();
+        // Adjust stupidly (should work for close range)
+        pivotAngle += -velocityTowardsGoal * NTData.SHOOTER_FADEAWAY_COEFFICIENT.get();
 
-        return new Aim(
-                sample(flywheelVelocityMap, distanceToSpeaker),
-                sample(pivotAngleMap, distanceToSpeaker),
-                distanceToSpeaker
-        );
+        return new Aim(flywheelVelocity, pivotAngle, distanceToSpeaker);
     }
 
     public double getSnapDistance(double currentDist, boolean wantMoveCloser) {