Feature/narwhal autonomous commands

src/main/java/frc/robot/Constants.java

@@ -265,18 +265,28 @@ public static class NarwhalConstants {
         public static final double UPPER_ASSEMBLY_MOI = 0.0; //Kg m^2
 
         public static class NarwhalIntakeOuttakeConstants {
+            // Sensor IDs
+            public static final int CORAL_LIMIT_SWITCH_PORT = 0; // Should be 0-9
+
+            // Motor IDs
             public static final int INTAKE_OUTTAKE_MOTOR_CAN_ID = 12;
             public static final int INTAKE_OUTTAKE_MOTOR_CURRENT_LIMIT = 40;
+
             // Settings for % of max power to use on intake and outtake
-            public static final double INTAKE_MOTOR_PERCENT = 0.3; // between -1.0 and 1.0
-            public static final double OUTTAKE_MOTOR_PERCENT = -0.5; // between -1.0 and 1.0
+            public static final double INTAKE_MOTOR_PERCENT = -0.3; // between -1.0 and 1.0
+            public static final double OUTTAKE_MOTOR_PERCENT = 0.5; // between -1.0 and 1.0
 
+            // PID configurations
             public static final double POSITION_PID_P = 0.7;
             public static final double POSITION_PID_I = 0;
             public static final double POSITION_PID_D = 0.2;
 
             public static final double PID_MAX_OUTPUT = 0.5;
             public static final double PID_MIN_OUTPUT = -0.5;
+
+            // Coral intake detection delay - used in the CoralIntakeCommand for auto to prevent the command from ending too early
+            public static final double CORAL_INTAKE_DETECTION_DELAY = 0.1; // seconds
+            public static final double CORAL_OUTTAKE_DETECTION_DELAY = 0.35; // seconds
         }
 
         public static class NarwhalWristConstants {
@@ -285,7 +295,7 @@ public static class NarwhalWristConstants {
 
             // Calculating encoder conversion factor
             public static final double GEAR_REDUCTION = 20;
-            public static final double ROTATIONS_TO_RADIANS = Math.PI * 2 / GEAR_REDUCTION; // Wrist target angles (radians) are multiplied by this to get the motor target position           
+            public static final double ENCODER_FACTOR = Math.PI * 2 / GEAR_REDUCTION; // Wrist target angles (radians) are multiplied by this to get the motor target position           
 
             // PID configurations
             public static final double POSITION_PID_P = 0.3;
@@ -295,19 +305,20 @@ public static class NarwhalWristConstants {
             public static final double PID_OUTPUT_RANGE_MIN = -0.35;        
 
             // Set position for wrist angles (Angle is relative to the world, with 0 being the down position and rotating away from 0 being positive)
-            public static final Rotation2d INTAKE_ANGLE = Rotation2d.fromRadians(Math.PI / 4.85); // -pi/4 TODO: update these placeholder values
+            public static final Rotation2d INTAKE_ANGLE = Rotation2d.fromRadians(Math.PI / 4.85);
             public static final Rotation2d L1_OUTTAKE_ANGLE = Rotation2d.fromRadians(1.12 * Math.PI);
             public static final Rotation2d L2_OUTTAKE_ANGLE = Rotation2d.fromRadians(1.12 * Math.PI);
             public static final Rotation2d L3_OUTTAKE_ANGLE = Rotation2d.fromRadians(1.12 * Math.PI);
             public static final Rotation2d L4_OUTTAKE_ANGLE = Rotation2d.fromRadians(1.08 * Math.PI);
-            public static final Rotation2d ALGAE_ANGLE =  Rotation2d.fromRadians(3 * Math.PI / 2); // pi/2 TODO: update these placeholder values
+            public static final Rotation2d CLIMB_ANGLE = Rotation2d.fromRadians(3 * Math.PI / 2); // This is the angle the wrist should be at when climbing
+            public static final Rotation2d ALGAE_ANGLE =  Rotation2d.fromRadians(3 * Math.PI / 2);
 
             /** The angle tolerance for the wrist to be considered at a specific state. */
-            public static final double WRIST_ANGLE_TOLERANCE = 0.3;
+            public static final Rotation2d WRIST_ANGLE_TOLERANCE = Rotation2d.fromRadians(0.89 * Math.PI);
         }
 
         public static class NarwhalClimberConstants {
-            public static final int CLIMBER_CAN_ID = 15;
+            public static final int CLIMBER_CAN_ID = 13;
 
             public static final boolean IS_CLIMBER_INVERTED = false;
 
@@ -325,10 +336,13 @@ public static class NarwhalClimberConstants {
 
             public static final Rotation2d DEPLOYED_ANGLE = Rotation2d.fromDegrees(40);
             public static final Rotation2d CLIMB_ANGLE = Rotation2d.fromDegrees(-13.5);
+
+            /** The angle tolerance for the climber to be considered at a specific state. */
+            public static final Rotation2d CLIMBER_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3);
         }
 
         public static class NarwhalElevatorConstants {
-            public static final int ELEVATOR_LEAD_MOTOR_CAN_ID = 13;
+            public static final int ELEVATOR_LEAD_MOTOR_CAN_ID = 15;
             public static final int ELEVATOR_LEAD_MOTOR_CURRENT_LIMIT = 40;
 
             public static final int ELEVATOR_FOLLOWER_CAN_ID = 16;
@@ -357,6 +371,7 @@ public static class NarwhalElevatorConstants {
             public static final double L4_ELEVATOR_HEIGHT = 1.7; // Meters
             public static final double INTAKE_ELEVATOR_HEIGHT_METERS = 0.05; // Meters
             public static final boolean MOTOR_INVERTED = true;
+            public static final double ELEVATOR_POSITION_TOLERANCE = 0.05; // Meters
         }
     }
 

src/main/java/frc/robot/commands/narwhal/NarwhalClimbCommand.java

@@ -0,0 +1,57 @@
+package frc.robot.commands.narwhal;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.narwhal.NarwhalClimber;
+import frc.robot.subsystems.narwhal.NarwhalElevator;
+import frc.robot.subsystems.narwhal.NarwhalWrist;
+import frc.robot.util.upper_assembly.narwhal.NarwhalClimberState;
+
+
+/** 
+ * A command that climbs using the Narwhal upper assembly.
+ * Will lower the elevator to the minimum height, move the wrist out of the way to the
+ * climb angle, and then climb. If the robot is not ready to climb, the climber arm will be moved to the deploy position.
+ */
+public class NarwhalClimbCommand extends Command{
+    private final NarwhalElevator narwhalElevator;
+    private final NarwhalWrist narwhalWrist;
+    private final NarwhalClimber narwhalClimber;
+    private final Supplier<Boolean> readyToClimbSupplier;
+
+    /**
+     * Creates a new NarwhalClimbCommand.
+     * @param narwhalElevator The NarwhalElevator subsystem to use.
+     * @param narwhalWrist The NarwhalWrist subsystem to use.
+     * @param narwhalClimber The NarwhalClimber subsystem to use.
+     * @param readyToClimbSupplier A supplier that returns true when the robot is ready to climb.
+     */
+    public NarwhalClimbCommand(NarwhalElevator narwhalElevator, NarwhalWrist narwhalWrist, NarwhalClimber narwhalClimber, Supplier<Boolean> readyToClimbSupplier){
+        this.narwhalElevator = narwhalElevator;
+        this.narwhalWrist = narwhalWrist;
+        this.narwhalClimber = narwhalClimber;
+        this.readyToClimbSupplier = readyToClimbSupplier;
+        addRequirements(narwhalElevator, narwhalWrist, narwhalClimber);        
+    }
+
+    @Override
+    public void execute(){
+        narwhalElevator.goToMinimumHeight();
+        narwhalWrist.goToClimbAngle();
+
+        // If the robot is ready to climb, climb, otherwise move the climber arm to the deploy position
+        if(readyToClimbSupplier.get() && narwhalClimber.isAtDeployAngle() && narwhalWrist.isAtTargetPosition() && narwhalElevator.isAtTargetPosition()){
+            narwhalClimber.climb();
+        }
+        else if (narwhalClimber.currentState != NarwhalClimberState.CLIMBING && !narwhalClimber.isAtDeployAngle()){
+            narwhalClimber.goToDeploy();
+        }
+    }
+
+    @Override
+    public boolean isFinished(){
+        // Finish the command if the Coral sensor is no longer triggered (i.e. the Coral has been scored)
+        return narwhalClimber.isAtClimbAngle();
+    }
+}

src/main/java/frc/robot/commands/narwhal/NarwhalCoralIntakeCommand.java

@@ -0,0 +1,84 @@
+package frc.robot.commands.narwhal;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.NarwhalConstants.NarwhalIntakeOuttakeConstants;
+import frc.robot.subsystems.narwhal.NarwhalElevator;
+import frc.robot.subsystems.narwhal.NarwhalIntakeOuttake;
+import frc.robot.subsystems.narwhal.NarwhalWrist;
+
+
+/** 
+ * A command that scores the Coral using the Narwhal upper assembly. (NOTE: IF CORAL IS STUCK OR THE CORAL SENSOR IS MALFUNCTIONING, THIS COMMAND WILL NOT END)
+ * Will raise the elevator to the appropriate height, move the wrist to the
+ * appropriate angle, wait for the ready to intake supplier to return true, and then intake the Coral.
+ */
+public class NarwhalCoralIntakeCommand extends Command{
+    private final NarwhalElevator narwhalElevator;
+    private final NarwhalWrist narwhalWrist;
+    private final NarwhalIntakeOuttake narwhalIntakeOuttake;
+    private final Supplier<Boolean> readyToIntakeSupplier;
+
+    /** Keeps track of how long the command has been running to allow the command to autonomously stop if it's running for too long*/
+    private final Timer intakeDelayTimer;
+
+    /**
+     * Creates a new NarwhalCoralIntakeCommand.
+     * @param narwhalElevator The NarwhalElevator subsystem to use.
+     * @param narwhalWrist The NarwhalWrist subsystem to use.
+     * @param narwhalIntakeOuttake The NarwhalIntakeOuttake subsystem to use.
+     * @param readyToIntakeSupplier A supplier that returns true when the robot is ready to intake.
+     */
+    public NarwhalCoralIntakeCommand(NarwhalElevator narwhalElevator, NarwhalWrist narwhalWrist, NarwhalIntakeOuttake narwhalIntakeOuttake, Supplier<Boolean> readyToIntakeSupplier){
+        this.narwhalElevator = narwhalElevator;
+        this.narwhalWrist = narwhalWrist;
+        this.narwhalIntakeOuttake = narwhalIntakeOuttake;
+        this.readyToIntakeSupplier = readyToIntakeSupplier;
+        addRequirements(narwhalElevator, narwhalWrist, narwhalIntakeOuttake);
+
+        intakeDelayTimer = new Timer();
+    }
+
+    @Override
+    public void initialize(){
+        // Ensures the intake delay timer is stopped when the command starts
+        intakeDelayTimer.stop();
+        intakeDelayTimer.reset();
+    }
+
+    @Override
+    public void execute(){
+        // Move the elevator and wrist to the appropriate positions
+        narwhalElevator.goToIntakeHeight();
+        narwhalWrist.goToIntakeAngle();
+
+        // If the robot is ready to intake, intake the Coral, otherwise do nothing with the intake/outtake
+        if(readyToIntakeSupplier.get() && narwhalElevator.isAtTargetPosition() && narwhalWrist.isAtTargetPosition()){
+            narwhalIntakeOuttake.intake();
+        }
+
+        // Start the intake delay timer if the Coral sensor is triggered and the intake delay timer isn't already running
+        if(narwhalIntakeOuttake.isCoralSensorTriggered()){
+            if (!intakeDelayTimer.isRunning()){
+                intakeDelayTimer.start();
+            }
+        }
+    }
+
+    @Override
+    public boolean isFinished(){
+        // Finish the command if the Coral sensor is triggered (i.e. the Coral has been intaked) and the intake delay timer has elapsed (prevents premature ending)
+        return narwhalIntakeOuttake.isCoralSensorTriggered() && intakeDelayTimer.hasElapsed(NarwhalIntakeOuttakeConstants.CORAL_INTAKE_DETECTION_DELAY);
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        // Stop the intake/outtake motor when the command ends
+        narwhalIntakeOuttake.hold();
+        // Stop & reset the intake delay timer when the command ends
+        intakeDelayTimer.stop();
+        intakeDelayTimer.reset();
+    }
+}

src/main/java/frc/robot/commands/narwhal/NarwhalCoralScoreCommand.java

@@ -0,0 +1,109 @@
+package frc.robot.commands.narwhal;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.NarwhalConstants.NarwhalIntakeOuttakeConstants;
+import frc.robot.subsystems.narwhal.NarwhalElevator;
+import frc.robot.subsystems.narwhal.NarwhalIntakeOuttake;
+import frc.robot.subsystems.narwhal.NarwhalWrist;
+import frc.robot.util.upper_assembly.ScoringHeight;
+
+
+/** 
+ * A command that scores the Coral using the Narwhal upper assembly. (NOTE: IF THERE IS NO CORAL OR THE CORAL SENSOR IS MALFUNCTIONING, THIS COMMAND WILL END PREMATURLY)
+ * Will raise the elevator to the appropriate height, move the wrist to the
+ * appropriate angle, wait for the ready to score supplier to return true, and then score the Coral.
+ */
+public class NarwhalCoralScoreCommand extends Command{
+    private final NarwhalElevator narwhalElevator;
+    private final NarwhalWrist narwhalWrist;
+    private final NarwhalIntakeOuttake narwhalIntakeOuttake;
+    private final Supplier<Boolean> readyToScoreSupplier;
+
+    private ScoringHeight targetScoringHeight;
+
+    private final Timer outtakeDelayTimer;
+
+    /**
+     * Creates a new NarwhalScoreCommand.
+     * @param narwhalElevator The NarwhalElevator subsystem to use.
+     * @param narwhalWrist The NarwhalWrist subsystem to use.
+     * @param narwhalIntakeOuttake The NarwhalIntakeOuttake subsystem to use.
+     * @param readyToScoreSupplier A supplier that returns true when the robot is ready to score.
+     * @param targetScoringHeight The height to score the Coral at.
+     */
+    public NarwhalCoralScoreCommand(NarwhalElevator narwhalElevator, NarwhalWrist narwhalWrist, NarwhalIntakeOuttake narwhalIntakeOuttake, Supplier<Boolean> readyToScoreSupplier, ScoringHeight targetScoringHeight){
+        this.narwhalElevator = narwhalElevator;
+        this.narwhalWrist = narwhalWrist;
+        this.narwhalIntakeOuttake = narwhalIntakeOuttake;
+        this.readyToScoreSupplier = readyToScoreSupplier;
+        this.targetScoringHeight = targetScoringHeight;
+        addRequirements(narwhalElevator, narwhalWrist, narwhalIntakeOuttake);
+
+        outtakeDelayTimer = new Timer();
+    }
+
+    public ScoringHeight getTargetScoringHeight(){
+        return targetScoringHeight;
+    }
+
+    @Override
+    public void initialize(){
+        // Reset the outtake delay timer when the command starts
+        outtakeDelayTimer.stop();
+        outtakeDelayTimer.reset();
+    }
+
+    @Override
+    public void execute(){
+        // Move the elevator and wrist to the appropriate positions
+        switch (targetScoringHeight){
+            case L1:
+                narwhalElevator.goToScoreHeightL1();
+                narwhalWrist.goToL1WristAngle();
+                break;
+            case L2:
+                narwhalElevator.goToScoreHeightL2();
+                narwhalWrist.goToL2WristAngle();
+                break;
+            case L3:
+                narwhalElevator.goToScoreHeightL3();
+                narwhalWrist.goToL3WristAngle();
+                break;
+            case L4:
+                narwhalElevator.goToScoreHeightL4();
+                narwhalWrist.goToL4WristAngle();
+                break;
+        }
+
+        // If the robot is ready to score, score the Coral, otherwise do nothing with the intake/outtake
+        if(readyToScoreSupplier.get() && narwhalElevator.isAtTargetPosition() && narwhalWrist.isAtTargetPosition()){
+            narwhalIntakeOuttake.outtake();
+        }
+
+        // Start the outtake delay timer if the Coral sensor is no longer triggered and the outtake delay timer isn't already running
+        if(!narwhalIntakeOuttake.isCoralSensorTriggered()){
+            if (!outtakeDelayTimer.isRunning()){
+                outtakeDelayTimer.start();
+            }
+        }
+    }
+
+    @Override
+    public boolean isFinished(){
+        // Finish the command if the Coral sensor is no longer triggered (i.e. the Coral has been scored) and the outtake delay timer has elapsed (prevents premature ending)
+        return !narwhalIntakeOuttake.isCoralSensorTriggered() && outtakeDelayTimer.hasElapsed(NarwhalIntakeOuttakeConstants.CORAL_OUTTAKE_DETECTION_DELAY);
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        // Stop the intake/outtake motor when the command ends
+        narwhalIntakeOuttake.stop();
+
+        // Stop & reset the outtake delay timer when the command ends
+        outtakeDelayTimer.stop();
+        outtakeDelayTimer.reset();
+    }
+}

src/main/java/frc/robot/subsystems/narwhal/NarwhalClimber.java

@@ -60,7 +60,7 @@ public NarwhalClimber() {
                 NarwhalClimberConstants.PID_D,
                 NarwhalClimberConstants.PID_F)
             .outputRange(
-                -NarwhalClimberConstants.CLIMBER_PID_MIN_OUTPUT, 
+                NarwhalClimberConstants.CLIMBER_PID_MIN_OUTPUT, 
                 NarwhalClimberConstants.CLIMBER_PID_MAX_OUTPUT
             );
 
@@ -100,6 +100,22 @@ public void climb() {
         currentState = NarwhalClimberState.CLIMBING; // must be after the set function because the set function will default to CUSTOM state
     }
 
+    /**
+     * Check if the climber is at the Climbing angle
+     */
+    public boolean isAtClimbAngle(){
+        double current_position = climber.getEncoder().getPosition();
+        return Math.abs(current_position - NarwhalClimberConstants.CLIMB_ANGLE.getRotations()) < NarwhalClimberConstants.CLIMBER_ANGLE_TOLERANCE.getRotations();
+    }
+
+    /**
+     * Check if the climber is at the Deploy angle
+     */
+    public boolean isAtDeployAngle(){
+        double current_position = climber.getEncoder().getPosition();
+        return Math.abs(current_position - NarwhalClimberConstants.DEPLOYED_ANGLE.getRotations()) < NarwhalClimberConstants.CLIMBER_ANGLE_TOLERANCE.getRotations();
+    }
+
     @Override
     public void periodic() {
         // This method will be called once per scheduler run