From 7fd6e776b5e043bfb5d791be433e025f958f665d Mon Sep 17 00:00:00 2001 From: Aarav Date: Thu, 7 Mar 2024 19:40:40 -0500 Subject: [PATCH 01/12] Added LED control --- .../commands/lights/FlashLEDCommand.java | 58 +++++++++ .../frc/robot/subsystems/LightControl.java | 52 +++++++++ .../java/frc/utils/BlinkinController.java | 16 +++ src/main/java/frc/utils/BlinkinState.java | 110 ++++++++++++++++++ 4 files changed, 236 insertions(+) create mode 100644 src/main/java/frc/robot/commands/lights/FlashLEDCommand.java create mode 100644 src/main/java/frc/robot/subsystems/LightControl.java create mode 100644 src/main/java/frc/utils/BlinkinController.java create mode 100644 src/main/java/frc/utils/BlinkinState.java diff --git a/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java b/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java new file mode 100644 index 0000000..007c73d --- /dev/null +++ b/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java @@ -0,0 +1,58 @@ +package frc.robot.commands.lights; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.utils.BlinkinState; +import frc.robot.subsystems.LightControl; + +public class FlashLEDCommand extends CommandBase { + + private LightControl blinkin; + private final BlinkinState flashPattern; + private final BlinkinState holdPattern; + private int flashTimeMillis; + private int timesToFlash; + + private int loops; + private long timer; + + public FlashLEDCommand(LightControl blinkin, BlinkinState flashPattern, BlinkinState holdPattern, int flashTimeMillis, int timesToFlash) { + this.blinkin = blinkin; + this.flashPattern = flashPattern; + this.holdPattern = holdPattern; + this.flashTimeMillis = flashTimeMillis; + this.timesToFlash = timesToFlash; + + addRequirements(blinkin); + } + + @Override + public void initialize() { + loops = 0; + timer = 0; + } + + @Override + public void execute() { + if (!blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { + timer = System.currentTimeMillis(); + blinkin.setPattern(flashPattern); + loops++; + } else if (blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { + timer = System.currentTimeMillis(); + blinkin.turnOff(); + } + if (loops > timesToFlash) blinkin.setPattern(holdPattern); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LightControl.java b/src/main/java/frc/robot/subsystems/LightControl.java new file mode 100644 index 0000000..9f2136e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LightControl.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; +import java.util.regex.Pattern; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.commands.lights.FlashLEDCommand; +import frc.utils.BlinkinState; +import frc.utils.BlinkinController; + +public class LightControl extends SubsystemBase { + private BlinkinController blinkin; + private BlinkinState pattern; + + public LightControl(int blinkinPort) { + this.blinkin = new BlinkinController(blinkinPort); + } + + public void turnOff() { setPattern(BlinkinState.Solid_Colors_Black); } + + public void setPattern(BlinkinState pattern) { + this.pattern = pattern; + } + + private double getSparkValue() { + if(pattern == null) { + return BlinkinState.Color_1_Pattern_Breath_Slow.sparkValue; + } + + return pattern.sparkValue; + } + + public boolean isActive() { return pattern != BlinkinState.Solid_Colors_Black; } + + @Override + public void periodic() { + double powerOutput = getSparkValue(); + blinkin.setPWM(powerOutput); + } + + public Command getSetLedCommand(Supplier pattern) { + return runOnce(() -> { + this.setPattern(pattern.get()); + }); + } + + public Command getSetLedCommand(BlinkinState state) { + return this.getSetLedCommand(() -> state); + } +} \ No newline at end of file diff --git a/src/main/java/frc/utils/BlinkinController.java b/src/main/java/frc/utils/BlinkinController.java new file mode 100644 index 0000000..b7c20ad --- /dev/null +++ b/src/main/java/frc/utils/BlinkinController.java @@ -0,0 +1,16 @@ +package frc.utils; + +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.motorcontrol.Spark; + +public class BlinkinController { + private Spark pwm; + + public BlinkinController(int pwmPort) { + pwm = new Spark(pwmPort); + } + + public void setPWM(double pwmValue) { + this.pwm.set(pwmValue); + } +} \ No newline at end of file diff --git a/src/main/java/frc/utils/BlinkinState.java b/src/main/java/frc/utils/BlinkinState.java new file mode 100644 index 0000000..37636f9 --- /dev/null +++ b/src/main/java/frc/utils/BlinkinState.java @@ -0,0 +1,110 @@ +package frc.utils; + +public enum BlinkinState { + Fixed_Rainbow_Rainbow_Palette(-0.99), + Fixed_Rainbow_Party_Palette(-0.97), + Fixed_Rainbow_Ocean_Palette(-0.95), + Fixed_Rainbow_Lave_Palette(-0.93), + Fixed_Rainbow_Forest_Palette(-0.91), + Fixed_Rainbow_with_Glitter(-0.89), + Fixed_Confetti(-0.87), + Fixed_Shot_Red(-0.85), + Fixed_Shot_Blue(-0.83), + Fixed_Shot_White(-0.81), + Fixed_Sinelon_Rainbow_Palette(-0.79), + Fixed_Sinelon_Party_Palette(-0.77), + Fixed_Sinelon_Ocean_Palette(-0.75), + Fixed_Sinelon_Lava_Palette(-0.73), + Fixed_Sinelon_Forest_Palette(-0.71), + Fixed_Beats_per_Minute_Rainbow_Palette(-0.69), + Fixed_Beats_per_Minute_Party_Palette(-0.67), + Fixed_Beats_per_Minute_Ocean_Palette(-0.65), + Fixed_Beats_per_Minute_Lava_Palette(-0.63), + Fixed_Beats_per_Minute_Forest_Palette(-0.61), + Fixed_Fire_Medium(-0.59), + Fixed_Fire_Large(-0.57), + Fixed_Twinkles_Rainbow_Palette(-0.55), + Fixed_Twinkles_Party_Palette(-0.53), + Fixed_Twinkles_Ocean_Palette(-0.51), + Fixed_Twinkles_Lava_Palette(-0.49), + Fixed_Twinkles_Forest_Palette(-0.47), + Fixed_Color_Waves_Rainbow_Palette(-0.45), + Fixed_Color_Waves_Party_Palette(-0.43), + Fixed_Color_Waves_Ocean_Palette(-0.41), + Fixed_Color_Waves_Lava_Palette(-0.39), + Fixed_Color_Waves_Forest_Palette(-0.37), + Fixed_Larson_Scanner_Red(-0.35), + Fixed_Larson_Scanner_Gray(-0.33), + Fixed_Light_Chase_Red(-0.31), + Fixed_Light_Chase_Blue(-0.29), + Fixed_Light_Chase_Gray(-0.27), + Fixed_Heartbeat_Red(-0.25), + Fixed_Heartbeat_Blue(-0.23), + Fixed_Heartbeat_White(-0.21), + Fixed_Heartbeat_Gray(-0.19), + Fixed_Breath_Red(-0.17), + Fixed_Breath_Blue(-0.15), + Fixed_Breath_Gray(-0.13), + Fixed_Strobe_Red(-0.11), + Fixed_Strobe_Blue(-0.09), + Fixed_Strobe_Gold(-0.07), + Fixed_Strobe_White(-0.05), + Color_1_Pattern_End_to_End_Blend_to_Black(-0.03), + Color_1_Pattern_Larson_Scanner(-0.01), + Color_1_Pattern_Light_Chase(0.01), + Color_1_Pattern_Heartbeat_Slow(0.03), + Color_1_Pattern_Heartbeat_Medium(0.05), + Color_1_Pattern_Heartbeat_Fast(0.07), + Color_1_Pattern_Breath_Slow(0.09), + Color_1_Pattern_Breath_Fast(0.11), + Color_1_Pattern_Shot(0.13), + Color_1_Pattern_Strobe(0.15), + Color_2_Pattern_End_to_End_Blend_to_Black(0.17), + Color_2_Pattern_Larson_Scanner(0.19), + Color_2_Pattern_Light_Chase_Dimming(0.21), + Color_2_Pattern_Heartbeat_Slow(0.23), + Color_2_Pattern_Heartbeat_Medium(0.25), + Color_2_Pattern_Heartbeat_Fast(0.27), + Color_2_Pattern_Breath_Slow(0.29), + Color_2_Pattern_Breath_Fast(0.31), + Color_2_Pattern_Shot(0.33), + Color_2_Pattern_Strobe(0.35), + Color_1_and_2_Pattern_Sparkle_Color_1_on_Color_2(0.37), + Color_1_and_2_Pattern_Sparkle_Color_2_on_Color_1(0.39), + Color_1_and_2_Pattern_Color_Gradient_Color_1_and_2(0.41), + Color_1_and_2_Pattern_Beats_per_Minute_Color_1_and_2(0.43), + Color_1_and_2_Pattern_End_to_End_Blend_Color_1_to_2(0.45), + Color_1_and_2_Pattern_End_to_End_Blend(0.47), + Color_1_and_2_Pattern_Color_1_and_Color(0.49), + Color_1_and_2_Pattern_Twinkles_Color_1_and_2(0.51), + Color_1_and_2_Pattern_Color_Waves_Color_1_and_2(0.53), + Color_1_and_2_Pattern_Sinelon_Color_1_and_2(0.55), + Solid_Colors_Hot_Pink(0.57), + Solid_Colors_Dark_red(0.59), + Solid_Colors_Red(0.61), + Solid_Colors_Red_Orange(0.63), + Solid_Colors_Orange(0.65), + Solid_Colors_Gold(0.67), + Solid_Colors_Yellow(0.69), + Solid_Colors_Lawn_Green(0.71), + Solid_Colors_Lime(0.73), + Solid_Colors_Dark_Green(0.75), + Solid_Colors_Green(0.77), + Solid_Colors_Blue_Green(0.79), + Solid_Colors_Aqua(0.81), + Solid_Colors_Sky_Blue(0.83), + Solid_Colors_Dark_Blue(0.85), + Solid_Colors_Blue(0.87), + Solid_Colors_Blue_Violet(0.89), + Solid_Colors_Violet(0.91), + Solid_Colors_White(0.93), + Solid_Colors_Gray(0.95), + Solid_Colors_Dark_Gray(0.97), + Solid_Colors_Black(0.99); + + public final double sparkValue; + + private BlinkinState(double sparkValue) { + this.sparkValue = sparkValue; + } +} \ No newline at end of file From 3e22e5435009bd6fa1d1bc29f01673833a5f12c6 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Thu, 21 Mar 2024 21:24:00 -0400 Subject: [PATCH 02/12] Brain Hurts --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/Robot.java | 60 +++++++++++++++++-- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/RobotState.java | 17 ++++++ .../lights/ContinueFlashLEDCommand.java | 52 ++++++++++++++++ .../commands/lights/FlashLEDCommand.java | 6 +- .../commands/lights/FlashStopLEDCommand.java | 58 ++++++++++++++++++ .../robot/commands/lights/HoldLEDCommand.java | 46 ++++++++++++++ .../frc/robot/subsystems/LightControl.java | 16 +++++ 9 files changed, 249 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java create mode 100644 src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java create mode 100644 src/main/java/frc/robot/commands/lights/HoldLEDCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ec644d4..5ee6be4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -254,5 +254,7 @@ public class CANConstants { } + public static final int blinkinPort = 0; + } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4d88701..0cb37e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,7 +23,13 @@ import monologue.Annotations.Log; import monologue.Logged; import edu.wpi.first.wpilibj.smartdashboard.*; - +import frc.utils.BlinkinState; +import frc.robot.subsystems.LightControl; +import frc.robot.Constants; +import frc.robot.commands.lights.FlashLEDCommand; +import frc.robot.commands.lights.HoldLEDCommand; +import frc.robot.commands.lights.ContinueFlashLEDCommand; +import frc.robot.commands.lights.FlashStopLEDCommand; /**f * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -35,6 +41,8 @@ public class Robot extends TimedRobot implements Logged { private RobotContainer m_robotContainer; + private LightControl m_LightControl = new LightControl(Constants.blinkinPort); + @Log.NT @Log.File @@ -42,6 +50,13 @@ public class Robot extends TimedRobot implements Logged { private final Field2d field = new Field2d(); + Command disabledLEDCommand = new HoldLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange); + Command defaultLEDCommand = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, BlinkinState.Solid_Colors_Red, 500); + Command initLEDCommand = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, 750, 4); + Command piecePickedUP = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, 750, 4); + Command readyToShoot = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, BlinkinState.Solid_Colors_Black, 500); + private boolean defaultStart = false; + private boolean shotPieceInverse = true; /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -102,10 +117,13 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} + public void disabledInit() { + disabledLEDCommand.schedule(); + } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override @@ -123,11 +141,24 @@ public void autonomousInit() { if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + + initLEDCommand.schedule(); + defaultStart = false; } /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + if(m_LightControl.checkflashState() && RobotState.getInstance().hasPiece()){ + piecePickedUP.schedule(); + RobotState.getInstance().setPieceSent(false); + defaultStart = false; + } + else if(m_LightControl.checkflashState() && !defaultStart){ + defaultLEDCommand.schedule(); + defaultStart = true; + } + } @Override public void teleopInit() { @@ -138,11 +169,30 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + initLEDCommand.schedule(); + defaultStart = false; } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + if(RobotState.getInstance().hasPiece()){ + piecePickedUP.schedule(); + RobotState.getInstance().setPieceSent(false); + defaultStart = false; + } + else if(RobotState.getInstance().checkReadytoShoot() && shotPieceInverse){ + readyToShoot.schedule(); + shotPieceInverse = false; + } + else if(m_LightControl.checkflashState() && !defaultStart){ + defaultLEDCommand.schedule(); + defaultStart = true; + } + else if(!RobotState.getInstance().checkReadytoShoot()){ + shotPieceInverse = true; + } + } @Override public void testInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a6ee64..041ac0d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -71,7 +71,6 @@ public class RobotContainer implements Logged { //private final UmbrellaSubsystem m_umbrella = new UmbrellaSubsystem(); private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - private final RobotState m_robotState = RobotState.getInstance(); //Robot preferences diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index f49749e..6fb56a3 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -13,6 +13,8 @@ public class RobotState { private Pose2d m_robotPose2d; private Pose2d m_speakerPose2d; private int m_speakerID = 0; + private boolean hasPiece = false; + private boolean readyToShoot = false; private static RobotState single_instance = null; @@ -54,4 +56,19 @@ public double getDistancetoSpeaker(){ return PhotonUtils.getDistanceToPose(m_robotPose2d, m_speakerPose2d); } + public void setPieceSent(boolean bool){ + hasPiece = bool; + } + public boolean hasPiece(){ + return hasPiece; + } + + public void setReadyToShoot(boolean bool){ + readyToShoot = bool; + } + + public boolean checkReadytoShoot(){ + return readyToShoot; + } + } diff --git a/src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java b/src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java new file mode 100644 index 0000000..817b506 --- /dev/null +++ b/src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java @@ -0,0 +1,52 @@ +package frc.robot.commands.lights; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.utils.BlinkinState; +import frc.robot.subsystems.LightControl; + +public class ContinueFlashLEDCommand extends Command { + + private LightControl blinkin; + private final BlinkinState flashPatternFirst; + private final BlinkinState flashPatternSecond; + private int flashTimeMillis; + + private long timer; + + public ContinueFlashLEDCommand(LightControl blinkin, BlinkinState flashPatternFirst, BlinkinState flashPatternSecond, int flashTimeMillis) { + this.blinkin = blinkin; + this.flashPatternFirst = flashPatternFirst; + this.flashPatternSecond = flashPatternSecond; + this.flashTimeMillis = flashTimeMillis; + + addRequirements(blinkin); + } + + @Override + public void initialize() { + timer = 0; + } + + @Override + public void execute() { + if (!blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { + timer = System.currentTimeMillis(); + blinkin.setPattern(flashPatternFirst); + } else if (blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { + timer = System.currentTimeMillis(); + blinkin.setPattern(flashPatternSecond); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + blinkin.turnOff(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java b/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java index 007c73d..493f7ee 100644 --- a/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java +++ b/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java @@ -2,11 +2,11 @@ import java.util.function.Supplier; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc.utils.BlinkinState; import frc.robot.subsystems.LightControl; -public class FlashLEDCommand extends CommandBase { +public class FlashLEDCommand extends Command { private LightControl blinkin; private final BlinkinState flashPattern; @@ -53,6 +53,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - + blinkin.turnOff(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java b/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java new file mode 100644 index 0000000..34b731c --- /dev/null +++ b/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java @@ -0,0 +1,58 @@ +package frc.robot.commands.lights; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.utils.BlinkinState; +import frc.robot.subsystems.LightControl; + +public class FlashStopLEDCommand extends Command { + + private LightControl blinkin; + private final BlinkinState flashPattern; + private int flashTimeMillis; + private int timesToFlash; + + private int loops; + private long timer; + + public FlashStopLEDCommand(LightControl blinkin, BlinkinState flashPattern, int flashTimeMillis, int timesToFlash) { + this.blinkin = blinkin; + this.flashPattern = flashPattern; + this.flashTimeMillis = flashTimeMillis; + this.timesToFlash = timesToFlash; + + addRequirements(blinkin); + } + + @Override + public void initialize() { + blinkin.StartFlashPeriod(); + loops = 0; + timer = 0; + } + + @Override + public void execute() { + if (!blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { + timer = System.currentTimeMillis(); + blinkin.setPattern(flashPattern); + loops++; + } else if (blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { + timer = System.currentTimeMillis(); + blinkin.turnOff(); + } + if (loops > timesToFlash) this.end(true); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + blinkin.turnOff(); + blinkin.endPeriod(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/lights/HoldLEDCommand.java b/src/main/java/frc/robot/commands/lights/HoldLEDCommand.java new file mode 100644 index 0000000..8bc8876 --- /dev/null +++ b/src/main/java/frc/robot/commands/lights/HoldLEDCommand.java @@ -0,0 +1,46 @@ +package frc.robot.commands.lights; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.utils.BlinkinState; +import frc.robot.subsystems.LightControl; + +public class HoldLEDCommand extends Command { + + private LightControl blinkin; + private final BlinkinState holdPattern; + + + private int loops; + private long timer; + + public HoldLEDCommand(LightControl blinkin, BlinkinState holdPattern) { + this.blinkin = blinkin; + this.holdPattern = holdPattern; + + + addRequirements(blinkin); + } + + @Override + public void initialize() { + loops = 0; + timer = 0; + } + + @Override + public void execute() { + blinkin.setPattern(holdPattern); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + blinkin.turnOff(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LightControl.java b/src/main/java/frc/robot/subsystems/LightControl.java index 9f2136e..e753be3 100644 --- a/src/main/java/frc/robot/subsystems/LightControl.java +++ b/src/main/java/frc/robot/subsystems/LightControl.java @@ -13,6 +13,8 @@ public class LightControl extends SubsystemBase { private BlinkinController blinkin; private BlinkinState pattern; + + private boolean flashPeriodActivated = false; public LightControl(int blinkinPort) { this.blinkin = new BlinkinController(blinkinPort); @@ -49,4 +51,18 @@ public Command getSetLedCommand(Supplier pattern) { public Command getSetLedCommand(BlinkinState state) { return this.getSetLedCommand(() -> state); } + + + public void StartFlashPeriod(){ + flashPeriodActivated = true; + } + + public void endPeriod(){ + flashPeriodActivated = false; + } + + public boolean checkflashState(){ + return flashPeriodActivated; + } + } \ No newline at end of file From 2f258880ddfed8e81ae65a91f5dd59f73c32fde2 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Fri, 22 Mar 2024 20:47:11 -0400 Subject: [PATCH 03/12] Better than brain garbage --- src/main/java/frc/robot/Robot.java | 43 ++----------------- src/main/java/frc/robot/RobotContainer.java | 40 +++++++++++++---- src/main/java/frc/robot/RobotState.java | 16 ------- .../robot/commands/Shooter/ShootPiece.java | 28 ++++++++++-- .../commands/lights/FlashStopLEDCommand.java | 2 - .../frc/robot/subsystems/LightControl.java | 13 ------ 6 files changed, 59 insertions(+), 83 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0cb37e2..a7c2069 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,8 +41,6 @@ public class Robot extends TimedRobot implements Logged { private RobotContainer m_robotContainer; - private LightControl m_LightControl = new LightControl(Constants.blinkinPort); - @Log.NT @Log.File @@ -50,13 +48,6 @@ public class Robot extends TimedRobot implements Logged { private final Field2d field = new Field2d(); - Command disabledLEDCommand = new HoldLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange); - Command defaultLEDCommand = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, BlinkinState.Solid_Colors_Red, 500); - Command initLEDCommand = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, 750, 4); - Command piecePickedUP = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, 750, 4); - Command readyToShoot = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, BlinkinState.Solid_Colors_Black, 500); - private boolean defaultStart = false; - private boolean shotPieceInverse = true; /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -118,7 +109,6 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { - disabledLEDCommand.schedule(); } @Override @@ -128,6 +118,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + m_robotContainer.getInitCommand().schedule(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); /* @@ -141,27 +132,16 @@ public void autonomousInit() { if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } - - initLEDCommand.schedule(); - defaultStart = false; } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() { - if(m_LightControl.checkflashState() && RobotState.getInstance().hasPiece()){ - piecePickedUP.schedule(); - RobotState.getInstance().setPieceSent(false); - defaultStart = false; - } - else if(m_LightControl.checkflashState() && !defaultStart){ - defaultLEDCommand.schedule(); - defaultStart = true; - } } @Override public void teleopInit() { + m_robotContainer.getInitCommand().schedule(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -169,29 +149,12 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - initLEDCommand.schedule(); - defaultStart = false; } /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - if(RobotState.getInstance().hasPiece()){ - piecePickedUP.schedule(); - RobotState.getInstance().setPieceSent(false); - defaultStart = false; - } - else if(RobotState.getInstance().checkReadytoShoot() && shotPieceInverse){ - readyToShoot.schedule(); - shotPieceInverse = false; - } - else if(m_LightControl.checkflashState() && !defaultStart){ - defaultLEDCommand.schedule(); - defaultStart = true; - } - else if(!RobotState.getInstance().checkReadytoShoot()){ - shotPieceInverse = true; - } + } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 041ac0d..aa8f296 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import frc.robot.Constants.OIConstants; @@ -18,6 +19,9 @@ import frc.robot.commands.intake.IntakePiece; import frc.robot.commands.intake.RunIntake; import frc.robot.commands.intake.StowIntake; +import frc.robot.commands.lights.ContinueFlashLEDCommand; +import frc.robot.commands.lights.FlashStopLEDCommand; +import frc.robot.commands.lights.HoldLEDCommand; import frc.robot.input.AxisButton; import frc.robot.commands.Shooter.RunShooterPower; import frc.robot.commands.Shooter.RunShooterRPM; @@ -29,9 +33,11 @@ import frc.robot.commands.Shooter.PositionShooter; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LightControl; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.UmbrellaSubsystem; import frc.robot.subsystems.drive.PhotonCameraWrapper; +import frc.utils.BlinkinState; import monologue.Logged; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -46,6 +52,8 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import javax.naming.InitialContext; + import com.pathplanner.lib.auto.AutoBuilder; import frc.robot.commands.ResetGyro; import edu.wpi.first.wpilibj2.command.button.POVButton; @@ -73,6 +81,9 @@ public class RobotContainer implements Logged { private final RobotState m_robotState = RobotState.getInstance(); + + private final LightControl m_LightControl = new LightControl(Constants.blinkinPort); + //Robot preferences private DoublePreference intakePower = new DoublePreference("intake/intakePower", 0.5); private DoublePreference intakePosition = new DoublePreference("intake/intakePosition", 100); @@ -117,14 +128,14 @@ public class RobotContainer implements Logged { private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition); private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower); private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower); - private final Command shootHighAngle = new ShootPiece(m_shooter, shooterHighAngle, shooterPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootMidAngle = new ShootPiece(m_shooter, shooterMidAngle, shooterPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootLowAngle = new ShootPiece(m_shooter, shooterLowAngle, shooterHighPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootHighAngle = new ShootPiece(m_shooter, m_LightControl, shooterHighAngle, shooterPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootMidAngle = new ShootPiece(m_shooter, m_LightControl, shooterMidAngle, shooterPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootLowAngle = new ShootPiece(m_shooter, m_LightControl, shooterLowAngle, shooterHighPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM); private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15); - private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shooterTune = new ShootPiece(m_shooter, m_LightControl, tuningPosition, tuningPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command shootInterpolated = new ShootInterpolated(m_shooter, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command driveToTarget = new DriveToTarget(m_robotDrive, m_photonCameraWrapper, @@ -142,11 +153,21 @@ public class RobotContainer implements Logged { private final SendableChooser autonChooser; + //light commands + Command defaultLEDCommand = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, BlinkinState.Solid_Colors_Red, 500); + Command initLEDCommand = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, 750, 4); + Command piecePickedUP = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, 750, 4); + Command readyToShoot = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, BlinkinState.Solid_Colors_Black, 500); + + private final double pathSpeed = 2; // The driver's controller XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); XboxController m_manipController = new XboxController(OIConstants.kManipControllerPort); + + private boolean autoInit = true; + private boolean teleopInit = true; private static class Operator { @@ -234,10 +255,8 @@ private void configureButtonBindings() { // new JoystickButton(m_driverController, XboxController.Button.kY.value) // .whileTrue(m_robotDrive.driveSysIdTestBuilder(6, 3)); // new JoystickButton(m_driverController, XboxController.Button.kB.value) - // .whileTrue(m_robotDrive.turnSysIdTestBuilder(10, 5)); + // .whileTrue(m_robotDrive.turnSysIdTestBuilder(10, 5); - - } private void configureDefaultCommands(){ @@ -255,7 +274,7 @@ private void configureDefaultCommands(){ m_robotIntake.setDefaultCommand(stowIntake); m_shooter.setDefaultCommand(stowShooter); - + m_LightControl.setDefaultCommand(defaultLEDCommand); } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -265,7 +284,10 @@ private void configureDefaultCommands(){ public Command getAutonomousCommand() { return autonChooser.getSelected(); } - + + public Command getInitCommand(){ + return initLEDCommand; + } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 6fb56a3..131bc41 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -55,20 +55,4 @@ public double getDistancetoSpeaker(){ if (null == m_robotPose2d || null == m_speakerPose2d) return 0.0; return PhotonUtils.getDistanceToPose(m_robotPose2d, m_speakerPose2d); } - - public void setPieceSent(boolean bool){ - hasPiece = bool; - } - public boolean hasPiece(){ - return hasPiece; - } - - public void setReadyToShoot(boolean bool){ - readyToShoot = bool; - } - - public boolean checkReadytoShoot(){ - return readyToShoot; - } - } diff --git a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java index f655f51..3545c2e 100644 --- a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java +++ b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj.shuffleboard.SuppliedValueWidget; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.LightControl; +import frc.utils.BlinkinState; import java.util.function.Supplier; @@ -17,26 +19,45 @@ public class ShootPiece extends Command { private final Supplier m_power; private final Supplier m_indexPower; private final Supplier m_ready; + private final LightControl m_LightControl; + + private int loops; + private long timer; /** Creates a new ShootPiece. */ - public ShootPiece(ShooterSubsystem shooter, Supplier position, Supplier power, Supplier indexpower, Supplier ready) { + public ShootPiece(ShooterSubsystem shooter, LightControl lightControl, Supplier position, Supplier power, Supplier indexpower, Supplier ready) { m_shooter = shooter; + m_LightControl = lightControl; m_position = position; m_power = power; m_indexPower = indexpower; m_ready = ready; // Use addRequirements() here to declare subsystem dependencies. - addRequirements(m_shooter); + addRequirements(m_shooter, m_LightControl); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + timer = 0; + loops = 0; + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { m_shooter.spinPower(m_power.get()); m_shooter.setAngleDegrees(m_position.get()); + if(m_shooter.armAtSetpoint() && m_shooter.atRPM()) + { + if (!m_LightControl.isActive() && System.currentTimeMillis() > timer + 500) { + timer = System.currentTimeMillis(); + m_LightControl.setPattern(BlinkinState.Solid_Colors_Green); + loops++; + } else if (m_LightControl.isActive() && System.currentTimeMillis() > timer + 500) { + timer = System.currentTimeMillis(); + m_LightControl.turnOff(); + } + } //if (m_ready.get() && m_shooter.atSetpoint()){ if (m_ready.get()){ m_shooter.runIndex(m_indexPower.get()); @@ -48,6 +69,7 @@ public void execute() { @Override public void end(boolean interrupted) { m_shooter.stopAll(); + m_LightControl.turnOff(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java b/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java index 34b731c..8f4a1b2 100644 --- a/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java +++ b/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java @@ -27,7 +27,6 @@ public FlashStopLEDCommand(LightControl blinkin, BlinkinState flashPattern, int @Override public void initialize() { - blinkin.StartFlashPeriod(); loops = 0; timer = 0; } @@ -53,6 +52,5 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { blinkin.turnOff(); - blinkin.endPeriod(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LightControl.java b/src/main/java/frc/robot/subsystems/LightControl.java index e753be3..8b81e2f 100644 --- a/src/main/java/frc/robot/subsystems/LightControl.java +++ b/src/main/java/frc/robot/subsystems/LightControl.java @@ -52,17 +52,4 @@ public Command getSetLedCommand(BlinkinState state) { return this.getSetLedCommand(() -> state); } - - public void StartFlashPeriod(){ - flashPeriodActivated = true; - } - - public void endPeriod(){ - flashPeriodActivated = false; - } - - public boolean checkflashState(){ - return flashPeriodActivated; - } - } \ No newline at end of file From 6adf1d3a5ea8e325e3f5d267029f0c413c903a04 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Fri, 22 Mar 2024 20:58:48 -0400 Subject: [PATCH 04/12] Merge Crap --- src/main/java/frc/robot/Robot.java | 18 +----------------- src/main/java/frc/robot/RobotContainer.java | 13 +++---------- 2 files changed, 4 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4ece0ab..0075401 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -80,23 +80,7 @@ public void robotInit() { boolean lazyLogging = false; Monologue.setupMonologue(this, "/Robot", fileOnly, lazyLogging); DriverStation.startDataLog(DataLogManager.getLog()); // same log used by monologue - StringLogEntry MetaData = new StringLogEntry(DataLogManager.getLog(), "MetaData"); - MetaData.append("Project Name: " + BuildConstants.MAVEN_NAME); - MetaData.append("Build Date: " + BuildConstants.BUILD_DATE); - MetaData.append("Commit Hash: " + BuildConstants.GIT_SHA); - MetaData.append("Git Date: " + BuildConstants.GIT_DATE); - MetaData.append("Git Branch: " + BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - MetaData.append("GitDirty: " + "All changes commited"); - break; - case 1: - MetaData.append("GitDirty: " + "Uncomitted changes"); - break; - default: - MetaData.append("GitDirty: " + "Unknown"); - break; - } + } private void getAllianceInfo(){ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 572e53e..720ceaa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -174,13 +174,9 @@ public class RobotContainer implements Logged { private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower); private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower); - private final Command shootHighAngle = new ShootPiece(m_shooter, m_LightControl, shooterHighAngle, shooterPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootMidAngle = new ShootPiece(m_shooter, m_LightControl, shooterMidAngle, shooterPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootLowAngle = new ShootPiece(m_shooter, m_LightControl, shooterLowAngle, shooterHighPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - - private final Command shootSubwoofer = new ShootPiece(m_shooter, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootPodium = new ShootPiece(m_shooter, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootWing = new ShootPiece(m_shooter, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootSubwoofer = new ShootPiece(m_shooter, m_LightControl, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootPodium = new ShootPiece(m_shooter, m_LightControl, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shootWing = new ShootPiece(m_shooter, m_LightControl, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM); private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM); @@ -193,9 +189,6 @@ public class RobotContainer implements Logged { private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15); private final Command shooterTune = new ShootPiece(m_shooter, m_LightControl, tuningPosition, tuningPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command shootInterpolated = new ShootInterpolated(m_shooter, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - - private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command shootInterpolated = new ShootInterpolated(m_shooter, m_photonCameraWrapper, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command driveToTarget = new DriveToTarget(m_robotDrive, From 9cb02a8ff06c1f4a7dd121f56e018df2145d3f3b Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Fri, 22 Mar 2024 21:07:34 -0400 Subject: [PATCH 05/12] Build Constants, yaaaah... --- src/main/java/frc/robot/Robot.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0075401..4ece0ab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -80,7 +80,23 @@ public void robotInit() { boolean lazyLogging = false; Monologue.setupMonologue(this, "/Robot", fileOnly, lazyLogging); DriverStation.startDataLog(DataLogManager.getLog()); // same log used by monologue - + StringLogEntry MetaData = new StringLogEntry(DataLogManager.getLog(), "MetaData"); + MetaData.append("Project Name: " + BuildConstants.MAVEN_NAME); + MetaData.append("Build Date: " + BuildConstants.BUILD_DATE); + MetaData.append("Commit Hash: " + BuildConstants.GIT_SHA); + MetaData.append("Git Date: " + BuildConstants.GIT_DATE); + MetaData.append("Git Branch: " + BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + MetaData.append("GitDirty: " + "All changes commited"); + break; + case 1: + MetaData.append("GitDirty: " + "Uncomitted changes"); + break; + default: + MetaData.append("GitDirty: " + "Unknown"); + break; + } } private void getAllianceInfo(){ From 000558b6bdf60e76cbbfe66858cca68b2e63ad64 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Fri, 22 Mar 2024 21:27:49 -0400 Subject: [PATCH 06/12] Shoot Ready done --- src/main/java/frc/robot/subsystems/LightControl.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/LightControl.java b/src/main/java/frc/robot/subsystems/LightControl.java index 8b81e2f..0b8ca02 100644 --- a/src/main/java/frc/robot/subsystems/LightControl.java +++ b/src/main/java/frc/robot/subsystems/LightControl.java @@ -52,4 +52,14 @@ public Command getSetLedCommand(BlinkinState state) { return this.getSetLedCommand(() -> state); } + public long ShootReady(long timer){ + if (!this.isActive() && System.currentTimeMillis() > timer + 500) { + this.setPattern(BlinkinState.Solid_Colors_Green); + } else if (this.isActive() && System.currentTimeMillis() > timer + 500) { + this.turnOff(); + } + timer = System.currentTimeMillis(); + return timer; + } + } \ No newline at end of file From afe6c7a2f18be9cfe6bbdc82eb2ee0ed13d52715 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Fri, 22 Mar 2024 21:29:48 -0400 Subject: [PATCH 07/12] Fixed function --- src/main/java/frc/robot/subsystems/LightControl.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/LightControl.java b/src/main/java/frc/robot/subsystems/LightControl.java index 0b8ca02..739d542 100644 --- a/src/main/java/frc/robot/subsystems/LightControl.java +++ b/src/main/java/frc/robot/subsystems/LightControl.java @@ -54,11 +54,12 @@ public Command getSetLedCommand(BlinkinState state) { public long ShootReady(long timer){ if (!this.isActive() && System.currentTimeMillis() > timer + 500) { + timer = System.currentTimeMillis(); this.setPattern(BlinkinState.Solid_Colors_Green); } else if (this.isActive() && System.currentTimeMillis() > timer + 500) { + timer = System.currentTimeMillis(); this.turnOff(); } - timer = System.currentTimeMillis(); return timer; } From f2f9a8845a65e4899ebcdfaa5ba93c6f605ce52f Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Sun, 24 Mar 2024 13:21:08 -0400 Subject: [PATCH 08/12] Better LED Control --- src/main/java/frc/robot/Robot.java | 13 ++++-- src/main/java/frc/robot/RobotContainer.java | 13 ++++-- .../robot/commands/Shooter/ShootPiece.java | 12 +---- .../robot/commands/intake/IntakePiece.java | 5 +- .../robot/commands/lights/HoldLEDCommand.java | 46 ------------------- .../frc/robot/subsystems/LightControl.java | 10 +++- 6 files changed, 34 insertions(+), 65 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/lights/HoldLEDCommand.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4ece0ab..275b509 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,9 +28,6 @@ import frc.robot.subsystems.LightControl; import frc.robot.Constants; import frc.robot.commands.lights.FlashLEDCommand; -import frc.robot.commands.lights.HoldLEDCommand; -import frc.robot.commands.lights.ContinueFlashLEDCommand; -import frc.robot.commands.lights.FlashStopLEDCommand; import edu.wpi.first.util.datalog.StringLogEntry; @@ -45,6 +42,8 @@ public class Robot extends TimedRobot implements Logged { private RobotContainer m_robotContainer; + private boolean runLEDBeamBreak = true; + @Log.NT @Log.File private CommandScheduler m_Scheduler; @@ -132,6 +131,14 @@ public void robotPeriodic() { Monologue.setFileOnly(DriverStation.isFMSAttached()); Monologue.updateAll(); + + if(runLEDBeamBreak && m_robotContainer.getIndexerBeamBreak()){ + m_robotContainer.getBeamBreakLEDCommand().schedule(); + runLEDBeamBreak = false; + } + else if(!runLEDBeamBreak && !m_robotContainer.getIndexerBeamBreak()){ + runLEDBeamBreak = true; + } } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0d8feb3..8a87424 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import frc.robot.commands.intake.StowIntake; import frc.robot.commands.lights.ContinueFlashLEDCommand; import frc.robot.commands.lights.FlashStopLEDCommand; -import frc.robot.commands.lights.HoldLEDCommand; import frc.robot.input.AxisButton; import frc.robot.commands.Shooter.RunShooterPower; import frc.robot.commands.Shooter.RunShooterRPM; @@ -77,6 +76,7 @@ + /* * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -170,7 +170,7 @@ public class RobotContainer implements Logged { private final Command resetGyro = new ResetGyro(m_robotDrive); private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition); private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition); - private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition); + private final Command intakePiece = new IntakePiece(m_robotIntake, m_LightControl, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition); private final Command stowIntake = new StowIntake(m_robotIntake); private final Command parkCommand = new ParkCommand(m_robotDrive); private final Command stowShooter = new PositionShooter(m_shooter, shooterHome); @@ -219,7 +219,6 @@ public class RobotContainer implements Logged { Command defaultLEDCommand = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, BlinkinState.Solid_Colors_Red, 500); Command initLEDCommand = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, 750, 4); Command piecePickedUP = new FlashStopLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Red_Orange, 750, 4); - Command readyToShoot = new ContinueFlashLEDCommand(m_LightControl, BlinkinState.Solid_Colors_Green, BlinkinState.Solid_Colors_Black, 500); private final double pathSpeed = 2; @@ -381,6 +380,14 @@ public Command getAutonomousCommand() { public Command getInitCommand(){ return initLEDCommand; } + + public boolean getIndexerBeamBreak(){ + return m_shooter.getIndexerBeamBreak(); + } + + public Command getBeamBreakLEDCommand(){ + return piecePickedUP; + } } diff --git a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java index c048895..60fa0be 100644 --- a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java +++ b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java @@ -40,8 +40,7 @@ public ShootPiece(ShooterSubsystem shooter, LightControl lightControl, Supplier< // Called when the command is initially scheduled. @Override public void initialize() { - timer = 0; - loops = 0; + timer = -1; } // Called every time the scheduler runs while the command is scheduled. @@ -51,14 +50,7 @@ public void execute() { m_shooter.setAngleDegrees(m_position.get()); if(m_shooter.armAtSetpoint() && m_shooter.atRPM()) { - if (!m_LightControl.isActive() && System.currentTimeMillis() > timer + 500) { - timer = System.currentTimeMillis(); - m_LightControl.setPattern(BlinkinState.Solid_Colors_Green); - loops++; - } else if (m_LightControl.isActive() && System.currentTimeMillis() > timer + 500) { - timer = System.currentTimeMillis(); - m_LightControl.turnOff(); - } + m_LightControl.ShootReady(timer); } //if (m_ready.get() && m_shooter.atSetpoint()){ if (m_ready.get()){ diff --git a/src/main/java/frc/robot/commands/intake/IntakePiece.java b/src/main/java/frc/robot/commands/intake/IntakePiece.java index 5a7ff38..c620866 100644 --- a/src/main/java/frc/robot/commands/intake/IntakePiece.java +++ b/src/main/java/frc/robot/commands/intake/IntakePiece.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.LightControl; import java.util.function.Supplier; @@ -19,6 +20,7 @@ public class IntakePiece extends Command { private final Supplier m_intakePosition; private final Supplier m_indexPower; private final Supplier m_indexPosition; + private final LightControl m_LightControl; private final Timer timer = new Timer(); private final double delaySeconds = 0.02; @@ -26,10 +28,11 @@ public class IntakePiece extends Command { /** Creates a new IntakePiece. */ - public IntakePiece(IntakeSubsystem intake, ShooterSubsystem shooter, Supplier intakePower, Supplier intakePosition, Supplier indexPower, Supplier indexPosition ) { + public IntakePiece(IntakeSubsystem intake, LightControl lightControl, ShooterSubsystem shooter, Supplier intakePower, Supplier intakePosition, Supplier indexPower, Supplier indexPosition ) { m_intake = intake; m_shooter = shooter; + m_LightControl = lightControl; m_intakePower = intakePower; m_intakePosition = intakePosition; m_indexPower = indexPower; diff --git a/src/main/java/frc/robot/commands/lights/HoldLEDCommand.java b/src/main/java/frc/robot/commands/lights/HoldLEDCommand.java deleted file mode 100644 index 8bc8876..0000000 --- a/src/main/java/frc/robot/commands/lights/HoldLEDCommand.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.commands.lights; - -import java.util.function.Supplier; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.utils.BlinkinState; -import frc.robot.subsystems.LightControl; - -public class HoldLEDCommand extends Command { - - private LightControl blinkin; - private final BlinkinState holdPattern; - - - private int loops; - private long timer; - - public HoldLEDCommand(LightControl blinkin, BlinkinState holdPattern) { - this.blinkin = blinkin; - this.holdPattern = holdPattern; - - - addRequirements(blinkin); - } - - @Override - public void initialize() { - loops = 0; - timer = 0; - } - - @Override - public void execute() { - blinkin.setPattern(holdPattern); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - blinkin.turnOff(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LightControl.java b/src/main/java/frc/robot/subsystems/LightControl.java index 739d542..82de486 100644 --- a/src/main/java/frc/robot/subsystems/LightControl.java +++ b/src/main/java/frc/robot/subsystems/LightControl.java @@ -53,7 +53,11 @@ public Command getSetLedCommand(BlinkinState state) { } public long ShootReady(long timer){ - if (!this.isActive() && System.currentTimeMillis() > timer + 500) { + if(timer == -1) { + timer = System.currentTimeMillis(); + this. turnOff(); + } + else if (!this.isActive() && System.currentTimeMillis() > timer + 500) { timer = System.currentTimeMillis(); this.setPattern(BlinkinState.Solid_Colors_Green); } else if (this.isActive() && System.currentTimeMillis() > timer + 500) { @@ -63,4 +67,6 @@ public long ShootReady(long timer){ return timer; } -} \ No newline at end of file + } + + From 023465a960a74dc96e9ffce11a915c24e40410ca Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Sun, 24 Mar 2024 13:23:36 -0400 Subject: [PATCH 09/12] Made a command easier to understand --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 275b509..9211ad0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -154,7 +154,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { - m_robotContainer.getInitCommand().schedule(); + m_robotContainer.getLEDInitCommand().schedule(); getAllianceInfo(); @@ -181,7 +181,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { - m_robotContainer.getInitCommand().schedule(); + m_robotContainer.getLEDInitCommand().schedule(); getAllianceInfo(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8a87424..acb0c32 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -377,7 +377,7 @@ public Command getAutonomousCommand() { return autonChooser.getSelected(); } - public Command getInitCommand(){ + public Command getLEDInitCommand(){ return initLEDCommand; } From 7e1c4a547ea618d3dae0bd2c314d3f7a6b080fc1 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Sun, 24 Mar 2024 13:29:29 -0400 Subject: [PATCH 10/12] Finish? --- src/main/java/frc/robot/RobotContainer.java | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index acb0c32..7d87772 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -60,9 +60,6 @@ import java.util.function.Supplier; -import javax.naming.InitialContext; - - import com.fasterxml.jackson.core.sym.Name; import com.pathplanner.lib.auto.AutoBuilder; @@ -192,7 +189,7 @@ public class RobotContainer implements Logged { private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15); - private final Command shooterTune = new ShootPiece(m_shooter, m_LightControl, tuningPosition, tuningPower, indexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + private final Command shooterTune = new ShootPiece(m_shooter, m_LightControl, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command shootInterpolated = new ShootInterpolated(m_shooter, m_photonCameraWrapper, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command driveToTarget = new DriveToTarget(m_robotDrive, @@ -347,7 +344,7 @@ private void configureButtonBindings() { // new JoystickButton(m_driverController, XboxController.Button.kY.value) // .whileTrue(m_robotDrive.driveSysIdTestBuilder(6, 3)); // new JoystickButton(m_driverController, XboxController.Button.kB.value) - // .whileTrue(m_robotDrive.turnSysIdTestBuilder(10, 5); + // .whileTrue(m_robotDrive.turnSysIdTestBuilder(10, 5)); } From 3186f843d1c5eed6848560bfb464cdbad67cedaf Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Sun, 24 Mar 2024 15:23:34 -0400 Subject: [PATCH 11/12] Created timer class --- .../robot/commands/Shooter/ShootPiece.java | 6 +++-- .../lights/ContinueFlashLEDCommand.java | 13 ++++++----- .../commands/lights/FlashLEDCommand.java | 2 +- .../commands/lights/FlashStopLEDCommand.java | 16 +++++++------- .../frc/robot/subsystems/LightControl.java | 17 +++++--------- src/main/java/frc/utils/Timer.java | 22 +++++++++++++++++++ 6 files changed, 48 insertions(+), 28 deletions(-) create mode 100644 src/main/java/frc/utils/Timer.java diff --git a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java index 60fa0be..503412c 100644 --- a/src/main/java/frc/robot/commands/Shooter/ShootPiece.java +++ b/src/main/java/frc/robot/commands/Shooter/ShootPiece.java @@ -11,6 +11,7 @@ import frc.utils.BlinkinState; import java.util.function.Supplier; +import frc.utils.Timer; public class ShootPiece extends Command { @@ -22,7 +23,7 @@ public class ShootPiece extends Command { private final LightControl m_LightControl; private int loops; - private long timer; + private Timer timer; /** Creates a new ShootPiece. */ public ShootPiece(ShooterSubsystem shooter, LightControl lightControl, Supplier position, Supplier rpm, Supplier indexpower, Supplier ready) { @@ -40,7 +41,7 @@ public ShootPiece(ShooterSubsystem shooter, LightControl lightControl, Supplier< // Called when the command is initially scheduled. @Override public void initialize() { - timer = -1; + timer.resetTimer(); } // Called every time the scheduler runs while the command is scheduled. @@ -52,6 +53,7 @@ public void execute() { { m_LightControl.ShootReady(timer); } + else timer.resetTimer(); //if (m_ready.get() && m_shooter.atSetpoint()){ if (m_ready.get()){ m_shooter.runIndex(m_indexPower.get()); diff --git a/src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java b/src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java index 817b506..363639c 100644 --- a/src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java +++ b/src/main/java/frc/robot/commands/lights/ContinueFlashLEDCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.utils.BlinkinState; import frc.robot.subsystems.LightControl; +import frc.utils.Timer; public class ContinueFlashLEDCommand extends Command { @@ -13,7 +14,7 @@ public class ContinueFlashLEDCommand extends Command { private final BlinkinState flashPatternSecond; private int flashTimeMillis; - private long timer; + private Timer m_Timer; public ContinueFlashLEDCommand(LightControl blinkin, BlinkinState flashPatternFirst, BlinkinState flashPatternSecond, int flashTimeMillis) { this.blinkin = blinkin; @@ -26,16 +27,16 @@ public ContinueFlashLEDCommand(LightControl blinkin, BlinkinState flashPatternFi @Override public void initialize() { - timer = 0; + m_Timer.resetTimer(); } @Override public void execute() { - if (!blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { - timer = System.currentTimeMillis(); + if (!blinkin.isActive() && m_Timer.timePassed(flashTimeMillis)){ + m_Timer.resetTimer(); blinkin.setPattern(flashPatternFirst); - } else if (blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { - timer = System.currentTimeMillis(); + } else if (blinkin.isActive() && m_Timer.timePassed(flashTimeMillis)) { + m_Timer.resetTimer(); blinkin.setPattern(flashPatternSecond); } } diff --git a/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java b/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java index 493f7ee..34749c6 100644 --- a/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java +++ b/src/main/java/frc/robot/commands/lights/FlashLEDCommand.java @@ -30,7 +30,7 @@ public FlashLEDCommand(LightControl blinkin, BlinkinState flashPattern, BlinkinS @Override public void initialize() { loops = 0; - timer = 0; + timer = System.currentTimeMillis(); } @Override diff --git a/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java b/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java index 8f4a1b2..f50a8ba 100644 --- a/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java +++ b/src/main/java/frc/robot/commands/lights/FlashStopLEDCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.utils.BlinkinState; import frc.robot.subsystems.LightControl; +import frc.utils.Timer; public class FlashStopLEDCommand extends Command { @@ -14,7 +15,7 @@ public class FlashStopLEDCommand extends Command { private int timesToFlash; private int loops; - private long timer; + private Timer m_Timer; public FlashStopLEDCommand(LightControl blinkin, BlinkinState flashPattern, int flashTimeMillis, int timesToFlash) { this.blinkin = blinkin; @@ -28,25 +29,24 @@ public FlashStopLEDCommand(LightControl blinkin, BlinkinState flashPattern, int @Override public void initialize() { loops = 0; - timer = 0; + m_Timer.resetTimer();; } @Override public void execute() { - if (!blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { - timer = System.currentTimeMillis(); + if (!blinkin.isActive() && m_Timer.timePassed(flashTimeMillis)) { + m_Timer.resetTimer(); blinkin.setPattern(flashPattern); loops++; - } else if (blinkin.isActive() && System.currentTimeMillis() > timer + flashTimeMillis) { - timer = System.currentTimeMillis(); + } else if (blinkin.isActive() && m_Timer.timePassed(flashTimeMillis)) { + m_Timer.resetTimer(); blinkin.turnOff(); } - if (loops > timesToFlash) this.end(true); } @Override public boolean isFinished() { - return false; + return loops > timesToFlash; } @Override diff --git a/src/main/java/frc/robot/subsystems/LightControl.java b/src/main/java/frc/robot/subsystems/LightControl.java index 82de486..fb77c8a 100644 --- a/src/main/java/frc/robot/subsystems/LightControl.java +++ b/src/main/java/frc/robot/subsystems/LightControl.java @@ -4,11 +4,11 @@ import java.util.regex.Pattern; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.commands.lights.FlashLEDCommand; import frc.utils.BlinkinState; import frc.utils.BlinkinController; +import frc.utils.Timer; public class LightControl extends SubsystemBase { private BlinkinController blinkin; @@ -52,19 +52,14 @@ public Command getSetLedCommand(BlinkinState state) { return this.getSetLedCommand(() -> state); } - public long ShootReady(long timer){ - if(timer == -1) { - timer = System.currentTimeMillis(); - this. turnOff(); - } - else if (!this.isActive() && System.currentTimeMillis() > timer + 500) { - timer = System.currentTimeMillis(); + public void ShootReady(Timer timer){ + if (!this.isActive() && timer.timePassed(500)) { + timer.resetTimer(); this.setPattern(BlinkinState.Solid_Colors_Green); - } else if (this.isActive() && System.currentTimeMillis() > timer + 500) { - timer = System.currentTimeMillis(); + } else if (this.isActive() && timer.timePassed(500)) { + timer.resetTimer(); this.turnOff(); } - return timer; } } diff --git a/src/main/java/frc/utils/Timer.java b/src/main/java/frc/utils/Timer.java new file mode 100644 index 0000000..f434163 --- /dev/null +++ b/src/main/java/frc/utils/Timer.java @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.utils; + +/** Add your docs here. */ +public class Timer { + long timer; + + public long getTimer(){ + return timer; + } + + public void resetTimer(){ + timer = 0; + } + + public boolean timePassed(int milliseconds){ + return System.currentTimeMillis() > timer + milliseconds; + } +} From 7e03a939f472e7d21c3e8585c2cb3c9319fe7516 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Mon, 1 Apr 2024 20:13:19 -0400 Subject: [PATCH 12/12] LED Merge conflicts --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d4603f0..a582c2c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -174,7 +174,7 @@ public class RobotContainer implements Logged { private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition); private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition); - private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition); + private final Command intakePiece = new IntakePiece(m_robotIntake, m_LightControl, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition); private final Command outTakePiece = new OuttakePiece(m_robotIntake, m_shooter, outtakePower, intakePosition, outdexPower, intakeShooterPosition, outtakeFlywheelPower); private final Command stowIntake = new StowIntake(m_robotIntake); private final Command parkCommand = new ParkCommand(m_robotDrive); @@ -214,7 +214,7 @@ public class RobotContainer implements Logged { private final Command autoAmpRingShot = new AutonShoot(m_shooter, autoAmpRingShotAngle, autoAmpRingShotRPM, shooterIndexPower).withTimeout(2); private final Command ampShotStart = new AutonShoot(m_shooter, ampRingShotOnlyAngle, ampRingShotOnlyRPM, shooterIndexPower).withTimeout(2); private final Command ringToss = new RingToss(m_robotIntake, m_shooter, intakePower, intakePosition, shooterIndexPower, intakeShooterPosition, () -> 1000.0); - private final Command autoIntake = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75); + private final Command autoIntake = new IntakePiece(m_robotIntake,m_LightControl, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition).withTimeout(3.75); //Autonomous Wait Command