add LED integration to shooter (#72)

Co-authored-by: Jade <[email protected]>

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

@@ -7,6 +7,8 @@
 import frc.robot.generated.TunerConstants;
 
 public final class Constants {
+  public static final int LEDport = 0;
+
   public static final int driverport = 0;
   public static final int codriverport = 1;
 

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

@@ -0,0 +1,34 @@
+// Copyright (c) 2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.PWM;
+import frc.robot.Constants;
+
+public final class LED {
+  private static final PWM ledpwm = new PWM(Constants.LEDport);
+
+  private LED() {}
+
+  public static void Intake() {
+    ledpwm.setSpeed(0.59); // dark red //
+  }
+
+  public static void Outake() {
+    ledpwm.setSpeed(0.69); // yellow //
+  }
+
+  public static void Spinup() {
+    ledpwm.setSpeed(0.63); // red orange, 0.65 for normal orange //
+  }
+
+  public static void Stop() {
+    ledpwm.setSpeed(0.73); // lime //
+  }
+
+  public static void Maintain() {
+    ledpwm.setSpeed(0.57); // hot pink //
+  }
+}

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

@@ -48,7 +48,7 @@ private Command achieveSpeeds(double speed) {
         () -> {
           var output =
               m_pid.calculate(
-                  -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity()));
+                  -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity()), speed);
           log_pid_output.append(output);
           m_ntPidError.set(m_pid.getVelocityError());
           m_ntPidOutput.set(m_pid.getVelocityError());
@@ -63,11 +63,12 @@ private Command achieveSpeeds(double speed) {
    * @return a {@link Command} to get to the desired speed.
    */
   public Command spinup(double speed) {
-    return achieveSpeeds(speed).until(m_pid::atSetpoint);
+    LED.Spinup();
+    return runOnce(() -> LED.Spinup()).andThen(achieveSpeeds(speed).until(m_pid::atSetpoint));
   }
 
   public Command stop() {
-    return runOnce(() -> m_motor.set(0));
+    return runOnce(() -> LED.Stop()).andThen(runOnce(() -> m_motor.set(0)));
   }
 
   /**
@@ -76,7 +77,7 @@ public Command stop() {
    * @return A {@link Command} to hold the speed at the setpoint.
    */
   public Command maintain() {
-    return defer(() -> achieveSpeeds(m_pid.getSetpoint()));
+    return defer(() -> runOnce(() -> LED.Maintain()).andThen(achieveSpeeds(m_pid.getSetpoint())));
   }
 
   public Command shoot() {