Manual shooting adjustment

src/main/java/frc/robot/controls/TeleopControls.java

@@ -245,7 +245,7 @@ public void configureBindings() {
         .whileTrue(this.robot.getTurret().moveAtVoltage(TurretConstants.FINE_CONTROL_VOLTAGE));
 
     operator
-        .povLeft()
+        .povRight()
         .and(() -> fineControl)
         .whileTrue(
             this.robot
@@ -272,13 +272,48 @@ public void configureBindings() {
                     .getIntakeExtension()
                     .moveAtVoltage(IntakeExtensionConstants.FINE_CONTROL_VOLTAGE.unaryMinus())));
 
-    // operator
-    //     .axisGreaterThan(Axis.kRightY.value, 0.5)
-    //     .and(() -> fineControl)
-    //     .whileTrue(this.robot.getClimb().moveAtVoltage(ClimbConstants.FINE_CONTROL_VOLTAGE));
+    operator
+        .axisMagnitudeGreaterThan(Axis.kRightX.value, 0.1)
+        .and(() -> fineControl)
+        .whileTrue(
+            Commands.run(
+                () -> {
+                  this.robot.getTurret().setOffsetAngle(Degrees.of((operator.getRightX()) * -2));
+                }));
+
+    operator
+        .axisGreaterThan(Axis.kRightX.value, 0.1)
+        .negate()
+        .and(() -> fineControl)
+        .onTrue(
+            Commands.run(
+                () -> {
+                  this.robot.getTurret().setOffsetAngle(Degrees.zero());
+                }));
+
+    operator
+        .axisMagnitudeGreaterThan(Axis.kRightY.value, 0.1)
+        .and(() -> fineControl)
+        .whileTrue(
+            Commands.run(
+                () -> {
+                  this.robot
+                      .getShooterHood()
+                      .setOffsetAngle(Degrees.of((operator.getLeftY()) * -2));
+                }));
+
+    operator
+        .axisGreaterThan(Axis.kRightY.value, 0.1)
+        .negate()
+        .and(() -> fineControl)
+        .onTrue(
+            Commands.run(
+                () -> {
+                  this.robot.getShooterHood().setOffsetAngle(Degrees.zero());
+                }));
 
     // operator
-    //     .axisLessThan(Axis.kRightY.value, -0.5)
+    //     .axisLessThan(Axis.kLeftY.value, -0.5)
     //     .and(() -> fineControl)
     //     .whileTrue(
     //

src/main/java/frc/robot/subsystems/hood/ShooterHood.java

@@ -59,6 +59,8 @@ public class ShooterHood extends SubsystemBase {
 
   private Supplier<Boolean> shouldLowerHoodSupplier;
 
+  private Angle manualOffset = Degrees.of(0);
+
   /** Initializes the motor and status signal */
   public ShooterHood(Supplier<Boolean> shouldLowerHoodSupplier) {
     hoodMotor =
@@ -215,6 +217,7 @@ public void periodic() {
         profileReferenceSignal,
         hallSensorTriggeredSignal);
     DogLog.log("Hood/Position", getPosition().in(Degrees), Degrees);
+    DogLog.log("Hood/ManualOffsetDegs", manualOffset);
     DogLog.log("Hood/Velocity", getVelocity());
     DogLog.log("Hood/Acceleration", getAcceleration());
     DogLog.log("Hood/AppliedVoltage", getAppliedVoltage());
@@ -325,6 +328,17 @@ public boolean isHallSensorTriggered() {
     return hallSensorTriggeredSignal.getValue();
   }
 
+  public void setOffsetAngle(Angle newOffset) {
+    manualOffset = newOffset;
+  }
+
+  /**
+   * Manual correction to shooter hood if auto shoot is consistently off
+   *
+   * @param targetAngle
+   * @return
+   */
+
   /**
    * Returns a command that moves the hood to the given target angle.
    *
@@ -355,7 +369,8 @@ public Command moveTo(Angle targetAngle) {
   public Command moveTo(Supplier<Angle> targetAngleSupplier) {
     return runEnd(
         () -> {
-          Angle clampedAngle = clampPositionToSafeRange(targetAngleSupplier.get());
+          Angle clampedAngle =
+              clampPositionToSafeRange(targetAngleSupplier.get().plus(manualOffset));
           DogLog.log("Hood/TargetPosition", clampedAngle.in(Degrees));
           setPositionControl(clampedAngle);
         },
@@ -467,7 +482,7 @@ public void initialize() {
 
           @Override
           public void execute() {
-            Angle unclampedTargetPosition = targetAngleSupplier.get();
+            Angle unclampedTargetPosition = targetAngleSupplier.get().plus(manualOffset);
             AngularVelocity targetVelocity = targetVelocitySupplier.get();
 
             if (unclampedTargetPosition == null) return;

src/main/java/frc/robot/subsystems/turret/Turret.java

@@ -70,6 +70,8 @@ public class Turret extends SubsystemBase {
    */
   private boolean isZeroed = false;
 
+  private Angle manualOffset = Degrees.of(0);
+
   /**
    * Indicates whether the hall sensor has been triggered. Used to detect when the turret has
    * finished being zeroed.
@@ -235,6 +237,7 @@ public void periodic() {
 
     // Log all status signals and the current control request to NetworkTables
     DogLog.log("Turret/Position", getPosition());
+    DogLog.log("Turret/ManualOffsetDegs", manualOffset);
     DogLog.forceNt.log("Turret/PositionDegrees", getPosition().in(Degrees), Degrees);
     DogLog.log("Turret/Velocity", getVelocity());
     DogLog.log("Turret/Acceleration", getAcceleration());
@@ -426,6 +429,15 @@ private Angle clampPositionToSafeRange(Angle position) {
     }
   }
 
+  /**
+   * Commands the manual offset to run once. It sets the manual offset to the provided angle.
+   * Afterwards, it moves the turret to the position that had the manual offset for improved
+   * accuracy.
+   */
+  public void setOffsetAngle(Angle newOffset) {
+    manualOffset = newOffset;
+  }
+
   /**
    * Optimizes a target angle to take the shortest path from the current angle, while also ensuring
    * the result is within the safe range defined by the given minimum and maximum angles.
@@ -467,7 +479,7 @@ public Command moveTo(Angle targetAngle) {
               Angle optimizedTargetPosition =
                   clampPositionToSafeRange(
                       optimizeTarget(
-                          clampPositionToSafeRange(targetAngle),
+                          clampPositionToSafeRange(targetAngle.plus(manualOffset)),
                           getPosition(),
                           TurretConstants.MIN_ANGLE,
                           TurretConstants.MAX_ANGLE));
@@ -525,7 +537,7 @@ public void execute() {
             Angle targetPosition =
                 clampPositionToSafeRange(
                     optimizeTarget(
-                        unoptimizedTargetAngle,
+                        unoptimizedTargetAngle.plus(manualOffset),
                         getPosition(),
                         TurretConstants.MIN_ANGLE,
                         TurretConstants.MAX_ANGLE));
@@ -574,7 +586,7 @@ public Command moveTo(Supplier<Angle> targetAngleSupplier) {
               Angle optimizedTargetPosition =
                   clampPositionToSafeRange(
                       optimizeTarget(
-                          targetAngleSupplier.get(),
+                          targetAngleSupplier.get().plus(manualOffset),
                           getPosition(),
                           TurretConstants.MIN_ANGLE,
                           TurretConstants.MAX_ANGLE));