fix: Fix inaccurate IMU readings

.vscode/settings.json

@@ -25,5 +25,6 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.debug.settings.onBuildFailureProceed": true
 }

src/main/java/frc/robot/Constants.java

@@ -211,12 +211,12 @@ public static final class AutoConstants {
         public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
                 MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED);
 
-        public static final Pose2d BLUE_1_STARTING_POSE =  new Pose2d(1.665, 6.636, new Rotation2d( (4/3) * Math.PI ));
-        public static final Pose2d BLUE_2_STARTING_POSE =  new Pose2d(2.362, 5.484, new Rotation2d( Math.PI ));
-        public static final Pose2d BLUE_3_STARTING_POSE =  new Pose2d(1.689, 4.451, new Rotation2d( (2/3) * Math.PI  ));
-        public static final Pose2d RED_1_STARTING_POSE =  new Pose2d(16.238, 6.650, new Rotation2d( (5/3) * Math.PI ));
-        public static final Pose2d RED_2_STARTING_POSE =  new Pose2d(15.481, 5.593, new Rotation2d( 0 ));
-        public static final Pose2d RED_3_STARTING_POSE =  new Pose2d(16.322, 4.367, new Rotation2d( (1/3) * Math.PI ));
+        public static final Pose2d BLUE_1_STARTING_POSE =  new Pose2d(1.665, 6.636, Rotation2d.fromRadians((4.0/3.0) * Math.PI));
+        public static final Pose2d BLUE_2_STARTING_POSE =  new Pose2d(2.362, 5.484, Rotation2d.fromRadians(Math.PI));
+        public static final Pose2d BLUE_3_STARTING_POSE =  new Pose2d(1.689, 4.451, Rotation2d.fromRadians((2.0/3.0) * Math.PI));
+        public static final Pose2d RED_1_STARTING_POSE =  new Pose2d(16.238, 6.650, Rotation2d.fromRadians((5.0/3.0) * Math.PI ));
+        public static final Pose2d RED_2_STARTING_POSE =  new Pose2d(15.481, 5.593, Rotation2d.fromRadians(0));
+        public static final Pose2d RED_3_STARTING_POSE =  new Pose2d(16.322, 4.367, Rotation2d.fromRadians((1.0/3.0) * Math.PI));
 
         public static final List<Pose2d> BLUE_1_TRAJECTORY = List.of(
                 new Pose2d(2.926,7.563,new Rotation2d()),
@@ -255,4 +255,18 @@ public static final class FieldConstants {
         public static final Pose2d SPEAKER_POSE = new Pose2d(0, 0, new Rotation2d());
         public static final double SPEAKER_HEIGHT = 81;
   }
+
+  public static final class IntakeConstants {
+    public static final int INTAKE = 14;
+  }
+
+  public static final class ShooterConstants {
+    public static final int SHOOTER = 13;
+  }
+
+  public static final class ArmConstants {
+    public static final int ARM1 = 11;
+    public static final int ARM2 = 12;
+    public static final int PIGEON = 15;
+  }
 }

