diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 92b6186..55b541a 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + /** * This class is the glue that binds the controls on the physical operator * interface to the code that allow control of the robot. @@ -36,6 +37,9 @@ public class OI extends Procedure { private static final double HighConeArm1 = 0; private static final double CHA2 = 0; private static final double CMA1 = 0; + + private int state = 0; + private boolean ignoreState = false; // enum generalControl{ // CONE_HIGH_NODE, // CUBE_HIGH_NODE, @@ -66,11 +70,55 @@ public void run(Context context) { context.takeOwnership(Robot.drive); // context.takeOwnership(Robot.intake); context.takeOwnership(Robot.gyro); + context.takeOwnership(Robot.lights); while (true) { context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); RobotProvider.instance.refreshDriverStationData(); + //TODO: ADD CODE DEPENDING ON WHAT THE STATE SHOULD BE FOR WHICH BUTTONS + if(DriverStation.getMatchTime() < 30 && DriverStation.getMatchTime() > 29){ + Robot.lights.rainbow(); + log("" + DriverStation.getMatchTime()); + ignoreState = true; + } + + if(DriverStation.getMatchTime() <29 && DriverStation.getMatchTime() > 28.7){ + Robot.lights.clearAnimation(); + ignoreState = false; + } + switch (state){ + case 1: + if(ignoreState){ break;} + Robot.lights.signalCube(); + break; + case 2: + if(ignoreState){ break;} + Robot.lights.signalCone(); + break; + case 3: + if(ignoreState){ break;} + Robot.lights.rainbow(); + break; + case 4: + if(ignoreState){ break;} + Robot.lights.hybridScore(); + break; + case 5: + if(ignoreState){ break;} + Robot.lights.midScore(); + break; + case 6: + if(ignoreState){ break;} + Robot.lights.highScore(); + break; + default: + if(!ignoreState){ + Robot.lights.resetLights(); + } + } + + // Add driver controls here - make sure to take/release ownership // of mechanisms when appropriate. @@ -87,6 +135,9 @@ public void run(Context context) { // SmartDashboard.putString("Alliance", "NULLLLLLLLL"); // } + if(DriverStation.isTeleop() && DriverStation.getMatchTime() < 30){ + Robot.lights.signalMalfunction(); + } if (leftJoystick.getButtonPressed(InputConstants.RESET_GYRO)) { Robot.gyro.resetGyro(); diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index baa970e..9bd0d5a 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -7,11 +7,13 @@ public class Robot { // public static Intake intake; public static Drive drive; public static Gyro gyro; + public static Lights lights; public static void robotInit() { // Initialize mechanisms here // intake = new Intake(); drive = new Drive(); gyro = new Gyro(); + lights = new Lights(); } } diff --git a/src/main/java/com/team766/robot/mechanisms/Lights.java b/src/main/java/com/team766/robot/mechanisms/Lights.java index 4e83f45..1c0af62 100644 --- a/src/main/java/com/team766/robot/mechanisms/Lights.java +++ b/src/main/java/com/team766/robot/mechanisms/Lights.java @@ -1,30 +1,185 @@ package com.team766.robot.mechanisms; +import org.apache.commons.math3.analysis.function.Ceil; +import org.apache.commons.math3.stat.correlation.StorelessCovariance; import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.StrobeAnimation; +import com.ctre.phoenix.led.TwinkleAnimation; +import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent; import com.team766.framework.Mechanism; +import edu.wpi.first.wpilibj.DriverStation; public class Lights extends Mechanism{ private CANdle candle; private static final int CANID = 5; + private int numLEDs = 42; + RainbowAnimation rainbowAnim = new RainbowAnimation(1, 3, numLEDs); + + int curAnimation = -1; public Lights(){ candle = new CANdle(CANID); + } + public void setNumLEDs(int num){ + checkContextOwnership(); + numLEDs = num; + rainbowAnim.setNumLed(num); } - public void purple(){ + public void signalCube(){ checkContextOwnership(); - candle.setLEDs(128, 0, 128); + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(142, 38, 252); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(142, 38, 252); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(142, 38, 252); + } else { + candle.setLEDs(0, 0, 0); + } + } + } } - public void white(){ + public void resetLights(){ checkContextOwnership(); candle.setLEDs(255, 255, 255); } - public void yellow(){ + public void signalCone(){ + checkContextOwnership(); + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(250, 196, 32); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(250, 196, 32); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(250, 196, 32); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + + } + + public void auton(){ + candle.setLEDs(0, 223, 247); + } + + public void signalMalfunction(){ + checkContextOwnership(); + candle.setLEDs(255, 0, 0); + } + + public void rainbow(){ + checkContextOwnership(); + candle.clearAnimation(curAnimation); + candle.animate(rainbowAnim,0); + curAnimation = 0; + + } + + public void clearAnimation(){ checkContextOwnership(); - candle.setLEDs(255, 255, 0); + if(curAnimation != -1){ + candle.clearAnimation(curAnimation); + curAnimation = -1; + } + + + } + + public void hybridScore(){ + checkContextOwnership(); + + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(165, 128, 65); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(165, 128, 65); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(165, 128, 65); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + } + + //color of justin and kapils shirts + public void midScore(){ + checkContextOwnership(); + + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(81,102,52); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(81,102,52); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(81,102,52); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + } + + public void highScore(){ + checkContextOwnership(); + + if(DriverStation.getMatchTime() > 30){ + if(curAnimation != -1){candle.clearAnimation(curAnimation);} + candle.setLEDs(202, 39, 75); + } else { + candle.clearAnimation(curAnimation); + if(DriverStation.getMatchTime() > 15){ + if((int) (DriverStation.getMatchTime() * 2) % 2 == 0){ + candle.setLEDs(202, 39, 75); + } else { + candle.setLEDs(0, 0, 0); + } + } else { + if((int) (DriverStation.getMatchTime() * 4) % 2 == 0){ + candle.setLEDs(202, 39, 75); + } else { + candle.setLEDs(0, 0, 0); + } + } + } + } + } diff --git a/src/main/java/com/team766/robot/procedures/AutonLED.java b/src/main/java/com/team766/robot/procedures/AutonLED.java new file mode 100644 index 0000000..f3c699d --- /dev/null +++ b/src/main/java/com/team766/robot/procedures/AutonLED.java @@ -0,0 +1,14 @@ +package com.team766.robot.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Robot; + +public class AutonLED extends Procedure { + public void run (Context context) { + context.takeOwnership(Robot.lights); + Robot.lights.clearAnimation(); + Robot.lights.auton(); + } + +}