From a3d3485c2ad9f6cd012e06b13717f2c75cb240e7 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 14 Feb 2026 11:34:56 -0800 Subject: [PATCH] a --- src/main/java/frc/robot/commands/gpm/AutoShootCommand.java | 7 ++++++- src/main/java/frc/robot/subsystems/hood/Hood.java | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index cc48137..db08005 100644 --- 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 33ed5eb..fa8b6fc 100644 --- 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 -- 2.39.5