import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.LED.LED;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.outtake.Outtake;
+// import frc.robot.subsystems.outtake.Outtake; // TODO: Outtake subsystem not implemented on current robot
import frc.robot.util.Vision.Vision;
public class LEDDefaultCommand extends Command {
private Vision vision;
private LED led;
- private Outtake outtake;
+ // private Outtake outtake;
private Drivetrain drivetrain;
- //TODO: change this to actual climb coordinate
+ // TODO: change this to actual climb coordinate
private double climbYCoordinate = 1.08;
private boolean allianceIsRed = DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
- public LEDDefaultCommand(LED led, Outtake outtake, Drivetrain drivetrain, Vision vision){
+ public LEDDefaultCommand(LED led, Drivetrain drivetrain, Vision vision) {
this.led = led;
- this.outtake = outtake;
+ // this.outtake = outtake; // TODO: Outtake subsystem not yet implemented
this.drivetrain = drivetrain;
this.vision = vision;
}
@Override
- public void execute(){
- if (climbAligned()){
- //When aligned to climb
+ public void execute() {
+ if (climbAligned()) {
+ // When aligned to climb
led.setTwoColorWave(255, 0, 100, 100, 0, 255);
- }else if(vision.oneCameraDisconnected()){
- //flash if camera disconnected
+ } else if (vision.oneCameraDisconnected()) {
+ // flash if camera disconnected
led.setStrobeLights(255, 0, 0);
- }else if (playingDefense()){
- //When playing defense
+ } else if (playingDefense()) {
+ // When playing defense
led.defenseLights();
}
- else if (outtake.coralLoaded()){
- //When coral detected
- led.setLEDs(0, 255, 0);
- }else if(allianceIsRed){
- //Red alliance
+
+ // else if (outtake.coralLoaded()){
+ // //When coral detected
+ // led.setLEDs(0, 255, 0);
+ // }
+ else if (allianceIsRed) {
+ // Red alliance
led.setTwoColorWave(255, 0, 0, 255, 255, 255);
- }else{
- //Blue alliance
+ } else {
+ // Blue alliance
led.setTwoColorWave(0, 0, 255, 255, 255, 255);
}
}
- private boolean climbAligned(){
+ private boolean climbAligned() {
double yCoordinate = drivetrain.getPose().getY();
return Math.abs(yCoordinate - climbYCoordinate) < 0.03;
}
- private boolean playingDefense(){
+ private boolean playingDefense() {
double xCoordinate = drivetrain.getPose().getX();
double xCoordinateHalfway = 50;
- if (allianceIsRed){
+ if (allianceIsRed) {
return xCoordinate > xCoordinateHalfway;
- }
- else if (!allianceIsRed) {
+ } else if (!allianceIsRed) {
return xCoordinate < xCoordinateHalfway;
}
return false;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.LED.LED;
-import frc.robot.subsystems.LaserCAN.Sensor;
+// import frc.robot.subsystems.LaserCAN.Sensor; // TODO: LaserCAN Sensor subsystem not implemented on current robot
-public class LEDSensorCommand extends Command{
+public class LEDSensorCommand extends Command {
private LED led;
- private Sensor sensor;
+ // private Sensor sensor;
private int alliance = 0;
- public LEDSensorCommand(LED led, Sensor sensor, int alliance){
+ public LEDSensorCommand(LED led, int alliance) {
this.led = led;
- this.sensor = sensor;
+ // this.sensor = sensor;
- addRequirements(led, sensor);
+ addRequirements(led);
}
@Override
public void execute() {
- if (sensor.detected()){
- led.setLEDs(0, 255, 0);
- }
- else if(alliance == 0){
+ // if (sensor.detected()){
+ // led.setLEDs(0, 255, 0);
+ // }
+ // else
+ if (alliance == 0) {
led.setLEDs(0, 0, 255);
- }else{
+ } else {
led.setLEDs(255, 0, 0);
}
}