leaf blower and intake pull back

src/main/java/frc/robot/BuildConstants.java

@@ -3,14 +3,14 @@
 /** Automatically generated file containing build version information. */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "2024RobotCode";
+  public static final String MAVEN_NAME = "2024 Robot Code";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 230;
-  public static final String GIT_SHA = "07a5e92dce913b6541180162f8cf3cb04c5c398f";
-  public static final String GIT_DATE = "2024-04-06 00:00:08 EDT";
-  public static final String GIT_BRANCH = "msc";
-  public static final String BUILD_DATE = "2024-04-06 11:05:30 EDT";
-  public static final long BUILD_UNIX_TIME = 1712415930608L;
+  public static final int GIT_REVISION = 231;
+  public static final String GIT_SHA = "5b58b584f2d149355cf085b0220bf1286721ac91";
+  public static final String GIT_DATE = "2024-04-07 20:06:05 EDT";
+  public static final String GIT_BRANCH = "Intake-fix";
+  public static final String BUILD_DATE = "2024-04-10 20:39:23 EDT";
+  public static final long BUILD_UNIX_TIME = 1712795963566L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

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

@@ -93,6 +93,13 @@ public static final class ShooterConstants {
     public static final double FLYWHEEL_SHOOT_RPM = 3000;
   }
 
+  public static enum NoteState {
+    Init,
+    NO_NOTE,
+    SENSOR,
+    CURRENT
+  }
+
   public static class ElevatorConstants {
     public static final double CURRENT_LIMIT = 40.0;
     public static final boolean CURRENT_LIMIT_ENABLED = true;

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

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
@@ -30,6 +31,7 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.LED_STATE;
+import frc.robot.Constants.NoteState;
 import frc.robot.commands.AimbotAuto;
 import frc.robot.commands.AimbotTele;
 import frc.robot.commands.AlignToNoteAuto;
@@ -81,6 +83,8 @@
 import frc.robot.subsystems.shooter.FeederIOTalonFX;
 import frc.robot.subsystems.shooter.FlywheelIOSim;
 import frc.robot.subsystems.shooter.FlywheelIOTalonFX;
+import frc.robot.subsystems.shooter.LeafBlowerIO;
+import frc.robot.subsystems.shooter.LeafBlowerIOTalonSRX;
 import frc.robot.subsystems.shooter.Shooter;
 import java.util.Map;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -142,7 +146,8 @@ public RobotContainer() {
                 new FlywheelIOTalonFX(
                     RobotMap.ShooterIDs.FLYWHEEL_LEFT, RobotMap.ShooterIDs.FLYWHEEL_RIGHT),
                 new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER),
-                new DistanceSensorIOAnalog());
+                new DistanceSensorIOAnalog(),
+                new LeafBlowerIOTalonSRX(18));
         elevator =
             new Elevator(
                 new ElevatorIOTalonFX(RobotMap.ElevatorIDs.LEFT, RobotMap.ElevatorIDs.RIGHT));
@@ -161,7 +166,12 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         intake = new Intake(new IntakeRollerIOSim());
-        shooter = new Shooter(new FlywheelIOSim(), new FeederIOSim(), new DistanceSensorIO() {});
+        shooter =
+            new Shooter(
+                new FlywheelIOSim(),
+                new FeederIOSim(),
+                new DistanceSensorIO() {},
+                new LeafBlowerIO() {});
         elevator = new Elevator(new ElevatorIOSim());
         pivot = new Pivot(new PivotIOSim());
         led = new LED(new LED_IOCANdle(20, Constants.CANBUS));
@@ -175,7 +185,12 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         intake = new Intake(new IntakeRollerIOSim());
-        shooter = new Shooter(new FlywheelIOSim(), new FeederIOSim(), new DistanceSensorIO() {});
+        shooter =
+            new Shooter(
+                new FlywheelIOSim(),
+                new FeederIOSim(),
+                new DistanceSensorIO() {},
+                new LeafBlowerIO() {});
         elevator = new Elevator(new ElevatorIOSim());
         pivot = new Pivot(new PivotIOSim());
         led = new LED(new LED_IO() {});
