3/5/2024 controls refactor

simgui.json

@@ -23,12 +23,17 @@
       "/SmartDashboard/arm mech": "Mechanism2d"
     },
     "windows": {
-      "/SmartDashboard/Mechanism": {
+      "/Shuffleboard/Auto/Auto Chooser": {
         "window": {
           "visible": true
         }
       },
-      "/SmartDashboard/SendableChooser[0]": {
+      "/Shuffleboard/Odometry/Field/Field": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Mechanism": {
         "window": {
           "visible": true
         }
@@ -66,12 +71,6 @@
             "open": true
           }
         },
-        "Intake": {
-          "Roller": {
-            "open": true
-          },
-          "open": true
-        },
         "Odometry": {
           "Position": {
             "open": true
@@ -86,7 +85,8 @@
           },
           "Serializer": {
             "open": true
-          }
+          },
+          "open": true
         },
         "Swerve": {
           "Constants": {
@@ -119,7 +119,12 @@
         "limitSwitch": {
           "open": true
         },
-        "open": true
+        "shoulderProfile": {
+          "open": true
+        },
+        "wristProfile": {
+          "open": true
+        }
       },
       "shooter": {
         "beamBreakSensorSim": {

src/main/java/frc/lib/TwoJointedArmFeedforward.java → src/main/java/frc/lib/DoubleJointedArmFeedforward.java

@@ -4,7 +4,7 @@
 import org.ejml.data.MatrixType;
 import org.ejml.simple.SimpleMatrix;
 
-public class TwoJointedArmFeedforward {
+public class DoubleJointedArmFeedforward {
 
   private final double m1;
   private final double m2;
@@ -22,7 +22,7 @@ public class TwoJointedArmFeedforward {
   private final SimpleMatrix Kb_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM);
   private final SimpleMatrix B_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM);
 
-  public TwoJointedArmFeedforward(JointConstants shoulder, JointConstants elbow) {
+  public DoubleJointedArmFeedforward(JointConstants shoulder, JointConstants elbow) {
     this.m1 = shoulder.massKg();
     this.m2 = elbow.massKg();
     this.l1 = shoulder.lengthMeters();

src/main/java/frc/lib/SingleJointedArmFeedforward.java

@@ -1,29 +1,79 @@
 package frc.lib;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public class SingleJointedArmFeedforward {
 
-  private final ArmFeedforward feedforward;
+  public static class SingleJointedArmFeedforwardConstants {
+    public double kS;
+    public double kG;
+    public double kV;
+    public double kA;
 
-  public SingleJointedArmFeedforward(double ks, double kg, double kv) {
-    feedforward = new ArmFeedforward(ks, kg, kv);
+    public SingleJointedArmFeedforwardConstants() {
+      this.kS = 0.0;
+      this.kG = 0.0;
+      this.kV = 0.0;
+      this.kA = 0.0;
+    }
+
+    public SingleJointedArmFeedforwardConstants withKs(double kS) {
+      this.kS = kS;
+      return this;
+    }
+
+    public SingleJointedArmFeedforwardConstants withKg(double kG) {
+      this.kG = kG;
+      return this;
+    }
+
+    /**
+     * Calculates the gravity compensation constant for an arm given the voltage applied at an
+     * equilibrium position.
+     *
+     * @param angle the equilibrium position of the arm.
+     * @param volts the voltage applied to the arm to hold it the equilibrium position.
+     * @return the gravity compensation constant for the arm.
+     */
+    public SingleJointedArmFeedforwardConstants withKg(Rotation2d angle, double volts) {
+      double kG = volts / angle.getCos();
+
+      return this.withKg(kG);
+    }
+
+    public SingleJointedArmFeedforwardConstants withKv(double kV) {
+      this.kV = kV;
+      return this;
+    }
+
+    public SingleJointedArmFeedforwardConstants withKa(double kA) {
+      this.kA = kA;
+      return this;
+    }
+  }
+
+  private final SingleJointedArmFeedforwardConstants constants;
+
+  public SingleJointedArmFeedforward() {
+    this.constants = new SingleJointedArmFeedforwardConstants();
   }
 
-  public SingleJointedArmFeedforward(double ks, double kg, double kv, double ka) {
-    feedforward = new ArmFeedforward(ks, kg, kv, ka);
+  public SingleJointedArmFeedforward(SingleJointedArmFeedforwardConstants constants) {
+    this.constants = constants;
   }
 
   public double calculate(Rotation2d position) {
-    return feedforward.calculate(position.getRadians(), 0);
+    return calculate(position, new Rotation2d());
   }
 
-  public double calculate(Rotation2d position, double velocity) {
-    return feedforward.calculate(position.getRadians(), velocity);
+  public double calculate(Rotation2d position, Rotation2d velocity) {
+    return calculate(position, new Rotation2d(), new Rotation2d());
   }
 
-  public double calculate(Rotation2d position, double velocity, double acceleration) {
-    return feedforward.calculate(position.getRadians(), velocity, acceleration);
+  public double calculate(Rotation2d position, Rotation2d velocity, Rotation2d acceleration) {
+    return constants.kS * Math.signum(velocity.getRotations())
+        + constants.kG * position.getCos()
+        + constants.kV * velocity.getRotations()
+        + constants.kA * acceleration.getRotations();
   }
 }

src/main/java/frc/lib/TrapezoidProfileTelemetry.java

@@ -0,0 +1,53 @@
+package frc.lib;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.networktables.DoubleEntry;
+
+public class TrapezoidProfileTelemetry {
+
+  private final DoubleEntry measuredPosition, measuredVelocity;
+  private final DoubleEntry setpointPosition, setpointVelocity;
+  private final DoubleEntry goalPosition, goalVelocity;
+
+  public TrapezoidProfileTelemetry(String table) {
+    measuredPosition = Telemetry.addDoubleEntry(table, "measuredPosition");
+    measuredVelocity = Telemetry.addDoubleEntry(table, "measuredVelocity");
+    setpointPosition = Telemetry.addDoubleEntry(table, "setpointPosition");
+    setpointVelocity = Telemetry.addDoubleEntry(table, "setpointVelocity");
+    goalPosition = Telemetry.addDoubleEntry(table, "goalPosition");
+    goalVelocity = Telemetry.addDoubleEntry(table, "goalVelocity");
+  }
+
+  public void updateMeasurement(double position, double velocity) {
+    measuredPosition.set(position);
+    measuredVelocity.set(velocity);
+  }
+
+  public void updateMeasurement(State state) {
+    updateMeasurement(state.position, state.velocity);
+  }
+
+  public void updateSetpoint(double position, double velocity) {
+    setpointPosition.set(position);
+    setpointVelocity.set(velocity);
+  }
+
+  public void updateSetpoint(State state) {
+    updateSetpoint(state.position, state.velocity);
+  }
+
+  public void updateGoal(double position, double velocity) {
+    goalPosition.set(position);
+    goalVelocity.set(velocity);
+  }
+
+  public void updateGoal(State state) {
+    updateGoal(state.position, state.velocity);
+  }
+
+  public void update(State measurement, State setpoint, State goal) {
+    updateMeasurement(measurement);
+    updateSetpoint(setpoint);
+    updateGoal(goal);
+  }
+}

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

@@ -2,7 +2,6 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.Telemetry;
 import frc.robot.arm.Arm;
@@ -11,7 +10,6 @@
 import frc.robot.intake.Intake;
 import frc.robot.odometry.Odometry;
 import frc.robot.shooter.Shooter;
-import frc.robot.swerve.DriveCommand;
 import frc.robot.swerve.Swerve;
 
 /** Initializes subsystems and commands. */
@@ -44,6 +42,7 @@ private RobotContainer() {
     operatorController = new CommandXboxController(1);
 
     initializeTelemetry();
+    configureDefaultCommands();
     configureBindings();
   }
 
@@ -66,36 +65,27 @@ private void initializeTelemetry() {
     SmartDashboard.putData("Mechanism", RobotMechanisms.getInstance().getMechanism());
   }
 
-  /** Configures operator controller bindings. */
-  private void configureBindings() {
-    swerve.setDefaultCommand(new DriveCommand(driverController));
+  /** Configures subsystem default commands. */
+  private void configureDefaultCommands() {
+    arm.setDefaultCommand(arm.stow());
+    intake.setDefaultCommand(intake.stow());
+    swerve.setDefaultCommand(swerve.driveWithController(driverController));
+  }
 
+  /** Configures controller bindings. */
+  private void configureBindings() {
     driverController.a().whileTrue(swerve.forwards());
     driverController.b().whileTrue(swerve.sideways());
     driverController.x().whileTrue(swerve.cross());
     driverController.y().onTrue(odometry.tare());
 
-    operatorController
-        .leftTrigger()
-        .whileTrue(auto.readyIntake().andThen(auto.intakeNote()))
-        .whileFalse((auto.stow()));
-    operatorController
-        .leftBumper()
-        .whileTrue(intake.out().andThen(intake.outtake()))
-        .whileFalse(auto.stow());
-
-    operatorController
-        .rightTrigger()
-        .whileTrue(auto.readyShoot().andThen(shooter.spin()))
-        .whileFalse(auto.stow());
-    operatorController.rightBumper().whileTrue(shooter.serialize());
+    operatorController.leftTrigger().whileTrue(auto.intakePosition().andThen(auto.intakeNote()));
+    operatorController.leftBumper().whileTrue(intake.unstow().andThen(intake.outtake()));
 
-    operatorController.povUp().whileTrue(climber.up());
-    operatorController
-        .povDown()
-        .whileTrue(Commands.parallel(auto.readyIntake().repeatedly(), climber.down()));
+    operatorController.rightTrigger().whileTrue(auto.shootPosition().andThen(shooter.spin()));
+    operatorController.rightBumper().whileTrue(shooter.serialize());
 
-    operatorController.a().whileTrue(arm.amp()).whileFalse(auto.stow());
+    operatorController.a().whileTrue(arm.amp());
   }
 
   /**