Feature/led

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

@@ -296,5 +296,7 @@ public class CANConstants {
 
   }
 
+  public static final int blinkinPort = 0;
+
 
 }

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

@@ -23,6 +23,12 @@
 import monologue.Annotations.Log;
 import monologue.Logged;
 import edu.wpi.first.wpilibj.smartdashboard.*;
+
+import frc.utils.BlinkinState;
+import frc.robot.subsystems.LightControl;
+import frc.robot.Constants;
+import frc.robot.commands.lights.FlashLEDCommand;
+
 import edu.wpi.first.util.datalog.StringLogEntry;
 
 /**f
@@ -36,6 +42,8 @@ public class Robot extends TimedRobot implements Logged {
 
   private RobotContainer m_robotContainer;
 
+  private boolean runLEDBeamBreak = true;
+
   @Log.NT
   @Log.File
   private CommandScheduler m_Scheduler;
@@ -123,19 +131,33 @@ public void robotPeriodic() {
 
     Monologue.setFileOnly(DriverStation.isFMSAttached());
     Monologue.updateAll();
+
+    if(runLEDBeamBreak && m_robotContainer.getIndexerBeamBreak()){
+      m_robotContainer.getBeamBreakLEDCommand().schedule();
+      runLEDBeamBreak = false;
+    }
+    else if(!runLEDBeamBreak && !m_robotContainer.getIndexerBeamBreak()){
+      runLEDBeamBreak = true;
+    }
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+  }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+  }
 
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
+
+    m_robotContainer.getLEDInitCommand().schedule();
+
     getAllianceInfo();
+
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     /*
@@ -153,11 +175,16 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+  }
 
   @Override
   public void teleopInit() {
+
+     m_robotContainer.getLEDInitCommand().schedule();
+
     getAllianceInfo();
+
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
@@ -169,7 +196,9 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+
+  }
 
   @Override
   public void testInit() {

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

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.Constants.ClimberConstants;
@@ -21,6 +22,8 @@
 import frc.robot.commands.intake.OuttakePiece;
 import frc.robot.commands.intake.RunIntake;
 import frc.robot.commands.intake.StowIntake;
+import frc.robot.commands.lights.ContinueFlashLEDCommand;
+import frc.robot.commands.lights.FlashStopLEDCommand;
 import frc.robot.input.AxisButton;
 import frc.robot.commands.Shooter.RunShooterPower;
 import frc.robot.commands.Shooter.RunShooterRPM;
@@ -38,10 +41,12 @@
 import frc.robot.commands.Shooter.PrepareShooter;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LightControl;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.UmbrellaSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.PhotonCameraWrapper;
+import frc.utils.BlinkinState;
 import monologue.Logged;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -57,7 +62,9 @@
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
+
 import com.fasterxml.jackson.core.sym.Name;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
@@ -69,6 +76,7 @@
 
 
 
+
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -86,11 +94,16 @@ public class RobotContainer implements Logged {
   //private final UmbrellaSubsystem m_umbrella  = new UmbrellaSubsystem();
   private final ShooterSubsystem m_shooter = new ShooterSubsystem();
 
+
   private final Climber m_Climber = new Climber();
 
 
+
   private final RobotState m_robotState = RobotState.getInstance();
 
+
+  private final LightControl m_LightControl = new LightControl(Constants.blinkinPort);
+
   //Robot preferences
   private DoublePreference intakePower = new DoublePreference("intake/intakePower", 0.5);
   private DoublePreference intakePosition = new DoublePreference("intake/intakePosition", 100);
@@ -160,17 +173,20 @@ public class RobotContainer implements Logged {
     private final Command resetGyro = new ResetGyro(m_robotDrive);
     private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition);
     private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition);
-    private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition);
+
+    private final Command intakePiece = new IntakePiece(m_robotIntake, m_LightControl, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition);
     private final Command outTakePiece = new OuttakePiece(m_robotIntake, m_shooter, outtakePower, intakePosition, outdexPower, intakeShooterPosition, outtakeFlywheelPower);
     private final Command stowIntake = new StowIntake(m_robotIntake);
     private final Command parkCommand = new ParkCommand(m_robotDrive);
     private final Command stowShooter = new StowShooter(m_shooter, shooterHome);
     private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition);
     private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower);
     private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower);
-    private final Command shootSubwoofer = new ShootPiece(m_shooter, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
-    private final Command shootPodium = new ShootPiece(m_shooter, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
-    private final Command shootWing = new ShootPiece(m_shooter, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+
+    private final Command shootSubwoofer = new ShootPiece(m_shooter, m_LightControl, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    private final Command shootPodium = new ShootPiece(m_shooter, m_LightControl, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    private final Command shootWing = new ShootPiece(m_shooter, m_LightControl, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+
     private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM);
     private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM);
     private final Command ejectPiece = new EjectPiece(m_shooter);
@@ -182,8 +198,9 @@ public class RobotContainer implements Logged {
 
     private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15);
 
-    private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    private final Command shooterTune = new ShootPiece(m_shooter, m_LightControl, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command shootInterpolated = new ShootInterpolated(m_shooter, m_photonCameraWrapper, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+
     private final Command driveToTarget = new DriveToTarget(m_robotDrive, 
             m_photonCameraWrapper, 
             m_robotState::getSpeakerID,
@@ -197,7 +214,7 @@ public class RobotContainer implements Logged {
   private final Command autoAmpRingShot = new AutonShoot(m_shooter, autoAmpRingShotAngle, autoAmpRingShotRPM, shooterIndexPower).withTimeout(2);
   private final Command ampShotStart = new AutonShoot(m_shooter, ampRingShotOnlyAngle, ampRingShotOnlyRPM, shooterIndexPower).withTimeout(2);
   private final Command ringToss = new RingToss(m_robotIntake, m_shooter, intakePower, intakePosition, shooterIndexPower, intakeShooterPosition, () -> 1000.0);
-  private final Command autoIntake = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75);
+  private final Command autoIntake = new IntakePiece(m_robotIntake,m_LightControl, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75);
 
 
   //Autonomous Wait Command
@@ -207,12 +224,21 @@ public class RobotContainer implements Logged {
 
   private final SendableChooser<Command> autonChooser;
 
+  //light commands
+  Command defaultLEDCommand = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, BlinkinState.Solid_Colors_Red, 500);
+  Command initLEDCommand = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, 750, 4);
+  Command piecePickedUP = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, 750, 4);
+
+
   private final double pathSpeed = 2;
 
 
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
   XboxController m_manipController = new XboxController(OIConstants.kManipControllerPort);  
+
+  private boolean autoInit = true;
+  private boolean teleopInit = true;
 
 private static class Operator {
 
@@ -334,8 +360,6 @@ private void configureButtonBindings() {
     // new JoystickButton(m_driverController, XboxController.Button.kB.value)
     //     .whileTrue(m_robotDrive.turnSysIdTestBuilder(10, 5));
 
-
-
   }
 
   private void configureDefaultCommands(){
@@ -353,7 +377,7 @@ private void configureDefaultCommands(){
 
     m_robotIntake.setDefaultCommand(stowIntake);
     m_shooter.setDefaultCommand(stowShooter);
-
+    m_LightControl.setDefaultCommand(defaultLEDCommand);
   }
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
@@ -363,7 +387,18 @@ private void configureDefaultCommands(){
   public Command getAutonomousCommand() {
     return autonChooser.getSelected();
   }
-
+
+  public Command getLEDInitCommand(){
+    return initLEDCommand;
+  }
+
+  public boolean getIndexerBeamBreak(){
+    return m_shooter.getIndexerBeamBreak();
+  }
+
+  public Command getBeamBreakLEDCommand(){
+    return piecePickedUP;
+  }
 
 }
 

src/main/java/frc/robot/RobotState.java

@@ -13,9 +13,12 @@ public class RobotState {
     private Pose2d m_robotPose2d;
     private Pose2d m_speakerPose2d;
     private int m_speakerID = 0;
+
+
     private boolean m_hasNote = false;
     private double m_distanceToSpeaker;
 
+
     private static RobotState single_instance = null;
 
     private RobotState() {
@@ -63,8 +66,10 @@ public void setDistanceToSpeaker(double d) {
         m_distanceToSpeaker = d;
     }
 
+
     public boolean hasNote() {
         return m_hasNote;
     }
 
+
 }

src/main/java/frc/robot/commands/Shooter/ShootPiece.java

@@ -7,8 +7,11 @@
 import edu.wpi.first.wpilibj.shuffleboard.SuppliedValueWidget;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.LightControl;
+import frc.utils.BlinkinState;
 
 import java.util.function.Supplier;
+import frc.utils.Timer;
 
 
 public class ShootPiece extends Command {
@@ -17,26 +20,41 @@ public class ShootPiece extends Command {
   private final Supplier<Double> m_rpm;
   private final Supplier<Double> m_indexPower;
   private final Supplier<Boolean> m_ready; 
+  private final LightControl m_LightControl;
+
+  private int loops;
+  private Timer timer;
   /** Creates a new ShootPiece. */
