Merge pull request #16 from grt192/leds

Leds

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

@@ -162,7 +162,7 @@ public static class AutoAlignConstants {
   public static class LEDConstants {
     public static final int LED_LENGTH = 140;
     public static final int LED_PWM_PORT = 0;
-    public static final double BRIGHTNESS_SCALE_FACTOR = .5;
+    public static final double BRIGHTNESS_SCALE_FACTOR = .6;
   }
 
   public static final class VisionConstants {

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

@@ -9,9 +9,11 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.intake.IntakePivotSubsystem;
 import frc.robot.subsystems.intake.IntakeRollersSubsystem;
+import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.shooter.ShooterFeederSubsystem;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
 import frc.robot.subsystems.shooter.ShooterPivotSubsystem;
+import frc.robot.subsystems.superstructure.NotePosition;
 
 public class IdleCommand extends ParallelCommandGroup{
     public IdleCommand(IntakePivotSubsystem intakePivotSubsystem,
@@ -20,7 +22,8 @@ public IdleCommand(IntakePivotSubsystem intakePivotSubsystem,
                        ShooterPivotSubsystem shooterPivotSubsystem,
                        ShooterFeederSubsystem shooterFeederSubsystem,
                        ShooterFlywheelSubsystem shooterFlywheelSubsystem,
-                       ClimbSubsystem climbSubsystem){
+                       ClimbSubsystem climbSubsystem,
+                       LEDSubsystem ledSubsystem){
         // addRequirements(intakePivotSubsystem, intakeRollersSubsystem,
         //                 elevatorSubsystem,
         //                 shooterPivotSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem,
@@ -30,7 +33,16 @@ public IdleCommand(IntakePivotSubsystem intakePivotSubsystem,
         addCommands(new InstantCommand(() -> intakeRollersSubsystem.setAllRollSpeed(0, 0), intakeRollersSubsystem),
                     new ElevatorToGroundCommand(elevatorSubsystem),
                     new InstantCommand(() -> shooterFeederSubsystem.setFeederMotorSpeed(0), shooterFeederSubsystem),
-                    new ShooterFlywheelStopCommand(shooterFlywheelSubsystem)//,
+                    new ShooterFlywheelStopCommand(shooterFlywheelSubsystem),
+                    new InstantCommand(() -> {
+                        if(intakeRollersSubsystem.sensorNow()){
+                            ledSubsystem.setNoteMode(NotePosition.INTAKE_HOLDING);
+                        } else if(shooterFeederSubsystem.getRed() > shooterFeederSubsystem.TOLERANCE){
+                            ledSubsystem.setNoteMode(NotePosition.SHOOTER_HOLDING);
+                        } else {
+                            ledSubsystem.setNoteMode(NotePosition.NONE);
+                        }
+                    })//,
                     // new ClimbLowerCommand(climbSubsystem) // NOT USING CLIMB FOR NOW
         );
     }

src/main/java/frc/robot/commands/auton/BaseAutonSequence.java

@@ -15,6 +15,7 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.intake.IntakePivotSubsystem;
 import frc.robot.subsystems.intake.IntakeRollersSubsystem;
+import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.shooter.ShooterFeederSubsystem;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
 import frc.robot.subsystems.shooter.ShooterPivotSubsystem;
@@ -33,21 +34,27 @@ public class BaseAutonSequence extends SequentialCommandGroup{
     private final ShooterFlywheelSubsystem shooterFlywheelSubsystem;
     private final ShooterPivotSubsystem shooterPivotSubsystem;
     private final ElevatorSubsystem elevatorSubsystem;
+    private final LEDSubsystem ledSubsystem;
     private final SwerveSubsystem swerveSubsystem;
     private final PIDController thetaController;
     private PIDController xPID;
     private PIDController yPID;
     private boolean isRed;
     private int driveforwardtime;
 
-    public BaseAutonSequence(IntakePivotSubsystem intakePivotSubsystem, IntakeRollersSubsystem intakeRollersSubsystem, ShooterFeederSubsystem shooterFeederSubsystem, ShooterFlywheelSubsystem shooterFlywheelSubsystem, ShooterPivotSubsystem shooterPivotSubsystem, ElevatorSubsystem elevatorSubsystem, SwerveSubsystem swerveSubsystem){
+    public BaseAutonSequence(IntakePivotSubsystem intakePivotSubsystem, IntakeRollersSubsystem intakeRollersSubsystem, 
+                             ShooterFeederSubsystem shooterFeederSubsystem, ShooterFlywheelSubsystem shooterFlywheelSubsystem, ShooterPivotSubsystem shooterPivotSubsystem, 
+                             ElevatorSubsystem elevatorSubsystem, 
+                             SwerveSubsystem swerveSubsystem,
+                             LEDSubsystem ledSubsystem){
         this.intakePivotSubsystem = intakePivotSubsystem; 
         this.intakeRollersSubsystem = intakeRollersSubsystem;
         this.shooterFeederSubsystem = shooterFeederSubsystem;
         this.shooterFlywheelSubsystem = shooterFlywheelSubsystem;
         this.shooterPivotSubsystem = shooterPivotSubsystem;
         this.elevatorSubsystem = elevatorSubsystem;
         this.swerveSubsystem = swerveSubsystem;
+        this.ledSubsystem = ledSubsystem;
 
         addRequirements(swerveSubsystem, intakeRollersSubsystem,intakePivotSubsystem);
 
@@ -82,15 +89,15 @@ public Command followPath(ChoreoTrajectory traj){
     public SequentialCommandGroup goIntake(ChoreoTrajectory intaketraj){
         return followPath(intaketraj)
         .andThen(new IntakePivotExtendedCommand(intakePivotSubsystem))
-        .andThen(new IntakeRollerIntakeCommand(intakeRollersSubsystem).raceWith(new DriveForwardCommand(swerveSubsystem).withTimeout(driveforwardtime)))
-        .andThen(new IntakeRollerIntakeCommand(intakeRollersSubsystem))//just in case the note isn't fully intaken above
+        .andThen(new IntakeRollerIntakeCommand(intakeRollersSubsystem, ledSubsystem).raceWith(new DriveForwardCommand(swerveSubsystem).withTimeout(driveforwardtime)))
+        .andThen(new IntakeRollerIntakeCommand(intakeRollersSubsystem, ledSubsystem))//just in case the note isn't fully intaken above
         .andThen(new IntakeRollerFeedCommand(intakeRollersSubsystem))
         .andThen(new IntakePivotVerticalCommand(intakePivotSubsystem));
     }
 
     public SequentialCommandGroup goShoot(ChoreoTrajectory shoottraj){
         return followPath(shoottraj)
-        .andThen(new ShootModeSequence(intakeRollersSubsystem, elevatorSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, shooterPivotSubsystem))
+        .andThen(new ShootModeSequence(intakeRollersSubsystem, elevatorSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, shooterPivotSubsystem, ledSubsystem))
         .andThen(new ShooterFeedShootCommand(shooterFeederSubsystem))
         .andThen(new ShooterFlywheelStopCommand(shooterFlywheelSubsystem));
     }

src/main/java/frc/robot/commands/auton/SimpleAutonSequence.java

@@ -5,6 +5,7 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.intake.IntakePivotSubsystem;
 import frc.robot.subsystems.intake.IntakeRollersSubsystem;
+import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.shooter.ShooterFeederSubsystem;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
 import frc.robot.subsystems.shooter.ShooterPivotSubsystem;
@@ -16,8 +17,10 @@ public class SimpleAutonSequence extends BaseAutonSequence{
     private ChoreoTrajectory intaketraj;
     private ChoreoTrajectory speakertraj;
 
-    public SimpleAutonSequence(IntakePivotSubsystem intakePivotSubsystem, IntakeRollersSubsystem intakeRollersSubsystem, ShooterFeederSubsystem shooterFeederSubsystem, ShooterFlywheelSubsystem shooterFlywheelSubsystem, ShooterPivotSubsystem shooterPivotSubsystem, ElevatorSubsystem elevatorSubsystem, SwerveSubsystem swerveSubsystem) {
-        super(intakePivotSubsystem, intakeRollersSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, shooterPivotSubsystem, elevatorSubsystem, swerveSubsystem);
+    public SimpleAutonSequence(IntakePivotSubsystem intakePivotSubsystem, IntakeRollersSubsystem intakeRollersSubsystem, 
+                               ShooterFeederSubsystem shooterFeederSubsystem, ShooterFlywheelSubsystem shooterFlywheelSubsystem, ShooterPivotSubsystem shooterPivotSubsystem, 
+                               ElevatorSubsystem elevatorSubsystem, SwerveSubsystem swerveSubsystem, LEDSubsystem ledSubsystem) {
+        super(intakePivotSubsystem, intakeRollersSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, shooterPivotSubsystem, elevatorSubsystem, swerveSubsystem, ledSubsystem);
 
         addCommands(
             goShoot(preloadedtraj), 

src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java

@@ -6,18 +6,23 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.intake.IntakeRollersSubsystem;
+import frc.robot.subsystems.leds.LEDSubsystem;
+import frc.robot.subsystems.superstructure.NotePosition;
 
 public class IntakeRollerIntakeCommand extends Command{
     private final IntakeRollersSubsystem intakeSubsystem;
+    private final LEDSubsystem ledSubsystem;
 
-    public IntakeRollerIntakeCommand(IntakeRollersSubsystem intakeSubsystem){
+    public IntakeRollerIntakeCommand(IntakeRollersSubsystem intakeSubsystem, LEDSubsystem ledSubsystem){
         this.intakeSubsystem = intakeSubsystem;
+        this.ledSubsystem = ledSubsystem;
         addRequirements(intakeSubsystem);
     }
 
     @Override
     public void initialize() {
-
+        ledSubsystem.setNoteMode(NotePosition.INTAKING);
+        System.out.println("SLDKJFHDSFLJKHDSFKHJDSFLKJKJLD");
     }
 
     @Override
@@ -30,6 +35,7 @@ public void execute() {
     public void end(boolean interrupted){
         // TODO Auto-generated method stub
         intakeSubsystem.setRollSpeed(0,0); 
+        ledSubsystem.setNoteMode(intakeSubsystem.sensorNow() ? NotePosition.INTAKE_HOLDING : NotePosition.NONE);
     }
 
     @Override

src/main/java/frc/robot/commands/sequences/AutoIntakeSequence.java

@@ -8,6 +8,7 @@
 import frc.robot.commands.swerve.NoteAlignCommand;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.intake.IntakeRollersSubsystem;
+import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.vision.NoteDetectionWrapper;
 
@@ -18,12 +19,13 @@ public AutoIntakeSequence(
         ElevatorSubsystem elevatorSubsystem, 
         IntakeRollersSubsystem intakeRollersSubsystem,
         SwerveSubsystem swerveSubsystem,
-        NoteDetectionWrapper noteDetector
+        NoteDetectionWrapper noteDetector,
+        LEDSubsystem ledSubsystem
     ){
         addCommands(new ElevatorToGroundCommand(elevatorSubsystem)
         .andThen(new NoteAlignCommand(swerveSubsystem, noteDetector))
         .andThen(new ParallelDeadlineGroup(
-            new IntakeRollerIntakeCommand(intakeRollersSubsystem).withTimeout(3),
+            new IntakeRollerIntakeCommand(intakeRollersSubsystem, ledSubsystem).withTimeout(3),
             new DriveForwardCommand(swerveSubsystem)))
         );
     }

src/main/java/frc/robot/commands/sequences/ShootModeSequence.java

@@ -1,6 +1,7 @@
 package frc.robot.commands.sequences;
 
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.elevator.ElevatorToGroundCommand;
@@ -10,9 +11,12 @@
 import frc.robot.commands.shooter.pivot.ShooterPivotSetAngleCommand;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.intake.IntakeRollersSubsystem;
+import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.shooter.ShooterFeederSubsystem;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
 import frc.robot.subsystems.shooter.ShooterPivotSubsystem;
+import frc.robot.subsystems.superstructure.NotePosition;
+
 import static frc.robot.Constants.ShooterConstants.*;
 
 public class ShootModeSequence extends SequentialCommandGroup {
@@ -38,16 +42,20 @@ public ShootModeSequence(
         ElevatorSubsystem elevatorSubsystem,
         ShooterFeederSubsystem shooterFeederSubsystem,
         ShooterFlywheelSubsystem shooterFlywheelSubsystem, 
-        ShooterPivotSubsystem shooterPivotSubsystem
+        ShooterPivotSubsystem shooterPivotSubsystem,
+        LEDSubsystem ledSubsystem
     ){
         addCommands(new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem).alongWith(
                     new SequentialCommandGroup(
                         new ShooterPivotSetAngleCommand(shooterPivotSubsystem, FEED_ANGLE).alongWith(
                             new ElevatorToGroundCommand(elevatorSubsystem)
                         ),
                         new ParallelDeadlineGroup(new ShooterFeedLoadCommand(shooterFeederSubsystem),
-                                                  new IntakeRollerFeedCommand(intakeRollerSubsystem)),
-                        new ShooterPivotSetAngleCommand(shooterPivotSubsystem, Units.degreesToRadians(20)) //STUB FOR AUTOAIM
+                                                  new IntakeRollerFeedCommand(intakeRollerSubsystem),
+                                                  new InstantCommand(() -> ledSubsystem.setNoteMode(NotePosition.TRANSFER_TO_SHOOTER))),
+                        new InstantCommand(() -> ledSubsystem.setNoteMode(NotePosition.SHOOTER_HOLDING)),
+                        new ShooterPivotSetAngleCommand(shooterPivotSubsystem, Units.degreesToRadians(20)), //STUB FOR AUTOAIM
+                        new InstantCommand(() -> ledSubsystem.setNoteMode(NotePosition.SHOOTER_READY_TO_SHOOT))
                     )
         ));
     }

src/main/java/frc/robot/subsystems/leds/LEDLayer.java

@@ -1,16 +1,16 @@
 package frc.robot.subsystems.leds;
 
 import edu.wpi.first.wpilibj.util.Color;
+import frc.robot.util.OpacityColor;
+import frc.robot.util.Util;
 
 public class LEDLayer {
-    protected final Color[] colorArray;
-    protected final double[] opacityArray;
+    protected final OpacityColor[] colorArray;
     protected final int ledLength;
 
     public LEDLayer(int length) {
         ledLength = length;
-        colorArray = new Color[length];
-        opacityArray = new double[length];
+        colorArray = new OpacityColor[length];
     }
 
     /**
@@ -19,66 +19,46 @@ public LEDLayer(int length) {
      * @param color The color to set the LED at index i to (null is equivalent to transparent).
      * @param opacity The opacity to set the LED at index i to
      */
-    public void setLED(int i, Color color, double opacity) {
+    public void setLED(int i, OpacityColor color) {
         colorArray[i] = color;
-        opacityArray[i] = opacity;
-    }
-
-    public void setLED(int i, Color color){
-        setLED(i, color, 1);
     }
 
     /**
      * Gets the color of the LED at a specified index.
      * @param i The LED index to retrieve.
      * @return The color of the LED at index i.
      */
-    public Color getLEDColor(int i) {
+    public OpacityColor getLEDColor(int i) {
         return colorArray[i];
     }
 
-    /**
-     * Gets the opacity of the LED at a specified index
-     * @param i The LED index to retrieve.
-     * @return The opacity of the LED at index i.
-     */
-    public double getLEDOpacity(int i){
-        return opacityArray[i];
-    }
 
     /**
      * Moves the leds up by an increment
      * @param inc The number of leds to move up by
      * @param color The color to set at the bottom
      * @param opacity The opacity to set the new leds at
      */
-    public void incrementColors(int inc, Color color, double opacity) {
+    public void incrementColors(int inc, OpacityColor color) {
         for (int i = 0; i < colorArray.length - inc; i++) {
-            setLED(i, getLEDColor(i + inc), getLEDOpacity(i +inc));
+            setLED(i, getLEDColor(i + inc));
         }
         for (int i = colorArray.length - inc; i < colorArray.length; i++) {
-            setLED(i, color, opacity);
+            setLED(i, color);
         }
     }
 
-    public void incrementColors(int inc, Color color){
-        incrementColors(inc, color, 1);
-    }
-
     /**
      * Fills the layer with a solid color.
      * @param color The color to fill the layer with.
      * @param opacity The opacity to fill the layer with
      */
-    public void fillColor(Color color, double opacity) {
+    public void fillColor(OpacityColor color) {
         for (int i = 0; i < colorArray.length; i++) {
-            setLED(i, color, opacity);
+            setLED(i, color);
         }
     }
 
-    public void fillColor(Color color){
-        fillColor(color, 1);
-    }
 
     /**
      * Fills the layer with alternating groups of "on" and "off" LEDs. "off" leds are set to null (transparent).
@@ -89,29 +69,35 @@ public void fillColor(Color color){
      * @param opacity The opacity of the "on" LEDs.
      * @param offset The number of LEDs to offset the base by
      */
-    public void fillGrouped(int onGroupLength, int offGroupLength, int borderLength, Color color, double opacity, int offset) {
+    public void fillGrouped(int onGroupLength, int offGroupLength, int borderLength, OpacityColor color, OpacityColor baseColor, int offset) {
         for (int i = 0; i < colorArray.length; i++) {
             int ledNumInSegment = (i + offset) % (2 * borderLength + onGroupLength + offGroupLength);
             if (ledNumInSegment < borderLength){
-                setLED(i, color, opacity *  (ledNumInSegment + 1) / (borderLength + 1));
+                setLED(i, OpacityColor.blendColors(baseColor, color, (ledNumInSegment + 1) / (borderLength + 1)));
             } else if (ledNumInSegment < onGroupLength + borderLength) {
-                setLED(i, color, opacity);
+                setLED(i, color);
             } else if(ledNumInSegment < onGroupLength + borderLength * 2){
-                setLED(i, color, opacity * (1 - ((ledNumInSegment - onGroupLength - borderLength + 1.) / (borderLength + 1))));                
+                setLED(i, OpacityColor.blendColors(baseColor, color, (1 - ((ledNumInSegment - onGroupLength - borderLength + 1.) / (borderLength + 1)))));                
             } else {
-                setLED(i, null, opacity);
+                setLED(i, baseColor);
             }
         }
     }
 
-    public void fillGrouped(int onGroupLength, int offGroupLength, Color color){
-        fillGrouped(onGroupLength, offGroupLength, 0, color, 1, 0);
+    public void fillGrouped(int onGroupLength, int offGroupLength, OpacityColor color){
+        fillGrouped(onGroupLength, offGroupLength, 0, color, new OpacityColor(), 0);
     }
 
     /**
      * Resets the layer by setting all LEDs to null (transparent).
      */
     public void reset() {
-        fillColor(null, 1);
+        fillColor(new OpacityColor());
+    }
+
+    public void scale(double factor){
+        for(int i = 0; i < colorArray.length; i++){
+            setLED(i, Util.scaleColor(getLEDColor(i), factor));
+        }
     }
 }