finish talonmodule onboard, reorg constants, style changes

src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java

@@ -9,10 +9,14 @@
 import edu.wpi.first.units.Current;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
 import edu.wpi.first.units.Velocity;
 import java.util.List;
 
 public final class DriveConstants {
+  // Rate at which sensors update periodicially
+  public static final Measure<Time> SENSOR_PERIOD = Seconds.of(0.001);
+
   // Distance between centers of right and left wheels on robot
   public static final Measure<Distance> TRACK_WIDTH = Meters.of(0.5715);
   // Distance between front and back wheels on robot
@@ -114,10 +118,19 @@ public static final class FF {
         // a linear: 0.17586, 0.13707, 0.23915, 0.26842
         // a rotation: 0.37587, 0.20079
         // 2 has 0.55 R^2
-        public static final double S = 0.23963;
-        public static final double V = 2.0681;
-        public static final double kA_linear = 0.205;
-        public static final double kA_angular = 0.376;
+        public static final class SPARK {
+          public static final double S = 0.23963;
+          public static final double V = 2.0681;
+          public static final double kA_linear = 0.205;
+          public static final double kA_angular = 0.376;
+        }
+
+        public static final class TALON {
+          public static final double S = -1; // TODO
+          public static final double V = 2.0681;
+          public static final double kA_linear = 0.205;
+          public static final double kA_angular = 0.376;
+        }
       }
     }
 

src/main/java/org/sciborgs1155/robot/drive/ModuleIO.java

