Use LinearProfile for flywheels

src/main/java/org/littletonrobotics/frc2024/Constants.java

@@ -20,7 +20,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final int loopPeriodMs = 20;
+  public static final double loopPeriodSecs = 0.02;
   private static RobotType robotType = RobotType.DEVBOT;
   public static final boolean tuningMode = true;
 

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -217,12 +217,12 @@ private void configureButtonBindings() {
         .a()
         .whileTrue(
             Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal)
-                .alongWith(superstructure.aim(), flywheels.shoot())
+                .alongWith(superstructure.aim(), flywheels.shootCommand())
                 .withName("Aim"));
     // Shoot
     Trigger readyToShoot =
         new Trigger(
-            () -> drive.isAutoAimGoalCompleted() && superstructure.atGoal() && flywheels.atGoal());
+            () -> drive.isAutoAimGoalCompleted() && flywheels.atGoal() && superstructure.atGoal());
     readyToShoot
         .whileTrue(
             Commands.run(
@@ -237,7 +237,8 @@ private void configureButtonBindings() {
             Commands.parallel(
                     Commands.waitSeconds(0.5),
                     Commands.waitUntil(controller.rightTrigger().negate()))
-                .deadlineWith(rollers.feedShooter(), superstructure.aim(), flywheels.shoot()));
+                .deadlineWith(
+                    rollers.feedShooter(), superstructure.aim(), flywheels.shootCommand()));
     // Intake Floor
     controller
         .leftTrigger()

src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java

@@ -10,21 +10,26 @@
 import org.littletonrobotics.frc2024.Constants;
 
 public class FlywheelConstants {
-  public static Config config =
+  public static FlywheelConfig flywheelConfig =
       switch (Constants.getRobot()) {
         case COMPBOT -> null;
-        case DEVBOT -> new Config(5, 4, (1.0 / 2.0), 100.0);
-        case SIMBOT -> new Config(0, 0, (1.0 / 2.0), 100.0);
+        case DEVBOT -> new FlywheelConfig(5, 4, (1.0 / 2.0), 6000.0, 100.0);
+        case SIMBOT -> new FlywheelConfig(0, 0, (1.0 / 2.0), 6000.0, 50.0);
       };
 
   public static Gains gains =
       switch (Constants.getRobot()) {
         case COMPBOT -> null;
         case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0);
-        case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0);
+        case SIMBOT -> new Gains(0.05, 0.0, 0.0, 0.01, 0.00103, 0.0);
       };
 
-  public record Config(int leftID, int rightID, double reduction, double toleranceRpm) {}
+  public record FlywheelConfig(
+      int leftID,
+      int rightID,
+      double reduction,
+      double maxAcclerationRpmPerSec,
+      double toleranceRPM) {}
 
   public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {}
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java

@@ -15,6 +15,8 @@
 import java.util.function.DoubleSupplier;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
+import org.littletonrobotics.frc2024.Constants;
+import org.littletonrobotics.frc2024.util.LinearProfile;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -27,40 +29,47 @@ public class Flywheels extends SubsystemBase {
   private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV());
   private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA());
   private static final LoggedTunableNumber shootingLeftRpm =
-      new LoggedTunableNumber("Superstructure/ShootingLeftRpm", 6000.0);
+      new LoggedTunableNumber("Flywheels/ShootingLeftRpm", 6000.0);
   private static final LoggedTunableNumber shootingRightRpm =
-      new LoggedTunableNumber("Superstructure/ShootingRightRpm", 4000.0);
+      new LoggedTunableNumber("Flywheels/ShootingRightRpm", 4000.0);
   private static final LoggedTunableNumber idleLeftRpm =
-      new LoggedTunableNumber("Superstructure/IdleLeftRpm", 1500.0);
+      new LoggedTunableNumber("Flywheels/IdleLeftRpm", 1500.0);
   private static final LoggedTunableNumber idleRightRpm =
-      new LoggedTunableNumber("Superstructure/IdleRightRpm", 1000.0);
+      new LoggedTunableNumber("Flywheels/IdleRightRpm", 1000.0);
   private static final LoggedTunableNumber intakingRpm =
