fixed problems with base unit conversions in the auto-aim command

Signed-off-by: a1cd <71281043+a1cd@users.noreply.github.com>
DurhamAcademy · Mar 26, 2024 · b85c189 · b85c189
1 parent 428246e
commit b85c189
Showing 1 changed file with 4 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -154,7 +154,7 @@ public static CommandAndReadySupplier aimAtSpeakerCommand(
 
         final Pose2d[] previousPose = {null};
         ProfiledPIDController rotationController =
-                new ProfiledPIDController(.5, 0, 0, new TrapezoidProfile.Constraints(1, 2));
+                new ProfiledPIDController(.5, 0, 0, new TrapezoidProfile.Constraints(RotationsPerSecond.of(1), RotationsPerSecond.per(Second).of(2)));
 
         LoggedDashboardBoolean invertVelocity = new LoggedDashboardBoolean("Disable Velocity", false);
 
@@ -189,17 +189,6 @@ public static CommandAndReadySupplier aimAtSpeakerCommand(
                                 } else movingWhileShootingTarget = targetPose;
                             } else movingWhileShootingTarget = ShooterCommands.getSpeakerPos().toPose2d();
                             Logger.recordOutput("speakerAimTargetPose", movingWhileShootingTarget);
-                    /*
-
-                    |--------|
-                    |-------|
-                    |------|
-                    |----|
-                    |---|
-                    |--|
-                    |-|*
-                    |=>
-                     */
 
 
                             Measure<Velocity<Angle>> goalAngleVelocity = null;
@@ -224,9 +213,9 @@ public static CommandAndReadySupplier aimAtSpeakerCommand(
                                     new TrapezoidProfile.State(
                                             Radians.of(goalAngle.getRadians()), goalAngleVelocity));
                             var value = rotationController.calculate(
-                                    inputModulus(drive.getPose().getRotation().getRotations(), 0, 1),
+                                    inputModulus(Rotations.toBaseUnits(drive.getPose().getRotation().getRotations()), Rotations.zero().baseUnitMagnitude(), Rotation.one().baseUnitMagnitude()),
 //                            new TrapezoidProfile.State(
-                                    inputModulus(goalAngle.getRotations(), 0, 1)
+                                    inputModulus(Rotations.toBaseUnits(goalAngle.getRotations()), Rotations.zero().baseUnitMagnitude(), Rotation.one().baseUnitMagnitude())
 //                                    goalAngleVelocity.in(RotationsPerSecond)*.0025
 //                            )
                             );
@@ -245,7 +234,7 @@ public static CommandAndReadySupplier aimAtSpeakerCommand(
                                             linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                                             linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
 
-                                            (rotationController.getSetpoint().velocity+value)*6.28,
+                                            RadiansPerSecond.fromBaseUnits(rotationController.getSetpoint().velocity + value),
                                             drive.getRotation().rotateBy(
                                                     getAllianceRotation())));
                             previousPose[0] = drive.getPose();