private Intake intake = null;
// this is inside addAuto()
- //private Command auto = new DoNothing();
+ // private Command auto = new DoNothing();
// Controllers are defined here
private BaseDriverConfig driver = null;
driver.configureControls();
operator.configureControls();
-
registerCommands();
PathGroupLoader.loadPathGroups();
-
+
initializeAutoBuilder();
autoChooserInit();
// put the Chooser on the SmartDashboard
SmartDashboard.putData("Auto chooser", autoChooser);
-
-
if (turret != null) {
turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
}
break;
}
- if (intake != null && hood != null && turret != null)
- CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret));
+ if (intake != null && hood != null && turret != null)
+ CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret));
// This is really annoying so it's disabled
DriverStation.silenceJoystickConnectionWarning(true);
}
- public void addAuto(String name){
- try{
+ public void addAuto(String name) {
+ try {
Command auto = new PathPlannerAuto(name);
autoChooser.addOption(name, auto);
}
// is this the right one??
catch (AutoBuilderException e) {
- e.printStackTrace();
- System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
- }
+ e.printStackTrace();
+ System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
+ }
}
/**
// add the options to the Chooser
String defaultAuto = "Test default auto";
String leftSideAuto = "Left Week V1";
- String rightSideAuto = "Right Week V1";
+ String rightSideAuto = "Right Week V1";
String shootOnlyAuto = "Shoot Only Left Week V1";
autoChooser.setDefaultOption("Default", new PathPlannerAuto(defaultAuto));
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.LED.LED;
-public class ChangeModeCommand extends Command{
+public class ChangeModeCommand extends Command {
private int mode;
private LED led;
- public ChangeModeCommand(int mode, LED led){
+
+ public ChangeModeCommand(int mode, LED led) {
this.mode = mode;
this.led = led;
addRequirements(led);
}
- public void initialize(){
+
+ public void initialize() {
mode++;
}
- public void execute(){
- if(mode == 0){
+
+ public void execute() {
+ if (mode == 0) {
led.setLEDs(0, 0, 0);
- }else if(mode == 1){
+ } else if (mode == 1) {
led.setLEDs(0, 130, 12);
- }else if(mode == 2){
+ } else if (mode == 2) {
led.setLEDs(255, 255, 255);
- }else{
+ } else {
mode = 0;
}
}
- public boolean isFinished(){
+
+ public boolean isFinished() {
return false;
}
- public void end(){
+
+ public void end() {
}
}
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.LED.LED;
-public class DefenseLightsCommand extends Command{
+public class DefenseLightsCommand extends Command {
private int counter;
private int startOffset;
private int length;
private LED led;
- public DefenseLightsCommand(LED led, int startOffset, int length){
+ public DefenseLightsCommand(LED led, int startOffset, int length) {
this.led = led;
this.startOffset = startOffset;
this.length = length;
addRequirements(led);
}
- public void initialize(){
+ public void initialize() {
counter = 0;
}
- public void execute(){
+ public void execute() {
counter++;
- if(counter == 1){
- //setLEDs(255, 0, 0);
+ if (counter == 1) {
+ // setLEDs(255, 0, 0);
led.alternate(255, 0, 0, 0, 0, 255, 5, startOffset, length);
- }else if(counter == 20){
- //setLEDs(0, 0, 255);
+ } else if (counter == 20) {
+ // setLEDs(0, 0, 255);
led.alternate(0, 0, 255, 255, 0, 0, 5, startOffset, length);
}
- if(counter >= 40){
+ if (counter >= 40) {
counter = 0;
}
-
+
}
- public boolean isFinished(){
+ public boolean isFinished() {
return false;
}
- public void end(boolean interrupted){
+ public void end(boolean interrupted) {
}
}
package frc.robot.commands.led_comm;
import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.PS5Controller;
-import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.RobotContainer;
-import frc.robot.constants.Constants;
-import frc.robot.controls.BaseDriverConfig;
-import frc.robot.controls.PS5ControllerDriverConfig;
import frc.robot.subsystems.LED.LED;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.util.Vision.Vision;
-import lib.controllers.PS5Controller.PS5Button;
public class LEDDefaultCommand extends Command {
private Vision vision;
private Drivetrain drivetrain;
private boolean allianceIsRed = DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
-
-
public LEDDefaultCommand(LED led) {
this.led = led;
// this.outtake = outtake;
@Override
public void execute() {
-
+
double matchTime = DriverStation.getMatchTime();
String gameData = DriverStation.getGameSpecificMessage();
- // if (vision.oneCameraDisconnected() || DriverStation.isJoystickConnected(Constants.DRIVER_JOY)) {
- // // flash if camera disconnected
- // led.setStrobeLights(255, 100, 0);
- // controller.endRumble();
- // } else
+ // if (vision.oneCameraDisconnected() ||
+ // DriverStation.isJoystickConnected(Constants.DRIVER_JOY)) {
+ // // flash if camera disconnected
+ // led.setStrobeLights(255, 100, 0);
+ // controller.endRumble();
+ // } else
if (fiveSecondsBeforeChange() && allianceIsRed) {
// blink alliance color and rumble if red alliance 5 seconds before hub shifts
led.setStrobeLights(255, 0, 0);
} else if (fiveSecondsBeforeChange()) {
// blink alliance color and rumble if blue alliance 5 seconds before hub shifts
led.setStrobeLights(0, 0, 255);
- } else
- if (playingDefense()) {
+ } else if (playingDefense()) {
new DefenseLightsCommand(led, 0, 67);
- } else
- if (DriverStation.isAutonomous() && allianceIsRed){
+ } else if (DriverStation.isAutonomous() && allianceIsRed) {
// Dimmer light for auto in red alliance
led.setLEDs(50, 0, 0);
- } else if (DriverStation.isAutonomous()){
+ } else if (DriverStation.isAutonomous()) {
// Dimmer light for auto in blue alliance
led.setLEDs(0, 0, 50);
- } else if ((allianceIsRed && gameData.equals("B") && matchTime <= 105 && matchTime >= 80) || (allianceIsRed && gameData.equals("B") && matchTime <= 55 && matchTime >= 30)) {
+ } else if ((allianceIsRed && gameData.equals("B") && matchTime <= 105 && matchTime >= 80)
+ || (allianceIsRed && gameData.equals("B") && matchTime <= 55 && matchTime >= 30)) {
// turn light off for inactive hub if red alliance and blue inactive first
led.setLEDs(0, 0, 0);
- } else if ((allianceIsRed && gameData.equals("R") && matchTime <= 130 && matchTime >= 105) || (allianceIsRed && gameData.equals("R") && matchTime <= 80 && matchTime >= 55)) {
+ } else if ((allianceIsRed && gameData.equals("R") && matchTime <= 130 && matchTime >= 105)
+ || (allianceIsRed && gameData.equals("R") && matchTime <= 80 && matchTime >= 55)) {
// turn light off for inactive hub if red alliance and red inactive first
led.setLEDs(0, 0, 0);
- } else if ((!allianceIsRed && gameData.equals("B") && matchTime <= 130 && matchTime >= 105) || (!allianceIsRed && gameData.equals("B") && matchTime <= 80 && matchTime >= 55)) {
+ } else if ((!allianceIsRed && gameData.equals("B") && matchTime <= 130 && matchTime >= 105)
+ || (!allianceIsRed && gameData.equals("B") && matchTime <= 80 && matchTime >= 55)) {
// turn off lights for inactive hub if blue alliance and blue inactive first
led.setLEDs(0, 0, 0);
- } else if ((!allianceIsRed && gameData.equals("R") && matchTime <= 105 && matchTime >= 80) || (!allianceIsRed && gameData.equals("R") && matchTime <= 55 && matchTime >= 30)) {
+ } else if ((!allianceIsRed && gameData.equals("R") && matchTime <= 105 && matchTime >= 80)
+ || (!allianceIsRed && gameData.equals("R") && matchTime <= 55 && matchTime >= 30)) {
// turn light off for inactive hub if blue alliance and red inactive first
led.setLEDs(0, 0, 0);
} else if (allianceIsRed) {
private boolean fiveSecondsBeforeChange() {
double time = DriverStation.getMatchTime();
- if((time <= 135 && time >= 130) || (time <= 110 && time >= 105) || (time <= 85 && time >= 80) || (time <= 60 && time >= 55) || (time <= 35 && time >= 30)){
+ if ((time <= 135 && time >= 130) || (time <= 110 && time >= 105) || (time <= 85 && time >= 80)
+ || (time <= 60 && time >= 55) || (time <= 35 && time >= 30)) {
return true;
}
return false;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.LED.LED;
-public class PaintCommand extends Command{
+public class PaintCommand extends Command {
private int start;
private int end;
private LED led;
- public PaintCommand(LED led, int start, int end){
+ public PaintCommand(LED led, int start, int end) {
this.led = led;
this.end = end;
this.start = start;
addRequirements(led);
}
- public void initialize(){
+
+ public void initialize() {
led.setSection(0, 0, 255, start, end);
}
- public void execute(){
+
+ public void execute() {
led.setTwoColorWave(30, 0, 80, 255, 255, 255);
}
- public boolean isFinished(){
+
+ public boolean isFinished() {
return false;
}
- public void end(boolean interrupted){
+
+ public void end(boolean interrupted) {
}
}
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.LED.LED;
-public class TurnOffLEDsCommand extends Command{
+public class TurnOffLEDsCommand extends Command {
private LED led;
- public TurnOffLEDsCommand(LED led){
+
+ public TurnOffLEDsCommand(LED led) {
this.led = led;
addRequirements(led);
}
- public void initialize(){
+
+ public void initialize() {
led.setLEDs(0, 0, 0);
}
- public void execute(){
+
+ public void execute() {
}
- public boolean isFinished(){
+
+ public boolean isFinished() {
return false;
}
- public void end(boolean interrupted){
+
+ public void end(boolean interrupted) {
}
}
import com.ctre.phoenix6.configs.LEDConfigs;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.SolidColor;
-import com.ctre.phoenix6.controls.StrobeAnimation;
import com.ctre.phoenix6.hardware.CANdle;
import com.ctre.phoenix6.signals.Enable5VRailValue;
import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue;
private double counter = 0;
private double waveOffset = 0;
- private final double waveSpeed = 0.08;
- private final double waveFrequency = 0.25;
+ private final double waveSpeed = 0.08;
+ private final double waveFrequency = 0.25;
private double period = 10;
private double midLine = 127.5;
public void setTwoColorWave(int r1, int g1, int b1, int r2, int g2, int b2) {
for (int i = 0; i < stripLength; i++) {
-
+
double wave = (Math.sin(i * waveFrequency + waveOffset) + 1) / 2.0;
- double inverseBias = 5; // higher = more color 2
+ double inverseBias = 5; // higher = more color 2
wave = 1 - Math.pow(1 - wave, inverseBias);
-
- int r = (int)(r1 * wave + r2 * (1 - wave));
- int g = (int)(g1 * wave + g2 * (1 - wave));
- int b = (int)(b1 * wave + b2 * (1 - wave));
+
+ int r = (int) (r1 * wave + r2 * (1 - wave));
+ int g = (int) (g1 * wave + g2 * (1 - wave));
+ int b = (int) (b1 * wave + b2 * (1 - wave));
setSection(r, g, b, i, i + 1);
}
-
+
waveOffset += waveSpeed;
}
// public void setTwoColorWave(int r1, int g1, int b1, int r2, int g2, int b2) {
- // for (int i = 0; i < stripLength; i++) {
-
- // double wave = 127.5 * Math.sin( ((2*Math.PI)/period) * (i + waveOffset)) + midLine;
- // double inverseBias = 5; // higher = more color 2
- // wave = 1 - Math.pow(1 - wave, inverseBias);
-
- // //int r = (int)(r1 * wave + r2 * (1 - wave));
- // //int g = (int)(g1 * wave + g2 * (1 - wave));
- // //int b = (int)(b1 * wave + b2 * (1 - wave));
- // // int b = (int)wave;
- // int b = 255;
- // int g = (int)(127.5 * Math.sin( ((2*Math.PI)/period) * (i + waveOffset + 5)) + midLine);
- // //int g = r;
-
- // setSection(0, g, b, i, i + 1);
- // }
-
- // waveOffset += waveSpeed;
+ // for (int i = 0; i < stripLength; i++) {
+
+ // double wave = 127.5 * Math.sin( ((2*Math.PI)/period) * (i + waveOffset)) +
+ // midLine;
+ // double inverseBias = 5; // higher = more color 2
+ // wave = 1 - Math.pow(1 - wave, inverseBias);
+
+ // //int r = (int)(r1 * wave + r2 * (1 - wave));
+ // //int g = (int)(g1 * wave + g2 * (1 - wave));
+ // //int b = (int)(b1 * wave + b2 * (1 - wave));
+ // // int b = (int)wave;
+ // int b = 255;
+ // int g = (int)(127.5 * Math.sin( ((2*Math.PI)/period) * (i + waveOffset + 5))
+ // + midLine);
+ // //int g = r;
+
+ // setSection(0, g, b, i, i + 1);
+ // }
+
+ // waveOffset += waveSpeed;
// }
/**