@@ -111,7 +111,10 @@ public static enum ControlMode {
    * @param angle The desired angle of the module.
    * @param voltage The voltage to supply to the drive motor.
    */
-  void updateDriveVoltage(Rotation2d angle, double voltage);
+  default void updateDriveVoltage(Rotation2d angle, double voltage) {
+    setDriveVoltage(voltage);
+    setTurnSetpoint(angle.getRadians());
+  }
 
   @Override
   void close();

src/main/java/org/sciborgs1155/robot/drive/SimModule.java

@@ -20,7 +20,7 @@ public class SimModule implements ModuleIO {
 
   private final DCMotorSim drive =
       new DCMotorSim(
-          LinearSystemId.createDCMotorSystem(Driving.FF.V, Driving.FF.kA_linear),
+          LinearSystemId.createDCMotorSystem(Driving.FF.SPARK.V, Driving.FF.SPARK.kA_linear),
           DCMotor.getNeoVortex(1),
           1 / Driving.GEARING);
   private final DCMotorSim turn =
@@ -35,9 +35,11 @@ public class SimModule implements ModuleIO {
       new PIDController(Turning.PID.SIM.P, Turning.PID.SIM.I, Turning.PID.SIM.D);
 
   private final SimpleMotorFeedforward driveTranslationFeedforward =
-      new SimpleMotorFeedforward(Driving.FF.S, Driving.FF.V, Driving.FF.kA_linear);
+      new SimpleMotorFeedforward(
+          Driving.FF.SPARK.S, Driving.FF.SPARK.V, Driving.FF.SPARK.kA_linear);
   private final SimpleMotorFeedforward driveRotationFeedforward =
-      new SimpleMotorFeedforward(Driving.FF.S, Driving.FF.V, Driving.FF.kA_angular);
+      new SimpleMotorFeedforward(
+          Driving.FF.SPARK.S, Driving.FF.SPARK.V, Driving.FF.SPARK.kA_angular);
 
   private SwerveModuleState setpoint = new SwerveModuleState();
 

src/main/java/org/sciborgs1155/robot/drive/SparkModule.java

@@ -50,7 +50,7 @@ public SparkModule(int drivePort, int turnPort, Rotation2d angularOffset, String
     drivePID = driveMotor.getPIDController();
     driveFF =
         new SimpleMotorFeedforward(
-            Driving.FF.S, Driving.FF.V, Driving.FF.kA_linear); // TODO: Re-tune
+            Driving.FF.SPARK.S, Driving.FF.SPARK.V, Driving.FF.SPARK.kA_linear); // TODO: Re-tune
 
     check(driveMotor, driveMotor.restoreFactoryDefaults());
 
@@ -202,7 +202,7 @@ public void updateSetpoint(SwerveModuleState setpoint, ControlMode mode) {
     // Scale setpoint by cos of turning error to reduce tread wear
     setpoint.speedMetersPerSecond *= setpoint.angle.minus(rotation()).getCos();
 
-    if (mode == ModuleIO.ControlMode.OPEN_LOOP_VELOCITY) {
+    if (mode == ControlMode.OPEN_LOOP_VELOCITY) {
       setDriveVoltage(driveFF.calculate(setpoint.speedMetersPerSecond));
     } else {
       setDriveSetpoint(setpoint.speedMetersPerSecond);
@@ -217,11 +217,4 @@ public void close() {
     driveMotor.close();
     turnMotor.close();
   }
-
-  @Override
-  public void updateDriveVoltage(Rotation2d angle, double voltage) {
-    setpoint.angle = angle;
-    setDriveVoltage(voltage);
-    setTurnSetpoint(angle.getRadians());
-  }
 }

src/main/java/org/sciborgs1155/robot/drive/TalonModule.java

@@ -1,35 +1,44 @@
 package org.sciborgs1155.robot.drive;
 
 import static edu.wpi.first.units.Units.*;
+import static org.sciborgs1155.lib.FaultLogger.*;
+import static org.sciborgs1155.robot.drive.DriveConstants.SENSOR_PERIOD;
 
 import com.ctre.phoenix6.StatusSignal;
 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.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkAbsoluteEncoder;
-import com.revrobotics.SparkAbsoluteEncoder.Type;
+import com.revrobotics.SparkPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import java.util.Set;
-import org.sciborgs1155.lib.FaultLogger;
 import org.sciborgs1155.lib.SparkUtils;
 import org.sciborgs1155.lib.SparkUtils.Data;
 import org.sciborgs1155.lib.SparkUtils.Sensor;
 import org.sciborgs1155.lib.TalonUtils;
+import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Driving;
 import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Turning;
 
 public class TalonModule implements ModuleIO {
   private final TalonFX driveMotor;
   private final CANSparkMax turnMotor;
-
   private final SparkAbsoluteEncoder turnEncoder;
 
+  private final SparkPIDController turnPID;
+  private final SimpleMotorFeedforward driveFF;
+
   private final StatusSignal<Double> drivePos;
-  private final StatusSignal<Double> driveVelo;
+  private final StatusSignal<Double> driveVelocity;
+
+  private final VelocityVoltage velocityOut = new VelocityVoltage(0);
 
   private SwerveModuleState setpoint = new SwerveModuleState();
 
@@ -38,40 +47,69 @@ public class TalonModule implements ModuleIO {
   public TalonModule(int drivePort, int turnPort, String name) {
     driveMotor = new TalonFX(drivePort);
     drivePos = driveMotor.getPosition();
-    driveVelo = driveMotor.getVelocity();
+    driveVelocity = driveMotor.getVelocity();
+    driveFF =
+        new SimpleMotorFeedforward(
+            Driving.FF.TALON.S, Driving.FF.TALON.V, Driving.FF.TALON.kA_linear);
+
+    drivePos.setUpdateFrequency(1 / SENSOR_PERIOD.in(Seconds));
+    driveVelocity.setUpdateFrequency(1 / SENSOR_PERIOD.in(Seconds));
 
-    drivePos.setUpdateFrequency(100);
-    driveVelo.setUpdateFrequency(100);
+    TalonFXConfiguration talonConfig = new TalonFXConfiguration();
+    talonConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    talonConfig.CurrentLimits.SupplyCurrentLimit = 80;
+    talonConfig.Slot0.kP = Driving.PID.TALON.P;
+    talonConfig.Slot1.kI = Driving.PID.TALON.I;
+    talonConfig.Slot0.kD = Driving.PID.TALON.D;
 
-    TalonFXConfiguration toApply = new TalonFXConfiguration();
-    toApply.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    toApply.CurrentLimits.SupplyCurrentLimit = 80;
-    driveMotor.getConfigurator().apply(toApply);
+    driveMotor.getConfigurator().apply(talonConfig);
 
     TalonUtils.addMotor(driveMotor);
-    resetEncoders();
 
     turnMotor = new CANSparkMax(turnPort, MotorType.kBrushless);
-    turnMotor.restoreFactoryDefaults();
-    turnMotor.setIdleMode(IdleMode.kBrake);
-    turnMotor.setSmartCurrentLimit((int) Turning.CURRENT_LIMIT.in(Amps));
+    turnEncoder = turnMotor.getAbsoluteEncoder();
+    turnPID = turnMotor.getPIDController();
 
-    turnEncoder = turnMotor.getAbsoluteEncoder(Type.kDutyCycle);
-    turnEncoder.setInverted(Turning.ENCODER_INVERTED);
-    turnEncoder.setPositionConversionFactor(Turning.POSITION_FACTOR.in(Radians));
-    turnEncoder.setVelocityConversionFactor(Turning.VELOCITY_FACTOR.in(RadiansPerSecond));
+    check(turnMotor, turnMotor.restoreFactoryDefaults());
 
-    SparkUtils.configureFrameStrategy(
-        turnMotor,
-        Set.of(Data.POSITION, Data.VELOCITY, Data.APPLIED_OUTPUT),
-        Set.of(Sensor.ABSOLUTE),
-        false);
+    check(turnMotor, turnPID.setP(Turning.PID.SPARK.P));
+    check(turnMotor, turnPID.setI(Turning.PID.SPARK.I));
+    check(turnMotor, turnPID.setD(Turning.PID.SPARK.D));
+    check(turnMotor, turnPID.setPositionPIDWrappingEnabled(true));
+    check(turnMotor, turnPID.setPositionPIDWrappingMinInput(-Math.PI));
+    check(turnMotor, turnPID.setPositionPIDWrappingMaxInput(Math.PI));
+    check(turnMotor, turnPID.setFeedbackDevice(turnEncoder));
 
-    // TODO: TalonFX FaultLogger support (there is code for it somewhere)
-    // FaultLogger.register(driveMotor);
-    FaultLogger.register(turnMotor);
-    turnMotor.burnFlash();
+    check(turnMotor, turnMotor.setIdleMode(IdleMode.kBrake));
+    check(turnMotor, turnMotor.setSmartCurrentLimit((int) Turning.CURRENT_LIMIT.in(Amps)));
+    turnEncoder.setInverted(Turning.ENCODER_INVERTED);
+    check(turnMotor);
+    check(turnMotor, turnEncoder.setPositionConversionFactor(Turning.POSITION_FACTOR.in(Radians)));
+    check(
+        turnMotor,
+        turnEncoder.setVelocityConversionFactor(Turning.VELOCITY_FACTOR.in(RadiansPerSecond)));
+    check(turnMotor, turnEncoder.setAverageDepth(2));
+    check(
+        turnMotor,
+        SparkUtils.configureFrameStrategy(
+            turnMotor,
+            Set.of(Data.POSITION, Data.VELOCITY, Data.APPLIED_OUTPUT),
+            Set.of(Sensor.ABSOLUTE),
+            false));
+    SparkUtils.addChecker(
+        () ->
+            check(
+                turnMotor,
+                SparkUtils.configureFrameStrategy(
+                    turnMotor,
+                    Set.of(Data.POSITION, Data.VELOCITY, Data.APPLIED_OUTPUT),
+                    Set.of(Sensor.ABSOLUTE),
+                    false)));
+    check(turnMotor, turnMotor.burnFlash());
+
+    register(turnMotor);
 
+    resetEncoders();
     this.name = name;
   }
 
@@ -97,7 +135,7 @@ public double drivePosition() {
 
   @Override
   public double driveVelocity() {
-    return driveVelo.getValueAsDouble();
+    return driveVelocity.getValueAsDouble();
   }
 
   @Override
@@ -133,22 +171,28 @@ public SwerveModuleState desiredState() {
 
   @Override
   public void setDriveSetpoint(double velocity) {
-    // TODO: Replace with Talon's onboard control
+    driveMotor.setControl(
+        velocityOut.withVelocity(velocity).withFeedForward(driveFF.calculate(velocity)));
   }
 
   @Override
   public void setTurnSetpoint(double angle) {
-    // TODO: Replace with Talon's onboard control
+    turnPID.setReference(angle, ControlType.kPosition);
   }
 
   @Override
   public void updateSetpoint(SwerveModuleState setpoint, ControlMode mode) {
-    // WARNING: DO NOT USE THIS CODE WHEN WORKING WITH KRAKENS, TALON ONBOARDING WILL BE WRITTEN
-    // SOON
-    // WARNING: NO CODE HERE
-    // TODO: Write code
+    setpoint = SwerveModuleState.optimize(setpoint, rotation());
+    // Scale setpoint by cos of turning error to reduce tread wear
+    setpoint.speedMetersPerSecond *= setpoint.angle.minus(rotation()).getCos();
+
+    if (mode == ControlMode.OPEN_LOOP_VELOCITY) {
+      setDriveVoltage(driveFF.calculate(setpoint.speedMetersPerSecond));
+    } else {
+      setDriveSetpoint(setpoint.speedMetersPerSecond);
+    }
+
+    setTurnSetpoint(setpoint.angle.getRadians());
+    this.setpoint = setpoint;
   }
-
-  @Override
-  public void updateDriveVoltage(Rotation2d angle, double voltage) {}
 }