[Robot] Make pivot angle tunable in reverse

SouthwestRoboticsProgramming · Mar 22, 2024 · 158360a · 158360a
1 parent 2e3b4e6
commit 158360a
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 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
@@ -114,6 +114,7 @@ public final class NTData {
     public static final NTEntry<Double> SHOOTER_PIVOT_ALLOWABLE_PCT_ERR = new NTDouble("Shooter/Pivot/Allowable Pct Error", 0.01).setPersistent(); // FIXME TUNE
     public static final NTEntry<Double> SHOOTER_PIVOT_ANGLE_ADJUST_BLUE = new NTDouble("Shooter/Pivot/Angle Adjust Blue (deg)", -1).setPersistent();
     public static final NTEntry<Double> SHOOTER_PIVOT_ANGLE_ADJUST_RED = new NTDouble("Shooter/Pivot/Angle Adjust Red (deg)", 1).setPersistent();
+    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();

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
@@ -112,7 +112,7 @@ public void periodic() {
             flywheel.setTargetVelocity(aim.flywheelVelocity());
             pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU);
         } else if (flywheelControl == FlywheelControl.REVERSE) {
-            // Don't touch pivot so it won't move and squish stuck note
+	    pivot.setTargetAngle(NTData.SHOOTER_PIVOT_REVERSE_ANGLE.get() / 360.0);
             flywheel.setDutyCycle(-NTData.SHOOTER_FLYWHEEL_REVERSE_SPEED.get());
         } else if (flywheelControl == FlywheelControl.SHOOT || afterShootDelay.calculate(indexer.hasPiece())) {
             if (aim != null) {
@@ -131,7 +131,10 @@ else if (flywheelControl == FlywheelControl.POOP)
                     flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get());
                 pivot.setIdle();
             }
-        } else {
+        } else if (flywheelControl == FlywheelControl.POOP) {
+	    flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get());
+	    pivot.setNeutral();
+	} else {
             flywheel.setNeutral();
             pivot.setNeutral();
         }