a

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index cc48137ea4b35432f86ea09b0949f84ea1a0434f..db0800538449fe51f0f7a5f8492bf136f4d373ea 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -155,8 +155,12 @@ public class AutoShootCommand extends Command {
             turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
         lastTurretAngle = turretAngle;
 
+        Logger.recordOutput("Lookahead Pose", lookaheadPose);
+
+        double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
+
         // Shortest path
-        double error = MathUtil.inputModulus(turretAngle.getDegrees() - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
+        double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
         double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error;
         // Stay within +/- 200 -- if  shortest path is past 200, we go long way around
         double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE;
@@ -200,6 +204,7 @@ public class AutoShootCommand extends Command {
 
         SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
         SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
+        System.out.println("COMMAND IS WORKINNGGG");
 
         /** Spindexer Stuff!! */
         if(spindexer != null){

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 33ed5eb9db80da30bc77e8cfe775213dd6bb20f0..fa8b6fc1f0f638e19b15b266af5236ed960461ba 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -122,6 +122,7 @@ public class Hood extends SubsystemBase implements HoodIO{
                SmartDashboard.putData("Hood PID", positionPID);
 
                SmartDashboard.putNumber("Turret Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO);
+               Logger.processInputs("Hood", inputs);
        }
 
     @Override