Ejection Commands (#14)

* Added Ejecting commands

* added ejecting coral from arm/gripper

* Update CoralEjectionCommands.java

* made atTargetAngle and atState functions that check for reversed states

* forget to add the logic for the reversed states

* added variable to know if the state is reversed or not

* fixed logic issues and renamed stuff

* switched reversed state functions to get called by regular ones

---------

Co-authored-by: ShmayaR <shmaya.rosenblatt@gmail.com>

Author: Nummun14

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)
+        );
+    }
+}

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,30 +108,48 @@ 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) {
         setTargetAngle(targetAngle);
         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));
     }

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
+        );
+    }
 }

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),