[arm] Use a motion profile for controls

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

@@ -25,10 +25,14 @@ public final class Constants {
   public static final double armP = 21.0;
   public static final double armI = 0;
   public static final double armD = 0.015;
-  public static final double armS = 0;
-  public static final double armG = 0;
-  public static final double armV = 0;
-  public static final double armA = 0;
+  public static final double armS = 0; // stiction gain
+  public static final double armG = 0; // gravity gain
+  public static final double armV = 0; // velocity gain
+  public static final double armA = 0; // acceleration gain
+  public static final int armMaxVelRadPerSec = 0;
+  public static final int armMaxAccelRadPerSec = 0;
+  public static final int armDistPerPulse = 0;
+  public static final int armOffsetRad = 0;
   public static final int armLeadPort = 21;
   public static final int armFollowerPort = 26;
   public static final int armEncoderPort = 3;

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

@@ -8,125 +8,151 @@
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.util.datalog.StringLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants;
-import java.util.function.DoubleSupplier;
 
-/** Our Arm Subsystem */
 public class Arm extends SubsystemBase {
+
+  private TrapezoidProfile m_profile;
+  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+  private PIDController m_pid;
+  private final CANSparkMax m_motor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless);
+  private DataLog m_log = DataLogManager.getLog();
+  private DoubleLogEntry log_trapezoid_goal = new DoubleLogEntry(m_log, "/arm/trapezoid/goal");
+  private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/arm/pid/output");
+  private DoubleLogEntry log_pid_setpoint = new DoubleLogEntry(m_log, "/arm/pid/setpoint");
+  private DoubleLogEntry log_ff_position_setpoint =
+      new DoubleLogEntry(m_log, "/arm/ff/position_setpoint");
+  private DoubleLogEntry log_ff_velocity_setpoint =
+      new DoubleLogEntry(m_log, "/arm/ff/velocity_setpoint");
+  private DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output");
+  private StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint");
+  private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort);
+  private ArmFeedforward m_feedforward =
+      new ArmFeedforward(
+          Constants.armS, Constants.armG,
+          Constants.armV, Constants.armA);
+
   public enum Setpoint {
     kAmp,
     kIntake,
     kSpeaker,
     kStowed
   }
 
-  private final CANSparkMax m_primaryMotor =
-      new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless);
-  private final CANSparkMax m_followerMotor =
-      new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless);
-  private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort);
-
-  private final PIDController m_pid =
-      new PIDController(Constants.armP, Constants.armI, Constants.armD);
-  private final ArmFeedforward m_feedforward =
-      new ArmFeedforward(Constants.armS, Constants.armG, Constants.armV, Constants.armA);
-
-  private final DataLog m_log = DataLogManager.getLog();
-  private final DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/arm/pid/output");
-  private final DoubleLogEntry log_pid_setpoint = new DoubleLogEntry(m_log, "/arm/pid/setpoint");
-  private final DoubleLogEntry log_ff_position_setpoint =
-      new DoubleLogEntry(m_log, "/arm/ff/position_setpoint");
-  private final DoubleLogEntry log_ff_velocity_setpoint =
-      new DoubleLogEntry(m_log, "/arm/ff/velocity_setpoint");
-  private final DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output");
-  private final StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint");
+  private void useOutput(double output, Double position, Double velocity) {
+    // Calculate the feedforward from the sepoint
+    double feedforward = m_feedforward.calculate(position, velocity);
 
-  public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
+    log_trapezoid_goal.append(m_goal.position);
+    log_trapezoid_goal.append(m_goal.velocity);
+    log_pid_output.append(output);
+    log_pid_setpoint.append(m_pid.getSetpoint());
+    log_ff_output.append(feedforward);
+    log_ff_position_setpoint.append(position);
+    log_ff_velocity_setpoint.append(velocity);
 
-  /** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
-  public Arm() {
-    m_followerMotor.follow(m_primaryMotor);
-  }
-
-  /** Achieves and maintains speed for the primary motor. */
-  private Command achievePosition(double position) {
-    return Commands.run(
-        () -> {
-          var pid_output = m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * Math.PI, position);
-          log_pid_output.append(pid_output);
-          log_pid_setpoint.append(m_pid.getSetpoint());
-          var ff_output = m_feedforward.calculate(position, (5676 / 250));
-          log_ff_output.append(ff_output);
-          log_ff_position_setpoint.append(position);
-          log_ff_velocity_setpoint.append((5676 / 250));
-          m_primaryMotor.setVoltage(-1 * ff_output + pid_output);
-        });
-  }
-
-  public Command stop() {
-    return runOnce(() -> m_primaryMotor.set(0));
+    // Add the feedforward to the PID output to get the motor output
+    m_motor.setVoltage(output + feedforward);
   }
 
   /**
-   * Moves the arm to the specified position then stops.
+   * Sets the goal for the ProfiledPIDController.
    *
-   * @param position The desired position.
-   * @return a {@link Command} to get to the desired position.
+   * @param goal The desired goal state.
    */
