diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f1a760..ae5425f 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/coral/CoralManipulatorState.java b/src/main/java/frc/robot/subsystems/coral/CoralManipulatorState.java index 9258153..00c688e 100644 --- a/src/main/java/frc/robot/subsystems/coral/CoralManipulatorState.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/subsystems/coral/CoralManipulatorSystem.java b/src/main/java/frc/robot/subsystems/coral/CoralManipulatorSystem.java index cbe5735..33187df 100644 --- a/src/main/java/frc/robot/subsystems/coral/CoralManipulatorSystem.java +++ b/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 { 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; diff --git a/src/main/java/frc/robot/subsystems/coral/wrist/WristConfigs.java b/src/main/java/frc/robot/subsystems/coral/wrist/WristConfigs.java new file mode 100644 index 0000000..c1a170d --- /dev/null +++ b/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)); +} diff --git a/src/main/java/frc/robot/subsystems/coral/wrist/WristPositions.java b/src/main/java/frc/robot/subsystems/coral/wrist/WristPositions.java new file mode 100644 index 0000000..0edccc0 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/coral/wrist/WristSubsystem.java new file mode 100644 index 0000000..41f67bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/wrist/WristSubsystem.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.coral.wrist; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.util.sim.PhysicsSim; +import frc.robot.util.state.StateUtils; +import frc.robot.util.state.StatefulSetpointSubsystem; + +@Logged +public class WristSubsystem + extends StatefulSetpointSubsystem { + private final TalonFX wristMotor = new TalonFX(WristConfigs.DEVICE_ID, Constants.RIO_BUS); + private final CANcoder wristEncoder = new CANcoder(WristConfigs.DEVICE_ID); + private final StatusSignal wristPosition = wristMotor.getPosition(); + private final MotionMagicTorqueCurrentFOC magicRequest = + new MotionMagicTorqueCurrentFOC(0).withSlot(1); + + public WristSubsystem() { + super( + WristPositions.SAFE, + StateUtils.mutableRotationSetpoint(), + Units.Rotations.of(WristConfigs.WRIST_TOLERANCE)); + wristMotor.getConfigurator().apply(WristConfigs.wristMotorConfigs); + wristEncoder.getConfigurator().apply(WristConfigs.wristEncoderConfigs); + if (Robot.isSimulation()) { + PhysicsSim.getInstance().addTalonFX(wristMotor, wristEncoder); + } + } + + public Command moveWristHorizontal() { + return runOnce(() -> moveTo(WristPositions.SAFE.getAngle())); + } + + public Command moveWristVertical() { + return runOnce(() -> moveTo(WristPositions.UNSAFE.getAngle())); + } + + @Override + public StatusSignal currentStateSignal() { + return wristPosition; + } + + @Override + public Angle determineSetpoint(WristPositions targetState) { + return targetState == WristPositions.HOLD + ? wristPosition.getValue() + : targetState.getAngle(); + } + + @Override + public StatusCode moveTo(Angle setpoint) { + return wristMotor.setControl(magicRequest.withPosition(setpoint)); + } +}