From 349799b22e3682923511c2ef9fa7644e3d26791d Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 24 Feb 2024 13:24:23 -0500 Subject: [PATCH] feat(arm): Update joint constants. --- src/main/java/frc/robot/RobotMechanisms.java | 3 ++- src/main/java/frc/robot/arm/Arm.java | 6 ++++-- src/main/java/frc/robot/arm/ArmConstants.java | 10 +++++----- src/main/java/frc/robot/arm/ArmState.java | 12 ++++++++++-- src/main/java/frc/robot/arm/WristMotorIOSim.java | 4 ++-- 5 files changed, 23 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotMechanisms.java b/src/main/java/frc/robot/RobotMechanisms.java index 8578704..887a4d6 100644 --- 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 index 9afc9ca..ff6d369 100644 --- 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 index cc06dff..d8b3a40 100644 --- 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 index 34e873f..3daa66e 100644 --- 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 index 361b6a1..299b4a8 100644 --- 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(),