Drive Talon?

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -61,8 +61,7 @@ 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();

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -22,6 +22,8 @@
 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);
 
@@ -44,6 +46,10 @@ public Module(ModuleIO io, int index) {
     // separate robot with different tuning)
     switch (Constants.currentMode) {
       case REAL:
+        driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
+        driveFeedback = new PIDController(0.05, 0.0, 0.0);
+        turnFeedback = new PIDController(7.0, 0.0, 0.0);
+        break;
       case REPLAY:
         driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
         driveFeedback = new PIDController(0.05, 0.0, 0.0);
@@ -95,6 +101,8 @@ public void periodic() {
         io.setDriveVoltage(
             driveFeedforward.calculate(velocityRadPerSec)
                 + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec));
+
+         // If Talon use:   io.runTalonPID(Units.radiansToRotations(velocityRadPerSec));
       }
     }
   }

src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -37,6 +37,8 @@ public default void updateInputs(ModuleIOInputs inputs) {}
   /** Run the drive motor at the specified voltage. */
   public default void setDriveVoltage(double volts) {}
 
+  public default void runTalonPID(double desiredStateRotPerSecond) {}
+
   /** Run the turn motor at the specified voltage. */
   public default void setTurnVoltage(double volts) {}
 

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -0,0 +1,193 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.drive;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.AbsoluteEncoder;
+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;
+
+/**
+ * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO
+ * or NEO 550), and analog absolute encoder connected to the RIO
+ *
+ * <p>NOTE: This implementation should be used as a starting point and adapted to different hardware
+ * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX")
+ *
+ * <p>To calibrate the absolute encoder offsets, point the modules straight (such that forward
+ * motion on the drive motor will propel the robot forward) and copy the reported values from the
+ * absolute encoders using AdvantageScope. These values are logged under
+ * "/Drive/ModuleX/TurnAbsolutePositionRad"
+ */
+public class ModuleIOTalonFX implements ModuleIO {
+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;
+
+  private final TalonFX driveTalonFX;
+  private final CANSparkFlex turnSparkMax;
+
+  private final RelativeEncoder turnRelativeEncoder;
+  private final AbsoluteEncoder turnAbsoluteEncoder;
+
+  private final boolean isTurnMotorInverted = true;
+  private final Rotation2d absoluteEncoderOffset;
+
+  public ModuleIOTalonFX(int index) {
+    switch (index) {
+      case 0:
+        driveTalonFX = new TalonFX(1);
+        turnSparkMax = new CANSparkFlex(2, MotorType.kBrushless);
+        turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        break;
+      case 1:
+        driveTalonFX = new TalonFX(3);
+        turnSparkMax = new CANSparkFlex(4, MotorType.kBrushless);
+        turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        break;
+      case 2:
+        driveTalonFX = new TalonFX(5);
+        turnSparkMax = new CANSparkFlex(6, MotorType.kBrushless);
+        turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        break;
+      case 3:
+        driveTalonFX = new TalonFX(7);
+        turnSparkMax = new CANSparkFlex(8, MotorType.kBrushless);
+        turnAbsoluteEncoder = turnSparkMax.getAbsoluteEncoder();
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        break;
+      default:
+        throw new RuntimeException("Invalid module index");
+    }
+
+    turnSparkMax.restoreFactoryDefaults();
+
+    turnAbsoluteEncoder.setPositionConversionFactor(2 * Math.PI); // Radians
+    turnAbsoluteEncoder.setVelocityConversionFactor((2 * Math.PI) / 60.0); // Radians per second
+
+    turnSparkMax.setCANTimeout(250);
+
+    turnRelativeEncoder = turnSparkMax.getEncoder();
+
+    turnSparkMax.setInverted(isTurnMotorInverted);
+    turnSparkMax.setSmartCurrentLimit(30);
+    turnSparkMax.enableVoltageCompensation(12.0);
+
+    driveTalonFX.setPosition(0);
+
+    turnRelativeEncoder.setPosition(0.0);
+    turnRelativeEncoder.setMeasurementPeriod(10);
+    turnRelativeEncoder.setAverageDepth(2);
+
+    turnSparkMax.setCANTimeout(0);
+
+    turnSparkMax.burnFlash();
+
+    var config = config();
+
+    driveTalonFX.optimizeBusUtilization();
+
+    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());
+        }
+  }
+
+  @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;
+    inputs.driveAppliedVolts = driveTalonFX.getMotorVoltage().getValueAsDouble();
+    inputs.driveCurrentAmps = new double[] {driveTalonFX.getSupplyCurrent().getValueAsDouble()};
+
+    inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnAbsoluteEncoder.getPosition());
+    inputs.turnPosition =
+        Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO);
+    inputs.turnVelocityRadPerSec =
+        Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity())
+            / TURN_GEAR_RATIO;
+    inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
+    inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()};
+  }
+
+  private TalonFXConfiguration config() {
+        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.CurrentLimits.StatorCurrentLimitEnable = true;
+        talonFXConfig.CurrentLimits.StatorCurrentLimit = 40; //CurrentLimits.drive;
+
+        talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+        talonFXConfig.Audio.BeepOnBoot = true;
+
+        return talonFXConfig;
+    }
+
+  @Override
+  public void setDriveVoltage(double volts) {
+    driveTalonFX.setVoltage(volts);
+  }
+
+  @Override
+  public void runTalonPID(double desiredStateRotPerSecond) {
+    driveTalonFX.setControl(m_request.withVelocity(desiredStateRotPerSecond));
+  }
+
+  @Override
+  public void setTurnVoltage(double volts) {
+    turnSparkMax.setVoltage(volts);
+  }
+
+//   @Override
+//   public void setDriveBrakeMode(boolean enable) {
+//     driveTalonFX.setBrakeMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+//   }
+
+  @Override
+  public void setTurnBrakeMode(boolean enable) {
+    turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
+  }
+}