From 5c41766a998dae2ab54da4caf0390a007518808f Mon Sep 17 00:00:00 2001 From: Tyler Seiford <50031864+TylerSeiford@users.noreply.github.com> Date: Mon, 3 Feb 2025 20:33:56 -0600 Subject: [PATCH 1/2] more elevator and updated drive constants (cherry picked from commit 2b1f57132d5f549ea7f6677324a66141b8b5aa7e) --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/subsystems/drive/DriveConstants.java | 10 ++++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c35d35..5e78b39 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1c829f2..46d5164 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -35,21 +35,23 @@ public class DriveConstants { }; // Zeroed rotation values for each module, see setup instructions - public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0); - public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0); - public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0); - public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0); + public static final Rotation2d frontLeftZeroRotation = Rotation2d.fromDegrees(-90.0); + public static final Rotation2d frontRightZeroRotation = Rotation2d.fromDegrees(0.0); + public static final Rotation2d backLeftZeroRotation = Rotation2d.fromDegrees(180.0); + public static final Rotation2d backRightZeroRotation = Rotation2d.fromDegrees(90.0); // Device CAN IDs public static final int gyroCanId = 9; public static final int frontLeftDriveCanId = 1; public static final int backLeftDriveCanId = 5; + public static final int frontRightDriveCanId = 3; public static final int backRightDriveCanId = 7; public static final int frontLeftTurnCanId = 2; public static final int backLeftTurnCanId = 6; + public static final int frontRightTurnCanId = 4; public static final int backRightTurnCanId = 8; From 2c1535319ee683e11848fbe43c3cb80f04abc91f Mon Sep 17 00:00:00 2001 From: Tyler Seiford <50031864+TylerSeiford@users.noreply.github.com> Date: Mon, 3 Feb 2025 20:34:15 -0600 Subject: [PATCH 2/2] more elevator and offset gyro 180 (cherry picked from commit 0239aac54832dcc9ae62dfaa579619a91837d22e) --- src/main/java/frc/robot/subsystems/drive/Drive.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 60b0076..c7e7855 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -56,6 +56,7 @@ public class Drive extends SubsystemBase { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; + private final Rotation2d gyroOffSet = Rotation2d.fromDegrees(180); private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); @@ -167,7 +168,7 @@ public void periodic() { // Update gyro angle if (gyroInputs.connected) { // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; + rawGyroRotation = gyroInputs.odometryYawPositions[i].plus(gyroOffSet); } else { // Use the angle delta from the kinematics and module deltas Twist2d twist = kinematics.toTwist2d(moduleDeltas);