FIX: Abandoned talonfx sims for wpilib motor

Author: dabeycorn

src/main/java/wmironpatriots/subsystems/Swerve/module/Module.java

@@ -40,7 +40,10 @@ public SwerveModuleState setSetpoints(SwerveModuleState setpointState) {
     setpointState.optimize(getRotation2d());
 
     // Decrease drive speed based on distance to angle setpoint (reduces thread wear)
-    setpointState.speedMetersPerSecond *= setpointState.angle.minus(getRotation2d()).getCos();
+    setpointState.cosineScale(getRotation2d());
+    // setpointState.speedMetersPerSecond *= setpointState.angle.minus(getRotation2d()).getCos();
+
+    // System.out.println(setpointState.speedMetersPerSecond);
 
     hardware.setDriveSetpointSpeed(setpointState.speedMetersPerSecond);
     hardware.setPivotSetpointPose(setpointState.angle.getRotations());
@@ -57,7 +60,7 @@ public void stop() {
    * @return {@link Rotation2d} representing the angle of the module
    */
   public Rotation2d getRotation2d() {
-    return Rotation2d.fromRotations(loggableState.cancoderRevs());
+    return Rotation2d.fromRotations(loggableState.pivotRevs());
   }
 
   /**

src/main/java/wmironpatriots/subsystems/Swerve/module/ModuleHardwareComp.java

@@ -6,6 +6,8 @@
 
 package wmironpatriots.subsystems.Swerve.module;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -20,6 +22,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import lib.utils.TalonFxUtil;
+import wmironpatriots.subsystems.Swerve.SwerveConstants;
 import wmironpatriots.subsystems.Swerve.SwerveConstants.ModuleConfig;
 
 /**
@@ -34,6 +37,9 @@
  * <p>CANcoder absolute encoder
  */
 public class ModuleHardwareComp implements ModuleHardware {
+  public static final double PIVOT_REDUCTION = 150 / 7;
+  public static final double DRIVE_REDUCTION = 6.12;
+
   private final int index;
 
   protected final TalonFX pivot, drive;
@@ -80,7 +86,7 @@ public ModuleHardwareComp(ModuleConfig moduleConfig) {
     pivotCfg.ClosedLoopGeneral.ContinuousWrap = true;
     pivotCfg.Feedback.FeedbackRemoteSensorID = moduleConfig.encoderId().getId();
     pivotCfg.Feedback.FeedbackRotorOffset = moduleConfig.encoderOffsetRevs();
-    pivotCfg.Feedback.RotorToSensorRatio = 0.0; // TODO
+    pivotCfg.Feedback.RotorToSensorRatio = PIVOT_REDUCTION;
     pivotCfg.Feedback.SensorToMechanismRatio = 0.0;
     pivotCfg.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
 
@@ -109,7 +115,8 @@ public ModuleHardwareComp(ModuleConfig moduleConfig) {
     driveCfg.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.02;
 
     driveCfg.ClosedLoopGeneral.ContinuousWrap = true;
-    driveCfg.Feedback.SensorToMechanismRatio = 0.0; // TODO
+    driveCfg.Feedback.SensorToMechanismRatio =
+        DRIVE_REDUCTION / (SwerveConstants.WHEEL_RADIUS.in(Meters) * 2 * Math.PI);
     driveCfg.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
 
     driveCfg.Slot0.kP = 35.0;

src/main/java/wmironpatriots/subsystems/Swerve/module/ModuleHardwareSim.java

@@ -9,116 +9,50 @@
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Seconds;
 
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
-import com.ctre.phoenix6.controls.VoltageOut;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.sim.ChassisReference;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import wmironpatriots.Constants;
 import wmironpatriots.subsystems.Swerve.SwerveConstants;
 import wmironpatriots.subsystems.Swerve.SwerveConstants.ModuleConfig;
 
 public class ModuleHardwareSim implements ModuleHardware {
+  public static final double PIVOT_REDUCTION = 150 / 7;
+  public static final double DRIVE_REDUCTION = 6.12;
+
   private final int index;
 
   private final DCMotor pivotModel = DCMotor.getKrakenX60Foc(1);
   private final DCMotor driveModel = DCMotor.getKrakenX60Foc(1);
 
   private final DCMotorSim pivotSim, driveSim;
-  private final TalonFX drive;
-  private final TalonFXConfiguration driveCfg;
-
-  private final VoltageOut voltReq = new VoltageOut(0.0);
-  private final VelocityTorqueCurrentFOC velReq = new VelocityTorqueCurrentFOC(0.0);
+  private double pivotAppliedVolts, driveAppliedVolts;
 
-  private double lastUpdateTimestamp = 0.0;
-  private double pivotAppliedVolts = 0.0;
-
-  private final PIDController pivotFeedback = new PIDController(1, 0.0, 0.0);
+  private PIDController pivotFeedback = new PIDController(50.0, 0.0, 0.0);
+  private PIDController driveFeedback = new PIDController(3.2, 0.0, 0.0);
+  private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.233, 2.02, 0.05);
 
   public ModuleHardwareSim(ModuleConfig moduleConfig) {
     index = moduleConfig.index();
 
     pivotSim =
         new DCMotorSim(
-            LinearSystemId.createDCMotorSystem(pivotModel, 0.004, 21.428571428571427),
-            pivotModel,
-            0.0,
-            0.0);
+            LinearSystemId.createDCMotorSystem(pivotModel, 0.004, 150 / 7), pivotModel, 0.0, 0.0);
 
     driveSim =
         new DCMotorSim(
-            LinearSystemId.createDCMotorSystem(driveModel, 0.025, 6.122448979591837),
-            driveModel,
-            0.0,
-            0.0);
-
-    drive = new TalonFX(moduleConfig.driveId().getId(), moduleConfig.driveId().getBusName());
-
-    // Drive Configs
-    driveCfg = new TalonFXConfiguration();
-    // Current limits
-    driveCfg.CurrentLimits.SupplyCurrentLimit = 40.0;
-    driveCfg.CurrentLimits.SupplyCurrentLimitEnable = true;
-    driveCfg.CurrentLimits.StatorCurrentLimit = 120.0;
-    driveCfg.CurrentLimits.StatorCurrentLimitEnable = true;
-    // Inverts
-    driveCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    driveCfg.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    // Sensor
-    // Meters per second
-    driveCfg.Feedback.SensorToMechanismRatio = 6.122448979591837;
-    // Current control gains
-    // Gains copied from AlphaSwerveConstants
-    driveCfg.Slot0.kV = 5.0;
-    // kT (stall torque / stall current) converted to linear wheel frame
-    driveCfg.Slot0.kA = 0.0; // (9.37 / 483.0) / getDriveRotorToMeters(); // 3.07135116146;
-    driveCfg.Slot0.kS = 10.0;
-    driveCfg.Slot0.kP = 1.0;
-    driveCfg.Slot0.kD = 0.0; // 1.0;
-
-    driveCfg.TorqueCurrent.TorqueNeutralDeadband = 10.0;
-
-    drive.getConfigurator().apply(driveCfg);
-
-    drive.getSimState().Orientation =
-        driveCfg.MotorOutput.Inverted == InvertedValue.CounterClockwise_Positive
-            ? ChassisReference.CounterClockwise_Positive
-            : ChassisReference.Clockwise_Positive;
-  }
+            LinearSystemId.createDCMotorSystem(driveModel, 0.025, 6.12), driveModel, 0.0, 0.0);
 
-  private double addFriction(double motorVoltage, double frictionVoltage) {
-    if (Math.abs(motorVoltage) < frictionVoltage) {
-      motorVoltage = 0.0;
-    } else if (motorVoltage > 0.0) {
-      motorVoltage -= frictionVoltage;
-    } else {
-      motorVoltage += frictionVoltage;
-    }
-    return motorVoltage;
+    pivotFeedback.enableContinuousInput(0, 0.5);
   }
 
   @Override
   public LoggableState getLoggableState() {
-    var driveSimState = drive.getSimState();
-    double driveSimVolts = addFriction(driveSimState.getMotorVoltage(), 0.25);
-
-    driveSim.setInput(driveSimVolts);
-    double timestamp = RobotController.getFPGATime();
     pivotSim.update(Constants.LOOPTIME.in(Seconds));
     driveSim.update(Constants.LOOPTIME.in(Seconds));
-    lastUpdateTimestamp = timestamp;
-
-    driveSimState.setRotorVelocity(
-        (driveSim.getAngularVelocityRPM() / 60.0) * driveCfg.Feedback.SensorToMechanismRatio);
 
     return new LoggableState(
         index,
@@ -131,12 +65,31 @@ public LoggableState getLoggableState() {
         true,
         driveSim.getAngularPositionRad() * SwerveConstants.WHEEL_RADIUS.in(Meters),
         driveSim.getAngularVelocityRadPerSec() * SwerveConstants.WHEEL_RADIUS.in(Meters),
-        0.0,
-        drive.getSimState().getMotorVoltage(),
+        driveFeedback.getSetpoint(),
+        driveAppliedVolts,
         driveSim.getCurrentDrawAmps(),
         pivotSim.getTorqueNewtonMeters() / driveModel.KtNMPerAmp,
         true,
-        pivotSim.getAngularPositionRad());
+        pivotSim.getAngularPositionRotations());
+  }
+
+  private double addFriction(double motorVoltage, double frictionVoltage) {
+    if (Math.abs(motorVoltage) < frictionVoltage) {
+      motorVoltage = 0.0;
+    } else if (motorVoltage > 0.0) {
+      motorVoltage -= frictionVoltage;
+    } else {
+      motorVoltage += frictionVoltage;
+    }
+    return motorVoltage;
+  }
+
+  /**
+   * @return the drive motor speed in Meters per Second
+   */
+  private double getDriveSpeedMps() {
+    return (driveSim.getAngularVelocityRPM() / 60)
+        * (SwerveConstants.WHEEL_RADIUS.in(Meters) * 2 * Math.PI);
   }
 
   @Override
@@ -147,7 +100,8 @@ public void setPivotAppliedVolts(double volts) {
 
   @Override
   public void setDriveAppliedVolts(double volts) {
-    drive.setControl(voltReq.withOutput(volts));
+    driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+    driveSim.setInputVoltage(driveAppliedVolts);
   }
 
   @Override
@@ -157,13 +111,13 @@ public void setPivotSetpointPose(double poseRevs) {
 
   @Override
   public void setDriveSetpointSpeed(double speedMps) {
-    drive.setControl(velReq.withVelocity(speedMps));
+    setDriveAppliedVolts(driveFeedback.calculate(getDriveSpeedMps(), speedMps));
   }
 
   @Override
   public void stop() {
     pivotSim.setInputVoltage(0.0);
-    drive.stopMotor();
+    driveSim.setInputVoltage(0.0);
   }
 
   @Override