Merge branch '27-handoff-command' of https://github.com/Patribots4738…

…/Crescendo2024 into 27-handoff-command

src/main/java/frc/robot/RobotContainer.java

@@ -47,7 +47,7 @@ public RobotContainer() {
         swerve = new Swerve();
         driverUI = new DriverUI();
         triggerWheel = new Indexer();
-
+      
         limelight.setDefaultCommand(Commands.run(() -> {
             // Create an "Optional" object that contains the estimated pose of the robot
             // This can be present (sees tag) or not present (does not see tag)

src/main/java/frc/robot/commands/handoff/Handoff.java → src/main/java/frc/robot/commands/handoff/PieceControl.java

@@ -7,31 +7,36 @@
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Swerve;
-import frc.robot.commands.ElevatorCommand;
+import frc.robot.subsystems.ElevatorSubs.Claw;
+import frc.robot.subsystems.ElevatorSubs.Elevator;
+import frc.robot.util.Constants.TrapConstants;
 import frc.robot.commands.ShooterCalc;
 
-public class Handoff {
+public class PieceControl {
     private final Command emptyCommand = null;
     private NotePosition notePosition = NotePosition.NONE;
 
     private Intake intake;
     private Indexer indexer;
-
-    private ElevatorCommand elevatorCommand;
+
+    private Elevator elevator;
+    private Claw claw;
+
     private ShooterCalc shooterCalc;
 
     private Swerve swerve;
 
-    public Handoff(
-            Intake intake, 
+    public PieceControl(
+            Intake intake,
             Indexer indexer,
-            ElevatorCommand elevatorCommand,
+            Elevator elevator,
+            Claw claw,
             ShooterCalc shooterCalc,
-            Swerve swerve) 
-        {
+            Swerve swerve) {
         this.intake = intake;
         this.indexer = indexer;
-        this.elevatorCommand = elevatorCommand;
+        this.elevator = elevator;
+        this.claw = claw;
         this.shooterCalc = shooterCalc;
         this.swerve = swerve;
     }
@@ -42,24 +47,23 @@ public Trigger readyToShoot() {
 
     public Command stopAllMotors() {
         return Commands.parallel(
-            intake.stop(),
-            indexer.stop(),
-            shooterCalc.stopMotors(),
-            elevatorCommand.stopMotors()
-        );
+                intake.stop(),
+                indexer.stop(),
+                shooterCalc.stopMotors(),
+                elevator.stop(),
+                claw.stop());
     }
 
     // TODO: We assume that the note is already in the claw when we want to shoot
-    public Command noteToShoot(){
+    public Command noteToShoot() {
         // this.notePosition = NotePosition.CLAW; ^^^
         Command shoot = emptyCommand;
         this.shooterCalc.prepareFireMovingCommand(() -> true, swerve);
-        if ( this.readyToShoot().getAsBoolean() ) {
+        if (this.readyToShoot().getAsBoolean()) {
             // run the indexer and intake to make sure the note gets to the shooter
-            shoot = 
-                indexer.toShooter()
+            shoot = indexer.toShooter()
                     .andThen(intake.inCommand())
-                    .andThen(elevatorCommand.outtake())
+                    .andThen(claw.outtake())
                     .andThen(() -> this.notePosition = NotePosition.SHOOTER)
                     .andThen(Commands.waitSeconds(1)) // TODO: Change this to a wait until the note is in the shooter?
                     .andThen(() -> this.notePosition = NotePosition.NONE)
@@ -73,12 +77,14 @@ public Command noteToShoot(){
     public Command noteToTrap() {
         Command shoot = emptyCommand;
 
-        if ( this.notePosition == NotePosition.CLAW ) {
-            // maybe make setPosition a command ORR Make the Elevator Command 
-            shoot = 
-                elevatorCommand.placeTrapCommand()
-                .andThen(new WaitCommand(1))
-                .andThen(stopAllMotors());
+        if (this.notePosition == NotePosition.CLAW) {
+            // maybe make setPosition a command ORR Make the Elevator Command
+            shoot = Commands.runOnce(
+                    () -> this.elevator.setPositionCommand(TrapConstants.TRAP_PLACE_POS)).andThen(
+                            Commands.waitUntil(elevator.isAtTargetPosition()))
+                    .andThen(claw.placeCommand())
+                    .andThen(new WaitCommand(1))
+                    .andThen(stopAllMotors());
 
             this.notePosition = NotePosition.CLAW;
 

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

@@ -3,26 +3,19 @@
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import frc.robot.util.Constants.IntakeConstants;
 import frc.robot.util.Neo;
 
 public class Indexer extends SubsystemBase {
     private final Neo triggerWheel;
-    private MotorRotation whichWay;
+    private double desiredSpeed;
 
     public Indexer() {
         triggerWheel = new Neo(IntakeConstants.TRIGGER_WHEEL_CAN_ID);
-        whichWay = MotorRotation.STOP;
+        desiredSpeed = 0;
         configMotor();
     }
 
-    enum MotorRotation {
-        CLOCKWISE,
-        COUNTER_CLOCKWISE,
-        STOP
-    }
-
     public void configMotor() {
         // See https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces      
         triggerWheel.setSmartCurrentLimit(IntakeConstants.TRIGGER_WHEEL_STALL_CURRENT_LIMIT_AMPS, IntakeConstants.TRIGGER_WHEEL_FREE_CURRENT_LIMIT_AMPS);
@@ -34,22 +27,28 @@ public void configMotor() {
         triggerWheel.setBrakeMode();
     }    
 
-    public MotorRotation getWhichWay() {
-        return whichWay;
+    public double getDesiredSpeed() {
+        return desiredSpeed;
+    }
+
+    public void setDesiredSpeed(double speed) {
+        desiredSpeed = speed;
+        triggerWheel.set(speed);
+    }
+
+    public Command setSpeedCommand(double speed) {
+        return runOnce(() -> setDesiredSpeed(speed));
     }
 
     public Command toShooter() {
-        whichWay = MotorRotation.CLOCKWISE;
-        return runOnce(() -> triggerWheel.set(IntakeConstants.SHOOTER_TRIGGER_WHEEL_SPEED));
+        return setSpeedCommand(IntakeConstants.SHOOTER_TRIGGER_WHEEL_SPEED);
     }
 
     public Command toTrap() {
-        whichWay = MotorRotation.COUNTER_CLOCKWISE;
-        return runOnce(() -> triggerWheel.set(IntakeConstants.TRAP_TRIGGER_WHEEL_SPEED));
+        return setSpeedCommand(IntakeConstants.TRAP_TRIGGER_WHEEL_SPEED);
     }
-        
+
     public Command stop() {
-        whichWay = MotorRotation.STOP;
-        return runOnce(() -> triggerWheel.set(IntakeConstants.STOP_SPEED));
+        return setSpeedCommand(IntakeConstants.STOP_SPEED);
     }
 }