@@ -196,7 +211,8 @@ public RobotContainer() {
                 new FlywheelIOTalonFX(
                     RobotMap.ShooterIDs.FLYWHEEL_LEFT, RobotMap.ShooterIDs.FLYWHEEL_RIGHT),
                 new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER),
-                new DistanceSensorIO() {});
+                new DistanceSensorIO() {},
+                new LeafBlowerIO() {});
         elevator = new Elevator(new ElevatorIO() {});
         pivot = new Pivot(new PivotIO() {});
         led = new LED(new LED_IO() {});
@@ -351,7 +367,7 @@ CLIMB_STATES.SHOOT_NOTE, new InstantCommand(() -> shooter.setFeedersRPM(500))),
     NamedCommands.registerCommand(
         "AlignToNote",
         new AlignToNoteAuto(led, drive, shooter, intake, pivot)
-            .until(() -> shooter.seesNote())
+            .until(() -> shooter.seesNote() == NoteState.SENSOR)
             // TODO:: adjust this delay
             .andThen(new InstantCommand(drive::stop))
             .andThen(new InstantCommand(() -> shooter.setFeedersRPM(500)))
@@ -410,10 +426,10 @@ CLIMB_STATES.SHOOT_NOTE, new InstantCommand(() -> shooter.setFeedersRPM(500))),
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    driverControls();
-    manipControls();
+    // driverControls();
+    // manipControls();
 
