diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2265ac21..30f5f505 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -25,14 +25,14 @@ import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; -import frc.trigon.robot.subsystems.arm.Arm; -import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmElevator; +import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; -import frc.trigon.robot.subsystems.elevator.Elevator; -import frc.trigon.robot.subsystems.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.endeffector.EndEffector; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -52,9 +52,9 @@ public class RobotContainer { CameraConstants.OBJECT_DETECTION_CAMERA ); public static final Swerve SWERVE = new Swerve(); - public static final Arm ARM = new Arm(); + public static final ArmElevator ARM_ELEVATOR = new ArmElevator(); public static final Climber CLIMBER = new Climber(); - public static final Elevator ELEVATOR = new Elevator(); + public static final EndEffector END_EFFECTOR = new EndEffector(); public static final Intake INTAKE = new Intake(); public static final Transporter TRANSPORTER = new Transporter(); @@ -80,9 +80,9 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); - ARM.setDefaultCommand(ArmCommands.getRestCommand()); + ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); - ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST)); + END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index f4d70ecb..bcd92619 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -7,10 +7,10 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.arm.ArmCommands; -import frc.trigon.robot.subsystems.arm.ArmConstants; -import frc.trigon.robot.subsystems.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; +import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -29,11 +29,11 @@ public static Command getCoralCollectionCommand() { // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) } - private static Command getLoadCoralCommand() { + public static Command getLoadCoralCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL) - ).until(RobotContainer.ARM::hasGamePiece).asProxy(); + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) + ).until(RobotContainer.END_EFFECTOR::hasGamePiece).onlyWhile(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); } private static Command getIntakeSequenceCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index ebefb93d..b0a9a4ed 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -5,8 +5,10 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; -import frc.trigon.robot.subsystems.arm.ArmCommands; -import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; +import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -16,7 +18,7 @@ public class CoralEjectionCommands { public static Command getCoralEjectionCommand() { return GeneralCommands.getContinuousConditionalCommand( getEjectCoralFromIntakeCommand(), - getEjectCoralFromArmCommand(), + getEjectCoralFromEndEffectorCommand(), () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() ).onlyIf(SimulationFieldHandler::isHoldingCoral); } @@ -28,10 +30,10 @@ private static Command getEjectCoralFromIntakeCommand() { ); } - private static Command getEjectCoralFromArmCommand() { + private static Command getEjectCoralFromEndEffectorCommand() { return new SequentialCommandGroup( - ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atPrepareAngle), - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT) + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.EJECT).until(RobotContainer.ARM_ELEVATOR::atTargetState), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT) ); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index a366475f..49194bd9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -13,10 +13,10 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; -import frc.trigon.robot.subsystems.arm.ArmCommands; -import frc.trigon.robot.subsystems.arm.ArmConstants; -import frc.trigon.robot.subsystems.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; +import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableTranslation2d; @@ -26,19 +26,22 @@ public class CoralPlacingCommands { private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { - return new ConditionalCommand( - getAutonomouslyScoreCommand(shouldScoreRight), - getScoreCommand(shouldScoreRight), - () -> SHOULD_SCORE_AUTONOMOUSLY + return new SequentialCommandGroup( + CoralCollectionCommands.getLoadCoralCommand(), + new ConditionalCommand( + getAutonomouslyScoreCommand(shouldScoreRight), + getScoreCommand(shouldScoreRight), + () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 + ) ); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)), + getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight)), new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), - ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore) + ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) ) ); } @@ -47,16 +50,15 @@ private static Command getScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), - ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore) + ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) ) ); } private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( - ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorState), - ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore), + ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).asProxy() ); } @@ -106,9 +108,8 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRig return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false); } - private static boolean isArmAndElevatorAtPrepareState(boolean shouldScoreRight) { - return RobotContainer.ELEVATOR.atPreparedTargetState() - && RobotContainer.ARM.atPrepareAngle() + private static boolean isReadyToScore(boolean shouldScoreRight) { + return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore()) && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); } @@ -134,8 +135,7 @@ public enum ScoringLevel { L3(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)), L4(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)); - public final ElevatorConstants.ElevatorState elevatorState; - public final ArmConstants.ArmState armState; + public final ArmElevatorConstants.ArmElevatorState armElevatorState; public final int level = calculateLevel(); final double xTransformMeters, positiveYTransformMeters; final Rotation2d rotationTransform; @@ -153,8 +153,7 @@ public enum ScoringLevel { this.xTransformMeters = xTransformMeters; this.positiveYTransformMeters = positiveYTransformMeters; this.rotationTransform = rotationTransform; - this.elevatorState = determineElevatorState(); - this.armState = determineArmState(); + this.armElevatorState = determineArmElevatorState(); } /** @@ -176,22 +175,12 @@ public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPo return new FlippablePose2d(reefCenterPose.plus(transform), true); } - private ElevatorConstants.ElevatorState determineElevatorState() { - return switch (level) { - case 1 -> ElevatorConstants.ElevatorState.SCORE_L1; - case 2 -> ElevatorConstants.ElevatorState.SCORE_L2; - case 3 -> ElevatorConstants.ElevatorState.SCORE_L3; - case 4 -> ElevatorConstants.ElevatorState.SCORE_L4; - default -> throw new IllegalStateException("Unexpected value: " + ordinal()); - }; - } - - private ArmConstants.ArmState determineArmState() { + private ArmElevatorConstants.ArmElevatorState determineArmElevatorState() { return switch (level) { - case 1 -> ArmConstants.ArmState.SCORE_L1; - case 2 -> ArmConstants.ArmState.SCORE_L2; - case 3 -> ArmConstants.ArmState.SCORE_L3; - case 4 -> ArmConstants.ArmState.SCORE_L4; + case 1 -> ArmElevatorConstants.ArmElevatorState.SCORE_L1; + case 2 -> ArmElevatorConstants.ArmElevatorState.SCORE_L2; + case 3 -> ArmElevatorConstants.ArmElevatorState.SCORE_L3; + case 4 -> ArmElevatorConstants.ArmElevatorState.SCORE_L4; default -> throw new IllegalStateException("Unexpected value: " + ordinal()); }; } diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index 9a185914..3c02d966 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -7,8 +7,7 @@ import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; -import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; import lib.utilities.flippable.FlippablePose2d; import java.util.function.Supplier; @@ -45,12 +44,8 @@ public FlippablePose2d calculateTargetScoringPose() { return scoringLevel.calculateTargetPlacingPosition(clockPosition, reefSide); } - public ArmConstants.ArmState getArmState() { - return scoringLevel.armState; - } - - public ElevatorConstants.ElevatorState getElevatorState() { - return scoringLevel.elevatorState; + public ArmElevatorConstants.ArmElevatorState getArmElevatorState() { + return scoringLevel.armElevatorState; } private void configureBindings() { diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java index 355fca6f..4a4972f6 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -3,7 +3,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.FlippablePose3d; @@ -78,7 +78,7 @@ private static void updateCollection() { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)), - algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose())); + algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose())); if (isCollectingCoral() && HELD_CORAL_INDEX == null) { HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralCollectionPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS); @@ -124,20 +124,20 @@ private static boolean isCollectingCoral() { } private static boolean isCollectingAlgae() { - return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3); + return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3); } private static boolean isCoralLoading() { - return RobotContainer.ARM.atState(ArmConstants.ArmState.LOAD_CORAL); + return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL); } private static void updateEjection() { if (HELD_CORAL_INDEX != null) updateCoralEjection(); - if (HELD_ALGAE_INDEX != null && RobotContainer.ARM.isEjecting()) { + if (HELD_ALGAE_INDEX != null && RobotContainer.END_EFFECTOR.isEjecting()) { final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); - heldAlgae.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); + heldAlgae.release(RobotContainer.END_EFFECTOR.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); HELD_ALGAE_INDEX = null; } } @@ -149,8 +149,8 @@ private static void updateCoralEjection() { heldCoral.release(RobotContainer.INTAKE.calculateLinearIntakeVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), IntakeConstants.CORAL_COLLECTION_POSE.getTranslation()); HELD_CORAL_INDEX = null; } - if (isCoralInEndEffector() && RobotContainer.ARM.isEjecting()) { - heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM.calculateGamePieceCollectionPose().getTranslation()); + if (isCoralInEndEffector() && RobotContainer.END_EFFECTOR.isEjecting()) { + heldCoral.release(RobotContainer.END_EFFECTOR.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose().getTranslation()); HELD_CORAL_INDEX = null; } } @@ -160,8 +160,8 @@ private static void updateCoralEjection() { */ private static void updateHeldGamePiecePoses() { final Pose3d - robotRelativeHeldCoralPosition = IS_CORAL_IN_END_EFFECTOR ? RobotContainer.ARM.calculateGamePieceCollectionPose() : TransporterConstants.COLLECTED_CORAL_POSE, - robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateGamePieceCollectionPose(); + robotRelativeHeldCoralPosition = IS_CORAL_IN_END_EFFECTOR ? RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose() : TransporterConstants.COLLECTED_CORAL_POSE, + robotRelativeHeldAlgaePosition = RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose(); updateHeldGamePiecePose(robotRelativeHeldCoralPosition, CORAL_ON_FIELD, HELD_CORAL_INDEX); updateHeldGamePiecePose(robotRelativeHeldAlgaePosition, ALGAE_ON_FIELD, HELD_ALGAE_INDEX); } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java deleted file mode 100644 index eb7a6b60..00000000 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ /dev/null @@ -1,255 +0,0 @@ -package frc.trigon.robot.subsystems.arm; - -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; -import frc.trigon.robot.subsystems.MotorSubsystem; -import lib.hardware.phoenix6.cancoder.CANcoderEncoder; -import lib.hardware.phoenix6.cancoder.CANcoderSignal; -import lib.hardware.phoenix6.talonfx.TalonFXMotor; -import lib.hardware.phoenix6.talonfx.TalonFXSignal; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Arm extends MotorSubsystem { - private final TalonFXMotor - armMasterMotor = ArmConstants.ARM_MASTER_MOTOR, - endEffectorMotor = ArmConstants.END_EFFECTOR_MOTOR; - private final CANcoderEncoder angleEncoder = ArmConstants.ANGLE_ENCODER; - private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ArmConstants.FOC_ENABLED); - private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( - 0, - ArmConstants.ARM_DEFAULT_MAXIMUM_VELOCITY, - ArmConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION, - ArmConstants.ARM_DEFAULT_MAXIMUM_JERK - ).withEnableFOC(ArmConstants.FOC_ENABLED); - public boolean isStateReversed = false; - private ArmConstants.ArmState targetState = ArmConstants.ArmState.REST; - - public Arm() { - setName("Arm"); - } - - @Override - public SysIdRoutine.Config getSysIDConfig() { - return ArmConstants.ARM_SYSID_CONFIG; - } - - @Override - public void setBrake(boolean brake) { - armMasterMotor.setBrake(brake); - } - - @Override - public void stop() { - armMasterMotor.stopMotor(); - endEffectorMotor.stopMotor(); - } - - @Override - public void updateLog(SysIdRoutineLog log) { - log.motor("Arm") - .angularPosition(Units.Rotations.of(getAngle().getRotations())) - .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.VELOCITY))) - .voltage(Units.Volts.of(armMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); - } - - @Override - public void updateMechanism() { - ArmConstants.ARM_MECHANISM.update( - Rotation2d.fromRotations(getAngle().getRotations() + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET), - Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET) - ); - ArmConstants.END_EFFECTOR_MECHANISM.update(endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); - - Logger.recordOutput("Poses/Components/ArmPose", calculateVisualizationPose()); - } - - @Override - public void updatePeriodically() { - armMasterMotor.update(); - endEffectorMotor.update(); - angleEncoder.update(); - ArmConstants.DISTANCE_SENSOR.updateSensor(); - Logger.recordOutput("Arm/CurrentPositionDegrees", getAngle().getDegrees()); - } - - @Override - public void sysIDDrive(double targetVoltage) { - armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); - } - - public Pose3d calculateGamePieceCollectionPose() { - return calculateVisualizationPose() - .transformBy(ArmConstants.ARM_TO_HELD_GAME_PIECE); - } - - public boolean isArmAboveSafeAngle() { - return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); - } - - public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { - if (isStateReversed) - return this.targetState == targetState && atAngle(subtractFrom360Degrees(targetState.targetAngle)); - return atState(targetState); - } - - public boolean atState(ArmConstants.ArmState targetState) { - return this.targetState == targetState && atAngle(targetState.targetAngle); - } - - public boolean atTargetAngle() { - if (isStateReversed) { - return atAngle(subtractFrom360Degrees(targetState.targetAngle)); - } - return atAngle(targetState.targetAngle); - } - - public boolean atPrepareAngle() { - if (isStateReversed) { - return atAngle(subtractFrom360Degrees(targetState.prepareAngle)); - } - return atAngle(targetState.prepareAngle); - } - - @AutoLogOutput(key = "Arm/HasCoral") - public boolean hasGamePiece() { - return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); - } - - public Rotation2d getAngle() { - return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); - } - - @AutoLogOutput(key = "Arm/InEndEffector") - public boolean inEndEffector() { - return SimulationFieldHandler.isCoralInEndEffector(); - } - - public Translation3d calculateLinearArmAndEndEffectorVelocity() { - double velocityMetersPerSecond = endEffectorMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * ArmConstants.WHEEL_RADIUS_METERS; - return calculateLinearArmVelocity().plus( - new Translation3d( - getAngle().getCos() * velocityMetersPerSecond, - getAngle().getSin() * velocityMetersPerSecond, - 0 - ) - ); - } - - public boolean isEjecting() { - return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; - } - - public Translation3d calculateLinearArmVelocity() { - double velocityRotationsPerSecond = armMasterMotor.getSignal(TalonFXSignal.VELOCITY); - double velocityMagnitude = velocityRotationsPerSecond * 2 * Math.PI * ArmConstants.ARM_LENGTH_METERS; - return new Translation3d( - getAngle().getCos() * velocityMagnitude, - 0, - getAngle().getSin() * velocityMagnitude - ); - } - - void setTargetState(ArmConstants.ArmState targetState) { - this.isStateReversed = false; - this.targetState = targetState; - setTargetState(targetState, false); - } - - void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { - this.isStateReversed = isStateReversed; - this.targetState = targetState; - - if (isStateReversed) { - setTargetState( - subtractFrom360Degrees(targetState.targetAngle) - , targetState.targetEndEffectorVoltage - ); - return; - } - setTargetState( - targetState.targetAngle, - targetState.targetEndEffectorVoltage); - } - - void setTargetState(Rotation2d targetAngle, double targetVoltage) { - setTargetAngle(targetAngle); - setEndEffectorTargetVoltage(targetVoltage); - } - - void setPrepareState(ArmConstants.ArmState targetState) { - setPrepareState(targetState, false); - } - - void setPrepareState(ArmConstants.ArmState targetState, boolean isStateReversed) { - this.isStateReversed = isStateReversed; - this.targetState = targetState; - - if (isStateReversed) { - setTargetAngle(subtractFrom360Degrees(targetState.prepareAngle)); - return; - } - setTargetAngle(targetState.prepareAngle); - } - - void setArmState(ArmConstants.ArmState targetState) { - setArmState(targetState, false); - } - - void setArmState(ArmConstants.ArmState targetState, boolean isStateReversed) { - this.isStateReversed = isStateReversed; - this.targetState = targetState; - - if (isStateReversed) { - setTargetAngle(subtractFrom360Degrees(targetState.targetAngle)); - return; - } - setTargetAngle(targetState.targetAngle); - } - - void setEndEffectorState(ArmConstants.ArmState targetState) { - setEndEffectorTargetVoltage(targetState.targetEndEffectorVoltage); - } - - private void setTargetAngle(Rotation2d targetAngle) { - armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()))); - } - - private Rotation2d calculateMinimumArmSafeAngle() { - final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); - final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); - final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); - return isElevatorAboveSafeZone - ? Rotation2d.fromRadians(0) - : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); - } - - private void setEndEffectorTargetVoltage(double targetVoltage) { - ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); - endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); - } - - private boolean atAngle(Rotation2d targetAngle) { - final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getAngle()).getDegrees()); - return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); - } - - private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { - return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); - } - - private Pose3d calculateVisualizationPose() { - final Transform3d armTransform = new Transform3d( - new Translation3d(0, 0, RobotContainer.ELEVATOR.getPositionMeters()), - new Rotation3d(0, getAngle().getRadians(), 0) - ); - return ArmConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform); - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java deleted file mode 100644 index 6bb7d384..00000000 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.trigon.robot.subsystems.arm; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import frc.trigon.robot.RobotContainer; -import lib.commands.ExecuteEndCommand; -import lib.commands.GearRatioCalculationCommand; -import lib.commands.NetworkTablesCommand; - -import java.util.Set; -import java.util.function.Supplier; - -public class ArmCommands { - public static Command getDebuggingCommand() { - return new NetworkTablesCommand( - (targetAngleDegrees, targetVoltage) -> RobotContainer.ARM.setTargetState( - Rotation2d.fromDegrees(targetAngleDegrees), - targetVoltage - ), - false, - Set.of(RobotContainer.ARM), - "Debugging/ArmTargetPositionDegrees", - "Debugging/EndEffectorTargetVoltage" - ); - } - - public static Command getGearRatioCalulationCommand() { - return new GearRatioCalculationCommand( - ArmConstants.ARM_MASTER_MOTOR, - ArmConstants.ANGLE_ENCODER, - 0.5, - RobotContainer.ARM - ); - } - - public static Command getSetTargetStateCommand(Supplier targetState, Supplier isStateReversed) { - return new FunctionalCommand( - () -> RobotContainer.ARM.setEndEffectorState(targetState.get()), - () -> RobotContainer.ARM.setArmState(targetState.get(), isStateReversed.get()), - interrupted -> RobotContainer.ARM.stop(), - () -> false, - RobotContainer.ARM - ); } - - public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { - return getSetTargetStateCommand(targetState, false); - } - - public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { - return new FunctionalCommand( - () -> RobotContainer.ARM.setEndEffectorState(targetState), - () -> RobotContainer.ARM.setArmState(targetState, isStateReversed), - interrupted -> RobotContainer.ARM.stop(), - () -> false, - RobotContainer.ARM - ); - } - - public static Command getPrepareForStateCommand(Supplier targetState, Supplier isStateReversed) { - return new ExecuteEndCommand( - () -> RobotContainer.ARM.setPrepareState(targetState.get(), isStateReversed.get()), - RobotContainer.ARM::stop, - RobotContainer.ARM - ); - } - - public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { - return getPrepareForStateCommand(targetState, false); - } - - public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { - return new ExecuteEndCommand( - () -> RobotContainer.ARM.setPrepareState(targetState, isStateReversed), - RobotContainer.ARM::stop, - RobotContainer.ARM - ); - } - - public static Command getRestCommand() { - return new ConditionalCommand( - getSetTargetStateCommand(ArmConstants.ArmState.REST_WITH_CORAL), - getSetTargetStateCommand(ArmConstants.ArmState.REST), - RobotContainer.ARM::hasGamePiece - ); - } -} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java deleted file mode 100644 index 53396fd4..00000000 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ /dev/null @@ -1,263 +0,0 @@ -package frc.trigon.robot.subsystems.arm; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.signals.*; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.event.BooleanEvent; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; -import lib.hardware.RobotHardwareStats; -import lib.hardware.misc.simplesensor.SimpleSensor; -import lib.hardware.phoenix6.cancoder.CANcoderEncoder; -import lib.hardware.phoenix6.cancoder.CANcoderSignal; -import lib.hardware.phoenix6.talonfx.TalonFXMotor; -import lib.hardware.phoenix6.talonfx.TalonFXSignal; -import lib.hardware.simulation.SimpleMotorSimulation; -import lib.hardware.simulation.SingleJointedArmSimulation; -import lib.utilities.Conversions; -import lib.utilities.mechanisms.SingleJointedArmMechanism2d; -import lib.utilities.mechanisms.SpeedMechanism2d; - -import java.util.function.DoubleSupplier; - -public class ArmConstants { - private static final int - ARM_MASTER_MOTOR_ID = 13, - ARM_FOLLOWER_MOTOR_ID = 14, - END_EFFECTOR_MOTOR_ID = 15, - ANGLE_ENCODER_ID = 13, - DISTANCE_SENSOR_CHANNEL = 3; - private static final String - ARM_MASTER_MOTOR_NAME = "ArmMasterMotor", - ARM_FOLLOWER_MOTOR_NAME = "ArmFollowerMotor", - END_EFFECTOR_MOTOR_NAME = "EndEffectorMotor", - ANGLE_ENCODER_NAME = "ArmEncoder", - DISTANCE_SENSOR_NAME = "EndEffectorDistanceSensor"; - static final TalonFXMotor - ARM_MASTER_MOTOR = new TalonFXMotor(ARM_MASTER_MOTOR_ID, ARM_MASTER_MOTOR_NAME), - ARM_FOLLOWER_MOTOR = new TalonFXMotor(ARM_FOLLOWER_MOTOR_ID, ARM_FOLLOWER_MOTOR_NAME), - END_EFFECTOR_MOTOR = new TalonFXMotor(END_EFFECTOR_MOTOR_ID, END_EFFECTOR_MOTOR_NAME); - static final CANcoderEncoder ANGLE_ENCODER = new CANcoderEncoder(ANGLE_ENCODER_ID, ANGLE_ENCODER_NAME); - static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); - - private static final double - ARM_GEAR_RATIO = 40, - END_EFFECTOR_GEAR_RATIO = 17; - private static final double ARM_MOTOR_CURRENT_LIMIT = 50; - private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0; - static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET; - private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false; - static final double - ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 0, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 0, - ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10; - static final boolean FOC_ENABLED = true; - - public static final double ARM_LENGTH_METERS = 0.52; - private static final int - ARM_MOTOR_AMOUNT = 2, - END_EFFECTOR_MOTOR_AMOUNT = 1; - private static final DCMotor - ARM_GEARBOX = DCMotor.getKrakenX60Foc(ARM_MOTOR_AMOUNT), - END_EFFECTOR_GEARBOX = DCMotor.getKrakenX60Foc(END_EFFECTOR_MOTOR_AMOUNT); - private static final double - ARM_MASS_KILOGRAMS = 3.5, - END_EFFECTOR_MOMENT_OF_INERTIA = 0.003, - END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY = 12; - private static final Rotation2d - ARM_MINIMUM_ANGLE = Rotation2d.fromDegrees(0), - ARM_MAXIMUM_ANGLE = Rotation2d.fromDegrees(360); - private static final boolean SHOULD_SIMULATE_GRAVITY = true; - private static final SingleJointedArmSimulation ARM_SIMULATION = new SingleJointedArmSimulation( - ARM_GEARBOX, - ARM_GEAR_RATIO, - ARM_LENGTH_METERS, - ARM_MASS_KILOGRAMS, - ARM_MINIMUM_ANGLE, - ARM_MAXIMUM_ANGLE, - SHOULD_SIMULATE_GRAVITY - ); - private static final SimpleMotorSimulation END_EFFECTOR_SIMULATION = new SimpleMotorSimulation( - END_EFFECTOR_GEARBOX, - END_EFFECTOR_GEAR_RATIO, - END_EFFECTOR_MOMENT_OF_INERTIA - ); - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> (SimulationFieldHandler.isHoldingCoral() && SimulationFieldHandler.isCoralInEndEffector()) || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; - - static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(1.5).per(Units.Seconds), - Units.Volts.of(3), - Units.Second.of(1000) - ); - - static final SingleJointedArmMechanism2d ARM_MECHANISM = new SingleJointedArmMechanism2d( - "ArmMechanism", - ARM_LENGTH_METERS, - Color.kBlue - ); - static final SpeedMechanism2d END_EFFECTOR_MECHANISM = new SpeedMechanism2d( - "EndEffectorMechanism", - END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY - ); - static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.0954, 0.3573), - new Rotation3d(0, 0, 0) - ); - - static final Transform3d ARM_TO_HELD_GAME_PIECE = new Transform3d( - new Translation3d(0, 0.1, -0.5855), - new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) - ); - static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); - /** - * The highest point of the arms angular zone where the safety logic applies. - */ - static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90); - - private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; - static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - DISTANCE_SENSOR::getBinaryValue - ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); - static final double WHEEL_RADIUS_METERS = edu.wpi.first.math.util.Units.inchesToMeters(1.5); - - static { - configureArmMasterMotor(); - configureArmFollowerMotor(); - configureEndEffectorMotor(); - configureAngleEncoder(); - configureDistanceSensor(); - } - - private static void configureArmMasterMotor() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Audio.BeepOnBoot = false; - config.Audio.BeepOnConfig = false; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - config.Feedback.RotorToSensorRatio = ARM_GEAR_RATIO; - config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID(); - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 38 : 0; - config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0; - - config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - - config.MotionMagic.MotionMagicCruiseVelocity = ARM_DEFAULT_MAXIMUM_VELOCITY; - config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION; - config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; - - ARM_MASTER_MOTOR.applyConfiguration(config); - ARM_MASTER_MOTOR.setPhysicsSimulation(ARM_SIMULATION); - - ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); - ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); - ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); - } - - private static void configureArmFollowerMotor() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Audio.BeepOnBoot = false; - config.Audio.BeepOnConfig = false; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; - - ARM_FOLLOWER_MOTOR.applyConfiguration(config); - - final Follower followerRequest = new Follower(ARM_MASTER_MOTOR.getID(), SHOULD_ARM_FOLLOWER_OPPOSE_MASTER); - ARM_FOLLOWER_MOTOR.setControl(followerRequest); - } - - private static void configureEndEffectorMotor() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - config.Audio.BeepOnBoot = false; - config.Audio.BeepOnConfig = false; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO; - - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.StatorCurrentLimit = 80; - - END_EFFECTOR_MOTOR.applyConfiguration(config); - END_EFFECTOR_MOTOR.setPhysicsSimulation(END_EFFECTOR_SIMULATION); - - END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); - END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); - END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); - } - - private static void configureAngleEncoder() { - final CANcoderConfiguration config = new CANcoderConfiguration(); - - config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_GRAVITY_OFFSET; - config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; - - ANGLE_ENCODER.applyConfiguration(config); - ANGLE_ENCODER.setSimulationInputsFromTalonFX(ARM_MASTER_MOTOR); - - ANGLE_ENCODER.registerSignal(CANcoderSignal.POSITION, 100); - ANGLE_ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); - } - - private static void configureDistanceSensor() { - DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); - } - - public enum ArmState { - REST(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0), - REST_WITH_CORAL(Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180), 0), - REST_FOR_CLIMB(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0), - LOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4), - HOLD_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), - EJECT(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(60), 4), - SCORE_L1(Rotation2d.fromDegrees(110), Rotation2d.fromDegrees(110), 4), - SCORE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), - SCORE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), - SCORE_L4(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(120), 4), - SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4), - SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4), - COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4); - - public final Rotation2d targetAngle; - public final Rotation2d prepareAngle; - public final double targetEndEffectorVoltage; - - ArmState(Rotation2d targetAngle, Rotation2d prepareAngle, double targetEndEffectorVoltage) { - this.targetAngle = targetAngle; - this.prepareAngle = prepareAngle; - this.targetEndEffectorVoltage = targetEndEffectorVoltage; - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java new file mode 100644 index 00000000..6e1541b1 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java @@ -0,0 +1,285 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.subsystems.MotorSubsystem; +import lib.hardware.phoenix6.cancoder.CANcoderEncoder; +import lib.hardware.phoenix6.cancoder.CANcoderSignal; +import lib.hardware.phoenix6.talonfx.TalonFXMotor; +import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import lib.utilities.Conversions; +import org.littletonrobotics.junction.Logger; + +public class ArmElevator extends MotorSubsystem { + private final TalonFXMotor + armMasterMotor = ArmElevatorConstants.ARM_MASTER_MOTOR, + elevatorMasterMotor = ArmElevatorConstants.ELEVATOR_MASTER_MOTOR; + private final CANcoderEncoder angleEncoder = ArmElevatorConstants.ANGLE_ENCODER; + private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ArmElevatorConstants.FOC_ENABLED); + private final DynamicMotionMagicVoltage armPositionRequest = new DynamicMotionMagicVoltage( + 0, + ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_VELOCITY, + ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION, + ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_JERK + ).withEnableFOC(ArmElevatorConstants.FOC_ENABLED); + private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( + 0, + ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY, + ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION, + ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * 10 + ).withEnableFOC(ArmElevatorConstants.FOC_ENABLED); + public boolean isStateReversed = false; + private ArmElevatorConstants.ArmElevatorState targetState = ArmElevatorConstants.ArmElevatorState.REST; + + public ArmElevator() { + setName("ArmElevator"); + } + + @Override + public SysIdRoutine.Config getSysIDConfig() { + return ArmElevatorConstants.ARM_SYSID_CONFIG; + //return ArmElevatorConstants.ELEVATOR_SYSID_CONFIG; + } + + @Override + public void setBrake(boolean brake) { + armMasterMotor.setBrake(brake); + elevatorMasterMotor.setBrake(brake); + } + + @Override + public void stop() { + armMasterMotor.stopMotor(); + elevatorMasterMotor.stopMotor(); + } + + @Override + public void updateLog(SysIdRoutineLog log) { + log.motor("Arm") + .angularPosition(Units.Rotations.of(getCurrentArmAngle().getRotations())) + .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(armMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); +// log.motor("Elevator") +// .linearPosition(Units.Meters.of(getPositionRotations())) +// .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) +// .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + } + + @Override + public void updateMechanism() { + ArmElevatorConstants.ARM_MECHANISM.update( + Rotation2d.fromRotations(getCurrentArmAngle().getRotations() + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET), + Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET) + ); + ArmElevatorConstants.ELEVATOR_MECHANISM.update( + getCurrentElevatorPositionMeters(), + rotationsToMeters(elevatorMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + ); + + Logger.recordOutput("Poses/Components/ArmPose", calculateVisualizationPose()); + Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); + Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); + } + + @Override + public void updatePeriodically() { + armMasterMotor.update(); + angleEncoder.update(); + elevatorMasterMotor.update(); + Logger.recordOutput("Elevator/CurrentPositionMeters", getCurrentElevatorPositionMeters()); + Logger.recordOutput("Arm/CurrentPositionDegrees", getCurrentArmAngle().getDegrees()); + } + + @Override + public void sysIDDrive(double targetVoltage) { + armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); +// elevatorMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); + } + + public Pose3d calculateGamePieceCollectionPose() { + return calculateVisualizationPose() + .transformBy(ArmElevatorConstants.ARM_TO_HELD_GAME_PIECE); + } + + public boolean isArmAboveSafeAngle() { + return getCurrentArmAngle().getDegrees() >= ArmElevatorConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); + } + + public boolean atTargetState() { + return atState(targetState, isStateReversed); + } + + public boolean atState(ArmElevatorConstants.ArmElevatorState targetState) { + return atState(targetState, false); + } + + public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { + if (targetState == null) + return false; + return armAtAngle(isStateReversed ? subtractFrom360Degrees(targetState.targetAngle) : targetState.targetAngle) + && elevatorAtPosition(targetState.targetPositionMeters); + } + + public boolean armAtAngle(Rotation2d targetAngle) { + final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getCurrentArmAngle()).getDegrees()); + return currentToTargetAngleDifferenceDegrees < ArmElevatorConstants.ANGLE_TOLERANCE.getDegrees(); + } + + public boolean elevatorAtPosition(double positionMeters) { + final double currentToTargetStateDifferenceMeters = Math.abs(positionMeters - getCurrentElevatorPositionMeters()); + return currentToTargetStateDifferenceMeters < ArmElevatorConstants.ELEVATOR_POSITION_TOLERANCE_METERS; + } + + public Rotation2d getCurrentArmAngle() { + return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); + } + + public double getCurrentElevatorPositionMeters() { + return rotationsToMeters(getElevatorPositionRotations()); + } + + public Translation3d calculateLinearArmVelocity() { + double velocityRotationsPerSecond = armMasterMotor.getSignal(TalonFXSignal.VELOCITY); + double velocityMagnitude = velocityRotationsPerSecond * 2 * Math.PI * ArmElevatorConstants.ARM_LENGTH_METERS; + return new Translation3d( + -getCurrentArmAngle().getSin() * velocityMagnitude, + 0, + getCurrentArmAngle().getCos() * velocityMagnitude + ); + } + + void prepareToState(ArmElevatorConstants.ArmElevatorState targetState) { + setTargetState(targetState.prepareState); + } + + void prepareToState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { + if (targetState.prepareState == null) { + setTargetState(targetState, isStateReversed); + return; + } + setTargetState(targetState.prepareState, isStateReversed); + } + + void setTargetState(ArmElevatorConstants.ArmElevatorState targetState) { + setTargetState(targetState, false); + } + + void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { + this.isStateReversed = isStateReversed; + this.targetState = targetState; + + setTargetArmState(targetState, isStateReversed); + setTargetElevatorState(targetState); + } + + void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { + if (isStateReversed) { + setTargetArmAngle(subtractFrom360Degrees(targetState.targetAngle), targetState.ignoreConstraints); + return; + } + setTargetArmAngle(targetState.targetAngle, targetState.ignoreConstraints); + } + + void setTargetElevatorState(ArmElevatorConstants.ArmElevatorState targetState) { + scaleElevatorPositionRequestSpeed(targetState.speedScalar); + setTargetElevatorPositionMeters(targetState.targetPositionMeters, targetState.ignoreConstraints); + } + + void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) { + armMasterMotor.setControl(armPositionRequest.withPosition(ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()))); + } + + void setTargetElevatorPositionMeters(double targetPositionMeters, boolean ignoreConstraints) { + setTargetElevatorPositionRotations(metersToRotations(targetPositionMeters), ignoreConstraints); + } + + void setTargetElevatorPositionRotations(double targetPositionRotations, boolean ignoreConstraints) { + elevatorMasterMotor.setControl(positionRequest.withPosition(ignoreConstraints ? targetPositionRotations : Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); + } + + private Rotation2d calculateMinimumArmSafeAngle() { + final double heightFromSafeZone = getElevatorHeightFromMinimumSafeZone(); + final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmElevatorConstants.ARM_LENGTH_METERS, 0, 1); + final double acos = Math.acos(cosOfMinimumSafeAngle); + return Double.isNaN(acos) + ? Rotation2d.fromRadians(0) + : Rotation2d.fromRadians(acos); + } + + private double calculateMinimumSafeElevatorHeightRotations() { + final double armCos = RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().getCos(); + final double elevatorHeightFromArm = armCos * ArmElevatorConstants.ARM_LENGTH_METERS; + final double minimumElevatorSafeZone = ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumSafeHeightMeters = (RobotContainer.ARM_ELEVATOR.isArmAboveSafeAngle() + ? 0 : elevatorHeightFromArm) + + minimumElevatorSafeZone; + return metersToRotations(minimumSafeHeightMeters); + } + + private double getElevatorHeightFromMinimumSafeZone() { + return getCurrentElevatorPositionMeters() - ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + } + + private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { + return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); + } + + private Pose3d calculateVisualizationPose() { + final Transform3d armTransform = new Transform3d( + new Translation3d(0, 0, getCurrentElevatorPositionMeters()), + new Rotation3d(0, getCurrentArmAngle().getRadians(), 0) + ); + return ArmElevatorConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform); + } + + private void scaleElevatorPositionRequestSpeed(double speedScalar) { + positionRequest.Velocity = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY * speedScalar; + positionRequest.Acceleration = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + positionRequest.Jerk = positionRequest.Acceleration * 10; + } + + private Pose3d getFirstStageComponentPose() { + return calculateComponentPose(ArmElevatorConstants.ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstStageComponentPoseHeight()); + } + + private Pose3d getSecondStageComponentPose() { + return calculateComponentPose(ArmElevatorConstants.ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getCurrentElevatorPositionMeters()); + } + + private double getFirstStageComponentPoseHeight() { + if (isSecondStageComponentLimitReached()) + return getCurrentElevatorPositionMeters() - ArmElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + return 0; + } + + private boolean isSecondStageComponentLimitReached() { + return getCurrentElevatorPositionMeters() > ArmElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; + } + + private Pose3d calculateComponentPose(Pose3d originPoint, double currentHeight) { + final Transform3d elevatorTransform = new Transform3d( + new Translation3d(0, 0, currentHeight), + new Rotation3d() + ); + return originPoint.transformBy(elevatorTransform); + } + + + private double rotationsToMeters(double positionsRotations) { + return Conversions.rotationsToDistance(positionsRotations, ArmElevatorConstants.DRUM_DIAMETER_METERS); + } + + private double metersToRotations(double positionsMeters) { + return Conversions.distanceToRotations(positionsMeters, ArmElevatorConstants.DRUM_DIAMETER_METERS); + } + + private double getElevatorPositionRotations() { + return elevatorMasterMotor.getSignal(TalonFXSignal.POSITION); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java new file mode 100644 index 00000000..87a0655d --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java @@ -0,0 +1,78 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.trigon.robot.RobotContainer; +import lib.commands.ExecuteEndCommand; +import lib.commands.GearRatioCalculationCommand; +import lib.commands.NetworkTablesCommand; + +import java.util.Set; +import java.util.function.Supplier; + +public class ArmElevatorCommands { + public static Command getDebuggingCommand(boolean ignoreConstraints) { + return new NetworkTablesCommand( + (targetArmAngleDegrees, targetElevatorPositionMeters) -> { + RobotContainer.ARM_ELEVATOR.setTargetArmAngle(Rotation2d.fromDegrees(targetArmAngleDegrees), ignoreConstraints); + RobotContainer.ARM_ELEVATOR.setTargetElevatorPositionMeters(targetElevatorPositionMeters, ignoreConstraints); + }, + true, + Set.of(RobotContainer.ARM_ELEVATOR), + "Debugging/ArmTargetPositionDegrees", + "Debugging/ElevatorTargetPositionMeters" + ); + } + + public static Command getArmGearRatioCalulationCommand() { + return new GearRatioCalculationCommand( + ArmElevatorConstants.ARM_MASTER_MOTOR, + ArmElevatorConstants.ANGLE_ENCODER, + 0.5, + RobotContainer.ARM_ELEVATOR + ); + } + + public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) { + return getSetTargetStateCommand(() -> targetState); + } + + public static Command getSetTargetStateCommand(Supplier targetState) { + return getSetTargetStateCommand(targetState, () -> false); + } + + public static Command getSetTargetStateCommand(Supplier targetState, Supplier isStateReversed) { + return new SequentialCommandGroup( + getPrepareForStateCommand(targetState, isStateReversed) + .onlyIf(() -> targetState.get().ignoreConstraints && targetState.get().prepareState != null) + .until(() -> RobotContainer.ARM_ELEVATOR.atState(targetState.get().prepareState, isStateReversed.get())), + new ExecuteEndCommand( + () -> RobotContainer.ARM_ELEVATOR.setTargetState(targetState.get(), isStateReversed.get()), + RobotContainer.ARM_ELEVATOR::stop, + RobotContainer.ARM_ELEVATOR + ) + ); + } + + public static Command getPrepareForStateCommand(Supplier targetState) { + return getPrepareForStateCommand(targetState, () -> false); + } + + public static Command getPrepareForStateCommand(Supplier targetState, Supplier isStateReversed) { + return new ExecuteEndCommand( + () -> RobotContainer.ARM_ELEVATOR.prepareToState(targetState.get(), isStateReversed.get()), + RobotContainer.ARM_ELEVATOR::stop, + RobotContainer.ARM_ELEVATOR + ); + } + + public static Command getDefaultCommand() { + return new ConditionalCommand( + getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_WITH_CORAL), + getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST), + RobotContainer.END_EFFECTOR::hasGamePiece + ); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java new file mode 100644 index 00000000..1ee51cef --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java @@ -0,0 +1,364 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.*; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import lib.hardware.RobotHardwareStats; +import lib.hardware.misc.simplesensor.SimpleSensor; +import lib.hardware.phoenix6.cancoder.CANcoderEncoder; +import lib.hardware.phoenix6.cancoder.CANcoderSignal; +import lib.hardware.phoenix6.talonfx.TalonFXMotor; +import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import lib.hardware.simulation.ElevatorSimulation; +import lib.hardware.simulation.SingleJointedArmSimulation; +import lib.utilities.Conversions; +import lib.utilities.mechanisms.ElevatorMechanism2d; +import lib.utilities.mechanisms.SingleJointedArmMechanism2d; + +import java.util.function.DoubleSupplier; + +public class ArmElevatorConstants { + private static final int + ARM_MASTER_MOTOR_ID = 13, + ARM_FOLLOWER_MOTOR_ID = 14, + ANGLE_ENCODER_ID = 13, + ELEVATOR_MASTER_MOTOR_ID = 16, + ELEVATOR_FOLLOWER_MOTOR_ID = 17, + REVERSE_LIMIT_SENSOR_CHANNEL = 0; + private static final String + ARM_MASTER_MOTOR_NAME = "ArmMasterMotor", + ARM_FOLLOWER_MOTOR_NAME = "ArmFollowerMotor", + ANGLE_ENCODER_NAME = "ArmEncoder", + ELEVATOR_MASTER_MOTOR_NAME = "ElevatorMasterMotor", + ELEVATOR_FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor", + REVERSE_LIMIT_SENSOR_NAME = "ElevatorReverseLimitSensor"; + static final TalonFXMotor + ARM_MASTER_MOTOR = new TalonFXMotor(ARM_MASTER_MOTOR_ID, ARM_MASTER_MOTOR_NAME), + ARM_FOLLOWER_MOTOR = new TalonFXMotor(ARM_FOLLOWER_MOTOR_ID, ARM_FOLLOWER_MOTOR_NAME), + ELEVATOR_MASTER_MOTOR = new TalonFXMotor(ELEVATOR_MASTER_MOTOR_ID, ELEVATOR_MASTER_MOTOR_NAME), + ELEVATOR_FOLLOWER_MOTOR = new TalonFXMotor(ELEVATOR_FOLLOWER_MOTOR_ID, ELEVATOR_FOLLOWER_MOTOR_NAME); + static final CANcoderEncoder ANGLE_ENCODER = new CANcoderEncoder(ANGLE_ENCODER_ID, ANGLE_ENCODER_NAME); + private static final SimpleSensor REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME); + + private static final double + ARM_GEAR_RATIO = 40, + ELEVATOR_GEAR_RATIO = 4; + private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; + private static final double + ARM_MOTOR_CURRENT_LIMIT = 50, + ELEVATOR_MOTOR_CURRENT_LIMIT = 50; + private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0; + static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET; + private static final boolean + SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false, + SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER = false; + static final double + ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 0, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 0, + ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10, + ELEVATOR_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, + ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; + static final boolean FOC_ENABLED = true; + + public static final double + ARM_LENGTH_METERS = 0.52, + ELEVATOR_MASS_KILOGRAMS = 7, + DRUM_RADIUS_METERS = 0.04, + MINIMUM_ELEVATOR_HEIGHT_METERS = 0, + MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.382; + private static final int + ARM_MOTOR_AMOUNT = 2, + ELEVATOR_MOTOR_AMOUNT = 2; + private static final DCMotor + ARM_GEARBOX = DCMotor.getKrakenX60Foc(ARM_MOTOR_AMOUNT), + ELEVATOR_GEARBOX = DCMotor.getKrakenX60Foc(ELEVATOR_MOTOR_AMOUNT); + private static final double + ARM_MASS_KILOGRAMS = 3.5; + private static final Rotation2d + ARM_MINIMUM_ANGLE = Rotation2d.fromDegrees(0), + ARM_MAXIMUM_ANGLE = Rotation2d.fromDegrees(360); + private static final boolean SHOULD_ARM_SIMULATE_GRAVITY = true; + private static final boolean SHOULD_ELEVATOR_SIMULATE_GRAVITY = true; + private static final SingleJointedArmSimulation ARM_SIMULATION = new SingleJointedArmSimulation( + ARM_GEARBOX, + ARM_GEAR_RATIO, + ARM_LENGTH_METERS, + ARM_MASS_KILOGRAMS, + ARM_MINIMUM_ANGLE, + ARM_MAXIMUM_ANGLE, + SHOULD_ARM_SIMULATE_GRAVITY + ); + + private static final ElevatorSimulation SIMULATION = new ElevatorSimulation( + ELEVATOR_GEARBOX, + ELEVATOR_GEAR_RATIO, + ELEVATOR_MASS_KILOGRAMS, + DRUM_RADIUS_METERS, + MINIMUM_ELEVATOR_HEIGHT_METERS, + MAXIMUM_ELEVATOR_HEIGHT_METERS, + SHOULD_ELEVATOR_SIMULATE_GRAVITY + ); + + static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(1.5).per(Units.Seconds), + Units.Volts.of(3), + Units.Second.of(1000) + ); + + static final SysIdRoutine.Config ELEVATOR_SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(1.25).per(Units.Seconds), + Units.Volts.of(3), + Units.Second.of(1000) + ); + + static final SingleJointedArmMechanism2d ARM_MECHANISM = new SingleJointedArmMechanism2d( + "ArmMechanism", + ARM_LENGTH_METERS, + Color.kBlue + ); + + static final ElevatorMechanism2d ELEVATOR_MECHANISM = new ElevatorMechanism2d( + "ElevatorMechanism", + MAXIMUM_ELEVATOR_HEIGHT_METERS + 0.1, + MINIMUM_ELEVATOR_HEIGHT_METERS, + Color.kYellow + ); + + static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, -0.0954, 0.3573), + new Rotation3d(0, 0, 0) + ); + + public static final Pose3d ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, -0.17, 0.0504), + new Rotation3d(0, 0, 0) + ); + + static final Pose3d ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, -0.17, 0.0814), + new Rotation3d(0, 0, 0) + ); + + static final Transform3d ARM_TO_HELD_GAME_PIECE = new Transform3d( + new Translation3d(0, 0.1, -0.5855), + new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) + ); + static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.603; + static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; + private static final double REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; + private static final BooleanEvent REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + REVERSE_LIMIT_SENSOR::getBinaryValue + ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); + + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); + + /** + * The highest point of the arms angular zone where the safety logic applies. + */ + static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90); + + /** + * The lowest point in the Elevators zone where the safety logic applies. + */ + public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; + + private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; + static final double ELEVATOR_POSITION_TOLERANCE_METERS = 0.02; + + static { + configureArmMasterMotor(); + configureArmFollowerMotor(); + configureElevatorMasterMotor(); + configureElevatorFollowerMotor(); + configureAngleEncoder(); + configureReverseLimitSensor(); + } + + private static void configureArmMasterMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.Feedback.RotorToSensorRatio = ARM_GEAR_RATIO; + config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0; + + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.MotionMagic.MotionMagicCruiseVelocity = ARM_DEFAULT_MAXIMUM_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION; + config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(270); + + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + + ARM_MASTER_MOTOR.applyConfiguration(config); + ARM_MASTER_MOTOR.setPhysicsSimulation(ARM_SIMULATION); + + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureArmFollowerMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; + + ARM_FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower followerRequest = new Follower(ARM_MASTER_MOTOR.getID(), SHOULD_ARM_FOLLOWER_OPPOSE_MASTER); + ARM_FOLLOWER_MOTOR.setControl(followerRequest); + } + + private static void configureElevatorMasterMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.SensorToMechanismRatio = ELEVATOR_GEAR_RATIO; + + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0; + + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; + + config.MotionMagic.MotionMagicCruiseVelocity = ELEVATOR_DEFAULT_MAXIMUM_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION; + config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = ELEVATOR_MOTOR_CURRENT_LIMIT; + + ELEVATOR_MASTER_MOTOR.applyConfiguration(config); + ELEVATOR_MASTER_MOTOR.setPhysicsSimulation(SIMULATION); + + ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureElevatorFollowerMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = ELEVATOR_MOTOR_CURRENT_LIMIT; + + ELEVATOR_FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower followerRequest = new Follower(ELEVATOR_MASTER_MOTOR.getID(), SHOULD_ELEVATOR_FOLLOWER_OPPOSE_MASTER); + ELEVATOR_FOLLOWER_MOTOR.setControl(followerRequest); + } + + private static void configureAngleEncoder() { + final CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_GRAVITY_OFFSET; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; + + ANGLE_ENCODER.applyConfiguration(config); + ANGLE_ENCODER.setSimulationInputsFromTalonFX(ARM_MASTER_MOTOR); + + ANGLE_ENCODER.registerSignal(CANcoderSignal.POSITION, 100); + ANGLE_ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); + } + + private static void configureReverseLimitSensor() { + REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); + REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> ELEVATOR_MASTER_MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); + } + + public enum ArmElevatorState { + PREPARE_SCORE_L2(Rotation2d.fromDegrees(100), 0.3, null, false, 1), + PREPARE_SCORE_L3(Rotation2d.fromDegrees(100), 0.7, null, false, 1), + PREPARE_SCORE_L4(Rotation2d.fromDegrees(120), 1.2, null, false, 1), + REST(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), + REST_WITH_CORAL(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7), + REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), + REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7), + EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7), + SCORE_L1(Rotation2d.fromDegrees(45), 0.603, null, false, 1), + SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1), + SCORE_L3(Rotation2d.fromDegrees(90), 0.7, PREPARE_SCORE_L3, false, 1), + SCORE_L4(Rotation2d.fromDegrees(100), 1.2, PREPARE_SCORE_L4, false, 1), + SCORE_NET(Rotation2d.fromDegrees(160), 1.382, null, false, 0.3), + SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), + COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.603, null, false, 1), + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.953, null, false, 1), + PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(60), 0.2, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(50), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1); + + public final Rotation2d targetAngle; + public final double targetPositionMeters; + public final ArmElevatorState prepareState; + public final boolean ignoreConstraints; + final double speedScalar; + + ArmElevatorState(Rotation2d targetAngle, double targetPositionMeters, ArmElevatorState prepareState, boolean ignoreConstraints, double speedScalar) { + this.targetAngle = targetAngle; + this.targetPositionMeters = targetPositionMeters; + this.prepareState = prepareState; + this.ignoreConstraints = ignoreConstraints; + this.speedScalar = speedScalar; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java deleted file mode 100644 index 080739d1..00000000 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ /dev/null @@ -1,179 +0,0 @@ -package frc.trigon.robot.subsystems.elevator; - -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.subsystems.MotorSubsystem; -import frc.trigon.robot.subsystems.arm.ArmConstants; -import lib.hardware.phoenix6.talonfx.TalonFXMotor; -import lib.hardware.phoenix6.talonfx.TalonFXSignal; -import lib.utilities.Conversions; -import org.littletonrobotics.junction.Logger; - -public class Elevator extends MotorSubsystem { - private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR; - private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ElevatorConstants.FOC_ENABLED); - private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( - 0, - ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY, - ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION, - ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10 - ).withEnableFOC(ElevatorConstants.FOC_ENABLED); - private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST; - - public Elevator() { - setName("Elevator"); - } - - @Override - public SysIdRoutine.Config getSysIDConfig() { - return ElevatorConstants.SYSID_CONFIG; - } - - @Override - public void setBrake(boolean brake) { - masterMotor.setBrake(brake); - } - - @Override - public void stop() { - masterMotor.stopMotor(); - } - - @Override - public void updateLog(SysIdRoutineLog log) { - log.motor("Elevator") - .linearPosition(Units.Meters.of(getPositionRotations())) - .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) - .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); - } - - @Override - public void sysIDDrive(double targetVoltage) { - masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); - } - - @Override - public void updateMechanism() { - ElevatorConstants.MECHANISM.update( - getPositionMeters(), - rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) - ); - - Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); - Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); - } - - @Override - public void updatePeriodically() { - masterMotor.update(); - Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); - } - - public boolean atState(ElevatorConstants.ElevatorState targetState) { - return targetState == this.targetState && atTargetState(); - } - - public boolean atTargetState() { - final double currentToTargetStateDifferenceMeters = Math.abs(targetState.targetPositionMeters - getPositionMeters()); - return currentToTargetStateDifferenceMeters < ElevatorConstants.HEIGHT_TOLERANCE_METERS; - } - - public boolean atPreparedTargetState() { - final double currentToTargetStateDifferenceMeters = Math.abs(targetState.prepareStatePositionMeters - getPositionMeters()); - return currentToTargetStateDifferenceMeters < ElevatorConstants.HEIGHT_TOLERANCE_METERS; - } - - public double getPositionMeters() { - return rotationsToMeters(getPositionRotations()); - } - - public boolean isElevatorAboveSafeZone() { - return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; - } - - public double getElevatorHeightFromMinimumSafeZone() { - return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - } - - void setTargetState(ElevatorConstants.ElevatorState targetState) { - this.targetState = targetState; - scalePositionRequestSpeed(targetState.speedScalar); - if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { - masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters))); - return; - } - setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); - } - - void prepareState(ElevatorConstants.ElevatorState targetState) { - this.targetState = targetState; - scalePositionRequestSpeed(targetState.speedScalar); - setTargetPositionRotations(metersToRotations(targetState.prepareStatePositionMeters)); - } - - void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); - } - - private double calculateMinimumSafeElevatorHeightRotations() { - final double armCos = RobotContainer.ARM.getAngle().getRadians(); - final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; - final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle() - ? 0 : elevatorHeightFromArm) - + minimumElevatorSafeZone; - return metersToRotations(minimumSafeHeightMeters); - } - - private Pose3d getFirstStageComponentPose() { - return calculateComponentPose(ElevatorConstants.ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstStageComponentPoseHeight()); - } - - private Pose3d getSecondStageComponentPose() { - return calculateComponentPose(ElevatorConstants.ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters()); - } - - private double getFirstStageComponentPoseHeight() { - if (isSecondStageComponentLimitReached()) - return getPositionMeters() - ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; - return 0; - } - - private boolean isSecondStageComponentLimitReached() { - return getPositionMeters() > ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS; - } - - private Pose3d calculateComponentPose(Pose3d originPoint, double currentHeight) { - final Transform3d elevatorTransform = new Transform3d( - new Translation3d(0, 0, currentHeight), - new Rotation3d() - ); - return originPoint.transformBy(elevatorTransform); - } - - private void scalePositionRequestSpeed(double speedScalar) { - positionRequest.Velocity = ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar; - positionRequest.Acceleration = ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar; - positionRequest.Jerk = positionRequest.Acceleration * 10; - } - - private double rotationsToMeters(double positionsRotations) { - return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS); - } - - private double metersToRotations(double positionsMeters) { - return Conversions.distanceToRotations(positionsMeters, ElevatorConstants.DRUM_DIAMETER_METERS); - } - - private double getPositionRotations() { - return masterMotor.getSignal(TalonFXSignal.POSITION); - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java deleted file mode 100644 index 988476fb..00000000 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ /dev/null @@ -1,59 +0,0 @@ -package frc.trigon.robot.subsystems.elevator; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.trigon.robot.RobotContainer; -import lib.commands.ExecuteEndCommand; -import lib.commands.NetworkTablesCommand; - -import java.util.Set; -import java.util.function.Supplier; - -public class ElevatorCommands { - public static Command getDebbugingCommand() { - return new NetworkTablesCommand( - RobotContainer.ELEVATOR::setTargetPositionRotations, - false, - Set.of(RobotContainer.ELEVATOR), - "Debugging/ElevatorTargetPositionRotations" - ); - } - - public static Command getSetTargetStateCommand(Supplier targetState) { - return new ExecuteEndCommand( - () -> RobotContainer.ELEVATOR.setTargetState(targetState.get()), - RobotContainer.ELEVATOR::stop, - RobotContainer.ELEVATOR - ); - } - - public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) { - return new ExecuteEndCommand( - () -> RobotContainer.ELEVATOR.setTargetState(targetState), - RobotContainer.ELEVATOR::stop, - RobotContainer.ELEVATOR - ); - } - - public static Command getSetTargetStateCommand(double targetPositionRotations) { - return new ExecuteEndCommand( - () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations), - RobotContainer.ELEVATOR::stop, - RobotContainer.ELEVATOR - ); - } - - public static Command getPrepareStateCommand(Supplier targetState) { - return new ExecuteEndCommand( - () -> RobotContainer.ELEVATOR.prepareState(targetState.get()), - RobotContainer.ELEVATOR::stop, - RobotContainer.ELEVATOR - ); - } - - public static Command getPrepareStateCommand(ElevatorConstants.ElevatorState targetState) { - return new ExecuteEndCommand( - () -> RobotContainer.ELEVATOR.prepareState(targetState), - RobotContainer.ELEVATOR::stop, - RobotContainer.ELEVATOR); - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java deleted file mode 100644 index afff2a4f..00000000 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ /dev/null @@ -1,198 +0,0 @@ -package frc.trigon.robot.subsystems.elevator; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.event.BooleanEvent; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.subsystems.arm.ArmConstants; -import lib.hardware.RobotHardwareStats; -import lib.hardware.misc.simplesensor.SimpleSensor; -import lib.hardware.phoenix6.talonfx.TalonFXMotor; -import lib.hardware.phoenix6.talonfx.TalonFXSignal; -import lib.hardware.simulation.ElevatorSimulation; -import lib.utilities.mechanisms.ElevatorMechanism2d; - -import java.util.function.DoubleSupplier; - -public class ElevatorConstants { - private static final int - MASTER_MOTOR_ID = 16, - FOLLOWER_MOTOR_ID = 17, - REVERSE_LIMIT_SENSOR_CHANNEL = 0; - private static final String - MASTER_MOTOR_NAME = "ElevatorMasterMotor", - FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor", - REVERSE_LIMIT_SENSOR_NAME = "ElevatorReverseLimitSensor"; - static final TalonFXMotor - MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME), - FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME); - private static final SimpleSensor REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME); - - private static final double GEAR_RATIO = 4; - private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; - private static final boolean SHOULD_FOLLOWER_OPPOSE_MASTER = false; - static final double - DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 25.178 : 20, - DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 20; - static final boolean FOC_ENABLED = true; - - private static final int MOTOR_AMOUNT = 2; - private static final DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); - private static final double - ELEVATOR_MASS_KILOGRAMS = 7, - DRUM_RADIUS_METERS = 0.04, - MINIMUM_ELEVATOR_HEIGHT_METERS = 0, - MAXIMUM_ELEVATOR_HEIGHT_METERS = 1.382; - private static final boolean SHOULD_SIMULATE_GRAVITY = true; - private static final ElevatorSimulation SIMULATION = new ElevatorSimulation( - GEARBOX, - GEAR_RATIO, - ELEVATOR_MASS_KILOGRAMS, - DRUM_RADIUS_METERS, - MINIMUM_ELEVATOR_HEIGHT_METERS, - MAXIMUM_ELEVATOR_HEIGHT_METERS, - SHOULD_SIMULATE_GRAVITY - ); - static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(1.25).per(Units.Seconds), - Units.Volts.of(3), - Units.Second.of(1000) - ); - - public static final Pose3d ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.17, 0.0504), - new Rotation3d(0, 0, 0) - ); - static final Pose3d ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.17, 0.0814), - new Rotation3d(0, 0, 0) - ); - static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( - "ElevatorMechanism", - MAXIMUM_ELEVATOR_HEIGHT_METERS + 0.1, - MINIMUM_ELEVATOR_HEIGHT_METERS, - Color.kYellow - ); - static final double HEIGHT_TOLERANCE_METERS = 0.01; - static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.603; - static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; - private static final double REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; - private static final BooleanEvent REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - REVERSE_LIMIT_SENSOR::getBinaryValue - ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); - - /** - * The lowest point in the Elevators zone where the safety logic applies. - */ - public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; - - /** - * The highest point in the Elevators zone where the safety logic applies. - */ - public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS; - private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; - static final double ELEVATOR_POSITION_TOLERANCE_METERS = 0.02; - - static { - configureMasterMotor(); - configureFollowerMotor(); - configureReverseLimitSensor(); - } - - private static void configureMasterMotor() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Audio.BeepOnBoot = false; - config.Audio.BeepOnConfig = false; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 0; - config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0; - - config.Slot0.GravityType = GravityTypeValue.Elevator_Static; - config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; - - config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; - config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; - config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - - MASTER_MOTOR.applyConfiguration(config); - MASTER_MOTOR.setPhysicsSimulation(SIMULATION); - - MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); - } - - private static void configureFollowerMotor() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Audio.BeepOnBoot = false; - config.Audio.BeepOnConfig = false; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - FOLLOWER_MOTOR.applyConfiguration(config); - - final Follower followerRequest = new Follower(MASTER_MOTOR.getID(), SHOULD_FOLLOWER_OPPOSE_MASTER); - FOLLOWER_MOTOR.setControl(followerRequest); - } - - private static void configureReverseLimitSensor() { - REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MASTER_MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); - } - - public enum ElevatorState { - REST(0.603, 0.603, 0.7), - LOAD_CORAL(0.5519, 0.5519, 0.7), - SCORE_L1(0.1, 0.1, 1), - SCORE_L2(0.3, 0.4, 1), - SCORE_L3(0.7, 0.8, 1), - SCORE_L4(1.2, 1.3, 1), - COLLECT_ALGAE_FROM_L2(0.603, 0.603, 1), - COLLECT_ALGAE_FROM_L3(0.953, 0.953, 1), - COLLECT_ALGAE_FROM_GROUND(0, 0, 0.7), - REST_WITH_ALGAE(0.603, 0.603, 0.3), - SCORE_NET(1.382, 1.382, 0.3); - - public final double targetPositionMeters; - public final double prepareStatePositionMeters; - final double speedScalar; - - ElevatorState(double targetPositionMeters, double prepareStatePositionMeters, double speedScalar) { - this.targetPositionMeters = targetPositionMeters; - this.prepareStatePositionMeters = prepareStatePositionMeters; - this.speedScalar = speedScalar; - } - } -} diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java new file mode 100644 index 00000000..1b669e41 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -0,0 +1,69 @@ +package frc.trigon.robot.subsystems.endeffector; + +import com.ctre.phoenix6.controls.VoltageOut; +import edu.wpi.first.math.geometry.Translation3d; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; +import frc.trigon.robot.subsystems.MotorSubsystem; +import lib.hardware.phoenix6.talonfx.TalonFXMotor; +import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import org.littletonrobotics.junction.AutoLogOutput; + +public class EndEffector extends MotorSubsystem { + private final TalonFXMotor endEffectorMotor = EndEffectorConstants.END_EFFECTOR_MOTOR; + private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(EndEffectorConstants.FOC_ENABLED); + + @Override + public void stop() { + endEffectorMotor.stopMotor(); + } + + @Override + public void updateMechanism() { + EndEffectorConstants.END_EFFECTOR_MECHANISM.update(endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); + } + + @Override + public void updatePeriodically() { + endEffectorMotor.update(); + EndEffectorConstants.DISTANCE_SENSOR.updateSensor(); + } + + @AutoLogOutput(key = "EndEffector/HasCoral") + public boolean hasGamePiece() { + return EndEffectorConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); + } + + @AutoLogOutput(key = "EndEffector/GamePieceInEndEffector") + public boolean isGamePieceInEndEffector() { + return SimulationFieldHandler.isCoralInEndEffector(); + } + + public Translation3d calculateLinearEndEffectorVelocity() { + double velocityMetersPerSecond = endEffectorMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * EndEffectorConstants.WHEEL_RADIUS_METERS; + return RobotContainer.ARM_ELEVATOR.calculateLinearArmVelocity().plus( + new Translation3d( + RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().getCos() * velocityMetersPerSecond, + RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().getSin() * velocityMetersPerSecond, + 0 + ) + ); + } + + public boolean isEjecting() { + return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; + } + + void setTargetState(EndEffectorConstants.EndEffectorState targetState) { + setEndEffectorTargetVoltage(targetState.targetVoltage); + } + + void setTargetState(double targetVoltage) { + setEndEffectorTargetVoltage(targetVoltage); + } + + private void setEndEffectorTargetVoltage(double targetVoltage) { + EndEffectorConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); + endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java new file mode 100644 index 00000000..93455350 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java @@ -0,0 +1,27 @@ +package frc.trigon.robot.subsystems.endeffector; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.trigon.robot.RobotContainer; +import lib.commands.NetworkTablesCommand; + +import java.util.Set; + +public class EndEffectorCommands { + public static Command getDebuggingCommand() { + return new NetworkTablesCommand( + RobotContainer.END_EFFECTOR::setTargetState, + false, + Set.of(RobotContainer.END_EFFECTOR), + "Debugging/EndEffectorTargetVoltage" + ); + } + + public static Command getSetTargetStateCommand(EndEffectorConstants.EndEffectorState targetState) { + return new StartEndCommand( + () -> RobotContainer.END_EFFECTOR.setTargetState(targetState), + RobotContainer.END_EFFECTOR::stop, + RobotContainer.END_EFFECTOR + ); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java new file mode 100644 index 00000000..e0c3afbe --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -0,0 +1,101 @@ +package frc.trigon.robot.subsystems.endeffector; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; +import lib.hardware.misc.simplesensor.SimpleSensor; +import lib.hardware.phoenix6.talonfx.TalonFXMotor; +import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import lib.hardware.simulation.SimpleMotorSimulation; +import lib.utilities.mechanisms.SpeedMechanism2d; + +import java.util.function.DoubleSupplier; + +public class EndEffectorConstants { + private static final int + END_EFFECTOR_MOTOR_ID = 15, + DISTANCE_SENSOR_CHANNEL = 4; + private static final String + END_EFFECTOR_MOTOR_NAME = "EndEffectorMotor", + DISTANCE_SENSOR_NAME = "EndEffectorDistanceSensor"; + static final TalonFXMotor END_EFFECTOR_MOTOR = new TalonFXMotor(END_EFFECTOR_MOTOR_ID, END_EFFECTOR_MOTOR_NAME); + static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); + + static final boolean FOC_ENABLED = true; + private static final double END_EFFECTOR_GEAR_RATIO = 17; + private static final int END_EFFECTOR_MOTOR_AMOUNT = 1; + private static final DCMotor END_EFFECTOR_GEARBOX = DCMotor.getKrakenX60Foc(END_EFFECTOR_MOTOR_AMOUNT); + private static final double + END_EFFECTOR_MOMENT_OF_INERTIA = 0.003, + END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY = 12; + private static final SimpleMotorSimulation END_EFFECTOR_SIMULATION = new SimpleMotorSimulation( + END_EFFECTOR_GEARBOX, + END_EFFECTOR_GEAR_RATIO, + END_EFFECTOR_MOMENT_OF_INERTIA + ); + + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> (SimulationFieldHandler.isHoldingCoral() && SimulationFieldHandler.isCoralInEndEffector()) || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; + + static final SpeedMechanism2d END_EFFECTOR_MECHANISM = new SpeedMechanism2d( + "EndEffectorMechanism", + END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY + ); + + private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; + static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + DISTANCE_SENSOR::getBinaryValue + ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); + static final double WHEEL_RADIUS_METERS = edu.wpi.first.math.util.Units.inchesToMeters(1.5); + + static { + configureEndEffectorMotor(); + configureDistanceSensor(); + } + + private static void configureEndEffectorMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO; + + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = 80; + + END_EFFECTOR_MOTOR.applyConfiguration(config); + END_EFFECTOR_MOTOR.setPhysicsSimulation(END_EFFECTOR_SIMULATION); + + END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + END_EFFECTOR_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureDistanceSensor() { + DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); + } + + public enum EndEffectorState { + REST(0), + EJECT(4), + LOAD_CORAL(-4), + SCORE_CORAL(4), + COLLECT_ALGAE(-4), + SCORE_ALGAE(4), + HOLD_ALGAE(-4); + + public final double targetVoltage; + + EndEffectorState(double targetVoltage) { + this.targetVoltage = targetVoltage; + } + } +}