-  private Command moveToPosition(double position) {
-    return achievePosition(position)
-        .until(
-            () ->
-                m_pid.atSetpoint()
-                    && ((m_encoder.getAbsolutePosition() * 2 * Math.PI) - position) < 0.001);
+  private void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
   }
 
   /**
-   * Holds the arm at the current position setpoint.
+   * Sets the goal for the ProfiledPIDController.
    *
-   * @return A {@link Command} to hold the position at the setpoint.
+   * @param goal The desired goal position.
    */
-  public Command maintain() {
-    return achievePosition(m_pid.getSetpoint());
+  private void setGoal(double goal) {
+    m_goal = new TrapezoidProfile.State(goal, 0);
   }
 
   /**
-   * Allows for manual control of the arm using a joystick.
+   * Gets the goal for the ProfiledPIDController.
    *
-   * @param speed The speed from the joystick input.
-   * @return A {@link Command} to control the arm manually.
+   * @return The goal.
    */
-  public Command manualControl(DoubleSupplier speed) {
-    return Commands.run(() -> m_primaryMotor.set(speed.getAsDouble()), this);
+  private TrapezoidProfile.State getGoal() {
+    return m_goal;
   }
 
-  /*
-   * Makes the arm go to a setpoint from the {@link Setpoint} enum
-   *
-   * @param setpoint The setpoint to go to, a {@link Setpoint}
+  private double getPeriod() {
+    return m_pid.getPeriod();
+  }
+
+  /**
+   * Returns the next output of the PID controller.
    *
-   * @return A {@link Command} to go to the setpoint
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   * @return The controller's next output.
    */
-  public Command goToSetpoint(Setpoint setpoint) {
+  private double calculate(double measurement) {
+
+    m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal);
+    return m_pid.calculate(measurement, m_setpoint.position);
+  }
+
+  private double calculate(double measurement, TrapezoidProfile.State goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  private double calculate(double measurement, double goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  public Arm() {
+
+    m_pid = new PIDController(Constants.armP, Constants.armI, Constants.armD);
+    new TrapezoidProfile(
+        new TrapezoidProfile.Constraints(
+            Constants.armMaxVelRadPerSec, Constants.armMaxAccelRadPerSec));
+
+    // Start arm at rest in neutral position
+    setGoal(Constants.armOffsetRad);
+  }
+
+  public double getMeasurement() {
+    return m_encoder.getDistance() + Constants.armOffsetRad;
+  }
+
+  public void goToSetpoint(Setpoint setpoint) {
     double position = 0;
     log_setpoint.append(setpoint.name());
 
     switch (setpoint) {
-      case kAmp -> position = 5.34;
-      case kIntake -> position = 3.7;
-      case kSpeaker -> position = 3.7;
-      case kStowed -> position = 3.7;
-    }
+      case kAmp:
+        position = 5.34;
+        break;
+
+      case kIntake:
+        position = 3.7;
+        break;
 
-    return moveToPosition(position);
+      case kSpeaker:
+        position = 3.7;
+        break;
+
+      case kStowed:
+        position = 3.7;
+        break;
+    }
   }
 }
+;