Add PID control for Arm and Flywheel subsystems

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

@@ -25,9 +25,10 @@ public RobotContainer() {
   }
 
   private void configureBindings() {
-    // When left trigger is pressed the arm will run at a fast speed, and when not pressed it will
+    // When left trigger is pressed the arm will go to vertical position, and when not pressed it
+    // will
     // stop
-    joystick.leftTrigger().onTrue(arm.runFast()).onFalse(arm.stopCommand());
+    joystick.leftTrigger().onTrue(arm.vertical());
 
     // When right trigger is pressed the flywheel will run at a fast speed, and when not pressed it
     // will spin slow

src/main/java/frc/robot/subsystems/Arm.java

@@ -6,12 +6,14 @@
 
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -24,8 +26,8 @@ public class Arm extends SubsystemBase {
   // Create and absolute encoder that the motor can refrence for position
   private final CANcoder encoder = new CANcoder(32, canivore);
 
-  // Voltage output control for the arm
-  private final VoltageOut voltageOut = new VoltageOut(0);
+  // Position output control for the arm
+  private final PositionVoltage positionOut = new PositionVoltage(0);
 
   public Arm() {
     // Create and apply the configuration for the leader motor
@@ -34,6 +36,12 @@ public Arm() {
     config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     // Configure the motor to make sure positive voltage is counter clockwise
     config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; // Use cosine gravity compensation
+    config.Slot0.kG = 0.0; // Gravity gain
+    config.Slot0.kS = 0.0; // Static gain
+    config.Slot0.kP = 0.0; // Proportional gain
+    config.Slot0.kD = 0.0; // Derivative gain
+    config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
     // Configure the leader motor to use the CANcoder for position feedback
     config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
     config.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID();
@@ -50,33 +58,33 @@ public void periodic() {
   }
 
   /**
-   * Sets the voltage for the arm.
+   * Sets the position for the arm.
    *
-   * @param voltage The voltage to set.
+   * @param position The position to set in rotations.
    */
-  public void setVoltage(double voltage) {
-    // Apply the voltage output to the leader motor
-    leader.setControl(voltageOut.withOutput(voltage));
+  public void setPosition(double position) {
+    // Apply the position output to the leader motor
+    leader.setControl(positionOut.withPosition(position));
   }
 
   /**
-   * Command to run the arm at a slow speed.
+   * Command to run the arm to vertical position.
    *
-   * @return The command to run the arm slowly.
+   * @return The command to run the arm vertical.
    */
-  public Command runSlow() {
-    // Command to run the arm at a slow speed and stop it afterward
-    return runOnce(() -> setVoltage(3));
+  public Command vertical() {
+    // Command to run the arm to vertical position and stop it afterward
+    return runOnce(() -> setPosition(0.25));
   }
 
   /**
-   * Command to run the arm at a fast speed.
+   * Command to run the arm to a horizontal position.
    *
-   * @return The command to run the arm quickly.
+   * @return The command to run the arm horizontal.
    */
-  public Command runFast() {
-    // Command to run the arm at a fast speed and stop it afterward
-    return runOnce(() -> setVoltage(6));
+  public Command horizontal() {
+    // Command to run the arm to horizontal position and stop it afterward
+    return runOnce(() -> setPosition(0.5));
   }
 
   /**

src/main/java/frc/robot/subsystems/Flywheel.java

@@ -7,7 +7,7 @@
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -23,8 +23,8 @@ public class Flywheel extends SubsystemBase {
 
   private final TalonFX follower = new TalonFX(22, canivore);
 
-  // Voltage output control for the flywheel
-  private final VoltageOut voltageOut = new VoltageOut(0);
+  // Velocity output control for the flywheel
+  private final VelocityVoltage velocityOut = new VelocityVoltage(0);
 
   public Flywheel() {
     // Set the follower to follow the leader motor
@@ -35,6 +35,9 @@ public Flywheel() {
     config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     // Configure the motor to make sure positive voltage is counter clockwise
     config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    config.Slot0.kS = 0.0; // Static gain
+    config.Slot0.kV = 0.0; // Velocity gain
+    config.Slot0.kP = 0.0; // Proportional gain
     // Try to apply config multiple time. Break after successfully applying
     for (int i = 0; i < 2; ++i) {
       var status = leader.getConfigurator().apply(config);
@@ -48,13 +51,13 @@ public void periodic() {
   }
 
   /**
-   * Sets the voltage for the flywheel.
+   * Sets the velocity for the flywheel.
    *
-   * @param voltage The voltage to set.
+   * @param velocity The velocity to set in rotations per second.
    */
-  public void setVoltage(double voltage) {
-    // Apply the voltage output to the leader motor
-    leader.setControl(voltageOut.withOutput(voltage));
+  public void setVelocity(double velocity) {
+    // Apply the velocity output to the leader motor
+    leader.setControl(velocityOut.withVelocity(velocity));
   }
 
   /**
@@ -64,7 +67,7 @@ public void setVoltage(double voltage) {
    */
   public Command runSlow() {
     // Command to run the flywheel at a slow speed
-    return runOnce(() -> setVoltage(3));
+    return runOnce(() -> setVelocity(25));
   }
 
   /**
@@ -74,7 +77,7 @@ public Command runSlow() {
    */
   public Command runFast() {
     // Command to run the flywheel at a fast speed
-    return runOnce(() -> setVoltage(6));
+    return runOnce(() -> setVelocity(75));
   }
 
   /**