Merge branch 'main' into 2_15_2024_endgame

simgui.json

@@ -13,6 +13,7 @@
       "/Shuffleboard/Auto/SendableChooser[0]": "String Chooser",
       "/Shuffleboard/Vision/Field": "Field2d",
       "/SmartDashboard/Arm Mechanism": "Mechanism2d",
+      "/SmartDashboard/Mechanism": "Mechanism2d",
       "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": "Field2d"
     },
     "windows": {
@@ -21,7 +22,7 @@
           "visible": true
         }
       },
-      "/SmartDashboard/Arm Mechanism": {
+      "/SmartDashboard/Mechanism": {
         "window": {
           "visible": true
         }
@@ -40,13 +41,13 @@
     "transitory": {
       "Shuffleboard": {
         "Arm": {
-          "Goal": {
+          "Feedforward": {
             "open": true
           },
           "Position": {
             "open": true
           },
-          "Setpoint": {
+          "Velocities": {
             "open": true
           }
         },

src/main/java/frc/lib/AccelerationCalculator.java

@@ -0,0 +1,20 @@
+package frc.lib;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class AccelerationCalculator {
+
+  private double previousVelocity;
+  private double previousTime;
+
+  public double calculate(double velocity) {
+    double time = Timer.getFPGATimestamp();
+
+    double acceleration = (velocity - previousVelocity) / (time - previousTime);
+
+    previousVelocity = velocity;
+    previousTime = time;
+
+    return acceleration;
+  }
+}

src/main/java/frc/lib/JointConstants.java

@@ -0,0 +1,75 @@
+package frc.lib;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import java.util.Objects;
+
+public record JointConstants(
+    double massKg,
+    double lengthMeters,
+    double radiusMeters,
+    double moiKgMetersSquared,
+    double gearing,
+    DCMotor motor,
+    int motorCount) {
+
+  /**
+   * Creates joint constants for the joint of an arm.
+   *
+   * @param massKg the joint's mass in kilograms.
+   * @param lengthMeters the joint's length in meters.
+   * @param radius the distance between the joint's pivot and the joint's center of mass in meters.
+   * @param moiKgMetersSquared the joint's moment of inertia in kilograms meters squared.
+   * @param gearing the gearing between the joint's motor and the joint's pivot.
+   * @param motor the type of the joint's driving motor.
+   * @param motorCount the number of the driving motors.
+   */
+  public JointConstants {
+    Objects.requireNonNull(massKg);
+    Objects.requireNonNull(lengthMeters);
+    Objects.requireNonNull(radiusMeters);
+    Objects.requireNonNull(moiKgMetersSquared);
+    Objects.requireNonNull(gearing);
+    Objects.requireNonNull(motor);
+    Objects.requireNonNull(motorCount);
+  }
+
+  /**
+   * Creates joint constants for the joint of an arm.
+   *
+   * @param massKg the joint's mass in kilograms.
+   * @param length the joint's length in meters.
+   * @param radius the distance between the joint's pivot and the joint's center of mass in meters.
+   * @param moi the joint's moment of inertia in kilograms meters squared.
+   * @param gearing the gearing between the joint's motor and the joint's pivot.
+   * @param motor the type of the joint's driving motor.
+   */
+  public JointConstants(
+      double massKg, double length, double radius, double moi, double gearing, DCMotor motor) {
+    this(massKg, length, radius, moi, gearing, motor, 1);
+  }
+
+  /**
+   * Calculates the torque generated by the joint's gearbox element for the feedforward matrix.
+   *
+   * @return the torque generated by the joint's gearbox element for the feedforward matrix.
+   */
+  // TODO Determine units
+  public double torque() {
+    return gearing * motorCount * motor.KtNMPerAmp / motor.rOhms;
+  }
+
+  /**
+   * Calculates the torque loss due to back-emf for the feedforward matrix.
+   *
+   * @return the torque loss due to back-emf for the feedforward matrix.
+   */
+  // TODO Determine units
+  public double torqueLoss() {
+    return gearing
+        * gearing
+        * motorCount
+        * motor.KtNMPerAmp
+        / motor.rOhms
+        / motor.KvRadPerSecPerVolt;
+  }
+}

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

@@ -0,0 +1,154 @@
+package frc.lib;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.ejml.data.MatrixType;
+import org.ejml.simple.SimpleMatrix;
+
+public class TwoJointedArmFeedforward {
+
+  private final double m1;
+  private final double m2;
+  private final double l1;
+  private final double r1;
+  private final double r2;
+  private final double I1;
+  private final double I2;
+  private final double g1;
+  private final double g2;
+
+  private final SimpleMatrix M_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM);
+  private final SimpleMatrix C_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM);
+  private final SimpleMatrix Tg_VECTOR = new SimpleMatrix(2, 1, MatrixType.DDRM);
+  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) {
+    this.m1 = shoulder.massKg();
+    this.m2 = elbow.massKg();
+    this.l1 = shoulder.lengthMeters();
+    this.r1 = shoulder.radiusMeters();
+    this.r2 = elbow.radiusMeters();
+    this.I1 = shoulder.moiKgMetersSquared();
+    this.I2 = elbow.moiKgMetersSquared();
+    this.g1 = shoulder.gearing();
+    this.g2 = elbow.gearing();
+
+    B_MATRIX.set(0, 0, shoulder.torque());
+    B_MATRIX.set(1, 1, elbow.torque());
+    B_MATRIX.set(1, 0, 0);
+    B_MATRIX.set(0, 1, 0);
+
+    Kb_MATRIX.set(0, 0, shoulder.torqueLoss());
+    Kb_MATRIX.set(1, 1, elbow.torqueLoss());
+    Kb_MATRIX.set(1, 0, 0);
+    Kb_MATRIX.set(0, 1, 0);
+  }
+
+  /**
+   * Calculates the M matrix.
+   *
+   * @param elbowPosition the position of the elbow joint.
+   */
+  public void updateMMatrix(Rotation2d elbowPosition) {
+    double c2 = elbowPosition.getCos();
+    double diagonal = m2 * (r2 * r2 + l1 * r2 * c2) + I2;
+
+    M_MATRIX.set(0, 0, m1 * r1 * r1 + m2 * (l1 * l1 + r2 * r2 + 2 * l1 * r2 * c2) + I1 + I2);
+    M_MATRIX.set(0, 1, diagonal);
+    M_MATRIX.set(1, 0, diagonal);
+    M_MATRIX.set(1, 1, m2 * r2 * r2 + I2);
+  }
+
+  /**
+   * Calculates the C matrix.
+   *
+   * @param elbowPosition the position of the elbow joint.
+   * @param shoulderVelocity the velocity of the shoulder joint.
+   * @param elbowVelocity the velocity of the elbow joint.
+   */
+  public void updateCMatrix(
+      Rotation2d elbowPosition, Rotation2d shoulderVelocity, Rotation2d elbowVelocity) {
+    double s2 = elbowPosition.getSin();
+
+    C_MATRIX.set(0, 0, -m2 * l1 * r2 * s2 * elbowVelocity.getRadians());
+    C_MATRIX.set(
+        0, 1, -m2 * l1 * r2 * s2 * (shoulderVelocity.getRadians() + elbowVelocity.getRadians()));
+    C_MATRIX.set(1, 0, m2 * l1 * r2 * s2 * shoulderVelocity.getRadians());
+    C_MATRIX.set(1, 1, 0);
+  }
+
+  /**
+   * Calculates the Tg vector.
+   *
+   * @param shoulderPosition the position of the shoulder joint.
+   * @param elbowPosition the position of the elbow joint.
+   */
+  public void updateTgVector(Rotation2d shoulderPosition, Rotation2d elbowPosition) {
+    double c1 = shoulderPosition.getCos();
+    double c12 = shoulderPosition.plus(elbowPosition).getCos();
+
+    Tg_VECTOR.set(0, 0, (m1 * r1 + m2 * l1) * g1 * c1 + m2 * r2 * g2 * c12);
+    Tg_VECTOR.set(1, 0, m2 * r2 * g2 * c12);
+  }
+
+  /**
+   * Calculates the voltage feedforward required to move the arm in a certain state.
+   *
+   * @param shoulderPosition the angle of the shoulder joint.
+   * @param elbowPosition the angle of the elbow joint.
+   * @param shoulderVelocity the angular velocity of the shoulder joint.
+   * @param elbowVelocity the angular velocity of the elbow joint.
+   * @param shoulderAcceleration the angular acceleration of the shoulder joint.
+   * @param elbowAcceleration the angular acceleration of the elbow joint.
+   * @return the voltage feedforward required to move the arm in the given state.
+   */
+  public TwoJointedArmFeedforwardResult calculateFeedforward(
+      Rotation2d shoulderPosition,
+      Rotation2d elbowPosition,
+      Rotation2d shoulderVelocity,
+      Rotation2d elbowVelocity,
+      Rotation2d shoulderAcceleration,
+      Rotation2d elbowAcceleration) {
+    updateMMatrix(elbowPosition);
+    updateCMatrix(elbowPosition, shoulderVelocity, elbowVelocity);
+    updateTgVector(shoulderPosition, elbowPosition);
+
+    var thetaDotVector = new SimpleMatrix(2, 1, MatrixType.DDRM);
+    thetaDotVector.set(0, 0, shoulderVelocity.getRadians());
+    thetaDotVector.set(1, 0, elbowVelocity.getRadians());
+
+    var thetaDotDotVector = new SimpleMatrix(2, 1, MatrixType.DDRM);
+    thetaDotDotVector.set(0, 0, shoulderAcceleration.getRadians());
+    thetaDotDotVector.set(1, 0, elbowAcceleration.getRadians());
+
+    var M = M_MATRIX.mult(thetaDotDotVector);
+    var C = C_MATRIX.mult(thetaDotVector);
+    var Kb = Kb_MATRIX.mult(thetaDotVector);
+    var B_INV = B_MATRIX.invert();
+
+    var u = B_INV.mult(M.plus(C).plus(Kb).plus(Tg_VECTOR));
+
+    return new TwoJointedArmFeedforwardResult(u.get(0, 0), u.get(1, 0));
+  }
+
+  /**
+   * Calculates the voltage feedforward required to move the arm in a certain (static) state.
+   *
+   * @param shoulderPosition the angle of the shoulder joint.
+   * @param elbowPosition the angle of the elbow joint.
+   * @return the voltage feedforward required to move the arm in the given (static) state.
+   */
+  public TwoJointedArmFeedforwardResult calculateFeedforward(
+      Rotation2d shoulderPosition, Rotation2d elbowPosition) {
+    return calculateFeedforward(
+        shoulderPosition,
+        elbowPosition,
+        new Rotation2d(),
+        new Rotation2d(),
+        new Rotation2d(),
+        new Rotation2d());
+  }
+
+  /** Calculates the voltage feedforward required to move the arm in a certain state. */
+  public static record TwoJointedArmFeedforwardResult(double shoulderVolts, double elbowVolts) {}
+}

src/main/java/frc/robot/RobotConstants.java

@@ -43,6 +43,5 @@ public enum Subsystem {
     VISION
   }
 
-  public static final Set<Subsystem> REAL_SUBSYSTEMS =
-      EnumSet.of(Subsystem.LIGHTS, Subsystem.ODOMETRY, Subsystem.SWERVE, Subsystem.VISION);
+  public static final Set<Subsystem> REAL_SUBSYSTEMS = EnumSet.of(Subsystem.INTAKE);
 }

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

@@ -2,11 +2,9 @@
 
 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;
-import frc.robot.arm.ArmState;
 import frc.robot.auto.Auto;
 import frc.robot.climber.Climber;
 import frc.robot.intake.Intake;
@@ -60,7 +58,7 @@ private void initializeTelemetry() {
     Telemetry.initializeShuffleboards(
         arm, auto, climber, intake, lights, odometry, shooter, swerve, vision);
 
-    SmartDashboard.putData("Arm Mechanism", RobotMechanisms.getInstance().getMechanism());
+    SmartDashboard.putData("Mechanism", RobotMechanisms.getInstance().getMechanism());
   }
 
   /** Configures operator controller bindings. */
@@ -70,20 +68,23 @@ private void configureBindings() {
     driverController.y().onTrue(odometry.tare());
     driverController.x().whileTrue(swerve.cross());
 
-    operatorController
-        .leftTrigger()
-        .whileTrue(
-            Commands.parallel(arm.to(ArmState.INTAKE))
-                .andThen(Commands.parallel(intake.intake(), shooter.intake())));
+    // operatorController
+    //     .leftTrigger()
+    //     .whileTrue(
+    //         Commands.parallel(arm.to(ArmState.INTAKE))
+    //             .andThen(Commands.parallel(intake.intake(), shooter.intake())));
 
-    operatorController
-        .rightTrigger()
-        .whileTrue(Commands.parallel(arm.to(ArmState.SHOOT)).andThen(shooter.shoot()));
+    // operatorController
+    //     .rightTrigger()
+    //     .whileTrue(Commands.parallel(arm.to(ArmState.SHOOT)).andThen(shooter.shoot()));
 
-    operatorController.rightBumper().whileTrue(shooter.shoot());
+    // operatorController.rightBumper().whileTrue(shooter.shoot());
 
-    operatorController.a().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.AMP)));
-    operatorController.b().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
+    // operatorController.a().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.AMP)));
+    // operatorController.b().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
+
+    operatorController.x().whileTrue(intake.out());
+    operatorController.y().whileTrue(intake.in());
   }
 
   /**