Merge pull request #37 from Team4388/Functional-LEDS

Functional leds

src/main/java/frc4388/robot/Constants.java

@@ -394,9 +394,16 @@ public final class Constants {
     }
     
     public static final class LEDConstants {
-        public static final int LED_SPARK_ID = 0;
+        public static final int LED_SPARK_ID = 9;
 
         public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+
+        public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
+        public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
+        public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
+
+        public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
+        public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
     }
 
     public static final class OIConstants {

src/main/java/frc4388/robot/RobotContainer.java

@@ -56,7 +56,7 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 // Subsystems
-// import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Elevator.CoordinationState;
 import frc4388.robot.subsystems.Lidar;
@@ -85,10 +85,10 @@ public class RobotContainer {
     public final RobotMap m_robotMap = new RobotMap();
     
     /* Subsystems */
-    // private final LED m_robotLED = new LED();
+    private final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
     public final Lidar m_lidar = new Lidar();
-    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch);
+    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -19,12 +19,17 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ElevatorConstants;
+import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.subsystems.LED;
+import frc4388.utility.LEDPatterns;
+import frc4388.utility.TimesNegativeOne;
 
 public class Elevator extends SubsystemBase {
   /** Creates a new Elevator. */
   private TalonFX elevatorMotor;
   private TalonFX endeffectorMotor;
+  private LED led;
 
   private long wait = 0;
   private long maxWait = 1000;
@@ -56,9 +61,10 @@ public class Elevator extends SubsystemBase {
 
   private CoordinationState currentState;
 
-  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch) {
+  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
     elevatorMotor = elevatorTalonFX;
     endeffectorMotor = endeffectorTalonFX;
+    this.led = led;
 
     this.basinBeamBreak = basinLimitSwitch;
     this.endeffectorLimitSwitch = endeffectorLimitSwitch;
@@ -89,6 +95,7 @@ public class Elevator extends SubsystemBase {
     endeffectorMotor.set(0);
   }
 
+
   public void transitionState(CoordinationState state) {
     currentState = state;
     switch (currentState) {
@@ -96,66 +103,77 @@ public class Elevator extends SubsystemBase {
         wait = System.currentTimeMillis() + maxWait;
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.WAITING_PATTERN);
         break;
       }
 
       case WatingBeamTripped: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.DOWN_PATTERN);
         break;
       }
 
       case Ready: {
         PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.DOWN_PATTERN);
         break;
       }
 
       case Hovering: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.READY_PATTERN);
         break;
       }
 
       case L2Score: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
       
       case PrimedFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
 
       case ScoringFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
 
       case PrimedThree: {
         PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
       
       case ScoringThree: {
         PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
 
       case BallRemoverL2Primed: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
 
       case BallRemoverL2Go: {
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
 
@@ -168,6 +186,7 @@ public class Elevator extends SubsystemBase {
       case BallRemoverL3Go: {
         PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
         break;
       }
 

src/main/java/frc4388/robot/subsystems/LED.java

@@ -22,69 +22,16 @@ import frc4388.utility.LEDPatterns;
  */
 public class LED extends SubsystemBase {
 
-  static AddressableLED m_led;
+  private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
-  static AddressableLEDBuffer m_ledBuffer;
+  private double mode = LEDConstants.DEFAULT_PATTERN.getValue();
-  static LED m_self;
-  /**
-   * Add your docs here.
-   */
 
-  public LED(){
+  public void setMode(LEDPatterns pattern){
-    if(m_self != null)
+    this.mode = pattern.getValue();
-      return;
-    m_led = new AddressableLED(9);
-    m_ledBuffer = new AddressableLEDBuffer(10);
-    m_led.setLength(m_ledBuffer.getLength());
-    m_led.setData(m_ledBuffer);
-    m_led.start();
-    System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
-  }
-  public static LED getInstance() {
-    if(m_self == null)
-      m_self = new LED();
-    return m_self;
   }
+
   @Override
-  public void periodic(){
+  public void periodic() {
-    //gamermode();
+    LEDController.set(mode);
-    //SmartDashboard.putNumber("LED", m_currentPattern.getValue());
-    return;
-  }
-  static int firstcolor = 0;
-  static void gamermode() {
-      for(int i = 0; i < m_ledBuffer.getLength(); i++) {
-          final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
-          setLEDHSV(i, hue, 255, 128);
-      }
-      firstcolor +=3;
-      firstcolor %= 180;
-  }
-  /**
-   * Add your docs here.
-   */
-  public static void updateLED (){
-    gamermode();
-   // m_LEDController.set(m_currentPattern.getValue());
   }
 
-  /**
-   * Add your docs here.
-   */
-  public static void setLEDRGB(int lednum, int r, int g, int b){
-    m_ledBuffer.setRGB(lednum, r, g, b);
-    //m_currentPattern = pattern;
-   // m_LEDController.set(m_currentPattern.getValue());
-  }
-  public static void setLEDHSV(int lednum, int hue, int sat, int val){
-    m_ledBuffer.setRGB(lednum, hue, sat, val);
-    //m_currentPattern = pattern;
-   // m_LEDController.set(m_currentPattern.getValue());
-  }
-  /**
-   * Add your docs here.
-   * @return
-   */
-  public AddressableLEDBuffer getBuffer() {
-    return m_ledBuffer;
-  }
 }