Switch gyro to NavX

Team-4795 · Sep 29, 2024 · 69724b2 · 69724b2
1 parent 89f30bb
commit 69724b2
Show file tree

Hide file tree

Showing 6 changed files with 109 additions and 40 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -24,7 +24,7 @@
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
-import frc.robot.subsystems.drive.GyroIOPigeon2;
+import frc.robot.subsystems.drive.GyroIONavX;
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOSparkMax;
@@ -61,7 +61,7 @@ public RobotContainer() {
         // Real robot, instantiate hardware IO implementations
         drive =
             Drive.initialize(
-                new GyroIOPigeon2(),
+                new GyroIONavX(),
                 new ModuleIOSparkMax(0),
                 new ModuleIOSparkMax(1),
                 new ModuleIOSparkMax(2),

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -61,7 +61,8 @@ public class Drive extends SubsystemBase {
   private final SysIdRoutine sysId;
   private final MutableMeasure<Voltage> m_appliedVoltage = MutableMeasure.ofBaseUnits(0, Volts);
   private final MutableMeasure<Angle> m_position = MutableMeasure.ofBaseUnits(0, Radians);
-  private final MutableMeasure<Velocity<Angle>> m_velocity = MutableMeasure.ofBaseUnits(0, RadiansPerSecond);
+  private final MutableMeasure<Velocity<Angle>> m_velocity =
+      MutableMeasure.ofBaseUnits(0, RadiansPerSecond);
 
   private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
   private Rotation2d rawGyroRotation = new Rotation2d();
@@ -136,7 +137,8 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
                   log.motor("driveSparkMax")
                       .voltage(m_appliedVoltage.mut_replace(inputs.driveAppliedVolts, Volts))
                       .angularPosition(m_position.mut_replace(inputs.drivePositionRad, Radians))
-                      .angularVelocity(m_velocity.mut_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond));
+                      .angularVelocity(
+                          m_velocity.mut_replace(inputs.driveVelocityRadPerSec, RadiansPerSecond));
                 },
                 this));
   }

diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
@@ -0,0 +1,29 @@
+package frc.robot.subsystems.drive;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.SPI;
+
+public class GyroIONavX implements GyroIO {
+  private final AHRS gyro = new AHRS(SPI.Port.kMXP);
+
+  public GyroIONavX() {
+    reset();
+  }
+
+  @Override
+  public void updateInputs(GyroIOInputs inputs) {
+    inputs.connected = gyro.isConnected();
+    inputs.yawPosition = gyro.getRotation2d();
+    inputs.yawVelocityRadPerSec = Units.degreesToRadians(gyro.getRate());
+  }
+
+  @Override
+  public void reset() {
+    gyro.reset();
+  }
+
+  public void setOffset(double offset) {
+    gyro.setAngleAdjustment(offset);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -22,8 +22,6 @@
 import frc.robot.Constants;
 import org.littletonrobotics.junction.Logger;
 
-import com.ctre.phoenix.sensors.Pigeon2;
-
 public class Module {
   private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0);
 
@@ -102,7 +100,7 @@ public void periodic() {
             driveFeedforward.calculate(velocityRadPerSec)
                 + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec));
 
-         // If Talon use:   io.runTalonPID(Units.radiansToRotations(velocityRadPerSec));
+        // If Talon use:   io.runTalonPID(Units.radiansToRotations(velocityRadPerSec));
       }
     }
   }

diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -22,7 +22,6 @@
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
@@ -40,7 +39,7 @@
  * "/Drive/ModuleX/TurnAbsolutePositionRad"
  */
 public class ModuleIOTalonFX implements ModuleIO {
-final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0);
+  final VelocityVoltage m_request = new VelocityVoltage(0).withSlot(0);
   // Gear ratios for SDS MK4i L2, adjust as necessary
   private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
   private static final double TURN_GEAR_RATIO = 150.0 / 7.0;
@@ -83,7 +82,7 @@ public ModuleIOTalonFX(int index) {
       default:
         throw new RuntimeException("Invalid module index");
     }
-    
+
     turnSparkMax.restoreFactoryDefaults();
 
     turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians
