feat(arm): Update joint constants.

Gongoliers · Feb 24, 2024 · 349799b · 349799b
1 parent a7ff8b0
commit 349799b
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+import frc.robot.arm.ArmConstants.WristMotorConstants;
 import frc.robot.arm.ArmState;
 import frc.robot.intake.IntakeConstants.PivotMotorConstants;
 
@@ -58,7 +59,7 @@ private void initializeArmMechanism() {
         shoulder.append(
             new MechanismLigament2d(
                 "wrist",
-                ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters() * 0.5,
+                WristMotorConstants.JOINT_CONSTANTS.lengthMeters(),
                 0,
                 armThickness,
                 new Color8Bit(Color.kGreen)));

diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -31,6 +31,8 @@ public class Arm extends Subsystem {
   /** Wrist motor values. */
   private final WristMotorIOValues wristMotorValues = new WristMotorIOValues();
 
+  private final ArmState initialState = ArmState.UP_SHOOTER_INSIDE;
+
   private ArmState goal, setpoint;
 
   /** Creates a new instance of the arm subsystem. */
@@ -41,9 +43,9 @@ private Arm() {
     shoulderMotor.configure();
     wristMotor.configure();
 
-    setPosition(ArmState.INIT);
+    setPosition(initialState);
 
-    goal = getPosition();
+    goal = initialState.withWrist(ArmState.STOW.wrist());
     setpoint = getPosition();
   }
 

diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java
@@ -70,10 +70,10 @@ public static class WristMotorConstants {
     /** Joint constants for the wrist joint. */
     public static final JointConstants JOINT_CONSTANTS =
         new JointConstants(
-            Units.lbsToKilograms(13.006), // massKg // TODO
-            Units.inchesToMeters(16.825), // lengthMeters // TODO
-            Units.inchesToMeters(12.251799915), // radiusMeters // TODO
-            0.5713, // TODO
+            Units.lbsToKilograms(8.016), // massKg
+            Units.inchesToMeters(5.135), // lengthMeters
+            Units.inchesToMeters(3.47629), // radiusMeters
+            0.02835,
             20.454545,
             DCMotor.getNEO(1), // motor
             1);
@@ -95,7 +95,7 @@ public static class WristMotorConstants {
 
     /** Maximum acceleration of the shoulder joint in rotations per second per second. */
     public static final double MAXIMUM_ACCELERATION =
-        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1);
+        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25);
 
     /** Maximum speed and acceleration of the shoulder joint. */
     public static final TrapezoidProfile.Constraints CONSTRAINTS =

diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java
@@ -12,7 +12,7 @@
 /** State of the arm. */
 public record ArmState(State shoulder, State wrist) {
 
-  public static final ArmState INIT = new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, WristMotorConstants.MINIMUM_ANGLE);
+  public static final ArmState UP_SHOOTER_INSIDE = new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, WristMotorConstants.MINIMUM_ANGLE);
   public static final ArmState STOW = new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE);
   public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(23.265));
   public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(6.81));
@@ -145,7 +145,15 @@ public ArmState nextWristSetpoint(ArmState goal) {
    * @return the next overall setpoint.
    */
   public ArmState nextSetpoint(ArmState goal) {
-    if (!atShoulderGoal(goal)) {
+    boolean shooterOnBottom = Rotation2d.fromRotations(wrist.position).getDegrees() < 0.0;
+    boolean shooterNeedsToBeOnTop = Rotation2d.fromRotations(goal.wrist.position).getDegrees() > 0.0;
+    boolean shooterOnWrongSide = shooterOnBottom && shooterNeedsToBeOnTop;
+
+    if (shooterOnWrongSide && !atWristGoal(goal)) {
+      return nextWristSetpoint(goal);
+    }
+
+    if (!atShoulderGoal(goal) ) {
       return nextShoulderSetpoint(goal);
     } else if (!atWristGoal(goal)) {
       return nextWristSetpoint(goal);

diff --git a/src/main/java/frc/robot/arm/WristMotorIOSim.java b/src/main/java/frc/robot/arm/WristMotorIOSim.java
@@ -24,8 +24,8 @@ public WristMotorIOSim() {
     // TODO
     singleJointedArmSim =
         new SingleJointedArmSim(
-            ShoulderMotorConstants.JOINT_CONSTANTS.motor(),
-            ShoulderMotorConstants.JOINT_CONSTANTS.gearing(),
+            WristMotorConstants.JOINT_CONSTANTS.motor(),
+            WristMotorConstants.JOINT_CONSTANTS.gearing(),
             ShoulderMotorConstants.JOINT_CONSTANTS.moiKgMetersSquared(),
             ShoulderMotorConstants.JOINT_CONSTANTS.lengthMeters() * 0.5,
             WristMotorConstants.MINIMUM_ANGLE.getRadians(),