Testing: Added Indicator LEDs

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

@@ -40,6 +40,7 @@
 import frc.team4909.robot.subsystems.intake.commands.CargoIntakeIn;
 import frc.team4909.robot.subsystems.intake.commands.CargoIntakeOut;
 import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeOpen;
+import frc.team4909.robot.subsystems.led.RGBStrip;
 
 import frc.team4909.robot.subsystems.climber.commands.ZeroStilts;
 import frc.team4909.robot.subsystems.elevator.commands.ZeroElevator;
@@ -93,6 +94,7 @@ public class Robot extends TimedRobot {
 
   // Sensors
   public static LidarLitePWM lidar;
+  public static RGBStrip LEDs;
 
   /**
    * This function is run when the robot is first started up and should be used
@@ -119,6 +121,7 @@ public void robotInit() {
     elevatorSubsystem = new ElevatorSubsystem();
     elevatorArmSubsystem = new ElevatorArmSubsystem();
     climberSubsystem = new ClimberSubsystem();
+    LEDs = new RGBStrip(2, 3, 4);
 
     // Sensors
     lidar = new LidarLitePWM(RobotMap.lidarPort);

src/main/java/frc/team4909/robot/subsystems/intake/IntakeSubsystem.java

@@ -12,6 +12,7 @@
 import frc.team4909.robot.RobotMap;
 import frc.team4909.robot.subsystems.intake.commands.CargoIntakeHold;
 import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeClose;
+import frc.team4909.robot.subsystems.led.RGBStrip.Color;
 
 public class IntakeSubsystem extends Subsystem {
     DoubleSolenoid hatchPanelSolenoid;
@@ -49,6 +50,7 @@ public void holdCargoIntake(){
     public void setCargoIntakeSpeed(double speed) {
         if(hasCargo() && speed > 0){
             speed = RobotConstants.cargoIntakeHoldSpeed;
+            Robot.LEDs.set(Color.Red);
         }
 
         cargoIntakeMotor.set(-speed);

src/main/java/frc/team4909/robot/subsystems/intake/commands/CargoIntakeIn.java

@@ -1,17 +1,19 @@
 package frc.team4909.robot.subsystems.intake.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import frc.team4909.robot.subsystems.led.RGBStrip.Color;
 import frc.team4909.robot.Robot;
 import frc.team4909.robot.RobotConstants;
 
 public class CargoIntakeIn extends Command {
     public CargoIntakeIn() {
         requires(Robot.intakeSubsystem);
+        requires(Robot.LEDs);
     }
 
     protected void execute() {
         Robot.intakeSubsystem.setCargoIntakeSpeed(RobotConstants.cargoIntakeInSpeed);
-
+        Robot.LEDs.set(Color.Red);
     }
 
     @Override

src/main/java/frc/team4909/robot/subsystems/intake/commands/CargoIntakeOut.java

@@ -3,14 +3,17 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.team4909.robot.Robot;
 import frc.team4909.robot.RobotConstants;
+import frc.team4909.robot.subsystems.led.RGBStrip.Color;
 
 public class CargoIntakeOut extends Command {
     public CargoIntakeOut() {
         requires(Robot.intakeSubsystem);
+        requires(Robot.LEDs);
     }
 
     protected void execute() {
         Robot.intakeSubsystem.setCargoIntakeSpeed(RobotConstants.cargoIntakeOutSpeed);
+        Robot.LEDs.set(Color.Lime);
     }
 
     @Override

src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeClose.java

@@ -1,15 +1,18 @@
 package frc.team4909.robot.subsystems.intake.commands;
 
 import edu.wpi.first.wpilibj.command.InstantCommand;
+import frc.team4909.robot.subsystems.led.RGBStrip.Color;
 import frc.team4909.robot.Robot;
 
 public class HatchPanelIntakeClose extends InstantCommand {
 
     public HatchPanelIntakeClose() {
         requires(Robot.intakeSubsystem);
+        requires(Robot.LEDs);
     }
 
     protected void initialize() {
         Robot.intakeSubsystem.hatchPanelIntakeClose();
+        Robot.LEDs.set(Color.Lime);
     }
 }

src/main/java/frc/team4909/robot/subsystems/intake/commands/HatchPanelIntakeOpen.java

@@ -2,15 +2,18 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeClose;
+import frc.team4909.robot.subsystems.led.RGBStrip.Color;
 import frc.team4909.robot.Robot;
 
 public class HatchPanelIntakeOpen extends Command {
     public HatchPanelIntakeOpen() {
         requires(Robot.intakeSubsystem);
+        requires(Robot.LEDs);
     }
 
-    protected void execute() {
+	protected void execute() {
         Robot.intakeSubsystem.hatchPanelIntakeOpen();
+        Robot.LEDs.set(Color.Yellow);
     }
 
     protected void end() {

src/main/java/frc/team4909/robot/subsystems/led/RGBStrip.java

@@ -0,0 +1,74 @@
+package frc.team4909.robot.subsystems.led;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+public class RGBStrip extends Subsystem{
+    private final Solenoid red, green, blue;
+    private Color currentColor = Color.Black;
+
+    public enum Color {
+        Black(false, false, false),
+        White(true, true, true),
+        Red(true, false, false),
+        Lime(false, true, false),
+        Blue(false, false, true),
+        Yellow(true, true, false),
+        Cyan(false, true, true),
+        Magenta(true, false, true);
+
+        public final boolean red;
+        public final boolean green;
+        public final boolean blue;
+
+        Color(boolean red, boolean green, boolean blue) {
+            this.red = red;
+            this.green = green;
+            this.blue = blue;
+        }
+    }
+
+    public RGBStrip(int redChannel, int greenChannel, int blueChannel) {
+        red = new Solenoid(redChannel);
+        green = new Solenoid(greenChannel);
+        blue = new Solenoid(blueChannel);
+    }
+
+    public Color getCurrentColor(){
+        return currentColor;
+    }
+
+    public void setColor(Color color){
+        currentColor = color;
+
+        red.set(color.red);
+        green.set(color.green);
+        blue.set(color.blue);
+    }
+
+    public void reset(){
+        setColor(Color.Black);
+    }
+
+    public InstantCommand set(Color color) {
+        return new SetColor(color);
+    }
+    private class SetColor extends InstantCommand {
+        Color color;
+
+        public SetColor(Color color){
+            this.color = color;
+        }
+
+        public void initialize(){
+            setColor(color);
+        }
+    }
+
+    @Override
+    protected void initDefaultCommand() {
+        this.set(Color.Lime);
+    }
+}