63 make safety logic for elevator and arm better

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

@@ -33,6 +33,7 @@
 public class RobotContainer {
 
     public final CommandXboxController primaryXboxController;
+    public final CommandJoystick simJoy;
 
     @Logged(name = "Drivetrain")
     final CommandSwerveDrivetrain drivetrain;
@@ -50,11 +51,11 @@ public class RobotContainer {
             new Mechanism2d(Units.inchesToMeters(60), Units.inchesToMeters(100));
     private MechanismLigament2d liftLigament;
     private MechanismLigament2d armLigament;
-    private final CommandJoystick joystick = new CommandJoystick(0);
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
         primaryXboxController = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT);
+        simJoy = new CommandJoystick(2);
         drivetrain = TunerConstants.createDrivetrain();
         coralManipulator = new CoralManipulatorSystem();
         configureBindings();
@@ -105,6 +106,19 @@ private void configureBindings() {
         primaryXboxController
                 .rightBumper()
                 .onTrue(coralManipulator.transitionTo(CoralManipulatorState.INTAKE_CORAL));
+
+        simJoy.button(1).onTrue(coralManipulator.wrist.moveWristHorizontal());
+        simJoy.button(2).onTrue(coralManipulator.wrist.moveWristVertical());
+
+        simJoy.button(3).onTrue(coralManipulator.transitionTo(CoralManipulatorState.INTAKE_CORAL));
+        simJoy.button(4).onTrue(coralManipulator.transitionTo(CoralManipulatorState.L1));
+        simJoy.button(5).onTrue(coralManipulator.transitionTo(CoralManipulatorState.L2));
+        simJoy.button(6).onTrue(coralManipulator.transitionTo(CoralManipulatorState.L3));
+        simJoy.button(7).onTrue(coralManipulator.transitionTo(CoralManipulatorState.L4));
+
+        simJoy.button(8).onTrue(coralManipulator.transitionTo(CoralManipulatorState.STOWED));
+
+        simJoy.button(9).onTrue(coralManipulator.transitionTo(CoralManipulatorState.HP_INTAKE));
     }
 
     private void assembleMechanisms() {

src/main/java/frc/robot/subsystems/coral/CoralManipulatorState.java

@@ -3,29 +3,48 @@
 import frc.robot.subsystems.coral.arm.ArmPosition;
 import frc.robot.subsystems.coral.elevator.ElevatorPosition;
 import frc.robot.subsystems.coral.grabber.GrabberState;
+import frc.robot.subsystems.coral.wrist.WristPositions;
 
 public enum CoralManipulatorState {
-    INTAKE_CORAL(ArmPosition.DOWN, ElevatorPosition.INTAKE, GrabberState.ROLL_IN),
-    L1(ArmPosition.POS_L1, ElevatorPosition.POS_L1, GrabberState.ROLL_OUT),
-    L2(ArmPosition.POS_L2, ElevatorPosition.POS_L2, GrabberState.OFF),
-    SCORE_L2(ArmPosition.SCORE_L2, ElevatorPosition.POS_L2, GrabberState.ROLL_OUT),
-    L3(ArmPosition.POS_L3, ElevatorPosition.SAFE_POSITION, GrabberState.OFF),
-    SCORE_L3(ArmPosition.SCORE_L3, ElevatorPosition.POS_L3, GrabberState.ROLL_OUT),
-    L4(ArmPosition.POS_L4, ElevatorPosition.POS_L4, GrabberState.OFF),
-    SCORE_L4(ArmPosition.SCORE_L4, ElevatorPosition.POS_L4, GrabberState.ROLL_OUT),
-    SCOREREEF(ArmPosition.DOWN, ElevatorPosition.SAFE_POSITION, GrabberState.OFF),
-    STOWED(ArmPosition.UP, ElevatorPosition.BOTTOM, GrabberState.OFF),
-    IDLE(ArmPosition.HOLD, ElevatorPosition.HOLD, GrabberState.OFF);
+    INTAKE_CORAL(
+            ArmPosition.DOWN, ElevatorPosition.INTAKE, GrabberState.ROLL_IN, WristPositions.SAFE),
+    L1(ArmPosition.POS_L1, ElevatorPosition.POS_L1, GrabberState.ROLL_OUT, WristPositions.UNSAFE),
+    L2(ArmPosition.POS_L2, ElevatorPosition.POS_L2, GrabberState.OFF, WristPositions.SAFE),
+    SCORE_L2(
+            ArmPosition.SCORE_L2,
+            ElevatorPosition.POS_L2,
+            GrabberState.ROLL_OUT,
+            WristPositions.SAFE),
+    L3(ArmPosition.POS_L3, ElevatorPosition.SAFE_POSITION, GrabberState.OFF, WristPositions.SAFE),
+    SCORE_L3(
+            ArmPosition.SCORE_L3,
+            ElevatorPosition.POS_L3,
+            GrabberState.ROLL_OUT,
+            WristPositions.SAFE),
+    L4(ArmPosition.POS_L4, ElevatorPosition.POS_L4, GrabberState.OFF, WristPositions.SAFE),
+    SCORE_L4(
+            ArmPosition.SCORE_L4,
+            ElevatorPosition.POS_L4,
+            GrabberState.ROLL_OUT,
+            WristPositions.SAFE),
+    HP_INTAKE(ArmPosition.HP, ElevatorPosition.HP, GrabberState.ROLL_IN, WristPositions.UNSAFE),
+    STOWED(ArmPosition.UP, ElevatorPosition.BOTTOM, GrabberState.OFF, WristPositions.SAFE),
+    IDLE(ArmPosition.HOLD, ElevatorPosition.HOLD, GrabberState.OFF, WristPositions.SAFE);
 
     private final ArmPosition armPosition;
     private final ElevatorPosition elevatorPosition;
     private final GrabberState grabberState;
+    private final WristPositions wristPosition;
 
     CoralManipulatorState(
-            ArmPosition armPosition, ElevatorPosition elevatorPosition, GrabberState grabberState) {
+            ArmPosition armPosition,
+            ElevatorPosition elevatorPosition,
+            GrabberState grabberState,
+            WristPositions wristPosition) {
         this.armPosition = armPosition;
         this.elevatorPosition = elevatorPosition;
         this.grabberState = grabberState;
+        this.wristPosition = wristPosition;
     }
 
     public ArmPosition getArmPosition() {
@@ -39,4 +58,8 @@ public ElevatorPosition getElevatorPosition() {
     public GrabberState getGrabberState() {
         return grabberState;
     }
+
+    public WristPositions getWristPosition() {
+        return wristPosition;
+    }
 }

src/main/java/frc/robot/subsystems/coral/CoralManipulatorSystem.java

@@ -8,6 +8,8 @@
 import frc.robot.subsystems.coral.elevator.ElevatorPosition;
 import frc.robot.subsystems.coral.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.coral.grabber.GrabberSubsystem;
+import frc.robot.subsystems.coral.wrist.WristPositions;
+import frc.robot.subsystems.coral.wrist.WristSubsystem;
 import frc.robot.util.state.StatefulSubsystem;
 
 @Logged
@@ -21,6 +23,9 @@ public class CoralManipulatorSystem extends StatefulSubsystem<CoralManipulatorSt
     @Logged(name = "Grabber")
     public final GrabberSubsystem grabber = new GrabberSubsystem();
 
+    @Logged(name = "Wrist")
+    public final WristSubsystem wrist = new WristSubsystem();
+
     public CoralManipulatorSystem() {
         super(CoralManipulatorState.IDLE);
     }
@@ -35,6 +40,11 @@ public String getCoralManipulatorTransState() {
         return transitioningTo().orElse(CoralManipulatorState.IDLE).toString();
     }
 
+    @Logged(name = "States/Wrist State")
+    public String getWristState() {
+        return wrist.getCurrentState().toString();
+    }
+
     @Logged(name = "States/Arm State")
     public String getArmState() {
         return arm.getCurrentState().toString();
@@ -70,21 +80,48 @@ public String elevatorTransitionState() {
         return elevator.transitioningTo().orElse(ElevatorPosition.HOLD).toString();
     }
 
+    public boolean isElevMovingUp(CoralManipulatorState targetState) {
+        return targetState
+                .getElevatorPosition()
+                .getHeight()
+                .gt(getCurrentState().getElevatorPosition().getHeight());
+    }
+
+    public boolean isMovingL2(CoralManipulatorState targetState) {
+        return targetState == CoralManipulatorState.L2 && arm.getCurrentState() != ArmPosition.DOWN;
+    }
+
+    public boolean isWristFirst() {
+        return getCurrentState().getWristPosition() == WristPositions.UNSAFE;
+    }
+
+    public boolean isArmMovingBelowZero(CoralManipulatorState targetState) {
+        return arm.getCurrentState().getAngle().lt(ArmPosition.AWAY.getAngle())
+                || targetState.getArmPosition().getAngle().lt(ArmPosition.AWAY.getAngle());
+    }
+
     @Override
     protected StatusCode initializeTransition(CoralManipulatorState targetState) {
         Command coralManipulatorCommand;
 
-        if (getCurrentState()
-                        .getElevatorPosition()
-                        .getHeight()
-                        .lt(ElevatorPosition.SAFE_POSITION.getHeight())
-                && getCurrentState() != targetState) {
-            if (targetState == CoralManipulatorState.SCORE_L2) {
+        if (isArmMovingBelowZero(targetState)) {
+            if (isMovingL2(targetState)) {
                 coralManipulatorCommand =
-                        elevator.transitionTo(targetState.getElevatorPosition())
-                                .andThen(arm.transitionTo(targetState.getArmPosition()))
+                        arm.transitionTo(targetState.getArmPosition())
+                                .alongWith(elevator.transitionTo(targetState.getElevatorPosition()))
+                                .andThen(grabber.transitionTo(targetState.getGrabberState()));
+            } else if (isElevMovingUp(targetState)) {
+                // safety stuff
+                coralManipulatorCommand =
+                        elevator.transitionTo(ElevatorPosition.SAFE_POSITION)
+                                .andThen(
+                                        arm.transitionTo(targetState.getArmPosition())
+                                                .alongWith(
+                                                        elevator.transitionTo(
+                                                                targetState.getElevatorPosition())))
                                 .andThen(grabber.transitionTo(targetState.getGrabberState()));
             } else {
+                // not safety stuff
                 coralManipulatorCommand =
                         elevator.transitionTo(ElevatorPosition.SAFE_POSITION)
                                 .andThen(arm.transitionTo(targetState.getArmPosition()))
@@ -93,18 +130,31 @@ && getCurrentState() != targetState) {
             }
         } else {
             coralManipulatorCommand =
-                    arm.transitionTo(targetState.getArmPosition())
-                            .alongWith(elevator.transitionTo(targetState.getElevatorPosition()))
+                    elevator.transitionTo(targetState.getElevatorPosition())
+                            .alongWith(arm.transitionTo(targetState.getArmPosition()))
                             .andThen(grabber.transitionTo(targetState.getGrabberState()));
         }
 
+        if (isWristFirst()) {
+            coralManipulatorCommand =
+                    wrist.transitionTo(targetState.getWristPosition())
+                            .andThen(coralManipulatorCommand);
+        } else {
+            coralManipulatorCommand =
+                    coralManipulatorCommand.andThen(
+                            wrist.transitionTo(targetState.getWristPosition()));
+        }
+
         coralManipulatorCommand.schedule();
 
         return StatusCode.OK;
     }
 
     @Override
     protected boolean isTransitionFinished() {
-        return !arm.isTransitioning() && !elevator.isTransitioning() && !grabber.isTransitioning();
+        return !arm.isTransitioning()
+                && !elevator.isTransitioning()
+                && !grabber.isTransitioning()
+                && !wrist.isTransitioning();
     }
 }

src/main/java/frc/robot/subsystems/coral/arm/ArmPosition.java

@@ -15,6 +15,7 @@ public enum ArmPosition implements SetpointEnum {
     POS_L4(.15),
     SCORE_L4(.05),
     AWAY(0),
+    HP(0.15), // get actual
     HOLD(-1); // special case, for when transitions are interrupted
 
     private final Angle angle;

src/main/java/frc/robot/subsystems/coral/elevator/ElevatorPosition.java

@@ -11,7 +11,7 @@ public enum ElevatorPosition implements Comparable<ElevatorPosition> {
     POS_L2(.32),
     POS_L3(1.8),
     POS_L4(3.95),
-
+    HP(2.1), // get actual
     HOLD(-1); // special case, for when transitions are interrupted
 
     private final Angle height;

src/main/java/frc/robot/subsystems/coral/wrist/WristConfigs.java

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.coral.wrist;
+
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+class WristConfigs {
+    static final double WRIST_TOLERANCE = 0.1;
+    static final int DEVICE_ID = 19;
+    static final double MAGNET_OFFSET = 0.501221;
+
+    static final Slot0Configs SLOT_0_CONFIGS =
+            new Slot0Configs()
+                    .withKP(0)
+                    .withKI(0)
+                    .withKD(0)
+                    .withKS(0)
+                    .withKV(0)
+                    .withKA(0)
+                    .withKG(0);
+
+    static final Slot1Configs SLOT_1_CONFIGS_SIM =
+            new Slot1Configs()
+                    .withKP(0)
+                    .withKI(0)
+                    .withKD(32)
+                    .withKS(0)
+                    .withKV(0)
+                    .withKA(1.8)
+                    .withKG(0);
+
+    static final MotionMagicConfigs MOTION_MAGIC_CONFIGS =
+            new MotionMagicConfigs()
+                    .withMotionMagicAcceleration(1)
+                    .withMotionMagicCruiseVelocity(0.5)
+                    .withMotionMagicJerk(0);
+
+    static final FeedbackConfigs FEEDBACK_CONFIGS =
+            new FeedbackConfigs().withRotorToSensorRatio(1).withSensorToMechanismRatio(3.75);
+
+    static final MotorOutputConfigs MOTOR_OUTPUT_CONFIGS =
+            new MotorOutputConfigs()
+                    .withInverted(InvertedValue.Clockwise_Positive)
+                    .withNeutralMode(NeutralModeValue.Brake);
+
+    static final TalonFXConfiguration wristMotorConfigs =
+            new TalonFXConfiguration()
+                    .withMotorOutput(MOTOR_OUTPUT_CONFIGS)
+                    .withSlot0(SLOT_0_CONFIGS)
+                    .withSlot1(SLOT_1_CONFIGS_SIM)
+                    .withMotionMagic(MOTION_MAGIC_CONFIGS)
+                    .withFeedback(FEEDBACK_CONFIGS);
+
+    static final CANcoderConfiguration wristEncoderConfigs =
+            new CANcoderConfiguration()
+                    .withMagnetSensor(
+                            new MagnetSensorConfigs()
+                                    .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
+                                    .withMagnetOffset(MAGNET_OFFSET)
+                                    .withAbsoluteSensorDiscontinuityPoint(0.625));
+}

src/main/java/frc/robot/subsystems/coral/wrist/WristPositions.java

@@ -0,0 +1,20 @@
+package frc.robot.subsystems.coral.wrist;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+
+public enum WristPositions {
+    SAFE(0),
+    UNSAFE(0.25),
+    HOLD(-1);
+
+    private final Angle angle;
+
+    WristPositions(double angle) {
+        this.angle = Units.Rotations.of(angle);
+    }
+
+    public Angle getAngle() {
+        return angle;
+    }
+}