From 85cbd38cca652ec0f5deb8da3215cf24cfc5db2c Mon Sep 17 00:00:00 2001 From: CrumblzTheEgg Date: Fri, 15 Mar 2024 11:32:45 -0500 Subject: [PATCH 1/3] Fix for driver offset broken code its fixed now --- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 84069e9..f26becd 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -334,8 +334,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)); From f907dc276ba1f72d5a61f93233eca32f8bd687af Mon Sep 17 00:00:00 2001 From: Cameron Myhre Date: Fri, 15 Mar 2024 18:52:22 -0500 Subject: [PATCH 2/3] fix: Fix IMU reset command --- src/main/java/frc/robot/Constants.java | 26 ++++++++++++++----- .../vision/ResetImuWithVisionCommand.java | 13 +++++++--- 2 files changed, 29 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ae224c3..7acfe99 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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, new Rotation2d((4.0/3.0) * 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.0/3.0) * Math.PI)); + public static final Pose2d RED_1_STARTING_POSE = new Pose2d(16.238, 6.650, new Rotation2d((5.0/3.0) * 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.0/3.0) * Math.PI)); public static final List 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; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java b/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java index d5797b3..5576fcf 100644 --- a/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java +++ b/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. } @@ -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()); From 19a9b91472217a7bacf83f5e0c7a97536023e571 Mon Sep 17 00:00:00 2001 From: Cameron Myhre Date: Sat, 16 Mar 2024 07:39:09 -0500 Subject: [PATCH 3/3] fix: Fix inaccurate IMU readings --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/Constants.java | 12 ++++++------ .../vision/ResetImuWithVisionCommand.java | 16 ++++++++-------- .../frc/robot/subsystems/DriveSubsystem.java | 16 +++++++++++++++- 4 files changed, 31 insertions(+), 16 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b..337aac5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,6 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.debug.settings.onBuildFailureProceed": true } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7acfe99..194ed94 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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.0/3.0) * 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.0/3.0) * Math.PI)); - public static final Pose2d RED_1_STARTING_POSE = new Pose2d(16.238, 6.650, new Rotation2d((5.0/3.0) * 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.0/3.0) * 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 BLUE_1_TRAJECTORY = List.of( new Pose2d(2.926,7.563,new Rotation2d()), diff --git a/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java b/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java index 5576fcf..fe4cf07 100644 --- a/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java +++ b/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java @@ -78,19 +78,19 @@ 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) { @@ -98,7 +98,7 @@ private void resetGyroWithVision() { 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; @@ -114,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; } @@ -150,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 diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index f26becd..e07c085 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/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())); } /** @@ -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 */