Adapt code for rev1 robot

AdvantageScope 1-20-2024.json → AdvantageScope 1-26-2024.json

@@ -121,12 +121,12 @@
           "fields": [
             {
               "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/speed",
-              "color": "#e5b31b",
+              "color": "#2b66a2",
               "show": false
             },
             {
               "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/speed",
-              "color": "#af2437",
+              "color": "#e5b31b",
               "show": false
             }
           ]
@@ -143,7 +143,7 @@
           "fields": [
             {
               "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value",
-              "color": "#2b66a2",
+              "color": "#af2437",
               "show": true
             },
             {
@@ -178,6 +178,12 @@
             "key": "NT:/AdvantageKit/RealOutputs/Drivetrain/TrajectorySetpoint",
             "sourceTypeIndex": 1,
             "sourceType": "Pose2d"
+          },
+          {
+            "type": "Vision Target",
+            "key": "NT:/AdvantageKit/RealOutputs/Vision/TagPoses2D",
+            "sourceTypeIndex": 2,
+            "sourceType": "Pose2d[]"
           }
         ]
       ],
@@ -198,7 +204,14 @@
       "type": 6,
       "fields": [],
       "listFields": [
-        [],
+        [
+          {
+            "type": "Vision Target",
+            "key": "NT:/AdvantageKit/RealOutputs/Vision/TagPoses",
+            "sourceTypeIndex": 2,
+            "sourceType": "Pose3d[]"
+          }
+        ],
         [
           {
             "type": "Robot",
@@ -217,6 +230,12 @@
             "key": "NT:/AdvantageKit/RealOutputs/Drivetrain/TrajectorySetpoint",
             "sourceTypeIndex": 1,
             "sourceType": "Pose2d"
+          },
+          {
+            "type": "Green Ghost",
+            "key": "NT:/AdvantageKit/RealOutputs/Vision/EstimatedPoses",
+            "sourceTypeIndex": 2,
+            "sourceType": "Pose2d[]"
           }
         ]
       ],
@@ -231,6 +250,25 @@
       },
       "configHidden": false,
       "title": "3D Field"
+    },
+    {
+      "type": 8,
+      "fields": [],
+      "listFields": [],
+      "options": {
+        "ids": [
+          0,
+          1,
+          2
+        ],
+        "layouts": [
+          "Xbox Controller (White)",
+          "None",
+          "None"
+        ]
+      },
+      "configHidden": false,
+      "title": "Joysticks"
     }
   ]
 }

src/main/java/frc/robot/Constants.java

@@ -1,7 +1,7 @@
 package frc.robot;
 
 public final class Constants {
-  public static final Mode kCurrentMode = Mode.kSim;
+  public static final Mode kCurrentMode = Mode.kReal;
   public static final RobotType kRobot = RobotType.kPractice;
   public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim);
 

src/main/java/frc/robot/Robot.java

