diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java new file mode 100644 index 00000000..cb52f00d --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -0,0 +1,36 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; +import frc.trigon.robot.subsystems.transporter.TransporterCommands; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; + +public class CoralEjectionCommands { + public static Command getCoralEjectionCommand() { + return GeneralCommands.getContinuousConditionalCommand( + getEjectCoralFromIntakeCommand(), + getEjectCoralFromArmCommand(), + () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() + ); + } + + private static Command getEjectCoralFromIntakeCommand() { + return new ParallelCommandGroup( + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.EJECT), + TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.EJECT) + ); + } + + private static Command getEjectCoralFromArmCommand() { + return new SequentialCommandGroup( + ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT) + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index d191f552..1b14eb1a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -25,6 +25,7 @@ public class Arm extends MotorSubsystem { 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() { @@ -80,10 +81,24 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { + if (isStateReversed) + return this.targetState == targetState && atTargetAngle(isStateReversed); + return atState(targetState); + } + public boolean atState(ArmConstants.ArmState targetState) { return this.targetState == targetState && atTargetAngle(); } + public boolean atTargetAngle(boolean isStateReversed) { + if (isStateReversed) { + final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees()); + return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + } + return atTargetAngle(); + } + public boolean atTargetAngle() { final double currentToTargetStateDifferenceDegrees = Math.abs(targetState.targetAngle.minus(getAngle()).getDegrees()); return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); @@ -93,23 +108,26 @@ public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + 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( - Rotation2d.fromDegrees(360 - targetState.targetAngle.getDegrees()) + ArmConstants.FULL_ROTATION.minus(targetState.targetAngle) , targetState.targetEndEffectorVoltage ); return; } - setTargetState(targetState); - } - - void setTargetState(ArmConstants.ArmState targetState) { - this.targetState = targetState; setTargetState( targetState.targetAngle, - targetState.targetEndEffectorVoltage - ); + targetState.targetEndEffectorVoltage); } void setTargetState(Rotation2d targetAngle, double targetVoltage) { @@ -117,6 +135,21 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetVoltage(targetVoltage); } + void prepareForState(ArmConstants.ArmState targetState) { + prepareForState(targetState, false); + } + + void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { + this.isStateReversed = isStateReversed; + this.targetState = targetState; + + if (isStateReversed) { + setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)); + return; + } + setTargetAngle(targetState.targetAngle); + } + private Rotation2d getAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 78c9a6cc..c7bcd58c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -47,4 +47,20 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState RobotContainer.ARM ); } + + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { + return new StartEndCommand( + () -> RobotContainer.ARM.prepareForState(targetState, isStateReversed), + RobotContainer.ARM::stop, + RobotContainer.ARM + ); + } + + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { + return new StartEndCommand( + () -> RobotContainer.ARM.prepareForState(targetState), + RobotContainer.ARM::stop, + RobotContainer.ARM + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 197dfc7e..0bdb08ef 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -120,6 +120,7 @@ public class ArmConstants { CommandScheduler.getInstance().getActiveButtonLoop(), DISTANCE_SENSOR::getBinaryValue ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); + static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360); static { configureArmMasterMotor(); @@ -229,6 +230,7 @@ public enum ArmState { REST(Rotation2d.fromDegrees(0), 0), REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0), HOLD_ALGAE(Rotation2d.fromDegrees(90), -4), + EJECT(Rotation2d.fromDegrees(60), 4), PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), SCORE_L1(Rotation2d.fromDegrees(75), 4), PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0),