-  public ShootPiece(ShooterSubsystem shooter, Supplier<Double> position, Supplier<Double> rpm, Supplier<Double> indexpower, Supplier<Boolean> ready) {
+
+  public ShootPiece(ShooterSubsystem shooter, LightControl lightControl, Supplier<Double> position, Supplier<Double> rpm, Supplier<Double> indexpower, Supplier<Boolean> ready) {
+
     m_shooter = shooter;
+    m_LightControl = lightControl;
     m_position = position;
     m_rpm = rpm;
     m_indexPower = indexpower;
     m_ready = ready;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(m_shooter);
+    addRequirements(m_shooter, m_LightControl);
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    timer.resetTimer();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_shooter.setAngleDegrees(m_position.get());
     m_shooter.spinRPM(m_rpm.get());
+    m_shooter.setAngleDegrees(m_position.get());
+
+    if(m_shooter.armAtSetpoint() && m_shooter.atRPM())
+    {
+      m_LightControl.ShootReady(timer);
+    }
+    else timer.resetTimer();
     //if (m_ready.get() && m_shooter.atSetpoint()){
     if (m_ready.get()){
       m_shooter.runIndex(m_indexPower.get());
@@ -47,6 +65,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_shooter.stopAll();
+    m_LightControl.turnOff();
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/intake/IntakePiece.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.LightControl;
 
 import java.util.function.Supplier;
 
@@ -19,17 +20,19 @@ public class IntakePiece extends Command {
   private final Supplier<Double> m_intakePosition;
   private final Supplier<Double> m_indexPower;
   private final Supplier<Double> m_indexPosition;
+  private final LightControl m_LightControl;
 
   private final Timer timer = new Timer();
   private final double delaySeconds = 0.02;
 
 
 
   /** Creates a new IntakePiece. */
-  public IntakePiece(IntakeSubsystem intake, ShooterSubsystem shooter, Supplier<Double> intakePower, Supplier<Double> intakePosition, Supplier<Double> indexPower, Supplier<Double> indexPosition ) {
+  public IntakePiece(IntakeSubsystem intake, LightControl lightControl, ShooterSubsystem shooter, Supplier<Double> intakePower, Supplier<Double> intakePosition, Supplier<Double> indexPower, Supplier<Double> indexPosition ) {
 
     m_intake = intake; 
     m_shooter = shooter;
+    m_LightControl = lightControl;
     m_intakePower = intakePower;
     m_intakePosition = intakePosition;
     m_indexPower = indexPower;