refactor(arm): WIP.

Author: haydenheroux

Diff for: src/main/java/frc/robot/RobotContainer.java

@@ -88,7 +88,7 @@ private void configureBindings() {
     operatorController.leftBumper().onTrue(superstructure.eject());
     operatorController.leftTrigger().onTrue(superstructure.intake());
 
-    operatorController.rightBumper().onTrue(superstructure.podium());
+    operatorController.rightBumper().onTrue(superstructure.skim());
     operatorController.rightTrigger().onTrue(superstructure.subwoofer());
 
     operatorController.a().onTrue(superstructure.amp());

Diff for: src/main/java/frc/robot/arm/Arm.java

@@ -10,34 +10,39 @@
 import frc.lib.controller.PositionControllerIO.PositionControllerIOValues;
 import frc.robot.arm.ArmConstants.ShoulderConstants;
 
-/** Subsystem class for the arm subsystem. */
+/** Arm subsystem. */
 public class Arm extends Subsystem {
 
   /** Instance variable for the arm subsystem singleton. */
   private static Arm instance = null;
 
-  /** Shoulder. */
+  /** Shoulder position controller. */
   private final PositionControllerIO shoulder;
 
-  /** Shoulder values. */
+  /** Shoulder position controller values. */
   private final PositionControllerIOValues shoulderValues;
 
-  private double lastTimeSeconds;
+  /** Arm's goal. Set by superstructure to use as end goal for setpoints. */
+  private ArmState goal;
 
-  /** Arm goal. */
-  private ArmState setpoint, goal;
+  /** Arm's setpoint. Updated periodically to reach the goal within constraints. */
+  private ArmState setpoint;
 
-  /** Creates a new instance of the arm subsystem. */
+  /** Time of the start of the previous periodic call. Used for calculating the time elapsed since the previous setpoint was generated. */
+  private double previousTimeSeconds;
+
+  /** Creates a new arm subsystem and configures arm hardware. */
   private Arm() {
     shoulder = ArmFactory.createShoulder();
-    shoulderValues = new PositionControllerIOValues();
     shoulder.configure(ShoulderConstants.CONTROLLER_CONSTANTS);
 
-    lastTimeSeconds = Timer.getFPGATimestamp();
+    shoulderValues = new PositionControllerIOValues();
+
+    previousTimeSeconds = Timer.getFPGATimestamp();
 
-    setPosition(ArmState.INITIAL);
-    setpoint = ArmState.INITIAL;
-    goal = ArmState.INITIAL;
+    setPosition(ArmState.STOW_POSITION);
+    setpoint = ArmState.STOW_POSITION;
+    goal = ArmState.STOW_POSITION;
   }
 
   /**
@@ -59,17 +64,18 @@ public void periodic() {
 
     double timeSeconds = Timer.getFPGATimestamp();
 
-    double dt = timeSeconds - lastTimeSeconds;
-
-    lastTimeSeconds = timeSeconds;
+    // Calculate the time elapsed in seconds since the previous setpoint was generated
+    double timeElapsedSeconds = timeSeconds - previousTimeSeconds;
 
     setpoint =
         new ArmState(
             ShoulderConstants.MOTION_PROFILE.calculate(
-                dt, setpoint.shoulderRotations(), goal.shoulderRotations()));
+                timeElapsedSeconds, setpoint.shoulderRotations(), goal.shoulderRotations()));
 
     shoulder.setSetpoint(
         setpoint.shoulderRotations().position, setpoint.shoulderRotations().velocity);
+
+    previousTimeSeconds = timeSeconds;
   }
 
   @Override
@@ -82,21 +88,41 @@ public void addToShuffleboard(ShuffleboardTab tab) {
             () -> Units.rotationsToDegrees(setpoint.shoulderRotations().position));
   }
 
+  /**
+   * Returns the arm's state.
+   * 
+   * @return the arm's state.
+   */
   public ArmState getState() {
     return new ArmState(
         new TrapezoidProfile.State(
             shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond));
   }
 
+  /**
+   * Sets the arm's goal state.
+   * 
+   * @param goal the arm's goal state.
+   */
   public void setGoal(ArmState goal) {
     this.goal = goal;
   }
 
+  /**
+   * Returns true if at the arm's set goal.
+   * 
+   * @return true if at the arm's set goal.
+   */
   public boolean atGoal() {
     return getState().at(goal);
   }
 
-  public void setPosition(ArmState armState) {
+  /**
+   * Sets the position of the shoulder motor encoders.
+   * 
+   * @param armState the position.
+   */
+  private void setPosition(ArmState armState) {
     shoulder.setPosition(armState.shoulderRotations().position);
   }
 }

Diff for: src/main/java/frc/robot/arm/ArmConstants.java

@@ -25,21 +25,35 @@ public static class ShoulderConstants {
     public static final PIDFConstants PIDF = new PIDFConstants();
 
     static {
-      PIDF.kS = 0.14;
-      PIDF.kG = 0.5125;
-      PIDF.kV = 4.0;
-      PIDF.kP = 4.0;
+      PIDF.kS = 0.14; // volts
+      PIDF.kG = 0.5125; // volts
+      PIDF.kV = 4.0; // volts per rotation per second
+      PIDF.kP = 4.0; // volts per rotation
     }
 
     /** Shoulder's controller constants. */
     public static final PositionControllerIOConstants CONTROLLER_CONSTANTS =
         new PositionControllerIOConstants();
 
     static {
+      // The leading shoulder motor has the correct reference frame
       CONTROLLER_CONSTANTS.ccwPositiveMotor = true;
+      
+      // The shoulder's absolute encoder has the wrong reference frame
       CONTROLLER_CONSTANTS.ccwPositiveAbsoluteEncoder = false;
+      
+      // Use brake mode to hold the arm in place
+      // Since the arm rests on a hard stop this isn't strictly necessary,
+      // but it prevents the arm from being knocked around if disabled
       CONTROLLER_CONSTANTS.neutralBrake = true;
+      
+      // Ask CAD if they can calculate this reduction or count the gears
       CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571;
+
+      // 1. Rest the arm in the stow position
+      // 2. Use a digital level to determine the actual angle of the stow position
+      // 3. Observe the value reported by the absolute encoder while the arm is in the stow position
+      // 4. Calculate the absolute encoder offset by subtracting the absolute encoder value from the actual angle
       CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-173.135);
     }
 
@@ -57,20 +71,10 @@ public static class ShoulderConstants {
     /** Motion profile of the shoulder. */
     public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS);
 
-    public static final Rotation2d STOW = Rotation2d.fromDegrees(-26);
-
-    public static final Rotation2d LOB = Rotation2d.fromDegrees(-26);
-
-    public static final Rotation2d SUBWOOFER = Rotation2d.fromDegrees(-26);
-
-    public static final Rotation2d PODIUM = Rotation2d.fromDegrees(-10);
-
-    public static final Rotation2d EJECT = Rotation2d.fromDegrees(30);
-
-    public static final Rotation2d SKIM = Rotation2d.fromDegrees(30);
-
-    public static final Rotation2d AMP = Rotation2d.fromDegrees(60);
+    /** Shoulder angle when the arm is stowed. */
+    public static final Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(-26);
 
-    public static final Rotation2d BLOOP = Rotation2d.fromDegrees(-26);
+    /** Shoulder angle when the shooter is parallel to the ground. Used for flat shots. */
+    public static final Rotation2d FLAT_ANGLE = Rotation2d.fromDegrees(30);
   }
 }

