Closed loop control working, seperate FFC for left and right

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

@@ -28,6 +28,7 @@ public final class Constants {
   public static final double INTAKE_SHOOTER_VOLTS = -8.0;
   public static final double INTAKE_HOPPER_VOLTS = -4.0;
 
+  public static final double TICKS_PER_REVOLUTION = 1440;
   public static final Mode currentMode = Mode.REAL;
 
   public static enum Mode {

src/main/java/frc/robot/RobotContainer.java

@@ -81,9 +81,13 @@ public RobotContainer() {
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
     // Set up feedforward characterization
     autoChooser.addOption(
-        "Drive FF Characterization",
+        "Drive FF Characterization Left",
         new FeedForwardCharacterization(
-            drive, (volts) -> drive.driveVolts(volts, volts), drive::getCharacterizationVelocity));
+            drive, (volts) -> drive.driveVolts(volts, volts), drive::getLeftVelocityMetersPerSec));
+    autoChooser.addOption(
+        "Drive FF Characterization Right",
+        new FeedForwardCharacterization(
+            drive, (volts) -> drive.driveVolts(volts, volts), drive::getRightVelocityMetersPerSec));
     autoChooser.addOption(
         "Flywheel FF Characterization",
         new FeedForwardCharacterization(

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

@@ -43,8 +43,11 @@ public class Drive extends SubsystemBase {
 
   // TODO: NON-SIM FEEDFORWARD GAINS MUST BE TUNED
   // Consider using SysId routines defined in RobotContainer
-  private static final double KS = Constants.currentMode == Mode.SIM ? 0.0 : 0.0;
-  private static final double KV = Constants.currentMode == Mode.SIM ? 0.227 : 0.0;
+  private static final double lKS = Constants.currentMode == Mode.SIM ? 0.0 : 0.90278;
+  private static final double lKV = Constants.currentMode == Mode.SIM ? 0.227 : 30.62084;
+
+  private static final double rKS = Constants.currentMode == Mode.SIM ? 0.0 : 0.82739;
+  private static final double rKV = Constants.currentMode == Mode.SIM ? 0.227 : 34.45523;
 
   private final PIDController pid = new PIDController(0, 0, 0);
 
@@ -54,7 +57,8 @@ public class Drive extends SubsystemBase {
       new DifferentialDriveOdometry(new Rotation2d(), 0.0, 0.0);
   private final DifferentialDriveKinematics kinematics =
       new DifferentialDriveKinematics(TRACK_WIDTH);
-  private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV);
+  private final SimpleMotorFeedforward feedforwardLeft = new SimpleMotorFeedforward(lKS, lKV);
+  private final SimpleMotorFeedforward feedforwardRight = new SimpleMotorFeedforward(rKS, rKV);
 
   /** Creates a new Drive. */
   public Drive(DriveIO io) {
@@ -106,22 +110,18 @@ public void driveVolts(double leftVolts, double rightVolts) {
   public void driveVelocity(double leftMetersPerSec, double rightMetersPerSec) {
     Logger.recordOutput("Drive/LeftVelocitySetpointMetersPerSec", leftMetersPerSec);
     Logger.recordOutput("Drive/RightVelocitySetpointMetersPerSec", rightMetersPerSec);
-    double leftRadPerSec = leftMetersPerSec / WHEEL_RADIUS;
-    double rightRadPerSec = rightMetersPerSec / WHEEL_RADIUS;
-    io.setVelocity(
-        leftRadPerSec,
-        rightRadPerSec,
-        feedforward.calculate(leftRadPerSec),
-        feedforward.calculate(rightRadPerSec));
+
+    io.setVoltage(
+        pid.calculate(inputs.leftVelocityRadPerSec * WHEEL_RADIUS, leftMetersPerSec)
+            + (feedforwardLeft.calculate(leftMetersPerSec)),
+        pid.calculate(inputs.rightVelocityRadPerSec * WHEEL_RADIUS, rightMetersPerSec)
+            + (feedforwardRight.calculate(rightMetersPerSec)));
   }
 
   /** Run open loop based on stick positions. */
   public void driveArcade(double xSpeed, double zRotation) {
     var speeds = DifferentialDrive.arcadeDriveIK(xSpeed, zRotation, true);
-    io.setVoltage(
-        pid.calculate(inputs.leftVelocityRadPerSec * WHEEL_RADIUS, speeds.left * MAX_SPEED_M_PER_S),
-        pid.calculate(
-            inputs.rightVelocityRadPerSec * WHEEL_RADIUS, speeds.right * MAX_SPEED_M_PER_S));
+    driveVelocity(speeds.left * MAX_SPEED_M_PER_S, speeds.right * MAX_SPEED_M_PER_S);
   }
 
   /** Stops the drive. */
@@ -164,8 +164,8 @@ public double getRightVelocityMetersPerSec() {
     return inputs.rightVelocityRadPerSec * WHEEL_RADIUS;
   }
 
-  /** Returns the average velocity in radians/second. */
+  /** Returns the average velocity in meters/second. */
   public double getCharacterizationVelocity() {
-    return (inputs.leftVelocityRadPerSec + inputs.rightVelocityRadPerSec) / 2.0;
+    return (inputs.leftVelocityRadPerSec + inputs.rightVelocityRadPerSec) / 2.0 * WHEEL_RADIUS;
   }
 }

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

@@ -17,19 +17,19 @@
 import static com.ctre.phoenix.motorcontrol.NeutralMode.Brake;
 
 import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
-import com.ctre.phoenix.motorcontrol.can.SlotConfiguration;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
 
 public class DriveIOTalonSRX implements DriveIO {
-  private static final double GEAR_RATIO = 10.0;
+  private static final double GEAR_RATIO = 10.0; //  was 10
 
   private static final double MAX_VOLTAGE = 12.0;
   private static final double KP = 1.0; // TODO: MUST BE TUNED, consider using Phoenix Tuner X
   private static final double KD = 0.0; // TODO: MUST BE TUNED, consider using Phoenix Tuner X
-  private PIDController pid = new PIDController(0.0, 0.0, 0.0);
+  private PIDController pid = new PIDController(0, 0.0, 0.0);
   private final TalonSRX leftLeader = new TalonSRX(2);
   private final TalonSRX leftFollower = new TalonSRX(0);
   private final TalonSRX rightLeader = new TalonSRX(3);
@@ -61,11 +61,6 @@ public DriveIOTalonSRX() {
     rightLeader.setNeutralMode(Brake);
     rightFollower.setNeutralMode(Brake);
 
-    SlotConfiguration slot = new SlotConfiguration();
-    slot.kP = KP;
-    slot.kD = KD;
-    config.slot0 = slot;
-
     leftLeader.configAllSettings(config);
     leftFollower.configAllSettings(config);
     rightLeader.configAllSettings(config);
@@ -94,13 +89,17 @@ public void updateInputs(DriveIOInputs inputs) {
     rightLeaderCurrent = rightLeader.getStatorCurrent();
     rightFollowerCurrent = rightFollower.getStatorCurrent();
 
-    inputs.leftPositionRad = Units.rotationsToRadians(leftPosition) / GEAR_RATIO;
-    inputs.leftVelocityRadPerSec = Units.rotationsToRadians(leftVelocity) / GEAR_RATIO;
+    inputs.leftPositionRad =
+        Units.rotationsToRadians(leftPosition / Constants.TICKS_PER_REVOLUTION);
+    inputs.leftVelocityRadPerSec =
+        Units.rotationsToRadians(leftVelocity / Constants.TICKS_PER_REVOLUTION);
     inputs.leftAppliedVolts = leftAppliedVolts;
     inputs.leftCurrentAmps = new double[] {leftLeaderCurrent, leftFollowerCurrent};
 
-    inputs.rightPositionRad = Units.rotationsToRadians(rightPosition) / GEAR_RATIO;
-    inputs.rightVelocityRadPerSec = Units.rotationsToRadians(rightVelocity) / GEAR_RATIO;
+    inputs.rightPositionRad =
+        Units.rotationsToRadians(rightPosition / Constants.TICKS_PER_REVOLUTION);
+    inputs.rightVelocityRadPerSec =
+        Units.rotationsToRadians(rightVelocity / Constants.TICKS_PER_REVOLUTION);
     inputs.rightAppliedVolts = rightAppliedVolts;
     inputs.rightCurrentAmps = new double[] {rightLeaderCurrent, rightFollowerCurrent};
   }
@@ -110,12 +109,4 @@ public void setVoltage(double leftVolts, double rightVolts) {
     leftLeader.set(TalonSRXControlMode.PercentOutput, leftVolts / MAX_VOLTAGE);
     rightLeader.set(TalonSRXControlMode.PercentOutput, -1 * rightVolts / MAX_VOLTAGE);
   }
-
-  @Override
-  public void setVelocity(
-      double leftRadPerSec, double rightRadPerSec, double leftFFVolts, double rightFFVolts) {
-    setVoltage(
-        pid.calculate(leftLeader.getSelectedSensorVelocity(), leftRadPerSec) + leftFFVolts,
-        pid.calculate(rightLeader.getSelectedSensorVelocity(), rightRadPerSec) + rightFFVolts);
-  }
 }