more elevator and offset gyro 180

firebears-frc · Feb 4, 2025 · 0239aac · 0239aac
1 parent 2b1f571
commit 0239aac
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git 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
@@ -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) {