@@ -114,26 +113,27 @@ public ModuleIOTalonFX(int index) {
     driveTalonFX.clearStickyFaults();
 
     StatusCode status = StatusCode.StatusCodeNotInitialized;
-        for (int i = 0; i < 5; i++) {
-            status = driveTalonFX.getConfigurator().apply(config);
-            if (status.isOK()) break;
-        }
-
-        if (!status.isOK()) {
-            System.out.println(
-                    "Talon ID "
-                            + driveTalonFX.getDeviceID()
-                            + " failed config with error "
-                            + status.toString());
-        }
+    for (int i = 0; i < 5; i++) {
+      status = driveTalonFX.getConfigurator().apply(config);
+      if (status.isOK()) break;
+    }
+
+    if (!status.isOK()) {
+      System.out.println(
+          "Talon ID "
+              + driveTalonFX.getDeviceID()
+              + " failed config with error "
+              + status.toString());
+    }
   }
 
   @Override
   public void updateInputs(ModuleIOInputs inputs) {
     inputs.drivePositionRad =
         Units.rotationsToRadians(driveTalonFX.getPosition().getValueAsDouble()) / DRIVE_GEAR_RATIO;
     inputs.driveVelocityRadPerSec =
-        Units.rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble()) / DRIVE_GEAR_RATIO;
+        Units.rotationsPerMinuteToRadiansPerSecond(driveTalonFX.getVelocity().getValueAsDouble())
+            / DRIVE_GEAR_RATIO;
     inputs.driveAppliedVolts = driveTalonFX.getMotorVoltage().getValueAsDouble();
     inputs.driveCurrentAmps = new double[] {driveTalonFX.getSupplyCurrent().getValueAsDouble()};
 
@@ -146,25 +146,25 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
     inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()};
   }
-  
+
   private TalonFXConfiguration config() {
-        var talonFXConfig = new TalonFXConfiguration();
+    var talonFXConfig = new TalonFXConfiguration();
 
-        talonFXConfig.Slot0.kP = 1;
-        talonFXConfig.Slot0.kI = 0;
-        talonFXConfig.Slot0.kD = 0;
-        talonFXConfig.Slot0.kS = 0;
-        talonFXConfig.Slot0.kV = 0;
+    talonFXConfig.Slot0.kP = 1;
+    talonFXConfig.Slot0.kI = 0;
+    talonFXConfig.Slot0.kD = 0;
+    talonFXConfig.Slot0.kS = 0;
+    talonFXConfig.Slot0.kV = 0;
 
-        talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true;
-        talonFXConfig.CurrentLimits.StatorCurrentLimit = 40; //CurrentLimits.drive;
+    talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+    talonFXConfig.CurrentLimits.StatorCurrentLimit = 40; // CurrentLimits.drive;
 
-        talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
 
-        talonFXConfig.Audio.BeepOnBoot = true;
+    talonFXConfig.Audio.BeepOnBoot = true;
 
-        return talonFXConfig;
-    }
+    return talonFXConfig;
+  }
 
   @Override
   public void setDriveVoltage(double volts) {
@@ -181,10 +181,10 @@ public void setTurnVoltage(double volts) {
     turnSparkMax.setVoltage(volts);
   }
 
-//   @Override
-//   public void setDriveBrakeMode(boolean enable) {
-//     driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
-//   }
+  //   @Override
+  //   public void setDriveBrakeMode(boolean enable) {
+  //     driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+  //   }
 
   @Override
   public void setTurnBrakeMode(boolean enable) {

diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json
@@ -0,0 +1,40 @@
+{
+  "fileName": "NavX.json",
+  "name": "NavX",
+  "version": "2024.1.0",
+  "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+  "frcYear": "2024",
+  "mavenUrls": [
+    "https://dev.studica.com/maven/release/2024/"
+  ],
+  "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
+  "javaDependencies": [
+    {
+      "groupId": "com.kauailabs.navx.frc",
+      "artifactId": "navx-frc-java",
+      "version": "2024.1.0"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "com.kauailabs.navx.frc",
+      "artifactId": "navx-frc-cpp",
+      "version": "2024.1.0",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": false,
+      "libName": "navx_frc",
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxraspbian",
+        "linuxarm32",
+        "linuxarm64",
+        "linuxx86-64",
+        "osxuniversal",
+        "windowsx86-64"
+      ]
+    }
+  ]
+}