Command cleanup (#97)

* - Commands
   - All: Use Timers instead of manual counter
   - ArmToPosition: setAngle only once in initialize() instead of repeatedly in execute()
   - ArmToPositionTP: deleted

* - Subsystems
   - Shooter
      - No longer setGoal during initialization.
      - No longer filter runVoltage()/runVelocity() calls if value is same as previous.

* Updated Sim Arm constants

src/main/java/frc/robot/commands/arm/ArmToPosition.java

@@ -1,5 +1,6 @@
 package frc.robot.commands.arm;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -11,7 +12,7 @@ public class ArmToPosition extends Command {
   Arm arm;
   DoubleSupplier positionDegrees;
   double targetPositionDegrees;
-  double timeMS;
+  Timer timer = new Timer();
 
   public ArmToPosition(Arm arm, DoubleSupplier positionDegrees) {
     this.arm = arm;
@@ -23,28 +24,24 @@ public ArmToPosition(Arm arm, DoubleSupplier positionDegrees) {
   @Override
   public void initialize() {
     targetPositionDegrees = positionDegrees.getAsDouble();
-    timeMS = 0.0;
+    timer.restart();
 
     if (Constants.debugCommands) {
       System.out.println(
           "START: " + this.getClass().getSimpleName() + " angle: " + targetPositionDegrees);
     }
-  }
 
-  @Override
-  public void execute() {
     arm.setAngle(targetPositionDegrees);
   }
 
   @Override
   public boolean isFinished() {
     if (Math.abs(arm.getAngle() - targetPositionDegrees) <= ArmConstants.pidAngleErrorInDegrees) {
-      timeMS += 20.0;
-      if (timeMS >= ArmConstants.pidSettlingTimeInMilliseconds) {
+      if (timer.get() >= ArmConstants.pidSettlingTimeInSeconds) {
         return true;
       }
     } else {
-      timeMS = 0.0;
+      timer.reset();
     }
     return false;
   }

src/main/java/frc/robot/commands/drive/DriveToYaw.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.Constants;
@@ -16,7 +17,7 @@ public class DriveToYaw extends Command {
   PIDController turnPID =
       new PIDController(
           DriveConstants.rotatePidKp, DriveConstants.rotatePidKi, DriveConstants.rotatePidKd);
-  double timeMS;
+  Timer timer = new Timer();
 
   public DriveToYaw(Drive drive, DoubleSupplier yawDegrees) {
     this.drive = drive;
@@ -32,7 +33,7 @@ public void initialize() {
     targetYaw = this.yawDegrees.getAsDouble();
     turnPID.reset();
     turnPID.setSetpoint(targetYaw);
-    timeMS = 0.0;
+    timer.reset();
     if (Constants.debugCommands) {
       System.out.println(
           "START: "
@@ -54,12 +55,11 @@ public void execute() {
   @Override
   public boolean isFinished() {
     if (turnPID.atSetpoint()) {
-      timeMS += 20.0;
-      if (timeMS >= DriveConstants.pidSettlingTimeInMilliseconds) {
+      if (timer.get() >= DriveConstants.pidSettlingTimeInSeconds) {
         return true;
       }
     } else {
-      timeMS = 0.0;
+      timer.reset();
     }
     return false;
   }

src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java

@@ -1,6 +1,7 @@
 package frc.robot.commands.shooter;
 
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -12,7 +13,7 @@ public class SetShooterVelocity extends Command {
   Shooter shooter;
   DoubleSupplier velocityRPM;
   double targetVelocityRPM;
-  double timeMS;
+  Timer timer = new Timer();
 
   public SetShooterVelocity(Shooter shooter, DoubleSupplier velocityRPM) {
     this.shooter = shooter;
@@ -26,7 +27,7 @@ public void initialize() {
       System.out.println(
           "START: " + this.getClass().getSimpleName() + " velocity: " + velocityRPM.getAsDouble());
     }
-    timeMS = 0.0;
+    timer.restart();
     targetVelocityRPM = velocityRPM.getAsDouble();
     shooter.runVelocity(targetVelocityRPM);
   }
@@ -37,12 +38,11 @@ public boolean isFinished() {
             Units.radiansPerSecondToRotationsPerMinute(shooter.getCurrentSpeed())
                 - targetVelocityRPM)
         <= ShooterConstants.pidVelocityErrorInRPMS) {
-      timeMS += 20.0;
-      if (timeMS >= ShooterConstants.pidSettlingTimeInMilliseconds) {
+      if (timer.get() >= ShooterConstants.pidSettlingTimeInSeconds) {
         return true;
       }
     } else {
-      timeMS = 0.0;
+      timer.reset();
     }
 
     return false;

src/main/java/frc/robot/config/RobotConfig.java

@@ -48,7 +48,7 @@ public static class DriveConstants {
     public static double rotatePidKd = 0.0;
     public static double rotatePidErrorInDegrees = 2.0;
     public static double pidTimeoutInSeconds = 0.5;
-    public static double pidSettlingTimeInMilliseconds = 0.1;
+    public static double pidSettlingTimeInSeconds = 0.1;
 
     public static double slewRateLimiterX = 3;
     public static double slewRateLimiterY = 3;
@@ -62,25 +62,26 @@ public static class ArmConstants {
     public static double absolutePositionOffset = 0; /* 0-1 */
     public static double absoluteEncoderInversion = 1; /* 1 for none, -1 to invert */
 
-    public static double pidKp = 0.1;
+    public static double pidKp = 0.12935;
     public static double pidKi = 0.0;
-    public static double pidKd = 0.0;
+    public static double pidKd = 0.0060174;
     public static double pidMaxOutput = 0.4;
     public static double pidMinOutput = -0.4;
 
-    public static double ffKs = 0.0;
-    public static double ffKv = 0.0;
-    public static double ffKa = 0.0;
-    public static double ffKg = 0.1;
+    public static double ffKs = 0.03402;
+    public static double ffKv = 0.072343;
+    public static double ffKa = 0.00036498;
+    public static double ffKg = 0.88138;
 
     public static double pidAngleErrorInDegrees = 2.0;
-    public static double pidSettlingTimeInMilliseconds = 0.1;
+    public static double pidSettlingTimeInSeconds = 0.0;
     public static double pidTimeoutInSeconds = 3.0;
 
     public static double maxAngleInDegrees = 90.0;
     public static double minAngleInDegrees = 0.0;
     public static double maxVelocityInDegreesPerSecond = 90;
-    public static double maxAccelerationInDegreesPerSecondSquared = 720;
+    public static double maxAccelerationInDegreesPerSecondSquared =
+        maxVelocityInDegreesPerSecond * 16;
 
     public static double intakeAngleInDegrees = 1;
     public static double ejectAngleInDegrees = 15;
@@ -105,7 +106,7 @@ public static class ShooterConstants {
 
     /* PID */
     public static double pidVelocityErrorInRPMS = 300;
-    public static double pidSettlingTimeInMilliseconds = 0.1;
+    public static double pidSettlingTimeInSeconds = 0.0;
     public static double pidTimeoutInSeconds = 2.0;
     public static double pidKp = 0.043566;
     public static double pidKi = 0.0;

src/main/java/frc/robot/config/RobotConfigInferno.java

@@ -36,7 +36,7 @@ public RobotConfigInferno() {
     DriveConstants.rotatePidKd = 0.001;
     DriveConstants.rotatePidErrorInDegrees = 0.5;
     DriveConstants.pidTimeoutInSeconds = 2;
-    DriveConstants.pidSettlingTimeInMilliseconds = 0.5;
+    DriveConstants.pidSettlingTimeInSeconds = 0.5;
 
     drive = new DriveSwerveYAGSL("yagsl/inferno");
 

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

@@ -46,7 +46,6 @@ public ShooterSubsystem(ShooterIO io) {
                 Units.rotationsPerMinuteToRadiansPerSecond(ShooterConstants.maxVelocityInRPMs),
                 Units.rotationsPerMinuteToRadiansPerSecond(
                     ShooterConstants.maxAccelerationInRPMsSquared))));
-    setGoal(0);
 
     this.io = io;
     useSoftwarePidVelocityControl = !io.supportsHardwarePid();
@@ -103,25 +102,21 @@ public double getMeasurement() {
   @Override
   // Sets the voltage to volts. the volts value is -12 to 12
   public void runVoltage(double volts) {
-    if (targetVoltage != volts) {
-      targetVoltage = volts;
-      targetVelocityRadPerSec = 0;
-      this.targetVelocityRPM = ShooterConstants.maxVelocityInRPMs * (volts / 12.0);
-      disable(); // disable PID control
-      io.setVoltage(targetVoltage);
-    }
+    targetVoltage = volts;
+    targetVelocityRadPerSec = 0;
+    this.targetVelocityRPM = ShooterConstants.maxVelocityInRPMs * (volts / 12.0);
+    disable(); // disable PID control
+    io.setVoltage(targetVoltage);
   }
 
   @Override
   public void runVelocity(double velocityRPM) {
     velocityRPM = MathUtil.clamp(velocityRPM, 0, ShooterConstants.maxVelocityInRPMs);
-    if (this.targetVelocityRPM != velocityRPM) {
-      targetVoltage = -1;
-      this.targetVelocityRPM = velocityRPM;
-      targetVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
-      setGoal(targetVelocityRadPerSec);
-      enable();
-    }
+    targetVoltage = -1;
+    this.targetVelocityRPM = velocityRPM;
+    targetVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
+    setGoal(targetVelocityRadPerSec);
+    enable();
   }
 
   @Override