Implement clawToShooter and rename triggerwheel

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

@@ -34,7 +34,7 @@ public class RobotContainer implements Logged {
 
     private final Limelight limelight;
     private final Climb climb;
-    private TriggerWheel triggerWheel;
+    private Indexer triggerWheel;
 
     public RobotContainer() {
         driver = new PatriBoxController(OIConstants.DRIVER_CONTROLLER_PORT, OIConstants.DRIVER_DEADBAND);
@@ -47,7 +47,7 @@ public RobotContainer() {
         climb = new Climb();
         swerve = new Swerve();
         driverUI = new DriverUI();
-        triggerWheel = new TriggerWheel();
+        triggerWheel = new Indexer();
 
         limelight.setDefaultCommand(Commands.run(() -> {
             // Create an "Optional" object that contains the estimated pose of the robot

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

@@ -4,74 +4,85 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.Intake;
-import frc.robot.subsystems.TriggerWheel;
+import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.ElevatorSubs.Claw;
 import frc.robot.subsystems.ElevatorSubs.Elevator;
 import frc.robot.subsystems.shooter.Pivot;
 import frc.robot.subsystems.shooter.Shooter;
 
 public class Handoff {
     private final Command emptyCommand = Commands.runOnce(() -> System.out.println(""));
-    private NotePosition currentNotePos;
+    private NotePosition notePosition = NotePosition.NONE;
 
     private Intake intake;
-    private TriggerWheel trigger;
+    private Indexer indexer;
 
+    // TODO: Use the commands for the subsystems instead of the subsystems themselves
     private Elevator elevator;
     private Claw claw;
 
+    //TODO: Use the commands for the subsystems instead of the subsystems themselves
     private Pivot pivot;
     private Shooter shooter;
 
 
     public Handoff(
             Intake intake, 
-            TriggerWheel trigger, 
+            Indexer indexer, 
             Elevator elevator, 
             Claw claw, 
             Pivot pivot, 
             Shooter shooter) 
         {
         this.intake = intake;
-        this.trigger = trigger;
+        this.indexer = indexer;
         this.elevator = elevator;
         this.claw = claw;
         this.pivot = pivot;
         this.shooter = shooter;
-
-        this.currentNotePos = trigger.hasPiece().getAsBoolean() 
-                                ? NotePosition.TRIGGER 
-                                : NotePosition.NONE;
-    }
-
-    /**
-     * @return
-     */
-    public NotePosition getCurrentNotePos(){
-        return this.currentNotePos;
     }
 
-    /**
-     * @param notePosition
-     */
-    public void setCurrentNotePos(NotePosition notePosition) {
-        this.currentNotePos = notePosition;
+    public Trigger readyToShoot() {
+        return new Trigger(() -> pivot.atDesiredAngle() && shooter.atDesiredRPM());
     }
 
-    public void triggerReady() {
-        trigger.setIsReady(intake.hasGamePieceTrigger());
-    }
+    // TODO: We assume that the note is already in the claw when we want to shoot
+    public Command clawToShooter(){
+        // this.notePosition = NotePosition.CLAW; ^^^
+        Command shoot = emptyCommand;
+        //TODO: impliment a way to tell the pivot and shooter to go to the correct angles etc... (before checking if ready to shoot)
+        if ( this.readyToShoot().getAsBoolean() ) {
+            // run the indexer and intake to make sure the note gets to the shooter
+            shoot = 
+                indexer.toShooter()
+                    .andThen(intake.inCommand()) // TODO: is this correct? because the top and bottom motors are seperate
+                    .andThen(claw.expel());
 
-    public Trigger readyToShoot() {
-        return new Trigger(trigger.hasPiece());
-    }
+            this.notePosition = NotePosition.SHOOTER;
+            //TODO: Then once we are done shooting, we want the variable to be set to NONE
+        }
 
-    public Command triggerToShooter() {
-        return Commands.either(trigger.toShooter(), emptyCommand, this.readyToShoot());
+        return shoot;
     }
 
-    public Command triggerToElevator() {
-        return Commands.either(trigger.toTrap(), emptyCommand, this.readyToShoot());
+    public Command clawToTrap() {
+        return emptyCommand;
     }
 
+    // public void triggerReady() {
+    //     trigger.setIsReady(intake.hasGamePieceTrigger());
+    // }
+
+    // public Trigger readyToShoot() {
+    //     return new Trigger(trigger.hasPiece());
+    // }
+
+    // public Command triggerToShooter() {
+    //     return Commands.either(trigger.toShooter(), emptyCommand, this.readyToShoot());
+    // }
+
+    // public Command triggerToElevator() {
+    //     return Commands.either(trigger.toTrap(), emptyCommand, this.readyToShoot());
+    // }
+
 }

src/main/java/frc/robot/commands/handoff/NotePosition.java

@@ -2,8 +2,8 @@
 
 public enum NotePosition {
     SHOOTER,
-    ELEVATOR,
+    CLAW,
     INTAKE,
-    TRIGGER,
+    INDEXER,
     NONE    
 }

src/main/java/frc/robot/subsystems/ElevatorSubs/Claw.java

@@ -55,8 +55,8 @@ public boolean getHasGamePiece() {
         return this.hasGamePiece;
     }
 
-    public void expel() {
-        claw.setTargetVelocity(1);
+    public Command expel() {
+        return Commands.runOnce(() -> claw.setTargetVelocity(1));
     }
 
     public void stop() {

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

@@ -10,12 +10,12 @@
 import frc.robot.util.Neo;
 import java.util.Optional;
 
-public class TriggerWheel extends SubsystemBase {
+public class Indexer extends SubsystemBase {
     private final Neo triggerWheel;
     private Optional<Boolean> whichWay;
     private BooleanSupplier hasPiece;
 
-    public TriggerWheel() {
+    public Indexer() {
         triggerWheel = new Neo(IntakeConstants.TRIGGER_WHEEL_CAN_ID);
         whichWay = Optional.empty();
         hasPiece = () -> false; 

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

@@ -11,6 +11,7 @@ public class Intake extends SubsystemBase {
     private final Neo intake;
 
     public Intake() {
+        // TODO: is this correct? because the top and bottom motors are seperate
         intake = new Neo(IntakeConstants.INTAKE_CAN_ID);
         configMotor();
     }
@@ -26,10 +27,12 @@ public void configMotor() {
         intake.setBrakeMode();
     }
 
+    // TODO: is this correct? because the top and bottom motors are seperate
     public Command inCommand() {
         return runOnce(() -> intake.set(IntakeConstants.INTAKE_SPEED));
     }
 
+    // TODO: is this correct? because the top and bottom motors are seperate
     public Command outCommand() {
         return runOnce(() -> intake.set(IntakeConstants.OUTTAKE_SPEED));
     }

src/main/java/frc/robot/subsystems/shooter/Pivot.java

@@ -39,4 +39,12 @@ public void setAngle(double angle) {
         // and we want to set the position between 1.0 and -1.0
         pivot.setTargetPosition(angle / ShooterConstants.PIVOT_MAX_ANGLE);
     }
+
+
+    public boolean atDesiredAngle() {
+        // TODO Auto-generated method stub
+        throw new UnsupportedOperationException("Unimplemented method 'atDesiredAngle'");
+    }
+
+    //TODO: Implement a way to get the angle desired angle of the pivot
 }

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -47,4 +47,10 @@ public void setSpeed(double speed) {
     public void stop() {
         motorLeft.setTargetVelocity(0);
     }
+
+    //TODO: Implement a way to get the RPM of the shooter
+    public boolean atDesiredRPM() {
+        // TODO Auto-generated method stub
+        throw new UnsupportedOperationException("Unimplemented method 'atDesiredRPM'");
+    }
 }