import frc.robot.controls.PS5ControllerDriverConfig;
import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.LED.LED2;
+import frc.robot.subsystems.LED.LED;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
import frc.robot.subsystems.hood.Hood;
private BaseDriverConfig driver = null;
private Operator operator = null;
private LinearClimb linearClimb = null;
- private LED2 led = null;
+ private LED led = null;
// TODO: move to correct robot and put the correct port?
private PS5Controller ps5 = new PS5Controller(0);
case PrimeJr: // AKA Valence
spindexer = new Spindexer();
intake = new Intake();
- // led = new LED();
- // led.setDefaultCommand(new LEDDefaultCommand(led));
- led = new LED2();
+ led = new LED();
case WaffleHouse: // AKA Betabot
turret = new Turret();
// fall-through
case Vivace:
- // linearClimb = new LinearClimb();
case Phil: // AKA "IHOP"
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.ScheduleCommand;
import frc.robot.constants.Constants;
import frc.robot.constants.IntakeConstants;
import frc.robot.subsystems.Intake.Intake;
--- /dev/null
+package frc.robot.subsystems.LED;
+
+import java.util.Optional;
+
+import com.ctre.phoenix6.configs.CANdleConfigurator;
+import com.ctre.phoenix6.configs.CANdleFeaturesConfigs;
+import com.ctre.phoenix6.configs.LEDConfigs;
+import com.ctre.phoenix6.controls.ColorFlowAnimation;
+import com.ctre.phoenix6.controls.FireAnimation;
+import com.ctre.phoenix6.controls.RainbowAnimation;
+import com.ctre.phoenix6.controls.RgbFadeAnimation;
+import com.ctre.phoenix6.controls.SolidColor;
+import com.ctre.phoenix6.controls.StrobeAnimation;
+import com.ctre.phoenix6.controls.TwinkleAnimation;
+import com.ctre.phoenix6.hardware.CANdle;
+import com.ctre.phoenix6.signals.Enable5VRailValue;
+import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue;
+import com.ctre.phoenix6.signals.RGBWColor;
+import com.ctre.phoenix6.signals.StatusLedWhenActiveValue;
+import com.ctre.phoenix6.signals.StripTypeValue;
+import com.ctre.phoenix6.signals.VBatOutputModeValue;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.util.HubActive;
+
+public class LED extends SubsystemBase {
+
+ private CANdle candle;
+ public static final int stripLength = 67;
+
+ /// Hz
+ public static final int FLASH_RATE = 4;
+
+ private Color color;
+
+ public LED() {
+ candle = new CANdle(IdConstants.CANDLE_ID, Constants.RIO_CAN);
+ CANdleConfigurator configurator = candle.getConfigurator();
+
+ LEDConfigs ledConf = new LEDConfigs()
+ .withStripType(StripTypeValue.GRB)
+ .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)
+ .withBrightnessScalar(1);
+
+ CANdleFeaturesConfigs featureConf = new CANdleFeaturesConfigs()
+ .withEnable5VRail(Enable5VRailValue.Enabled) // Turns off LEDs
+ .withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)
+ .withVBatOutputMode(VBatOutputModeValue.On);
+
+ configurator.apply(featureConf);
+ configurator.apply(ledConf);
+
+ setColor();
+
+ candle.clearAllAnimations();
+ lightsOff();
+
+ System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
+
+ SmartDashboard.putData("LED Disable", new InstantCommand(() -> {forceOff = true; lightsOff();}).ignoringDisable(true));
+ SmartDashboard.putData("LED Enable", new InstantCommand(() -> forceOff = false).ignoringDisable(true));
+ SmartDashboard.putData("LED Strobe", new InstantCommand(() -> setStrobe()).ignoringDisable(true));
+ SmartDashboard.putData("LED Static", new InstantCommand(() -> setStatic()).ignoringDisable(true));
+ SmartDashboard.putData("LED Fire", new InstantCommand(() -> setFire()).ignoringDisable(true));
+ SmartDashboard.putData("LED Rainbow", new InstantCommand(() -> setRainbow()).ignoringDisable(true));
+ SmartDashboard.putData("LED Fade", new InstantCommand(() -> setRgbFadeAnimation()).ignoringDisable(true));
+ SmartDashboard.putData("LED Twinkle", new InstantCommand(() -> setTwinkle()).ignoringDisable(true));
+ SmartDashboard.putData("LED Color Flow", new InstantCommand(() -> setColorFlow()).ignoringDisable(true));
+ SmartDashboard.putData("LED Color Team Reset", new InstantCommand(() -> setColor()).ignoringDisable(true));
+ }
+
+ public void setColor() {
+ var alliance = DriverStation.getAlliance();
+ if (alliance.isEmpty()) {
+ color = Color.kOrangeRed;
+ } else if (alliance.get() == Alliance.Red) {
+ color = Color.kRed;
+ } else if (alliance.get() == Alliance.Blue) {
+ color = Color.kBlue;
+ } else {
+ color = Color.kOrangeRed;
+ }
+ }
+
+ private enum State { OFF, ON, AUTO, SLOW, FAST, ENDGAME };
+
+ private State lastState = State.OFF;
+ private boolean forceOff = false;
+
+ @Override
+ public void periodic() {
+ State targetState = State.ON;
+ if (underSecsToFlip(5)) targetState = State.SLOW;
+ if (underSecsToFlip(1)) targetState = State.FAST;
+ if (DriverStation.isAutonomous()) targetState = State.AUTO;
+ if (DriverStation.getMatchTime() < 30) targetState = State.ENDGAME;
+ if (forceOff) targetState = State.OFF;
+
+ if (targetState != lastState) {
+ switch (targetState) {
+ case OFF: lightsOff(); break;
+ case ON: setStatic(); break;
+ case AUTO: setTwinkle(); break;
+ case SLOW: setStrobe(); break;
+ case FAST: setFastStrobe(); break;
+ case ENDGAME: setRainbow(); break;
+ }
+ lastState = targetState;
+ }
+ }
+
+ public void setFire() {
+ candle.clearAllAnimations();
+ candle.setControl(new FireAnimation(8, 8 + stripLength).withSparking(0.5));
+ }
+
+ public void setRainbow() {
+ candle.clearAllAnimations();
+ candle.setControl(new RainbowAnimation(8, 8 + stripLength));
+ }
+
+ public void setRgbFadeAnimation() {
+ candle.clearAllAnimations();
+ candle.setControl(new RgbFadeAnimation(8, 8 + stripLength));
+ }
+
+ public void setTwinkle() {
+ candle.clearAllAnimations();
+ candle.setControl(new TwinkleAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kViolet)));
+ }
+
+ public void setColorFlow() {
+ candle.clearAllAnimations();
+ candle.setControl(new ColorFlowAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kAzure)));
+ }
+
+ public void setStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE).withColor(new RGBWColor(color)));
+ }
+
+ public void setFastStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE * 4).withColor(new RGBWColor(color)));
+ }
+
+ public void setStatic() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
+ }
+
+ public void lightsOff() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8 , 8 + stripLength).withColor(new RGBWColor(0, 0, 0, 0)));
+ }
+
+
+ private boolean underSecsToFlip(double secs) {
+ Optional<Double> timeToActive = HubActive.timeToActive();
+ Optional<Double> timeToInactive = HubActive.timeToInactive();
+
+ if (timeToActive.isEmpty() && timeToInactive.isEmpty()) {
+ return false;
+ } else if (timeToActive.isPresent() && timeToActive.get() != 0) {
+ return (timeToActive.get() <= secs);
+
+ } else if (timeToInactive.isPresent() && timeToInactive.get() != 0) {
+ return (timeToInactive.get() <= secs);
+ } else {
+ return false;
+ }
+ }
+
+}
+++ /dev/null
-package frc.robot.subsystems.LED;
-
-import java.util.Optional;
-
-import com.ctre.phoenix6.configs.CANdleConfigurator;
-import com.ctre.phoenix6.configs.CANdleFeaturesConfigs;
-import com.ctre.phoenix6.configs.LEDConfigs;
-import com.ctre.phoenix6.controls.ColorFlowAnimation;
-import com.ctre.phoenix6.controls.FireAnimation;
-import com.ctre.phoenix6.controls.RainbowAnimation;
-import com.ctre.phoenix6.controls.RgbFadeAnimation;
-import com.ctre.phoenix6.controls.SolidColor;
-import com.ctre.phoenix6.controls.StrobeAnimation;
-import com.ctre.phoenix6.controls.TorqueCurrentFOC;
-import com.ctre.phoenix6.controls.TwinkleAnimation;
-import com.ctre.phoenix6.hardware.CANdle;
-import com.ctre.phoenix6.signals.Enable5VRailValue;
-import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue;
-import com.ctre.phoenix6.signals.RGBWColor;
-import com.ctre.phoenix6.signals.StatusLedWhenActiveValue;
-import com.ctre.phoenix6.signals.StripTypeValue;
-import com.ctre.phoenix6.signals.VBatOutputModeValue;
-
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-import frc.robot.util.HubActive;
-
-public class LED2 extends SubsystemBase {
-
- private CANdle candle;
- public static final int stripLength = 67;
-
- /// Hz
- public static final int FLASH_RATE = 4;
-
- private Color color;
-
- public LED2() {
- candle = new CANdle(IdConstants.CANDLE_ID, Constants.RIO_CAN);
- CANdleConfigurator configurator = candle.getConfigurator();
-
- LEDConfigs ledConf = new LEDConfigs()
- .withStripType(StripTypeValue.GRB)
- .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)
- .withBrightnessScalar(1);
-
- CANdleFeaturesConfigs featureConf = new CANdleFeaturesConfigs()
- .withEnable5VRail(Enable5VRailValue.Enabled) // Turns off LEDs
- .withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)
- .withVBatOutputMode(VBatOutputModeValue.On);
-
- configurator.apply(featureConf);
- configurator.apply(ledConf);
-
- setColor();
-
- candle.clearAllAnimations();
- lightsOff();
-
- System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
-
- SmartDashboard.putData("LED Disable", new InstantCommand(() -> {forceOff = true; lightsOff();}).ignoringDisable(true));
- SmartDashboard.putData("LED Enable", new InstantCommand(() -> forceOff = false).ignoringDisable(true));
- SmartDashboard.putData("LED Strobe", new InstantCommand(() -> setStrobe()).ignoringDisable(true));
- SmartDashboard.putData("LED Static", new InstantCommand(() -> setStatic()).ignoringDisable(true));
- SmartDashboard.putData("LED Fire", new InstantCommand(() -> setFire()).ignoringDisable(true));
- SmartDashboard.putData("LED Rainbow", new InstantCommand(() -> setRainbow()).ignoringDisable(true));
- SmartDashboard.putData("LED Fade", new InstantCommand(() -> setRgbFadeAnimation()).ignoringDisable(true));
- SmartDashboard.putData("LED Twinkle", new InstantCommand(() -> setTwinkle()).ignoringDisable(true));
- SmartDashboard.putData("LED Color Flow", new InstantCommand(() -> setColorFlow()).ignoringDisable(true));
- SmartDashboard.putData("LED Color Team Reset", new InstantCommand(() -> setColor()).ignoringDisable(true));
- }
-
- public void setColor() {
- var alliance = DriverStation.getAlliance();
- if (alliance.isEmpty()) {
- color = Color.kOrangeRed;
- } else if (alliance.get() == Alliance.Red) {
- color = Color.kRed;
- } else if (alliance.get() == Alliance.Blue) {
- color = Color.kBlue;
- } else {
- color = Color.kOrangeRed;
- }
- }
-
- private enum State { OFF, ON, AUTO, SLOW, FAST, ENDGAME };
-
- private State lastState = State.OFF;
- private boolean forceOff = false;
-
- @Override
- public void periodic() {
- State targetState = State.ON;
- if (underSecsToFlip(5)) targetState = State.SLOW;
- if (underSecsToFlip(1)) targetState = State.FAST;
- if (DriverStation.isAutonomous()) targetState = State.AUTO;
- if (DriverStation.getMatchTime() < 30) targetState = State.ENDGAME;
- if (forceOff) targetState = State.OFF;
-
- if (targetState != lastState) {
- switch (targetState) {
- case OFF: lightsOff(); break;
- case ON: setStatic(); break;
- case AUTO: setTwinkle(); break;
- case SLOW: setStrobe(); break;
- case FAST: setFastStrobe(); break;
- case ENDGAME: setRainbow(); break;
- }
- lastState = targetState;
- }
- }
-
- public void setFire() {
- candle.clearAllAnimations();
- candle.setControl(new FireAnimation(8, 8 + stripLength).withSparking(0.5));
- }
-
- public void setRainbow() {
- candle.clearAllAnimations();
- candle.setControl(new RainbowAnimation(8, 8 + stripLength));
- }
-
- public void setRgbFadeAnimation() {
- candle.clearAllAnimations();
- candle.setControl(new RgbFadeAnimation(8, 8 + stripLength));
- }
-
- public void setTwinkle() {
- candle.clearAllAnimations();
- candle.setControl(new TwinkleAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kViolet)));
- }
-
- public void setColorFlow() {
- candle.clearAllAnimations();
- candle.setControl(new ColorFlowAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kAzure)));
- }
-
- public void setStrobe() {
- candle.clearAllAnimations();
- candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE).withColor(new RGBWColor(color)));
- }
-
- public void setFastStrobe() {
- candle.clearAllAnimations();
- candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE * 4).withColor(new RGBWColor(color)));
- }
-
- public void setStatic() {
- candle.clearAllAnimations();
- candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
- }
-
- public void lightsOff() {
- candle.clearAllAnimations();
- candle.setControl(new SolidColor(8 , 8 + stripLength).withColor(new RGBWColor(0, 0, 0, 0)));
- }
-
-
- private boolean underSecsToFlip(double secs) {
- Optional<Double> timeToActive = HubActive.timeToActive();
- Optional<Double> timeToInactive = HubActive.timeToInactive();
-
- if (timeToActive.isEmpty() && timeToInactive.isEmpty()) {
- return false;
- } else if (timeToActive.isPresent() && timeToActive.get() != 0) {
- return (timeToActive.get() <= secs);
-
- } else if (timeToInactive.isPresent() && timeToInactive.get() != 0) {
- return (timeToInactive.get() <= secs);
- } else {
- return false;
- }
- }
-
-}
}
// for current limit setting (brownout protection)
- public void applyNewModuleCurrents(double steerCurrent, double driveCurren) {
+ public void applyNewModuleCurrents(double steerCurrent, double driveCurrent) {
for (Module module : modules) { // iterate over our modules
- module.setNewCurrentLimit(steerCurrent, driveCurren);
+ module.setNewCurrentLimit(steerCurrent, driveCurrent);
}
}
angleMotor.getConfigurator().apply(steerConfig); // apply
// drive
- CurrentLimitsConfigs dirveConfig = new CurrentLimitsConfigs();
- dirveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- dirveConfig.SupplyCurrentLimit = currentDrive;
- dirveConfig.SupplyCurrentLowerLimit = currentDrive;
- dirveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- dirveConfig.StatorCurrentLimit = currentDrive;
- dirveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveMotor.getConfigurator().apply(dirveConfig); // apply
+ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
+ driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveConfig.SupplyCurrentLimit = currentDrive;
+ driveConfig.SupplyCurrentLowerLimit = currentDrive;
+ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ driveConfig.StatorCurrentLimit = currentDrive;
+ driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(driveConfig); // apply
}
private void configDriveMotor() {
DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.45 // lower drive movement
);
-}
\ No newline at end of file
+}