diff --git a/src/main/java/team1403/robot/Constants.java b/src/main/java/team1403/robot/Constants.java index d0e5b53..d95771b 100644 --- a/src/main/java/team1403/robot/Constants.java +++ b/src/main/java/team1403/robot/Constants.java @@ -203,4 +203,9 @@ public static class AlgaeWrist { public static final double Ki = 0.0; public static final double Kd = 0.0; } + + public static class LED { + public static final double speed = 0.7; + public static final int kLedCount = 0; //Placeholder + } } \ No newline at end of file diff --git a/src/main/java/team1403/robot/RobotContainer.java b/src/main/java/team1403/robot/RobotContainer.java index 2a4e9bd..871dffc 100644 --- a/src/main/java/team1403/robot/RobotContainer.java +++ b/src/main/java/team1403/robot/RobotContainer.java @@ -337,7 +337,7 @@ private void configureBindings() { ).withTimeout(2)); */ m_coralIntake.setDefaultCommand(new DefaultIntakeCommand(m_coralIntake)); - m_led.setDefaultCommand(new LightCommand(m_led)); + m_led.setDefaultCommand(new LightCommand(m_led, m_elevator).ignoringDisable(true)); // m_algaeIntake.setDefaultCommand(new DefaultAlgaeIntakeCommand(m_algaeIntake)); // coral intake diff --git a/src/main/java/team1403/robot/commands/LightCommand.java b/src/main/java/team1403/robot/commands/LightCommand.java index 89ace47..13d06b2 100644 --- a/src/main/java/team1403/robot/commands/LightCommand.java +++ b/src/main/java/team1403/robot/commands/LightCommand.java @@ -1,21 +1,26 @@ package team1403.robot.commands; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import team1403.robot.subsystems.Blackbox; +import team1403.robot.subsystems.ElevatorSubsystem; import team1403.robot.subsystems.LEDSubsystem; +import team1403.robot.subsystems.LEDSubsystem.LEDConfig; public class LightCommand extends Command{ //Variable for local copy of LED subsystem private LEDSubsystem m_LED; + private ElevatorSubsystem m_elevator; /*** * Runs the lights based of robot state * * @param LED LED subsystem */ - public LightCommand(LEDSubsystem LED) { + public LightCommand(LEDSubsystem LED, ElevatorSubsystem elevator) { m_LED = LED; + m_elevator = elevator; addRequirements(m_LED); } @@ -24,28 +29,49 @@ public LightCommand(LEDSubsystem LED) { public void initialize() {} @Override + //Runs every 20 milliseconds public void execute() { - switch(Blackbox.robotState){ - case loading: - m_LED.setLEDcolor(LEDSubsystem.Color.Yellow); - break; - case driving: - m_LED.setLEDcolor(LEDSubsystem.Color.Green); - break; - case aligning: - m_LED.setLEDcolor(LEDSubsystem.Color.Blue); - break; - case placing: - m_LED.setLEDcolor(LEDSubsystem.Color.Pink); - break; - case exiting: - m_LED.setLEDcolor(LEDSubsystem.Color.Red); - break; - case ManualElevator: - m_LED.setLEDcolor(LEDSubsystem.Color.White); - break; + if(true){ + if(!m_elevator.isAtSetpoint()) { + if(m_elevator.isGoingUp){ + m_LED.setLEDcolor(LEDConfig.Style.Upwards, LEDConfig.Color.Green); + } + else if(!m_elevator.isGoingUp) { + m_LED.setLEDcolor(LEDConfig.Style.Downwards, LEDConfig.Color.Grey); + } } + else if(DriverStation.isDisabled()) { + m_LED.setLEDcolor(LEDConfig.Style.Strobe, LEDConfig.Color.White); + } + + else if(DriverStation.isEStopped()) { + m_LED.setLEDcolor(LEDConfig.Style.Strobe, LEDConfig.Color.Red); + } + + else { + switch(Blackbox.robotState){ + case loading: + m_LED.setLEDcolor(LEDConfig.Color.Yellow); + break; + case driving: + m_LED.setLEDcolor(LEDConfig.Color.Green); + break; + case aligning: + m_LED.setLEDcolor(LEDConfig.Color.Blue); + break; + case placing: + m_LED.setLEDcolor(LEDConfig.Color.Pink); + break; + case exiting: + m_LED.setLEDcolor(LEDConfig.Color.Red); + break; + case ManualElevator: + m_LED.setLEDcolor(LEDConfig.Color.White); + break; + } + } + } } @Override diff --git a/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java b/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java index 61cfb2c..d380d40 100644 --- a/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java @@ -27,199 +27,199 @@ public class ElevatorSubsystem extends SubsystemBase { private SparkMax m_rightMotor; private ElevatorFeedforward m_ElevatorFeedforward; private double currentPos; - private double currMotorOutput; - private double desiredMotorOutput; - private boolean isRampDone; - private boolean isGoingUp; - private boolean isGoingDown; - private boolean directionFlag; - private double posError; - private double setpoint; - private double minSpeed; - private double maxSpeed; - - private SysIdRoutine m_sysIdRoutine; - - public ElevatorSubsystem() { - m_leftMotor = new SparkMax(Constants.CanBus.leftElevatorMotorID, MotorType.kBrushless); - m_rightMotor = new SparkMax(Constants.CanBus.rightElevatorMotorID, MotorType.kBrushless); - configMotors(); - - m_ElevatorFeedforward = new ElevatorFeedforward(0, Constants.Elevator.kFeedforwardG, Constants.Elevator.kFeedforwardV, 0, Constants.kLoopTime); - - m_sysIdRoutine = new SysIdRoutine(new SysIdRoutine.Config(null, null, null, - (state) -> Logger.recordOutput("SysIDElevatorFeedforward", state.toString())), - new SysIdRoutine.Mechanism((voltage) -> { - m_rightMotor.setVoltage(voltage.in(Volts)); - }, null, this)); - -} - - private void configMotors() { - SparkMaxConfig leftconfig = new SparkMaxConfig(); - leftconfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(45) - .voltageCompensation(12) - .follow(m_rightMotor, true); - SparkMaxConfig rightconfig = new SparkMaxConfig(); - rightconfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(45) - .voltageCompensation(12) - .inverted(true); - - m_leftMotor.configure(leftconfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - m_rightMotor.configure(rightconfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - public void setMotorSpeed(double speed) { - m_rightMotor.set(MathUtil.clamp(speed, -1.0, 1.0)); - } - - public void stopMotors() { - setMotorSpeed(0); - } - - public double getSpeed() { - return m_rightMotor.getEncoder().getVelocity(); - } - - public double getPosition() { - return m_rightMotor.getEncoder().getPosition(); - } - - public double FFcalculation() { - return m_ElevatorFeedforward.calculate(0); - } - - public Command getSysIDQ(SysIdRoutine.Direction dir) { - return m_sysIdRoutine.quasistatic(dir); - } - - public Command getSysIDD(SysIdRoutine.Direction dir) { - return m_sysIdRoutine.dynamic(dir); - } - - public void moveToSetpoint(double setPoint) { - //setpoint = setPoint; - safeMove(setPoint); - } - - public void safeMove(double target) { - if(Blackbox.isElevatorSafe()) - setpoint = target; - setpoint = Math.max(setpoint, Constants.Elevator.Setpoints.Min); - } - - public double getSetpoint() { - return setpoint; - } - - //check whether component is moving up or down - private void checkDirection(double setPoint) { - if (currentPos < 1.0) { - isGoingUp = false; - isGoingDown = false; - } - else if(setPoint > currentPos + Constants.Elevator.Command.setPointMargin) { - isGoingUp = true; - isGoingDown = false; - } - else if(setPoint < currentPos - Constants.Elevator.Command.setPointMargin) { - isGoingUp = false; - isGoingDown = true; - } - else { - isGoingUp = false; - isGoingDown = false; - } - } - - private double getDesiredOutput(double setPoint) { - // set desired motor output equal to the difference between current position and setpoint * a gain constant - posError = setPoint - currentPos; - if(isGoingUp) { - minSpeed = Constants.Elevator.Command.upMinSpeed; - maxSpeed = Constants.Elevator.Command.upMaxSpeed; - posError *= Constants.Elevator.Command.movementUpGain; - } - else if(isGoingDown){ - minSpeed = Constants.Elevator.Command.downMinSpeed; - maxSpeed = Constants.Elevator.Command.downMaxSpeed; - posError *= Constants.Elevator.Command.movementDownGain; - } - double desiredOutput = posError; - // checks conditions that don't require any output by the motor - if(!isGoingUp && !isGoingDown || (isGoingUp && desiredOutput < 0) || (isGoingDown && desiredOutput > 0)) { - desiredOutput = 0; - } - // clamp desired motor output to a maximum value - desiredOutput = Math.abs(desiredOutput); - if(desiredOutput > maxSpeed) { - desiredOutput = maxSpeed; - } - return desiredOutput; - } - - // ramp function gradually brings up the output of the motor to the desired motor output - private double ramp(double rampUpTime, double rampDownTime, double currentOutput, double desiredOutput) { - // if desired output is greater than current output, run upwards ramp function - if(desiredOutput > currentOutput) { - // increment current output by 100/rampUpTime/cycle rate and if that output is less than desired - if((currentOutput + (100/(rampUpTime/0.02)) < desiredOutput)) { - currentOutput += (100/(rampUpTime/0.02)); - } - // set current output to desired output - else { - currentOutput = desiredOutput; - } + private double currMotorOutput; + private double desiredMotorOutput; + private boolean isRampDone; + public boolean isGoingUp; + private boolean isGoingDown; + private boolean directionFlag; + private double posError; + private double setpoint; + private double minSpeed; + private double maxSpeed; + + private SysIdRoutine m_sysIdRoutine; + + public ElevatorSubsystem() { + m_leftMotor = new SparkMax(Constants.CanBus.leftElevatorMotorID, MotorType.kBrushless); + m_rightMotor = new SparkMax(Constants.CanBus.rightElevatorMotorID, MotorType.kBrushless); + configMotors(); + + m_ElevatorFeedforward = new ElevatorFeedforward(0, Constants.Elevator.kFeedforwardG, Constants.Elevator.kFeedforwardV, 0, Constants.kLoopTime); + + m_sysIdRoutine = new SysIdRoutine(new SysIdRoutine.Config(null, null, null, + (state) -> Logger.recordOutput("SysIDElevatorFeedforward", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> { + m_rightMotor.setVoltage(voltage.in(Volts)); + }, null, this)); + + } + + private void configMotors() { + SparkMaxConfig leftconfig = new SparkMaxConfig(); + leftconfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(45) + .voltageCompensation(12) + .follow(m_rightMotor, true); + SparkMaxConfig rightconfig = new SparkMaxConfig(); + rightconfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(45) + .voltageCompensation(12) + .inverted(true); + + m_leftMotor.configure(leftconfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + m_rightMotor.configure(rightconfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void setMotorSpeed(double speed) { + m_rightMotor.set(MathUtil.clamp(speed, -1.0, 1.0)); } - // if desired output is less than current output, run downwards ramp function - else if(desiredOutput < currentOutput) { - if((currentOutput - (100/(rampDownTime/0.02)) > desiredOutput)) { - // decrement current output by 100/rampDownTime/cycle rate and if that output is less than desired - currentOutput -= (100/(rampDownTime/0.02)); - } - else { - currentOutput = desiredOutput; - } + + public void stopMotors() { + setMotorSpeed(0); } - // if current motor output reaches the desired output set isRampDone to true - if(desiredOutput == currentOutput) { - isRampDone = true; + + public double getSpeed() { + return m_rightMotor.getEncoder().getVelocity(); } - // returns our current output - return currentOutput; - } - - private void adjustCurrentOutput() { - // once ramp function is done and the elevator is moving up or down, set velocity to a minimum value - if((isGoingUp || isGoingDown) && isRampDone && currMotorOutput < minSpeed) { - currMotorOutput = minSpeed; + + public double getPosition() { + return m_rightMotor.getEncoder().getPosition(); } - // invert output if elevator is moving down - if(isGoingDown) { - currMotorOutput *= -1; + + public double FFcalculation() { + return m_ElevatorFeedforward.calculate(0); } - } - - private void checkIfReachedSetPoint(double setPoint) { - // if elevator is within the window of the setpoint, stop the motor from running and set booleans to false - if(currentPos > setPoint - Constants.Elevator.Command.setPointMargin && currentPos < setPoint + Constants.Elevator.Command.setPointMargin) { - currMotorOutput = 0; - isGoingUp = false; - isGoingDown = false; - directionFlag = true; + + public Command getSysIDQ(SysIdRoutine.Direction dir) { + return m_sysIdRoutine.quasistatic(dir); } - // else set isRampDone to false and continue the following steps above - else { - isRampDone = false; + + public Command getSysIDD(SysIdRoutine.Direction dir) { + return m_sysIdRoutine.dynamic(dir); } - } - - public boolean isAtSetpoint() { - return Math.abs(setpoint - currentPos) <= 3; + + public void moveToSetpoint(double setPoint) { + //setpoint = setPoint; + safeMove(setPoint); + } + + public void safeMove(double target) { + if(Blackbox.isElevatorSafe()) + setpoint = target; + setpoint = Math.max(setpoint, Constants.Elevator.Setpoints.Min); + } + + public double getSetpoint() { + return setpoint; + } + + //check whether component is moving up or down + private void checkDirection(double setPoint) { + if (currentPos < 1.0) { + isGoingUp = false; + isGoingDown = false; + } + else if(setPoint > currentPos + Constants.Elevator.Command.setPointMargin) { + isGoingUp = true; + isGoingDown = false; + } + else if(setPoint < currentPos - Constants.Elevator.Command.setPointMargin) { + isGoingUp = false; + isGoingDown = true; + } + else { + isGoingUp = false; + isGoingDown = false; + } + } + + private double getDesiredOutput(double setPoint) { + // set desired motor output equal to the difference between current position and setpoint * a gain constant + posError = setPoint - currentPos; + if(isGoingUp) { + minSpeed = Constants.Elevator.Command.upMinSpeed; + maxSpeed = Constants.Elevator.Command.upMaxSpeed; + posError *= Constants.Elevator.Command.movementUpGain; + } + else if(isGoingDown){ + minSpeed = Constants.Elevator.Command.downMinSpeed; + maxSpeed = Constants.Elevator.Command.downMaxSpeed; + posError *= Constants.Elevator.Command.movementDownGain; + } + double desiredOutput = posError; + // checks conditions that don't require any output by the motor + if(!isGoingUp && !isGoingDown || (isGoingUp && desiredOutput < 0) || (isGoingDown && desiredOutput > 0)) { + desiredOutput = 0; + } + // clamp desired motor output to a maximum value + desiredOutput = Math.abs(desiredOutput); + if(desiredOutput > maxSpeed) { + desiredOutput = maxSpeed; + } + return desiredOutput; + } + + // ramp function gradually brings up the output of the motor to the desired motor output + private double ramp(double rampUpTime, double rampDownTime, double currentOutput, double desiredOutput) { + // if desired output is greater than current output, run upwards ramp function + if(desiredOutput > currentOutput) { + // increment current output by 100/rampUpTime/cycle rate and if that output is less than desired + if((currentOutput + (100/(rampUpTime/0.02)) < desiredOutput)) { + currentOutput += (100/(rampUpTime/0.02)); + } + // set current output to desired output + else { + currentOutput = desiredOutput; + } + } + // if desired output is less than current output, run downwards ramp function + else if(desiredOutput < currentOutput) { + if((currentOutput - (100/(rampDownTime/0.02)) > desiredOutput)) { + // decrement current output by 100/rampDownTime/cycle rate and if that output is less than desired + currentOutput -= (100/(rampDownTime/0.02)); + } + else { + currentOutput = desiredOutput; + } + } + // if current motor output reaches the desired output set isRampDone to true + if(desiredOutput == currentOutput) { + isRampDone = true; + } + // returns our current output + return currentOutput; + } + + private void adjustCurrentOutput() { + // once ramp function is done and the elevator is moving up or down, set velocity to a minimum value + if((isGoingUp || isGoingDown) && isRampDone && currMotorOutput < minSpeed) { + currMotorOutput = minSpeed; + } + // invert output if elevator is moving down + if(isGoingDown) { + currMotorOutput *= -1; + } + } + + private void checkIfReachedSetPoint(double setPoint) { + // if elevator is within the window of the setpoint, stop the motor from running and set booleans to false + if(currentPos > setPoint - Constants.Elevator.Command.setPointMargin && currentPos < setPoint + Constants.Elevator.Command.setPointMargin) { + currMotorOutput = 0; + isGoingUp = false; + isGoingDown = false; + directionFlag = true; + } + // else set isRampDone to false and continue the following steps above + else { + isRampDone = false; + } + } + + public boolean isAtSetpoint() { + return Math.abs(setpoint - currentPos) <= 3; } public void periodic() { diff --git a/src/main/java/team1403/robot/subsystems/LEDSubsystem.java b/src/main/java/team1403/robot/subsystems/LEDSubsystem.java index 6525e79..61fc623 100644 --- a/src/main/java/team1403/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/LEDSubsystem.java @@ -1,113 +1,124 @@ package team1403.robot.subsystems; -import com.ctre.phoenix.led.CANdle; -import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.*; +import com.ctre.phoenix.led.CANdle.LEDStripType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import team1403.robot.Constants; public class LEDSubsystem extends SubsystemBase { - private final CANdle m_candle; - - public enum Color { - Green, - Red, - Off, - Blue, - Grey, - Pink, - White, - Yellow, - Brown, - Orange, - Purple + private final int m_ledCount = Constants.LED.kLedCount; + private final double m_animSpeed = Constants.LED.speed; + + public enum LEDConfig { + ; //It just works don't question it + public enum Style { + Solid, + Rainbow, + Strobe, + Upwards, + Downwards + } + + public enum Color { + Green(0, 255, 0), + Red(255, 0, 0), + Blue(0, 0, 255), + Grey(128, 128, 128), + Pink(255, 182, 193), + White(255, 255, 255), + Yellow(255, 255, 0), + Brown(139, 69, 19), + Orange(255, 165, 0), + Purple(128, 0, 128), + Off(0,0,0); + + private final int red; + private final int green; + private final int blue; + + Color(int red, int green, int blue) { + this.red = red; + this.green = green; + this.blue = blue; + } + + public int getRed() { + return red; + } + + public int getGreen() { + return green; + } + + public int getBlue() { + return blue; + } + } } public LEDSubsystem() { m_candle = new CANdle(Constants.CanBus.kCandleID); CANdleConfiguration config = new CANdleConfiguration(); + config.stripType = LEDStripType.RGB; + config.brightnessScalar = 1; m_candle.configAllSettings(config); + m_candle.setLEDs(0, 0, 0); } - public void setLEDcolor(Color color) { - switch(color) { - case Green: - m_candle.setLEDs(0, 255, 0); - break; - case Red: - m_candle.setLEDs(255, 0, 0); - break; - case Off: + public void setLEDcolor(LEDConfig.Color color) { + setLEDcolor(LEDConfig.Style.Solid, color); + } + + + public void setLEDcolor(LEDConfig.Style style, LEDConfig.Color primary) { + if (primary == null) primary = LEDConfig.Color.Off; + + int P_red = primary.getRed(); + int P_green = primary.getGreen(); + int P_blue = primary.getBlue(); + + switch(style) { + case Solid: + m_candle.setLEDs(P_red, P_green, P_blue); + break; + + case Rainbow: + m_candle.animate(new RainbowAnimation(1.0, m_animSpeed, m_ledCount)); + break; + + case Strobe: + m_candle.animate(new LarsonAnimation(P_red, P_green, P_blue, 0, + m_animSpeed, + m_ledCount, + LarsonAnimation.BounceMode.Center, 7)); + break; + + case Upwards: + m_candle.animate(new ColorFlowAnimation(P_red, P_green, P_blue, 0, + m_animSpeed, + m_ledCount, + ColorFlowAnimation.Direction.Forward)); + break; + + case Downwards: + m_candle.animate(new ColorFlowAnimation(P_red, P_green, P_blue, 0, + m_animSpeed, + m_ledCount, + ColorFlowAnimation.Direction.Backward)); + break; + + default: m_candle.setLEDs(0, 0, 0); break; - case Blue: - m_candle.setLEDs(0, 0, 255); - break; - case Grey: - m_candle.setLEDs(128, 128, 128); - break; - case Pink: - m_candle.setLEDs(255, 182, 193); - break; - case White: - m_candle.setLEDs(255, 255, 255); - break; - case Yellow: - m_candle.setLEDs(255, 255, 0); - break; - case Brown: - m_candle.setLEDs(139, 69, 19); - break; - case Orange: - m_candle.setLEDs(255, 165, 0); - break; - case Purple: - m_candle.setLEDs(128, 0, 128); - break; } } - public void setLEDcolor(Color color, int startIndex, int count) { - switch(color) { - case Green: - m_candle.setLEDs(0, 255, 0, 0, startIndex, count); // RGB for Green - break; - case Red: - m_candle.setLEDs(255, 0, 0, 0, startIndex, count); // RGB for Red - break; - case Off: - m_candle.setLEDs(0, 0, 0, 0, startIndex, count); // RGB for Black (off) - break; - case Blue: - m_candle.setLEDs(0, 0, 255, 0, startIndex, count); // RGB for Blue - break; - case Grey: - m_candle.setLEDs(128, 128, 128, 0, startIndex, count); // RGB for Grey - break; - case Pink: - m_candle.setLEDs(255, 182, 193, 0, startIndex, count); // RGB for Pink - break; - case White: - m_candle.setLEDs(255, 255, 255, 0, startIndex, count); // RGB for White - break; - case Yellow: - m_candle.setLEDs(255, 255, 0, 0, startIndex, count); // RGB for Yellow - break; - case Brown: - m_candle.setLEDs(139, 69, 19, 0, startIndex, count); // RGB for Brown - break; - case Orange: - m_candle.setLEDs(255, 165, 0, 0, startIndex, count); // RGB for Orange - break; - case Purple: - m_candle.setLEDs(128, 0, 128, 0, startIndex, count); // RGB for Purple - break; - } + public void setBrightness(double brightness) { + m_candle.configBrightnessScalar(brightness); } - - @Override public void periodic() {} }