src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.cameras.RobotVision;
@@ -26,7 +27,7 @@ public class ResetImuWithVisionCommand extends Command {
      * position than the initial state. By storing the robot's previous heading, we ensure a fallback 
      * option and maintain awareness of potential challenges.
      */
-    private double previousHeading = 0;
+    private double previousHeading = -999999999;
 
     // Keep track of whether or not the command has finished running.
     private boolean isFinished = false;
@@ -42,6 +43,9 @@ public ResetImuWithVisionCommand(DriveSubsystem driveSubsystem, RobotVision robo
         // Gain acsess to the robot's subsystems so that we are able to interact with them.
         this.driveSubsystem = driveSubsystem;
         this.robotVision = robotVision;
+
+        // Start the timer
+        timer.start();
     }
 
     @Override
@@ -50,10 +54,11 @@ public void initialize() {
         // Make it possible to run the comand again. If we don't set isFinished to false every tine
         // this command is run, then the command will only sucsessfully run once, since once it completes 
         isFinished = false;
-
+        
         // Determine whether or not the driver wants to use vision to reset the robot's gyro. 
         // This is done by checking if we pressed the back button within the last 750 milliseconds.
-        if (timer.get() > 0.75) {
+        if (timer.get() > 0.75 || previousHeading == -999999999) {
+            System.out.println("------------------------------------------------");
             resetGyro(); // We don't want to use vision to rset the robot's gyro.
         }
 
@@ -73,27 +78,27 @@ public void execute() {
     }
 
     /**
-     * Attemps to reset the robot's heading using the various april tags located around the field. If 
-     * the robot is unable acomplish this in under 250 milliseconds, then in
+     * Attempt to reset the robot's heading using the various april tags located around the field. If 
+     * the robot is unable accomplish this in under 250 milliseconds, then in
      */
     private void resetGyroWithVision() {
 
-        // Try and get an estimante of the robot's position. If we are unable to get a good estimante,
-        // then wait until we are able to get a good estimante of the robot's position. After that, then 
+        // Try and get an estimate of the robot's position. If we are unable to get a good estimate,
+        // then wait until we are able to get a good estimate of the robot's position. After that, then 
         // see if it has taken more than 250 milliseconds have passed since we started trying to reset the IMU
         // with vision.
         Pose2d estimatedRobotPose = robotVision.estimateRobotPose();
         if (estimatedRobotPose == null) {
 
-            // If more then 250 milliseconds have passed then resore the robot's previous orientation and 
+            // If more then 250 milliseconds have passed then restore the robot's previous orientation and 
             // print out an error.
             if (timer.get() > .25) {
 
                 // Restore the robot's original orientation.
                 restoreOrientation();
 
                 // Inform the user of the failure.
-                System.err.println("Unble to reset robot's IMU with vision.");
+                System.err.println("Unable to reset robot's IMU with vision.");
 
                 // Stop running the command
                 isFinished = true;
@@ -109,7 +114,7 @@ private void resetGyroWithVision() {
         // it's facing the center of the field. 
         driveSubsystem.setYaw(estimatedRobotYaw);
 
-        // Tell the robot that the command has finished runnng.
+        // Tell the robot that the command has finished running.
         isFinished = true;
     }
 
@@ -120,7 +125,7 @@ private void resetGyro() {
 
         // Store the robot's current heading in case we need to revert back to it later.
         previousHeading = driveSubsystem.getHeading();
-
+        
         // Reset the robot's gyro.
         driveSubsystem.setYaw(driveSubsystem.getDriverRotationalOffset());
 
@@ -145,7 +150,7 @@ private void restoreOrientation() {
         double updatedPreviousHeading = previousHeading + driveSubsystem.getHeading();
 
         // Revert the robot's imu's zero position back to what it was previously.
-        driveSubsystem.setYaw(Rotation2d.fromDegrees(updatedPreviousHeading));
+        driveSubsystem.setYaw(Rotation2d.fromRadians(updatedPreviousHeading));
     }
 
     @Override

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -184,6 +184,7 @@ public void periodic() {
          SmartDashboard.putNumber("Robot X: ", robotPose.getX());
          SmartDashboard.putNumber("Robot Y: ", robotPose.getY());
          SmartDashboard.putNumber("Robot Heading: ", robotPose.getRotation().getDegrees());
+         SmartDashboard.putNumber("IMU Heading: ", gyro.getAngle());
 
          // Output the current driver controlelr offset to check whether or not our code works.
          SmartDashboard.putNumber("Rotation Offset: ", driverRotationalOffset.getDegrees());
@@ -271,8 +272,12 @@ public void resetTrajectory() {
     public void setAutonomous(AutonomousOption selectedAuto) {
         setDriverRotationalOffset(selectedAuto.getTeleopRotationalOffset());
         setTrajectory(selectedAuto.getTrajectory());
-        resetOdometry(selectedAuto.getStartingPosition());
         setYaw(selectedAuto.getStartingPosition().getRotation());
+
+        // Set the robot's position to the starting location specified in the selected autonomous. We get
+        // this position independent of rotation, since the position's rotation is added to the IMU's rotation,
+        // which results in inaccurate readings
+        resetOdometry(new Pose2d(selectedAuto.getStartingPosition().getTranslation(), gyro.getRotation2d()));
     }
 
     /**
@@ -334,8 +339,8 @@ public void driveFromController(double leftX, double leftY, double rightX, doubl
         //rotates the commanded linear speed
         double oldX = leftX;
         double oldY = leftY;
-        leftX = oldX * driverRotationalOffset.getCos() - oldY * driverRotationalOffset.getSin();
-        leftX = oldY * driverRotationalOffset.getSin() + driverRotationalOffset.getCos();
+        leftX = (oldX * driverRotationalOffset.getCos()) - (oldY * driverRotationalOffset.getSin());
+        leftY = (oldX * driverRotationalOffset.getSin()) + (oldY * driverRotationalOffset.getCos());
 
         double turnJoystickOrientation = Math.atan2(rightY, rightX);
         double turnJoystickMagnitude = Math.sqrt(Math.pow(rightX, 2) + Math.pow(rightY, 2));
@@ -612,6 +617,15 @@ public void setYaw(Rotation2d newYaw) {
         gyro.setYaw(newYaw.getDegrees());
     }
 
+    /**
+     * Sets the direciton that the robot is facing to the sepecified value.
+     * 
+     * @param newYaw The direciton you want the robot to think it's facing
+     */
+    public void setYaw(double newYaw) {
+        gyro.setYaw(newYaw);
+    }
+
     /**
      * resets the heading of the robot
      */