-    // testControls();
+    testControls();
   }
 
   private void testControls() {
@@ -472,6 +488,8 @@ private void testControls() {
     // driveController.y().onTrue(new AimbotTele(drive, driveController, shooter, pivot, led));
     driveController.y().onTrue(new SetPivotTarget(39, pivot));
 
+    driveController.x().onTrue(new InstantCommand(() -> shooter.turnOnFan(), shooter));
+    driveController.x().onFalse(new InstantCommand(() -> shooter.turnOffFan(), shooter));
     // driveController.a().onTrue(new TurnToSpeaker(drive, driveController));
     // driveController.a().onTrue(new SetPivotTarget(40, pivot));
 
@@ -521,7 +539,9 @@ private void testControls() {
 
     driveController
         .rightTrigger()
-        .onTrue(new InstantCommand(() -> shooter.setFlywheelRPMs(4000, 4000)));
+        .onTrue(
+            new InstantCommand(
+                () -> shooter.setFlywheelRPMs(flywheelSpeed.get(), flywheelSpeed.get())));
     driveController.rightTrigger().onFalse(new InstantCommand(() -> shooter.stopFlywheels()));
 
     driveController.leftTrigger().onTrue(new InstantCommand(() -> shooter.setFeedersRPM(1000)));
@@ -599,15 +619,26 @@ private void driverControls() {
             new SequentialCommandGroup(
                 new SetElevatorTarget(0, 1.5, elevator),
                 new AlignToNoteTele(intake, pivot, shooter, drive, led)));
+
     driveController
         .rightBumper()
         .onFalse(
-            new InstantCommand(() -> led.setState(LED_STATE.BLUE))
+            new ConditionalCommand(
+                    new WaitCommand(0.25),
+                    new InstantCommand(),
+                    () -> (shooter.seesNote() == NoteState.CURRENT))
                 .andThen(
-                    new InstantCommand(() -> shooter.setFeedersRPM(500))
-                        .andThen(new WaitCommand(0.15))
-                        .andThen(new InstantCommand(shooter::stopFeeders)))
-                .andThen(new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot)));
+                    (new ConditionalCommand(
+                            new WaitCommand(0.10),
+                            new InstantCommand(),
+                            () -> (shooter.seesNote() == NoteState.SENSOR)))
+                        .andThen(
+                            new ParallelCommandGroup(
+                                new InstantCommand(() -> intake.stopRollers(), intake),
+                                new PositionNoteInFeeder(shooter, intake),
+                                new SetPivotTarget(
+                                    Constants.PivotConstants.STOW_SETPOINT_DEG, pivot)))
+                        .andThen(new InstantCommand(() -> shooter.stopFeeders()))));
   }
 
   private void manipControls() {

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

@@ -20,10 +20,10 @@ public static class ElevatorIDs {
 
   public static class PivotIDs {
     public static final int GYRO = 10;
-    public static final int LEFT = 14;
-    public static final int RIGHT = 15;
-    // public static final int LEFT = 10000;
-    // public static final int RIGHT = 99999;
+    // public static final int LEFT = 14;
+    // public static final int RIGHT = 15;
+    public static final int LEFT = 10000;
+    public static final int RIGHT = 99999;
   }
 
   public static class LEDIDs {

src/main/java/frc/robot/commands/AlignToNoteAuto.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.Constants.LED_STATE;
+import frc.robot.Constants.NoteState;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.led.LED;
@@ -56,7 +57,7 @@ public void initialize() {
   @Override
   public void execute() {
 
-    finished = shooter.seesNote();
+    finished = shooter.seesNote() == NoteState.SENSOR;
     pathCommand.execute();
     Logger.recordOutput("path is finished", finished);
   }
@@ -74,6 +75,6 @@ public void end(boolean interrupted) {
   @Override
   public boolean isFinished() {
     Logger.recordOutput("isFinished align note", shooter.seesNote());
-    return shooter.seesNote() || finished;
+    return shooter.seesNote() == NoteState.SENSOR || finished;
   }
 }

src/main/java/frc/robot/commands/AlignToNoteTele.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.Constants.LED_STATE;
+import frc.robot.Constants.NoteState;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.led.LED;
@@ -61,7 +62,7 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (shooter.seesNote()) led.setState(LED_STATE.GREEN);
-    return shooter.seesNote();
+    if (shooter.seesNote() == NoteState.SENSOR) led.setState(LED_STATE.GREEN);
+    return shooter.seesNote() == NoteState.SENSOR;
   }
 }

src/main/java/frc/robot/commands/InFeederLEDCheck.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.LED_STATE;
+import frc.robot.Constants.NoteState;
 import frc.robot.subsystems.led.LED;
 import frc.robot.subsystems.shooter.Shooter;
 
@@ -24,7 +25,7 @@ public InFeederLEDCheck(Shooter shooter, LED led) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    if (shooter.seesNote()) {
+    if (shooter.seesNote() == NoteState.SENSOR) {
       led.setState(LED_STATE.GREEN);
     } else led.setState(LED_STATE.BLUE);
   }

src/main/java/frc/robot/commands/IntakeNote.java

@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.NoteState;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.led.LED;
 import frc.robot.subsystems.shooter.Shooter;
@@ -47,6 +48,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return shooter.seesNote();
+    return shooter.seesNote() == NoteState.SENSOR;
   }
 }

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

@@ -0,0 +1,17 @@
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface LeafBlowerIO {
+  @AutoLog
+  static class LeafBlowerIOInputs {
+    public double appliedVolts = 0.0;
+    public double currentAmps = 0.0;
+  }
+
+  public default void updateInputs(LeafBlowerIOInputs inputs) {}
+
+  public default void setSpeed(double percentSpeed) {}
+
+  public default void stop() {}
+}

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

@@ -0,0 +1,33 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+
+public class LeafBlowerIOTalonSRX implements LeafBlowerIO {
+
+  private final TalonSRX leafBlower;
+
+  //    private final double currentAmps;
+  //    private final double appliedVolts;
+
+  public LeafBlowerIOTalonSRX(int id) {
+
+    leafBlower = new TalonSRX(id);
+  }
+
+  @Override
+  public void updateInputs(LeafBlowerIOInputs inputs) {
+    inputs.appliedVolts = leafBlower.getMotorOutputVoltage();
+    inputs.currentAmps = leafBlower.getStatorCurrent();
+  }
+
+  @Override
+  public void setSpeed(double percentSpeed) {
+    leafBlower.set(ControlMode.PercentOutput, percentSpeed);
+  }
+
+  @Override
+  public void stop() {
+    leafBlower.set(ControlMode.PercentOutput, 0);
+  }
+}