-      new LoggedTunableNumber("Superstructure/IntakingRpm", -2000.0);
+      new LoggedTunableNumber("Flywheels/IntakingRpm", -2000.0);
   private static final LoggedTunableNumber ejectingRpm =
-      new LoggedTunableNumber("Superstructure/EjectingRpm", 2000.0);
-  private static final LoggedTunableNumber shooterTolerance =
-      new LoggedTunableNumber("Flywheels/ToleranceRpm", config.toleranceRpm());
+      new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0);
+  private static final LoggedTunableNumber maxAcceleration =
+      new LoggedTunableNumber(
+          "Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec());
 
   private final FlywheelsIO io;
   private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged();
 
+  private final LinearProfile leftProfile;
+  private final LinearProfile rightProfile;
+  private boolean wasClosedLoop = false;
+  private boolean closedLoop = false;
+
   @RequiredArgsConstructor
   public enum Goal {
+    STOP(() -> 0, () -> 0),
     IDLE(idleLeftRpm, idleRightRpm),
     SHOOT(shootingLeftRpm, shootingRightRpm),
     INTAKE(intakingRpm, intakingRpm),
     EJECT(ejectingRpm, ejectingRpm),
     CHARACTERIZING(() -> 0.0, () -> 0.0);
 
-    private final DoubleSupplier leftSetpoint;
-    private final DoubleSupplier rightSetpoint;
+    private final DoubleSupplier leftGoal;
+    private final DoubleSupplier rightGoal;
 
-    private double getLeftSetpoint() {
-      return leftSetpoint.getAsDouble();
+    private double getLeftGoal() {
+      return leftGoal.getAsDouble();
     }
 
-    private double getRightSetpoint() {
-      return rightSetpoint.getAsDouble();
+    private double getRightGoal() {
+      return rightGoal.getAsDouble();
     }
   }
 
@@ -69,7 +78,10 @@ private double getRightSetpoint() {
   public Flywheels(FlywheelsIO io) {
     this.io = io;
 
-    setDefaultCommand(runOnce(this::goIdle).withName("Flywheels Idle"));
+    leftProfile = new LinearProfile(maxAcceleration.get(), Constants.loopPeriodSecs);
+    rightProfile = new LinearProfile(maxAcceleration.get(), Constants.loopPeriodSecs);
+
+    setDefaultCommand(runOnce(() -> setGoal(Goal.IDLE)).withName("Flywheels Idle"));
   }
 
   @Override
@@ -81,24 +93,61 @@ public void periodic() {
     LoggedTunableNumber.ifChanged(hashCode(), pid -> io.setPID(pid[0], pid[1], pid[2]), kP, kI, kD);
     LoggedTunableNumber.ifChanged(
         hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA);
-
+    LoggedTunableNumber.ifChanged(
+        hashCode(),
+        () -> {
+          leftProfile.setMaxAcceleration(maxAcceleration.get());
+          rightProfile.setMaxAcceleration(maxAcceleration.get());
+        },
+        maxAcceleration);
+
+    // Stop when disabled
     if (DriverStation.isDisabled()) {
-      io.stop();
+      setGoal(Goal.STOP);
     }
 
-    if (goal != Goal.CHARACTERIZING) {
-      io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
+    // Check if profile needs to be reset
+    if (!closedLoop && wasClosedLoop) {
+      leftProfile.reset();
+      rightProfile.reset();
+      wasClosedLoop = false;
+    }
+
+    // Run to setpoint
+    if (closedLoop) {
+      // Update goals
+      leftProfile.setGoal(goal.getLeftGoal());
+      rightProfile.setGoal(goal.getRightGoal());
+      io.runVelocity(leftProfile.calculateSetpoint(), rightProfile.calculateSetpoint());
     }
 
     Logger.recordOutput("Flywheels/Goal", goal);
-    Logger.recordOutput("Flywheels/LeftSetpointRpm", goal.getLeftSetpoint());
-    Logger.recordOutput("Flywheels/RightSetpointRpm", goal.getRightSetpoint());
-    Logger.recordOutput("Flywheels/LeftRpm", inputs.leftVelocityRpm);
-    Logger.recordOutput("Flywheels/RightRpm", inputs.rightVelocityRpm);
+    Logger.recordOutput("Flywheels/SetpointLeftRpm", leftProfile.getCurrentSetpoint());
+    Logger.recordOutput("Flywheels/SetpointRightRpm", rightProfile.getCurrentSetpoint());
+    Logger.recordOutput("Flywheels/GoalLeftRpm", goal.getLeftGoal());
+    Logger.recordOutput("Flywheels/GoalRightRpm", goal.getRightGoal());
+  }
+
+  private void setGoal(Goal goal) {
+    if (goal == Goal.CHARACTERIZING || goal == Goal.STOP) {
+      wasClosedLoop = closedLoop;
+      closedLoop = false;
+      return; // Don't set a goal
+    }
+    // If not already controlling to requested goal
+    // set closed loop false
+    closedLoop = this.goal == goal;
+    // Enable close loop
+    if (!closedLoop) {
+      leftProfile.setGoal(goal.getLeftGoal(), inputs.leftVelocityRpm);
+      rightProfile.setGoal(goal.getRightGoal(), inputs.rightVelocityRpm);
+      closedLoop = true;
+    }
+    this.goal = goal;
   }
 
   public void runCharacterizationVolts(double volts) {
-    goal = Goal.CHARACTERIZING;
+    setGoal(Goal.CHARACTERIZING);
     io.runCharacterizationLeftVolts(volts);
     io.runCharacterizationRightVolts(volts);
   }
@@ -109,23 +158,16 @@ public double getCharacterizationVelocity() {
 
   @AutoLogOutput(key = "Flywheels/AtGoal")
   public boolean atGoal() {
-    return Math.abs(inputs.leftVelocityRpm - goal.getLeftSetpoint()) <= shooterTolerance.get()
-        && Math.abs(inputs.rightVelocityRpm - goal.getRightSetpoint()) <= shooterTolerance.get();
-  }
-
-  private void goIdle() {
-    goal = Goal.IDLE;
-  }
-
-  public Command shoot() {
-    return startEnd(() -> goal = Goal.SHOOT, this::goIdle).withName("Flywheels Shooting");
+    return leftProfile.getCurrentSetpoint() == goal.getLeftGoal()
+        && rightProfile.getCurrentSetpoint() == goal.getRightGoal();
   }
 
-  public Command intake() {
-    return startEnd(() -> goal = Goal.INTAKE, this::goIdle).withName("Flywheels Intaking");
+  public Command shootCommand() {
+    return startEnd(() -> setGoal(Goal.SHOOT), () -> setGoal(Goal.IDLE)).withName("FlywheelsShoot");
   }
 
-  public Command eject() {
-    return startEnd(() -> goal = Goal.EJECT, this::goIdle).withName("Flywheels Ejecting");
+  public Command intakeCommand() {
+    return startEnd(() -> setGoal(Goal.INTAKE), () -> setGoal(Goal.IDLE))
+        .withName("FlywheelsIntake");
   }
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java

@@ -18,9 +18,9 @@
 
 public class FlywheelsIOSim implements FlywheelsIO {
   private final FlywheelSim leftSim =
-      new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292);
+      new FlywheelSim(DCMotor.getKrakenX60Foc(1), flywheelConfig.reduction(), 0.00363458292);
   private final FlywheelSim rightSim =
-      new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292);
+      new FlywheelSim(DCMotor.getKrakenX60Foc(1), flywheelConfig.reduction(), 0.00363458292);
 
   private final PIDController leftController =
       new PIDController(gains.kP(), gains.kI(), gains.kD());

src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java

@@ -31,8 +31,8 @@ public class FlywheelsIOSparkFlex implements FlywheelsIO {
 
   public FlywheelsIOSparkFlex() {
     // Init Hardware
-    leftMotor = new CANSparkFlex(config.leftID(), CANSparkFlex.MotorType.kBrushless);
-    rightMotor = new CANSparkFlex(config.rightID(), CANSparkFlex.MotorType.kBrushless);
+    leftMotor = new CANSparkFlex(flywheelConfig.leftID(), CANSparkFlex.MotorType.kBrushless);
+    rightMotor = new CANSparkFlex(flywheelConfig.rightID(), CANSparkFlex.MotorType.kBrushless);
     leftEncoder = leftMotor.getEncoder();
     rightEncoder = rightMotor.getEncoder();
 
@@ -72,15 +72,15 @@ public FlywheelsIOSparkFlex() {
   @Override
   public void updateInputs(FlywheelsIOInputs inputs) {
     inputs.leftPositionRads =
-        Units.rotationsToRadians(leftEncoder.getPosition()) / config.reduction();
-    inputs.leftVelocityRpm = leftEncoder.getVelocity() / config.reduction();
+        Units.rotationsToRadians(leftEncoder.getPosition()) / flywheelConfig.reduction();
+    inputs.leftVelocityRpm = leftEncoder.getVelocity() / flywheelConfig.reduction();
     inputs.leftAppliedVolts = leftMotor.getAppliedOutput();
     inputs.leftOutputCurrent = leftMotor.getOutputCurrent();
     inputs.leftTempCelsius = leftMotor.getMotorTemperature();
 
     inputs.rightPositionRads =
-        Units.rotationsToRadians(rightEncoder.getPosition()) / config.reduction();
-    inputs.rightVelocityRpm = rightEncoder.getVelocity() / config.reduction();
+        Units.rotationsToRadians(rightEncoder.getPosition()) / flywheelConfig.reduction();
+    inputs.rightVelocityRpm = rightEncoder.getVelocity() / flywheelConfig.reduction();
     inputs.rightAppliedVolts = rightMotor.getAppliedOutput();
     inputs.rightOutputCurrent = rightMotor.getOutputCurrent();
     inputs.rightTempCelsius = rightMotor.getMotorTemperature();
@@ -95,13 +95,13 @@ public void runVolts(double leftVolts, double rightVolts) {
   @Override
   public void runVelocity(double leftRpm, double rightRpm) {
     leftController.setReference(
-        leftRpm * config.reduction(),
+        leftRpm * flywheelConfig.reduction(),
         CANSparkBase.ControlType.kVelocity,
         0,
         ff.calculate(leftRpm),
         SparkPIDController.ArbFFUnits.kVoltage);
     rightController.setReference(
-        rightRpm * config.reduction(),
+        rightRpm * flywheelConfig.reduction(),
         CANSparkBase.ControlType.kVelocity,
         0,
         ff.calculate(rightRpm),

src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java

@@ -0,0 +1,75 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package org.littletonrobotics.frc2024.util;
+
+import lombok.Getter;
+import lombok.Setter;
+
+/** Ramps up and down to setpoint for velocity closed loop control */
+public class LinearProfile {
+  private double dv;
+  @Getter private final double period;
+  @Getter private double currentSetpoint = 0;
+  @Getter @Setter private double goal = 0;
+
+  /**
+   * Creates a new LinearProfile
+   *
+   * @param maxAcceleration The max ramp rate in velocity in rpm/sec
+   * @param period Period of control loop (0.02)
+   */
+  public LinearProfile(double maxAcceleration, double period) {
+    this.period = period;
+    setMaxAcceleration(maxAcceleration);
+  }
+
+  /** Set the max acceleration constraint in rpm/sec */
+  public void setMaxAcceleration(double maxAcceleration) {
+    dv = maxAcceleration * period;
+  }
+
+  /**
+   * Sets the target setpoint, starting from the current speed
+   *
+   * @param goal Target setpoint
+   * @param currentSpeed Current speed, to be used as the starting setpoint
+   */
+  public void setGoal(double goal, double currentSpeed) {
+    this.goal = goal;
+    currentSetpoint = currentSpeed;
+  }
+
+  /** Resets target setpoint and current setpoint */
+  public void reset() {
+    currentSetpoint = 0;
+    goal = 0;
+  }
+
+  /**
+   * Returns the current setpoint to send to motors
+   *
+   * @return Setpoint to send to motors
+   */
+  public double calculateSetpoint() {
+    if (EqualsUtil.epsilonEquals(goal, currentSetpoint)) {
+      return currentSetpoint;
+    }
+    if (goal > currentSetpoint) {
+      currentSetpoint += dv;
+      if (currentSetpoint > goal) {
+        currentSetpoint = goal;
+      }
+    } else if (goal < currentSetpoint) {
+      currentSetpoint -= dv;
+      if (currentSetpoint < goal) {
+        currentSetpoint = goal;
+      }
+    }
+    return currentSetpoint;
+  }
+}