Command groups

src/main/java/frc/robot/Robot.java

@@ -1,22 +1,22 @@
 package frc.robot;
 
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.*;
+import frc.robot.commands.*;
 import frc.robot.subsystems.CoralGripper;
-import frc.robot.commands.CoralArmCommand;
 import frc.robot.subsystems.CoralArm;
 import frc.robot.subsystems.AlgaeArm;
 import frc.robot.subsystems.AlgaeGripper;
 import frc.robot.subsystems.CoralElevator;
 
+import java.util.Set;
+
 public class Robot extends TimedRobot {
     private AlgaeArm algaeArm;
     private AlgaeGripper algaeGripper;
     private CoralElevator coralElevator;
     private CoralGripper coralGripper;
     private CoralArm coralArm;
-
     private CoralArmCommand coralArmCommand;
 
     @Override
@@ -29,6 +29,39 @@ public void robotInit() {
 
         coralArmCommand = new CoralArmCommand(coralArm);
         coralArm.setDefaultCommand(coralArmCommand);
+
+        Command checkIfAlgaeRetract = Commands.defer(()->{
+            if(algaeArm.isExtended()){
+                return new RetractAlgaeArm(algaeArm);
+            }
+            return Commands.idle(algaeArm);
+        }, Set.of(algaeArm));
+        algaeArm.setDefaultCommand(checkIfAlgaeRetract);
+
+        Command checkIfLow = Commands.defer(()-> {
+            if(coralElevator.isRaised()){
+                return new LowerCoralElevator(coralElevator);
+            }
+            return Commands.idle(coralElevator);
+        }, Set.of(coralElevator));
+        coralElevator.setDefaultCommand(checkIfLow);
+
+        Command checkCoral = Commands.defer(()-> {
+            if (coralGripper.hasCoral()) {
+                return new HoldCoral(coralGripper);
+            }
+            return Commands.idle(coralGripper);
+        }, Set.of(coralGripper));
+        coralGripper.setDefaultCommand(checkCoral);
+
+        Command checkAlgae = Commands.defer(()-> {
+            if (algaeGripper.hasAlgae()) {
+                return new HoldAlgae(algaeGripper);
+            }
+            return Commands.idle(algaeGripper);
+        }, Set.of(algaeGripper));
+        algaeGripper.setDefaultCommand(checkAlgae);
+
     }
 
     @Override
@@ -106,4 +139,46 @@ public void testPeriodic() {
     public void testExit() {
 
     }
+
+    private Command coralLevel2Place(){
+        return new SequentialCommandGroup(
+                Commands.runOnce(()-> coralArmCommand.setNewTargetPosition(RobotMap.ARM_CORAL_ANGLE_A)),
+                new ParallelCommandGroup(
+                        new LowerCoralElevator(coralElevator),
+                        Commands.waitUntil(()-> coralArmCommand.didReachTargetPosition())
+                ),
+                new CollectCoral(coralGripper));
+    }
+
+    private Command coralLevel3Place(){
+        return new SequentialCommandGroup(
+                Commands.runOnce(()-> coralArmCommand.setNewTargetPosition(RobotMap.ARM_CORAL_ANGLE_A)),
+                new ParallelCommandGroup(
+                        new RaiseCoralElevator(coralElevator),
+                        Commands.waitUntil(()-> coralArmCommand.didReachTargetPosition())
+                ),
+                new CollectCoral(coralGripper));
+    }
+
+    private Command coralCollect(){
+        return new SequentialCommandGroup(
+                Commands.runOnce(()-> coralArmCommand.setNewTargetPosition(RobotMap.ARM_CORAL_ANGLE_B)),
+                new ParallelCommandGroup(
+                        new LowerCoralElevator(coralElevator),
+                        Commands.waitUntil(()-> coralArmCommand.didReachTargetPosition())
+                ),
+                new ReleaseCoral(coralGripper));
+    }
+
+    private Command algaeCollect(){
+        return new SequentialCommandGroup(
+                new ExtendedAlgaeArm(algaeArm),
+                new CollectAlgae(algaeGripper));
+    }
+
+    private Command algaeOut(){
+        return new SequentialCommandGroup(
+                new RetractAlgaeArm(algaeArm),
+                new ReleaseAlgae(algaeGripper));
+    }
 }

src/main/java/frc/robot/RobotMap.java

@@ -1,7 +1,5 @@
 package frc.robot;
 
-import com.revrobotics.spark.SparkMax;
-
 public class RobotMap {
 
     private RobotMap() {}
@@ -22,6 +20,8 @@ private RobotMap() {}
     public static final int ARM_CORAL_END_PULSE_US = 1024;
     public static final double ARM_CORAL_ZERO_OFFSET = 0;
     public static final double ARM_CORAL_KF = 0;
+    public static final double ARM_CORAL_ANGLE_A = 0;
+    public static final double ARM_CORAL_ANGLE_B = 0;
 
     // coral elevator
     public static final int CORAL_ELEVATOR_UPPER_LIMIT_SWITCH = 1;

src/main/java/frc/robot/subsystems/CoralGripper.java

@@ -29,7 +29,7 @@ public class CoralGripper extends SubsystemBase {
     public static final double ROTATE_HOLD = 0.2;
 
     public CoralGripper() {
-        motor = new TalonFX(RobotMap.MOTOR_IDENTIFIER);
+        motor = new TalonFX(RobotMap.CORAL_GRIPPER_MOTOR);
         motor.getConfigurator().apply(new TalonFXConfiguration());
         limitSwitch = new DigitalInput(RobotMap.CORAL_GRIPPER_LIMIT_SWITCH);