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); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index f36d572..2367ef7 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -61,7 +61,7 @@ public Elevator() { PersistMode.kPersistParameters)); } - public void setShoulderSetpoint(Rotation2d setpoint) { // change values later + public void elevatorSetpoint(Rotation2d setpoint) { // change values later if (setpoint.getDegrees() < -5) { setpoint = Rotation2d.fromDegrees(-5); } else if (setpoint.getDegrees() > 100) {