Diff for: src/main/java/frc/robot/arm/ArmState.java

@@ -7,25 +7,17 @@
 import frc.robot.arm.ArmConstants.ShoulderConstants;
 import java.util.Objects;
 
+/** Represents an arm's state */
 public record ArmState(State shoulderRotations) {
 
-  public static final ArmState INITIAL = new ArmState(ShoulderConstants.STOW);
+  /** State for stow position. */
+  public static final ArmState STOW_POSITION = new ArmState(ShoulderConstants.STOW_ANGLE);
 
-  public static final ArmState STOW = new ArmState(ShoulderConstants.STOW);
+  /** State for flat position. */
+  public static final ArmState FLAT_POSITION = new ArmState(ShoulderConstants.FLAT_ANGLE);
 
-  public static final ArmState SUBWOOFER = new ArmState(ShoulderConstants.SUBWOOFER);
-
-  public static final ArmState PODIUM = new ArmState(ShoulderConstants.PODIUM);
-
-  public static final ArmState EJECT = new ArmState(ShoulderConstants.EJECT);
-
-  public static final ArmState SKIM = new ArmState(ShoulderConstants.SKIM);
-
-  public static final ArmState LOB = new ArmState(ShoulderConstants.LOB);
-
-  public static final ArmState AMP = new ArmState(ShoulderConstants.AMP);
-
-  public static final ArmState BLOOP = new ArmState(ShoulderConstants.BLOOP);
+  /** State for amp position. */
+  public static final ArmState AMP_POSITION = new ArmState(Rotation2d.fromDegrees(60));
 
   public ArmState {
     Objects.requireNonNull(shoulderRotations);

Diff for: src/main/java/frc/robot/auto/Auto.java

@@ -5,8 +5,6 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -17,8 +15,6 @@
 import frc.robot.superstructure.Superstructure;
 import frc.robot.swerve.Swerve;
 import frc.robot.swerve.SwerveConstants;
-import java.util.function.Consumer;
-import java.util.function.Supplier;
 
 /** Subsystem class for the auto subsystem. */
 public class Auto extends Subsystem {
@@ -44,40 +40,38 @@ private Auto() {
     superstructure = Superstructure.getInstance();
     swerve = Swerve.getInstance();
 
-    Supplier<Pose2d> robotPositionSupplier = () -> odometry.getPosition();
-
-    Consumer<Pose2d> robotPositionConsumer = position -> odometry.setPosition(position);
-
-    Supplier<ChassisSpeeds> swerveChassisSpeedsSupplier = () -> swerve.getChassisSpeeds();
-
-    Consumer<ChassisSpeeds> swerveChassisSpeedsConsumer =
-        chassisSpeeds -> swerve.setChassisSpeeds(chassisSpeeds);
-
-    HolonomicPathFollowerConfig holonomicPathFollowerConfig =
+    AutoBuilder.configureHolonomic(
+        odometry::getPosition,
+        odometry::setPosition,
+        swerve::getChassisSpeeds,
+        swerve::setChassisSpeeds,
+        // TODO Move some of these constants to AutoConstants
         new HolonomicPathFollowerConfig(
             new PIDConstants(1.0),
             new PIDConstants(1.0),
             SwerveConstants.MAXIMUM_ATTAINABLE_SPEED,
             SwerveConstants.NORTH_EAST_MODULE_CONFIG.position().getNorm(),
-            new ReplanningConfig());
-
-    AutoBuilder.configureHolonomic(
-        robotPositionSupplier,
-        robotPositionConsumer,
-        swerveChassisSpeedsSupplier,
-        swerveChassisSpeedsConsumer,
-        holonomicPathFollowerConfig,
+            new ReplanningConfig()),
         AllianceFlipHelper::shouldFlip,
         swerve);
 
+    registerNamedCommands();
+
+    autoChooser = AutoBuilder.buildAutoChooser();
+  }
+
+  /**
+   * Registers PathPlanner named commands.
+   */
+  private void registerNamedCommands() {
     NamedCommands.registerCommand("stow", superstructure.stow());
     NamedCommands.registerCommand(
-        "shoot", superstructure.subwoofer().withTimeout(1.5)); // 1 second could work
-    NamedCommands.registerCommand(
-        "shootNoPull", superstructure.subwooferNoPull().withTimeout(1.5)); // 1 second could work
+        "shoot", superstructure.subwoofer().withTimeout(1.5));
     NamedCommands.registerCommand("intake", superstructure.intakeInstant());
 
-    autoChooser = AutoBuilder.buildAutoChooser();
+    // TODO Deprecate after competition
+    NamedCommands.registerCommand(
+        "shootNoPull", superstructure.subwooferNoPull().withTimeout(1.5));
   }
 
   /**
@@ -93,9 +87,6 @@ public static Auto getInstance() {
     return instance;
   }
 
-  @Override
-  public void periodic() {}
-
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
     Telemetry.makeFullscreen(tab.add("Auto Chooser", autoChooser));

Diff for: src/main/java/frc/robot/shooter/ShooterState.java

@@ -20,12 +20,6 @@ public record ShooterState(
   public static final ShooterState SUBWOOFER =
       new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FAST_FEED_SPEED);
 
-  public static final ShooterState PODIUM =
-      new ShooterState(FlywheelConstants.PODIUM_SPEED, SerializerConstants.FAST_FEED_SPEED);
-
-  public static final ShooterState LOB =
-      new ShooterState(FlywheelConstants.LOB_SPEED, SerializerConstants.FAST_FEED_SPEED);
-
   public static final ShooterState SKIM =
       new ShooterState(FlywheelConstants.SKIM_SPEED, SerializerConstants.FAST_FEED_SPEED);
 

Diff for: src/main/java/frc/robot/superstructure/Superstructure.java

@@ -37,8 +37,6 @@ private Superstructure() {
     intake = Intake.getInstance();
     shooter = Shooter.getInstance();
 
-    setPosition(SuperstructureState.STOW);
-
     goal = SuperstructureState.STOW;
   }
 
@@ -103,10 +101,6 @@ private void addStateToShuffleboard(
         () -> state.get().shooterState().serializerVelocityRotationsPerSecond());
   }
 
-  public void setPosition(SuperstructureState state) {
-    arm.setPosition(state.armState());
-  }
-
   public void setGoal(SuperstructureState goal) {
     this.goal = goal;
 
@@ -183,14 +177,6 @@ public Command subwoofer() {
     return shoot(SuperstructureState.SUBWOOFER).withName("SUBWOOFER");
   }
 
-  public Command podium() {
-    return shoot(SuperstructureState.PODIUM).withName("PODIUM");
-  }
-
-  public Command lob() {
-    return shoot(SuperstructureState.LOB).withName("LOB");
-  }
-
   public Command skim() {
     return shoot(SuperstructureState.SKIM).withName("SKIM");
   }