Added feedforward to flywheel and tuned PID

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

@@ -14,6 +14,7 @@
 package frc.robot;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
@@ -28,7 +29,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final boolean tuningMode = false;
+  public static final boolean tuningMode = true;
 
   public static final class ShooterConstants {
 
@@ -42,7 +43,7 @@ public static final class ShooterConstants {
     public static final Rotation2d kPivotOffset = Rotation2d.fromDegrees(170);
 
     public static final LoggedTunableNumber kPivotP =
-        new LoggedTunableNumber("Shooter Pivot P", 1.0);
+        new LoggedTunableNumber("Shooter Pivot P", 4.0);
     public static final LoggedTunableNumber kPivotI =
         new LoggedTunableNumber("Shooter Pivot I", 0.0);
     public static final LoggedTunableNumber kPivotD =
@@ -60,11 +61,11 @@ public static final class ShooterConstants {
     public static final double kFlywheelDiameter = Units.inchesToMeters(4.0);
 
     public static final LoggedTunableNumber kLeftFlywheelP =
-        new LoggedTunableNumber("Shooter Flywheel P", 0.7);
+        new LoggedTunableNumber("Shooter Left Flywheel P", 1.0);
     public static final LoggedTunableNumber kLeftFlywheelI =
-        new LoggedTunableNumber("Shooter Flywheel I", 0.0);
+        new LoggedTunableNumber("Shooter Left Flywheel I", 0.0);
     public static final LoggedTunableNumber kLeftFlywheelD =
-        new LoggedTunableNumber("Shooter Flywheel D", 0.0);
+        new LoggedTunableNumber("Shooter Left Flywheel D", 0.0);
     public static final double kLeftFlywheelVelocity = 10.0;
     public static final double kLeftFlywheelAcceleration = 15.0;
     public static final ProfiledPIDController kLeftFlywheelController =
@@ -75,11 +76,11 @@ public static final class ShooterConstants {
             new Constraints(kLeftFlywheelVelocity, kLeftFlywheelAcceleration));
 
     public static final LoggedTunableNumber kRightFlywheelP =
-        new LoggedTunableNumber("Shooter Flywheel P", 0.7);
+        new LoggedTunableNumber("Shooter Right Flywheel P", 1.0);
     public static final LoggedTunableNumber kRightFlywheelI =
-        new LoggedTunableNumber("Shooter Flywheel I", 0.0);
+        new LoggedTunableNumber("Shooter Right Flywheel I", 0.0);
     public static final LoggedTunableNumber kRightFlywheelD =
-        new LoggedTunableNumber("Shooter Flywheel D", 0.0);
+        new LoggedTunableNumber("Shooter Right Flywheel D", 0.0);
     public static final double kRightFlywheelVelocity = 10.0;
     public static final double kRightFlywheelAcceleration = 15.0;
     public static final ProfiledPIDController kRightFlywheelController =
@@ -88,6 +89,25 @@ public static final class ShooterConstants {
             kRightFlywheelI.get(),
             kRightFlywheelD.get(),
             new Constraints(kRightFlywheelVelocity, kRightFlywheelAcceleration));
+
+    public static final LoggedTunableNumber kRightFlywheelKs =
+        new LoggedTunableNumber("Shooter Right Flywheel kS", 0.38);
+    public static final LoggedTunableNumber kRightFlywheelKv =
+        new LoggedTunableNumber("Shooter Right Flywheel kV", 0.27);
+    public static final SimpleMotorFeedforward kRightFlywheelFeedforward =
+        new SimpleMotorFeedforward(kRightFlywheelKs.get(), kRightFlywheelKv.get());
+
+    public static final LoggedTunableNumber kLeftFlywheelKs =
+        new LoggedTunableNumber("Shooter Left Flywheel kS", 0.32);
+    public static final LoggedTunableNumber kLeftFlywheelKv =
+        new LoggedTunableNumber("Shooter Left Flywheel kV", 0.27);
+    public static final SimpleMotorFeedforward kLeftFlywheelFeedforward =
+        new SimpleMotorFeedforward(kLeftFlywheelKs.get(), kLeftFlywheelKv.get());
+
+    public static final LoggedTunableNumber kTestLeftFlywheelSpeed =
+        new LoggedTunableNumber("Left test flywheel speed", 0.0);
+    public static final LoggedTunableNumber kTestRightFlywheelSpeed =
+        new LoggedTunableNumber("Right test flywheel speed", 20.0);
   }
 
   public static final class Ports {

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

@@ -77,7 +77,9 @@ private void configureSubsystems() {
               new FlywheelIOKraken(Ports.kFlywheelLeft, Ports.kFlywheelRight),
               ShooterConstants.kPivotController,
               ShooterConstants.kLeftFlywheelController,
-              ShooterConstants.kRightFlywheelController);
+              ShooterConstants.kRightFlywheelController,
+              ShooterConstants.kLeftFlywheelFeedforward,
+              ShooterConstants.kRightFlywheelFeedforward);
       m_indexer =
           new Indexer(
               new IndexerIOFalcon(
@@ -99,7 +101,9 @@ private void configureSubsystems() {
               new FlywheelIOSim(),
               ShooterConstants.kPivotController,
               ShooterConstants.kLeftFlywheelController,
-              ShooterConstants.kRightFlywheelController);
+              ShooterConstants.kRightFlywheelController,
+              ShooterConstants.kLeftFlywheelFeedforward,
+              ShooterConstants.kRightFlywheelFeedforward);
       m_indexer = new Indexer(new IndexerIOSim());
     }
 
@@ -130,7 +134,9 @@ private void configureButtonBindings() {
         .onTrue(
             Commands.runOnce(
                 () -> {
-                  m_shooter.setFlywheelVelocity(10, 13);
+                  m_shooter.setFlywheelVelocity(
+                      ShooterConstants.kTestLeftFlywheelSpeed.get(),
+                      ShooterConstants.kTestRightFlywheelSpeed.get());
                   m_shooter.setPivotAngle(Rotation2d.fromDegrees(50));
                 }))
         .onFalse(

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

@@ -69,28 +69,28 @@ public class ModuleIOTalonFX implements ModuleIO {
   public ModuleIOTalonFX(int index) {
     switch (index) {
       case 0:
-        turnTalon = new TalonFX(1);
-        driveTalon = new TalonFX(2);
-        cancoder = new CANcoder(3);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        driveTalon = new TalonFX(1, "Drivetrain");
+        turnTalon = new TalonFX(2, "Drivetrain");
+        cancoder = new CANcoder(3, "Drivetrain");
+        absoluteEncoderOffset = new Rotation2d(1.226); // MUST BE CALIBRATED
         break;
       case 1:
-        turnTalon = new TalonFX(4);
-        driveTalon = new TalonFX(5);
-        cancoder = new CANcoder(6);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        driveTalon = new TalonFX(4, "Drivetrain");
+        turnTalon = new TalonFX(5, "Drivetrain");
+        cancoder = new CANcoder(6, "Drivetrain");
+        absoluteEncoderOffset = new Rotation2d(0.175); // MUST BE CALIBRATED
         break;
       case 2:
-        turnTalon = new TalonFX(7);
-        driveTalon = new TalonFX(8);
-        cancoder = new CANcoder(9);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        driveTalon = new TalonFX(7, "Drivetrain");
+        turnTalon = new TalonFX(8, "Drivetrain");
+        cancoder = new CANcoder(9, "Drivetrain");
+        absoluteEncoderOffset = new Rotation2d(-2.945); // MUST BE CALIBRATED
         break;
       case 3:
-        turnTalon = new TalonFX(10);
-        driveTalon = new TalonFX(11);
-        cancoder = new CANcoder(12);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
+        driveTalon = new TalonFX(10, "Drivetrain");
+        turnTalon = new TalonFX(11, "Drivetrain");
+        cancoder = new CANcoder(12, "Drivetrain");
+        absoluteEncoderOffset = new Rotation2d(2.827); // MUST BE CALIBRATED
         break;
       default:
         throw new RuntimeException("Invalid module index");

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.shooter;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -39,12 +40,17 @@ public ShooterPosition(
   private ProfiledPIDController m_leftFlywheelController;
   private ProfiledPIDController m_rightFlywheelController;
 
+  private SimpleMotorFeedforward m_rightFlywheelFeedforward;
+  private SimpleMotorFeedforward m_leftFlywheelFeedforward;
+
   public Shooter(
       ShooterPivotIO pivotIO,
       FlywheelIO flywheelIO,
       ProfiledPIDController pivotController,
       ProfiledPIDController leftFlywheelController,
-      ProfiledPIDController rightFlywheelController) {
+      ProfiledPIDController rightFlywheelController,
+      SimpleMotorFeedforward leftFlywheelFeedforward,
+      SimpleMotorFeedforward rightFlywheelFeedforward) {
 
     m_pivotIO = pivotIO;
     m_flywheelIO = flywheelIO;
@@ -53,6 +59,9 @@ public Shooter(
     m_leftFlywheelController = leftFlywheelController;
     m_rightFlywheelController = rightFlywheelController;
 
+    m_rightFlywheelFeedforward = rightFlywheelFeedforward;
+    m_leftFlywheelFeedforward = leftFlywheelFeedforward;
+
     m_pivotInputs = new ShooterPivotInputsAutoLogged();
     m_flywheelInputs = new FlywheelInputsAutoLogged();
   }
@@ -93,6 +102,26 @@ public void periodic() {
         ShooterConstants.kRightFlywheelI,
         ShooterConstants.kRightFlywheelD);
 
+    LoggedTunableNumber.ifChanged(
+        hashCode(),
+        () -> {
+          m_rightFlywheelFeedforward =
+              new SimpleMotorFeedforward(
+                  ShooterConstants.kRightFlywheelKs.get(), ShooterConstants.kRightFlywheelKv.get());
+        },
+        ShooterConstants.kRightFlywheelKs,
+        ShooterConstants.kRightFlywheelKv);
+
+    LoggedTunableNumber.ifChanged(
+        hashCode(),
+        () -> {
+          m_leftFlywheelFeedforward =
+              new SimpleMotorFeedforward(
+                  ShooterConstants.kLeftFlywheelKs.get(), ShooterConstants.kLeftFlywheelKv.get());
+        },
+        ShooterConstants.kLeftFlywheelKs,
+        ShooterConstants.kLeftFlywheelKv);
+
     m_pivotIO.updateInputs(m_pivotInputs);
     m_flywheelIO.updateInputs(m_flywheelInputs);
 
@@ -103,7 +132,15 @@ public void periodic() {
         m_leftFlywheelController.calculate(m_flywheelInputs.linearVelocity[0]);
     double rightFlywheelPidVoltage =
         m_rightFlywheelController.calculate(m_flywheelInputs.linearVelocity[1]);
-    m_flywheelIO.setVoltage(leftFlywheelPidVoltage, rightFlywheelPidVoltage);
+
+    double leftFlywheelFeedforwardVoltage =
+        m_leftFlywheelFeedforward.calculate(m_leftFlywheelController.getGoal().position);
+    double rightFlywheelFeedforwardVoltage =
+        m_rightFlywheelFeedforward.calculate(m_rightFlywheelController.getGoal().position);
+
+    m_flywheelIO.setVoltage(
+        leftFlywheelPidVoltage + leftFlywheelFeedforwardVoltage,
+        rightFlywheelPidVoltage + rightFlywheelFeedforwardVoltage);
 
     Logger.processInputs("Shooter Pivot", m_pivotInputs);
     Logger.processInputs("Shooter Flywheel", m_flywheelInputs);
@@ -116,6 +153,17 @@ public void periodic() {
 
     Logger.recordOutput("Shooter/Flywheel/LeftFlywheelPIDVoltage", leftFlywheelPidVoltage);
     Logger.recordOutput("Shooter/Flywheel/RightFlywheelPIDVoltage", rightFlywheelPidVoltage);
+
+    Logger.recordOutput(
+        "Shooter/Flywheel/LeftFlywheelFeedforwardVoltage", leftFlywheelFeedforwardVoltage);
+    Logger.recordOutput(
+        "Shooter/Flywheel/RightFlywheelFeedforwardVoltage", rightFlywheelFeedforwardVoltage);
+
+    Logger.recordOutput(
+      "Shooter/Flywheel/LeftFlywheelSetVoltage", leftFlywheelPidVoltage + leftFlywheelFeedforwardVoltage);
+    Logger.recordOutput(
+      "Shooter/Flywheel/RightFlywheelSetVoltage", rightFlywheelPidVoltage + rightFlywheelFeedforwardVoltage);
+
     Logger.recordOutput(
         "Shooter/Flywheel/LeftFlywheelVelocity", m_flywheelInputs.linearVelocity[0]);
     Logger.recordOutput(

src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java

@@ -6,7 +6,8 @@
 public interface FlywheelIO extends LoggedIO<FlywheelIO.FlywheelInputs> {
   @AutoLog
   public static class FlywheelInputs {
-    public double[] angularVelocity;
+    public double[] angularVelocityRad;
+    public double[] angularVelocityRPS;
     public double[] linearVelocity;
     public double[] curVoltage;
     public double[] curAmps;

src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOKraken.java

@@ -40,12 +40,17 @@ public void updateInputs(FlywheelInputs inputs) {
         m_rightCurrentSignal,
         m_rightVelocitySignal);
 
-    inputs.angularVelocity =
+    inputs.angularVelocityRad =
         new double[] {
           Units.rotationsToRadians(m_leftVelocitySignal.getValueAsDouble()),
           Units.rotationsToRadians(m_rightVelocitySignal.getValueAsDouble())
         };
 
+    inputs.angularVelocityRPS =
+        new double[] {
+          m_leftVelocitySignal.getValueAsDouble(), m_rightVelocitySignal.getValueAsDouble()
+        };
+
     inputs.linearVelocity =
         new double[] {
           rotationsPerSecondToMeterPerSecond(m_leftVelocitySignal.getValueAsDouble()),

src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java

@@ -24,10 +24,15 @@ public void updateInputs(FlywheelInputs inputs) {
 
     inputs.curVoltage = new double[] {m_leftVoltage, m_rightVoltage};
 
-    inputs.angularVelocity =
+    inputs.angularVelocityRad =
         new double[] {
           m_leftSim.getAngularVelocityRadPerSec(), m_leftSim.getAngularVelocityRadPerSec()
         };
+
+    inputs.angularVelocityRPS =
+        new double[] {
+          m_leftSim.getAngularVelocityRPM() / 60.0, m_rightSim.getAngularVelocityRPM() / 60.0
+        };
 
     inputs.linearVelocity =
         new double[] {