@@ -55,7 +55,7 @@ public Robot() {
     switch (Constants.kCurrentMode) {
       case kReal:
         m_drivetrain = Subsystems.createTalonFXDrivetrain();
-        m_vision = Subsystems.createFourCameraVision();
+        m_vision = Subsystems.createBlankFourCameraVision();
         break;
       case kSim:
         m_drivetrain = Subsystems.createTalonFXDrivetrain();
@@ -72,8 +72,8 @@ public Robot() {
             () -> -m_driverController.getLeftY(),
             () -> -m_driverController.getLeftX(),
             // This needs to be getRawAxis(2) when using sim on a Mac
-            () -> -m_driverController.getRawAxis(2)));
-    m_driverController.y().onTrue(m_drivetrain.zeroRotation());
+            () -> -m_driverController.getRightX()));
+    m_driverController.y().onTrue(m_drivetrain.zeroGyro());
 
     m_autoChooser = new LoggedDashboardChooser<>("Auto Chooser", AutoBuilder.buildAutoChooser());
     m_autoChooser.addOption(

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -45,7 +45,7 @@ public class Drivetrain extends SubsystemBase {
   private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16.5);
   private final double kMaxAngularSpeedRadPerSec = 2 * Math.PI;
   private final double kDeadband = 0.05;
-  private final boolean kUseVisionCorrection = true;
+  private final boolean kUseVisionCorrection = false;
 
   public static final Lock odometryLock = new ReentrantLock();
   private final ImuIO m_imuIO;
@@ -153,7 +153,9 @@ public void periodic() {
       }
       if (Constants.kIsSim) {
         Logger.recordOutput("Drivetrain/dtheta", m_kinematics.toTwist2d(moduleDeltas).dtheta);
-        m_imuIO.updateSim(m_kinematics.toTwist2d(moduleDeltas).dtheta);
+        // TODO we cannot update the gyro sim state in the same loop we read it, it will always be
+        // behind causing inaccurate behavior
+        // m_imuIO.updateSim(m_kinematics.toTwist2d(moduleDeltas).dtheta);
       }
 
       // The reason we are bothering with timestamps here is so vision updates can be properly
@@ -166,6 +168,9 @@ public void periodic() {
   }
 
   private void runVelocity(ChassisSpeeds speeds) {
+    if (Constants.kIsSim) {
+      m_imuIO.updateSim(speeds.omegaRadiansPerSecond * 0.02);
+    }
     ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
     SwerveModuleState[] setpointStates = m_kinematics.toSwerveModuleStates(discreteSpeeds);
     SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, kMaxLinearSpeedMetersPerSecond);
@@ -208,9 +213,8 @@ public Command joystickDrive(
         .withName("Joystick Drive");
   }
 
-  public Command zeroRotation() {
-    return Commands.runOnce(() -> setPose(new Pose2d(getPose().getTranslation(), new Rotation2d())))
-        .ignoringDisable(true);
+  public Command zeroGyro() {
+    return Commands.runOnce(() -> m_imuIO.setGyroAngle(0.0)).ignoringDisable(true);
   }
 
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {

src/main/java/frc/robot/drivetrain/ImuIO.java

@@ -15,5 +15,7 @@ public static class ImuIOInputs {
 
   public default void updateInputs(ImuIOInputs inputs) {}
 
+  public default void setGyroAngle(double angleRad) {}
+
   public default void updateSim(double dThetaRad) {}
 }

src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java

@@ -12,7 +12,7 @@
 import java.util.Queue;
 
 public class ImuIOPigeon2 implements ImuIO {
-  private final Pigeon2 m_imu = new Pigeon2(1, "CANivore1");
+  private final Pigeon2 m_imu = new Pigeon2(20, "CANivore1");
   private final StatusSignal<Double> yaw = m_imu.getYaw();
   private final Queue<Double> m_yawPositionQueue, m_yawTimestampQueue;
   private final StatusSignal<Double> yawVelocity = m_imu.getAngularVelocityZWorld();
@@ -43,6 +43,10 @@ public void updateInputs(ImuIOInputs inputs) {
     m_yawTimestampQueue.clear();
   }
 
+  public void setGyroAngle(double angleRad) {
+    m_imu.setYaw(angleRad);
+  }
+
   public void updateSim(double dThetaRad) {
     final Pigeon2SimState imuSimState = m_imu.getSimState();
     imuSimState.setSupplyVoltage(RobotController.getBatteryVoltage());

src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java

@@ -31,6 +31,7 @@ public class ModuleIOTalonFX implements ModuleIO {
   private final double kDrivekV = 0.1;
   private final double kSteerkP = 100.0;
   private final double kSteerkD = 0.2;
+  private final double kSlipCurrent = 400.0;
   private final String kCanBusName = "CANivore1";
 
   private final TalonFX m_driveMotor, m_steerMotor;
@@ -51,37 +52,37 @@ public class ModuleIOTalonFX implements ModuleIO {
   private final VelocityVoltage m_driveControl;
 
   private final DCMotorSim m_driveSim =
-      new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kDriveRatio, 0.025);
+      new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kDriveRatio, 0.001);
   private final DCMotorSim m_steerSim =
-      new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kSteerRatio, 0.004);
+      new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kSteerRatio, 0.00001);
   private final int m_index;
 
   public ModuleIOTalonFX(int index) {
     m_index = index;
     switch (index) {
-      case 0:
-        m_driveMotor = new TalonFX(1, kCanBusName);
-        m_steerMotor = new TalonFX(2, kCanBusName);
-        m_azimuthEncoder = new CANcoder(1, kCanBusName);
-        m_absoluteEncoderMagnetOffset = 0.100830078125;
+      case 0: // FL
+        m_driveMotor = new TalonFX(4, kCanBusName);
+        m_steerMotor = new TalonFX(3, kCanBusName);
+        m_azimuthEncoder = new CANcoder(12, kCanBusName);
+        m_absoluteEncoderMagnetOffset = -0.380126953125;
         break;
-      case 1:
-        m_driveMotor = new TalonFX(3, kCanBusName);
-        m_steerMotor = new TalonFX(4, kCanBusName);
-        m_azimuthEncoder = new CANcoder(2, kCanBusName);
-        m_absoluteEncoderMagnetOffset = 0.028076171875;
+      case 1: // FR
+        m_driveMotor = new TalonFX(6, kCanBusName);
+        m_steerMotor = new TalonFX(5, kCanBusName);
+        m_azimuthEncoder = new CANcoder(13, kCanBusName);
+        m_absoluteEncoderMagnetOffset = -0.196533203125;
         break;
-      case 2:
-        m_driveMotor = new TalonFX(5, kCanBusName);
-        m_steerMotor = new TalonFX(6, kCanBusName);
-        m_azimuthEncoder = new CANcoder(3, kCanBusName);
-        m_absoluteEncoderMagnetOffset = -0.166259765625;
+      case 2: // BL
+        m_driveMotor = new TalonFX(2, kCanBusName);
+        m_steerMotor = new TalonFX(1, kCanBusName);
+        m_azimuthEncoder = new CANcoder(11, kCanBusName);
+        m_absoluteEncoderMagnetOffset = 0.265380859375;
         break;
-      case 3:
+      case 3: // BR
         m_driveMotor = new TalonFX(7, kCanBusName);
         m_steerMotor = new TalonFX(8, kCanBusName);
-        m_azimuthEncoder = new CANcoder(4, kCanBusName);
-        m_absoluteEncoderMagnetOffset = -0.173583984375;
+        m_azimuthEncoder = new CANcoder(14, kCanBusName);
+        m_absoluteEncoderMagnetOffset = 0.314208984375;
         break;
       default:
         throw new RuntimeException("Invalid module index");
@@ -102,6 +103,10 @@ public ModuleIOTalonFX(int index) {
     driveMotorConfig.Slot0.kP = kDrivekP;
     driveMotorConfig.Slot0.kS = kDrivekS;
     driveMotorConfig.Slot0.kV = kDrivekV;
+    driveMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = kSlipCurrent;
+    driveMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -kSlipCurrent;
+    driveMotorConfig.CurrentLimits.StatorCurrentLimit = kSlipCurrent;
+    driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true;
     m_driveMotor.getConfigurator().apply(driveMotorConfig);
     m_driveMotor.setPosition(0.0);