From c24d662cb8f294881e772b7ada885cd935deafbe Mon Sep 17 00:00:00 2001 From: NightForts7798 Date: Wed, 6 Mar 2024 20:17:30 -0500 Subject: [PATCH 01/14] Scheduled LED command --- src/main/java/frc/robot/Robot.java | 10 ++++++++++ src/main/java/frc/robot/RobotContainer.java | 22 +++++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 49b315c2..6dff628f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -169,6 +169,11 @@ public void autonomousInit() { )){ _robotContainer.getRunShooterPIDCommand().schedule(); } + // if the runLED command is NOT scheduled then schedule it + if(!CommandScheduler.getInstance().isScheduled( + _robotContainer.getRunLEDs())){ + _robotContainer.getRunLEDs().schedule(); + } // schedule the autonomous command (example) if (_autonomousCommand != null) { @@ -215,6 +220,11 @@ public void teleopInit() { )){ _robotContainer.getRunShooterPIDCommand().schedule(); } + //if the runLED command is NOT scheduled then schedule it + if(!CommandScheduler.getInstance().isScheduled( + _robotContainer.getRunLEDs())){ + _robotContainer.getRunLEDs().schedule(); + } } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 96aa1111..1af93622 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,11 +20,16 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.RealClimberIO; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.intake.Intake; @@ -40,6 +45,8 @@ import frc.robot.subsystems.drive.commands.CmdDriveRotateAboutSpeaker; import frc.robot.subsystems.drive.commands.DriveCommands; import frc.robot.subsystems.intake.Intake.IntakePosition; +import frc.robot.subsystems.leds.LEDs; +import frc.robot.subsystems.leds.commands.CmdLEDsRunLEDs; import frc.robot.subsystems.multisubsystemcommands.GrpShootNoteInZone; import frc.robot.subsystems.shooter.RealShooterIO; import frc.robot.subsystems.shooter.Shooter; @@ -64,9 +71,12 @@ public class RobotContainer { private final Drive _drive; private final Intake _intake; private final Shooter _shooter; + private final Climber _climber; + private final LEDs _leds; private ShooterZone _zone; private Command _runShooterPIDCommand; private Command _runIntakePIDCommand; + private Command _runLEDsCommand; // logged dashboard inputs private final LoggedDashboardChooser _autoChooser; @@ -135,6 +145,8 @@ public RobotContainer() { new ModuleIOSparkFlex(3)); _intake = new Intake(new RealIntakeIO()); _shooter = new Shooter(new RealShooterIO()); + _leds = new LEDs(); + _climber = new Climber(new RealClimberIO()); break; case SIM: @@ -152,6 +164,9 @@ public RobotContainer() { }); _shooter = new Shooter(new ShooterIO() { }); + _leds = new LEDs(); + _climber = new Climber(new ClimberIO() { + }); break; default: // Replayed robot, disable IO implementations @@ -172,12 +187,16 @@ public RobotContainer() { }); _shooter = new Shooter(new ShooterIO() { }); + _leds = new LEDs(); + _climber = new Climber(new ClimberIO() { + }); break; } // setup commands for PID this._runShooterPIDCommand = new CmdShooterRunPids(_shooter); this._runIntakePIDCommand = _intake.buildCommand().runPID(); + this._runLEDsCommand = new CmdLEDsRunLEDs(_leds, _shooter, _intake, _climber); NamedCommands.registerCommand("intake GroundPos", new IntakeCommandFactory(_intake).setPosition(IntakePosition.GroundPickup)); NamedCommands.registerCommand("intake Stow", new IntakeCommandFactory(_intake).setPosition(IntakePosition.Stowed)); @@ -330,4 +349,7 @@ public Command getRunShooterPIDCommand() { public Command getRunIntakePIDCommand() { return this._runIntakePIDCommand; } + public Command getRunLEDs() { + return this._runLEDsCommand; + } } From 580a0ad8ef578d30597c9a8e631b2d2df3d5d22f Mon Sep 17 00:00:00 2001 From: LunaMoon9860 Date: Fri, 8 Mar 2024 19:55:26 -0500 Subject: [PATCH 02/14] added an LED animation for when the amp should be activated by the human player --- src/main/java/frc/robot/RobotContainer.java | 14 ++++++++++++++ src/main/java/frc/robot/subsystems/leds/LEDs.java | 13 +++++++++++-- .../subsystems/leds/commands/CmdLEDsRunLEDs.java | 5 +++++ 3 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1af93622..b1319117 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -288,6 +288,20 @@ private void configureButtonBindings() { _op2ButtonTwo.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Podium)); _op2ButtonOne.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Subwoofer)); + _op2ButtonFour.onTrue(new Command() { + + @Override + public void initialize(){ + _leds.setAmpReady(true); + } + + @Override + public boolean isFinished(){ + return true; + } + } + ); + _op2ButtonSix.onTrue(new Command() { @Override diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index 76318347..e48c1af1 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -13,13 +13,14 @@ public class LEDs { static int _numLEDs = 8; LEDAnimation _currentAnimation = LEDAnimation.None; boolean _prevHasNote; + boolean _ampReady = false; public enum LEDAnimation { None(null, null, 0), ReadyToShoot(new LEDColor(0, 255, 0), null, 3), PickedUpNote(null, new StrobeAnimation(255, 128, 0, 0, 0.5, _numLEDs), 3), - ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3); - + ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3), + AmpReady(null, new StrobeAnimation(204, 0, 255, 0, 0.5, _numLEDs), 3); LEDColor _color; Animation _animation; int _secondsToRun; @@ -89,4 +90,12 @@ public boolean shouldRunHasNoteAnimation(boolean hasNote) { _prevHasNote = hasNote; return ret; } + + public boolean getAmpReady() { + return _ampReady; + } + + public void setAmpReady(boolean isReady) { + _ampReady = isReady; + } } diff --git a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java index cfee6d48..ccce9718 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.leds.commands; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.leds.LEDs; @@ -41,6 +42,10 @@ public void execute() { } else if (_climber.isClimbing()) { // Runs when climber is active _leds.runAnimation(LEDAnimation.ClimberAscending); + } else if (_leds.getAmpReady()) { + // Runs when human player should activate Amp + _leds.runAnimation(LEDAnimation.AmpReady); + _leds.setAmpReady(false); } // Increment the timer for another loop From 2faeb0e6b5f18fe781ba2e4a70f47030499145e3 Mon Sep 17 00:00:00 2001 From: LunaMoon9860 Date: Fri, 8 Mar 2024 20:26:33 -0500 Subject: [PATCH 03/14] added rainbow led animations for when robot is disabled --- src/main/java/frc/robot/subsystems/leds/LEDs.java | 5 ++++- .../frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index e48c1af1..0bb3b040 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -3,6 +3,7 @@ import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.LarsonAnimation; +import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; import com.ctre.phoenix.led.LarsonAnimation.BounceMode; @@ -20,7 +21,9 @@ public enum LEDAnimation { ReadyToShoot(new LEDColor(0, 255, 0), null, 3), PickedUpNote(null, new StrobeAnimation(255, 128, 0, 0, 0.5, _numLEDs), 3), ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3), - AmpReady(null, new StrobeAnimation(204, 0, 255, 0, 0.5, _numLEDs), 3); + AmpReady(null, new StrobeAnimation(204, 0, 255, 0, 0.5, _numLEDs), 3), + //Change time on RobotDisabled + RobotDisabled(null, new RainbowAnimation(100, 0.5, _numLEDs), 3); LEDColor _color; Animation _animation; int _secondsToRun; diff --git a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java index ccce9718..55bee33f 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.leds.commands; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.leds.LEDs; @@ -46,6 +46,9 @@ public void execute() { // Runs when human player should activate Amp _leds.runAnimation(LEDAnimation.AmpReady); _leds.setAmpReady(false); + } else if (DriverStation.isDisabled()) { + // Rainbows while robot is disabled + _leds.runAnimation(LEDAnimation.RobotDisabled); } // Increment the timer for another loop From fcf27fe13f7a9e3747757596fe2653ec95370e93 Mon Sep 17 00:00:00 2001 From: Mackenzie Racoop Date: Sat, 9 Mar 2024 13:40:05 -0500 Subject: [PATCH 04/14] made it build --- src/main/java/frc/robot/RobotContainer.java | 2 -- .../frc/robot/subsystems/shooter/ShooterCommandFactory.java | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c10ed84..f8c7e186 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,9 +20,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterCommandFactory.java b/src/main/java/frc/robot/subsystems/shooter/ShooterCommandFactory.java index 7bb0ec7c..20d7fe0c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterCommandFactory.java @@ -20,7 +20,7 @@ public ShooterCommandFactory(Shooter shooter) { /** * Sets the angle of the shooter in degrees * - * @param angle andle in degrees + * @param angle angle in degrees * @return Command to set shooter angle */ public Command setAngle(double angle) { From 69a9121bc3e8f34f13675766f9701ad5cd8e834f Mon Sep 17 00:00:00 2001 From: NightForts7798 Date: Wed, 20 Mar 2024 19:40:34 -0400 Subject: [PATCH 05/14] resolve merge issues --- src/main/java/frc/robot/RobotContainer.java | 43 +++++++++------------ 1 file changed, 19 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e1807cd0..3c3cae83 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,6 +50,7 @@ import frc.robot.subsystems.intake.Intake.IntakePosition; import frc.robot.subsystems.leds.LEDs; import frc.robot.subsystems.leds.commands.CmdLEDsRunLEDs; +import frc.robot.subsystems.multisubsystemcommands.CmdAdjustShooterAutomatically; import frc.robot.subsystems.multisubsystemcommands.GrpShootNoteInZone; import frc.robot.subsystems.shooter.RealShooterIO; import frc.robot.subsystems.shooter.Shooter; @@ -75,14 +76,13 @@ public class RobotContainer { private final Drive _drive; private final Intake _intake; private final Shooter _shooter; - private final Climber _climber; - private final LEDs _leds; private final Climber _climber; + private final LEDs _leds; private double _driveMultiplier = 1.0; private Command _adjustShooterAutomaticallyCommand; - private Command _runLEDsCommand; + private Command _runLEDsCommand; // logged dashboard inputs private final LoggedDashboardChooser _autoChooser; @@ -150,7 +150,6 @@ public RobotContainer() { _shooter = new Shooter(new RealShooterIO()); _climber = new Climber(new RealClimberIO()); _leds = new LEDs(); - _climber = new Climber(new RealClimberIO()); break; case SIM: @@ -172,9 +171,7 @@ public RobotContainer() { }); _climber = new Climber(new ClimberIO() { }); - _leds = new LEDs(); - _climber = new Climber(new ClimberIO() { - }); + _leds = new LEDs(); break; default: // Replayed robot, disable IO implementations @@ -199,9 +196,7 @@ public RobotContainer() { }); _climber = new Climber(new ClimberIO() { }); - _leds = new LEDs(); - _climber = new Climber(new ClimberIO() { - }); + _leds = new LEDs(); break; } @@ -352,19 +347,19 @@ private void configureButtonBindings() { // _op2ButtonOne.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Subwoofer)); // _op2ButtonTwo.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Podium)); - _op2ButtonFour.onTrue(new Command() { + _op2ButtonFour.onTrue(new Command() { - @Override - public void initialize(){ - _leds.setAmpReady(true); - } + @Override + public void initialize(){ + _leds.setAmpReady(true); + } - @Override - public boolean isFinished(){ - return true; - } - } - ); + @Override + public boolean isFinished(){ + return true; + } + } + ); // // Reset hasNote in case the robot thinks that it has a note when it doesn't // _op2ButtonSix.onTrue(Commands.runOnce(() -> _intake.resetHasNote())); @@ -431,7 +426,7 @@ public Command getSetInitalShooterPosition() { public Command getFeederInitialStateCommand() { return Commands.runOnce(() -> _intake.setFeederMotorPickupSpeed()); } - public Command getRunLEDs() { - return this._runLEDsCommand; - } + public Command getRunLEDs() { + return this._runLEDsCommand; + } } From 8a6f310f6a5d71df27c21402d1525cfa02a9681d Mon Sep 17 00:00:00 2001 From: NightForts7798 Date: Wed, 20 Mar 2024 20:19:23 -0400 Subject: [PATCH 06/14] fix checkstyle --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c3cae83..bb5e7bfa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,7 +49,6 @@ import frc.robot.subsystems.drive.commands.DriveCommands; import frc.robot.subsystems.intake.Intake.IntakePosition; import frc.robot.subsystems.leds.LEDs; -import frc.robot.subsystems.leds.commands.CmdLEDsRunLEDs; import frc.robot.subsystems.multisubsystemcommands.CmdAdjustShooterAutomatically; import frc.robot.subsystems.multisubsystemcommands.GrpShootNoteInZone; import frc.robot.subsystems.shooter.RealShooterIO; From 5536911b53efa0ca2460a5d88fbbfa0b6e0ecf29 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 20 Mar 2024 21:03:38 -0400 Subject: [PATCH 07/14] fix merge --- src/main/java/frc/robot/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 80d109a8..7b6536c8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -202,9 +202,9 @@ public void teleopInit() { } //if the runLED command is NOT scheduled then schedule it if(!CommandScheduler.getInstance().isScheduled( - _robotContainer.getRunLEDs())){ - _robotContainer.getRunLEDs().schedule(); - } + _robotContainer.getRunLEDs())){ + _robotContainer.getRunLEDs().schedule(); + } _robotContainer.getSetInitalShooterPosition().schedule(); //_robotContainer.getRunAnglePIDCommand().schedule(); From c431cc9b0382b6fda6c22c74811d67b79533b071 Mon Sep 17 00:00:00 2001 From: area5188 Date: Wed, 20 Mar 2024 21:24:03 -0400 Subject: [PATCH 08/14] fixed merge error part 2 --- src/main/java/frc/robot/Robot.java | 7 +-- src/main/java/frc/robot/RobotContainer.java | 66 +++++++-------------- 2 files changed, 24 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b6536c8..5f4ea9b4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -166,13 +166,12 @@ public void autonomousInit() { _robotContainer.getAdjustShooterAutomaticallyCommand().schedule(); _robotContainer.getFeederInitialStateCommand().schedule(); } - // if the runLED command is NOT scheduled then schedule it + if(!CommandScheduler.getInstance().isScheduled( _robotContainer.getRunLEDs())){ _robotContainer.getRunLEDs().schedule(); } - // _robotContainer.getRunAnglePIDCommand().schedule(); _robotContainer.getSetInitalShooterPosition().schedule(); // schedule the autonomous command (example) @@ -200,7 +199,7 @@ public void teleopInit() { _robotContainer.getAdjustShooterAutomaticallyCommand().schedule(); _robotContainer.getFeederInitialStateCommand().schedule(); } - //if the runLED command is NOT scheduled then schedule it + if(!CommandScheduler.getInstance().isScheduled( _robotContainer.getRunLEDs())){ _robotContainer.getRunLEDs().schedule(); @@ -233,4 +232,4 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 67eaf1c4..8e8ac405 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -33,13 +34,13 @@ import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.RealClimberIO; -import frc.robot.subsystems.climber.commands.CmdClimberMove; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.RealIntakeIO; +import frc.robot.subsystems.leds.LEDs; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIONavX2; import frc.robot.subsystems.drive.ModuleIO; @@ -49,10 +50,7 @@ import frc.robot.subsystems.drive.commands.CmdDriveAutoAim; import frc.robot.subsystems.drive.commands.DriveCommands; import frc.robot.subsystems.intake.Intake.IntakePosition; -import frc.robot.subsystems.leds.LEDs; -import frc.robot.subsystems.leds.commands.CmdLEDsRunLEDs; import frc.robot.subsystems.multisubsystemcommands.CmdAdjustShooterAutomatically; -import frc.robot.subsystems.multisubsystemcommands.CmdShootOnTheMove; import frc.robot.subsystems.multisubsystemcommands.GrpShootNoteInZone; import frc.robot.subsystems.shooter.RealShooterIO; import frc.robot.subsystems.shooter.Shooter; @@ -82,9 +80,6 @@ public class RobotContainer { private final LEDs _leds; private double _driveMultiplier = 1.0; - private Command _runShooterPIDCommand; - private Command _runIntakePIDCommand; - private Command _runLEDsCommand; private Command _adjustShooterAutomaticallyCommand; private Command _runLEDsCommand; @@ -177,7 +172,6 @@ public RobotContainer() { _climber = new Climber(new ClimberIO() { }); _leds = new LEDs(); - _leds = new LEDs(); break; default: // Replayed robot, disable IO implementations @@ -203,7 +197,6 @@ public RobotContainer() { _climber = new Climber(new ClimberIO() { }); _leds = new LEDs(); - _leds = new LEDs(); break; } @@ -307,21 +300,6 @@ private void configureButtonBindings() { Commands.runOnce( () -> _drive.setPose(robotOnSubwooferRed), _drive).ignoringDisable(true)); - _op2ButtonFour.onTrue(new Command() { - - @Override - public void initialize(){ - _leds.setAmpReady(true); - } - - @Override - public boolean isFinished(){ - return true; - } - } - ); - - // _op2ButtonSix.onTrue(new Command() { /* * ================================ * Climber Controller @@ -364,25 +342,25 @@ public boolean isFinished(){ // Run intake rollers, stop when we let go of button _opButtonNine.onTrue(this._intake.buildCommand().acquire()) .onFalse(this._intake.buildCommand().stop()); + + // _op2ButtonFour.onTrue(new Command() { + + // @Override + // public void initialize(){ + // _leds.setAmpReady(true); + // } + + // @Override + // public boolean isFinished(){ + // return true; + // } + // } + // ); // Move to shooter positions manually // _op2ButtonOne.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Subwoofer)); // _op2ButtonTwo.onTrue(new GrpShootNoteInZone(_intake, _shooter, ShooterZone.Podium)); - _op2ButtonFour.onTrue(new Command() { - - @Override - public void initialize(){ - _leds.setAmpReady(true); - } - - @Override - public boolean isFinished(){ - return true; - } - } - ); - // // Reset hasNote in case the robot thinks that it has a note when it doesn't // _op2ButtonSix.onTrue(Commands.runOnce(() -> _intake.resetHasNote())); @@ -441,12 +419,6 @@ public Command getRunAnglePIDCommand() { return _shooter.buildCommand().runAnglePID(); } - public Command getRunIntakePIDCommand() { - return this._runIntakePIDCommand; - } - public Command getRunLEDs() { - return this._runLEDsCommand; - } public Command getSetInitalShooterPosition() { return _shooter.buildCommand().setPositionByZone(ShooterZone.Unknown); } @@ -454,4 +426,8 @@ public Command getSetInitalShooterPosition() { public Command getFeederInitialStateCommand() { return Commands.runOnce(() -> _intake.setFeederMotorPickupSpeed()); } -} + + public Command getRunLEDs() { + return this._runLEDsCommand; + } +} \ No newline at end of file From 6313a0cdb2ecec94995835c5cdaa62adc56d23b4 Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Wed, 27 Mar 2024 17:13:56 -0400 Subject: [PATCH 09/14] save code changes --- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../subsystems/drive/DriveConstants.java | 4 +-- .../drive/commands/CmdDriveAutoAim.java | 26 ++++++++++--------- 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0c6d958..b4d5061e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -253,8 +253,8 @@ private void configureButtonBindings() { // () -> _driveController.getLeftX(), // () -> _driveController.getRightX())); _driveController.leftBumper().whileTrue(new CmdDriveAutoAim(_drive, - () -> -_driveController.getLeftY()*_driveMultiplier, - () -> -_driveController.getLeftX()*_driveMultiplier)); + () -> _driveController.getLeftY()*_driveMultiplier, + () -> _driveController.getLeftX()*_driveMultiplier)); _driveController.a().whileTrue( diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 438e577b..0ac4dc36 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -81,8 +81,8 @@ public class DriveConstants { public static final double MODULE_TURNPID_KD = 0.0; // PID constants for auto aim command - public static final double AUTO_ROTATE_P = 0.13; - public static final double AUTO_ROTATE_I = 0.003; + public static final double AUTO_ROTATE_P = 0.08; + public static final double AUTO_ROTATE_I = 0.002; public static final double AUTO_ROTATE_D = 0.00075; public static final double AUTO_ROTATE_TOLERANCE = 3.0; diff --git a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java index 0fd0d8db..21881bc6 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java @@ -77,12 +77,16 @@ public void execute() { Logger.recordOutput("Drive/autoaim/autorotatedactualDegrees", currentAngleDegrees); // this is what drives the robot - this._drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - _translationXSupplier.getAsDouble(), - _translationYSupplier.getAsDouble(), - rotationVal, - _drive.getRotation())); + // drive the robot based on the calculations from above + // _drive.runVelocity( + // _drive.transformJoystickInputsToChassisSpeeds( + // _translationXSupplier.getAsDouble(), + // _translationYSupplier.getAsDouble(), + // rotationVal, true)); + _drive.runVelocity(_drive.transformJoystickInputsToChassisSpeeds( + _translationXSupplier.getAsDouble(), + _translationYSupplier.getAsDouble(), + 0, false)); } @@ -90,12 +94,10 @@ public void execute() { @Override public void end(boolean interrupted) { // when we finish set the rotation to 0 but keep driving - this._drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - _translationXSupplier.getAsDouble(), - _translationYSupplier.getAsDouble(), - 0, - _drive.getGyroscopeRotation())); + _drive.runVelocity(_drive.transformJoystickInputsToChassisSpeeds( + _translationXSupplier.getAsDouble(), + _translationYSupplier.getAsDouble(), + 0, false)); } From 1181e79e09b1c2ee4a0d353f5ba5c6845afd0225 Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Thu, 28 Mar 2024 12:27:59 -0400 Subject: [PATCH 10/14] pass check syyle --- .../frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java index 260e51e7..c5075701 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; From 3ba78c307c99d5fee4e4b0dc731180860bd08fac Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Thu, 28 Mar 2024 12:32:54 -0400 Subject: [PATCH 11/14] remove climber from leds --- src/main/java/frc/robot/RobotContainer.java | 3 +++ .../subsystems/leds/commands/CmdLEDsRunLEDs.java | 16 +++++----------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e4aa11e..707f85d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.RealIntakeIO; import frc.robot.subsystems.leds.LEDs; +import frc.robot.subsystems.leds.commands.CmdLEDsRunLEDs; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIONavX2; import frc.robot.subsystems.drive.ModuleIO; @@ -83,6 +84,7 @@ public class RobotContainer { private Command _adjustShooterAutomaticallyCommand; private Command _runLEDsCommand; + // logged dashboard inputs private final LoggedDashboardChooser _autoChooser; @@ -201,6 +203,7 @@ public RobotContainer() { // setup hand-scheduled commands _adjustShooterAutomaticallyCommand = new CmdAdjustShooterAutomatically(_drive, _shooter, _intake); + _runLEDsCommand = new CmdLEDsRunLEDs(_leds, _shooter, _intake); NamedCommands.registerCommand("Pickup_Note_Without_Limelight", _intake.buildCommand().pickUpFromGround(4000)); NamedCommands.registerCommand("Pickup_Note_With_Limelight", new PrintCommand("[ERROR] Not implemented")); diff --git a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java index 55bee33f..fd5cea27 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.leds.LEDs; import frc.robot.subsystems.leds.LEDs.LEDAnimation; @@ -12,14 +11,12 @@ public class CmdLEDsRunLEDs extends Command { private LEDs _leds; private Shooter _shooter; private Intake _intake; - private Climber _climber; private int _timer; - public CmdLEDsRunLEDs(LEDs leds, Shooter shooter, Intake intake, Climber climber) { + public CmdLEDsRunLEDs(LEDs leds, Shooter shooter, Intake intake) { _leds = leds; _shooter = shooter; _intake = intake; - _climber = climber; } @Override @@ -39,13 +36,10 @@ public void execute() { } else if (_leds.shouldRunHasNoteAnimation(_intake.hasNote())) { // Only run this animation one time, right when the intake first picks up a note _leds.runAnimation(LEDAnimation.PickedUpNote); - } else if (_climber.isClimbing()) { - // Runs when climber is active - _leds.runAnimation(LEDAnimation.ClimberAscending); - } else if (_leds.getAmpReady()) { - // Runs when human player should activate Amp - _leds.runAnimation(LEDAnimation.AmpReady); - _leds.setAmpReady(false); + // } else if (_leds.getAmpReady()) { + // // Runs when human player should activate Amp + // _leds.runAnimation(LEDAnimation.AmpReady); + // _leds.setAmpReady(false); } else if (DriverStation.isDisabled()) { // Rainbows while robot is disabled _leds.runAnimation(LEDAnimation.RobotDisabled); From 8315862ae3aaa2ed0a4d2299084ab1a375f1b6f3 Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Thu, 28 Mar 2024 13:54:09 -0400 Subject: [PATCH 12/14] LEDS WORKING --- src/main/java/frc/robot/Robot.java | 5 +++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../drive/commands/CmdDriveAutoAim.java | 2 + .../java/frc/robot/subsystems/leds/LEDs.java | 41 +++++++++++++++++-- .../leds/commands/CmdLEDsRunLEDs.java | 24 ++++++----- 5 files changed, 59 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bfd3b355..3c3ab53a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -162,6 +162,11 @@ public void robotPeriodic() { public void disabledInit() { _robotContainer.getRunAnglePIDCommand().cancel(); SmartDashboard.putData("Autonomous Selection Preview", this._autonomousTrajectory); + + if(!CommandScheduler.getInstance().isScheduled(_robotContainer.getRunLEDs())){ + _robotContainer.getRunLEDs().schedule(); + } + } /** This function is called periodically when disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 707f85d7..cb8b159d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -203,7 +203,7 @@ public RobotContainer() { // setup hand-scheduled commands _adjustShooterAutomaticallyCommand = new CmdAdjustShooterAutomatically(_drive, _shooter, _intake); - _runLEDsCommand = new CmdLEDsRunLEDs(_leds, _shooter, _intake); + _runLEDsCommand = new CmdLEDsRunLEDs(_leds, _shooter, _intake).ignoringDisable(true); NamedCommands.registerCommand("Pickup_Note_Without_Limelight", _intake.buildCommand().pickUpFromGround(4000)); NamedCommands.registerCommand("Pickup_Note_With_Limelight", new PrintCommand("[ERROR] Not implemented")); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java index 260e51e7..d2efe4f3 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java @@ -88,6 +88,7 @@ public void execute() { rotationVal, true)); Logger.recordOutput("Drive/autoaim/isReady", _angleController.atSetpoint()); + Logger.recordOutput("Drive/autoaim/enabled", true); } @@ -100,6 +101,7 @@ public void end(boolean interrupted) { _translationYSupplier.getAsDouble(), 0, false)); Logger.recordOutput("Drive/autoaim/isReady", false); + Logger.recordOutput("Drive/autoaim/enabled", false); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index 0bb3b040..8fd3ffd6 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -1,7 +1,11 @@ package frc.robot.subsystems.leds; +import org.easymock.bytebuddy.dynamic.scaffold.TypeInitializer.None; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.FireAnimation; import com.ctre.phoenix.led.LarsonAnimation; import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; @@ -16,14 +20,17 @@ public class LEDs { boolean _prevHasNote; boolean _ampReady = false; + public enum LEDAnimation { None(null, null, 0), + // RobotIdle(null, new FireAnimation(1.0, 0.02, _numLEDs, 0.01, 0.0), 0), + RobotIdle(new LEDColor(255, 128, 0), null, 0), ReadyToShoot(new LEDColor(0, 255, 0), null, 3), PickedUpNote(null, new StrobeAnimation(255, 128, 0, 0, 0.5, _numLEDs), 3), ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3), AmpReady(null, new StrobeAnimation(204, 0, 255, 0, 0.5, _numLEDs), 3), //Change time on RobotDisabled - RobotDisabled(null, new RainbowAnimation(100, 0.5, _numLEDs), 3); + RobotDisabled(null, new RainbowAnimation(100, 0.5, _numLEDs), 0); LEDColor _color; Animation _animation; int _secondsToRun; @@ -56,14 +63,19 @@ public LEDAnimation getCurrentAnimation() { } public void runAnimation(LEDAnimation animation) { + Logger.recordOutput("LEDS/currentAnimation", _currentAnimation); + Logger.recordOutput("LEDS/requestedAnimation", animation); if (animation != _currentAnimation) { _currentAnimation = animation; if (animation.getColor() == null) { _candle.clearAnimation(0); + _candle.setLEDs(0,0,0); _candle.animate(animation.getAnimation()); } else if (animation.getAnimation() == null){ LEDColor color = animation.getColor(); + _candle.clearAnimation(0); + _candle.setLEDs(0,0,0); _candle.setLEDs(color.getR(), color.getB(), color.getG()); } else { _candle.clearAnimation(0); @@ -75,17 +87,38 @@ public void runAnimation(LEDAnimation animation) { } } - public void stopAnimationIfTimedOut(int currentRunTimeIn20Ms) { + /** + * returns true if the animation was stopped so we can reset the timer + * @param currentRunTimeIn20Ms + * @return + */ + public boolean stopAnimationIfTimedOut(int currentRunTimeIn20Ms) { // Stop the current animation if it has run for its time int animationTime = _currentAnimation.getSecondsToRun(); + boolean wasAnimationStopped = false; if (animationTime > 0) { // We actually have a timeout if (currentRunTimeIn20Ms >= animationTime * 50) { - // We have finished running, so stop the animation + Logger.recordOutput("LEDS/animationTimeout",true); + wasAnimationStopped = true; + if (_currentAnimation.getColor() == null) { _candle.clearAnimation(0); - _currentAnimation = LEDAnimation.None; + } else if (_currentAnimation.getAnimation() == null){ + _candle.setLEDs(0, 0, 0); + } else { + _candle.clearAnimation(0); + _candle.setLEDs(0, 0, 0); + } + _currentAnimation = LEDAnimation.None; } + else{ + Logger.recordOutput("LEDS/animationTimeout",false); + } + } + else{ + Logger.recordOutput("LEDS/animationTimeout",false); } + return wasAnimationStopped; } public boolean shouldRunHasNoteAnimation(boolean hasNote) { diff --git a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java index fd5cea27..43bbdc1b 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -27,22 +27,26 @@ public void initialize() { @Override public void execute() { // Stop running the current animation if it's run for long enough - _leds.stopAnimationIfTimedOut(_timer); + boolean wasAnimationStopped = _leds.stopAnimationIfTimedOut(_timer); + if(wasAnimationStopped){ + _timer = 0; + } - // Start a new animation if we meet conditions - if (_shooter.isReady()) { + // // Start a new animation if we meet conditions + if (DriverStation.isDisabled()) { + // rainbow when DS is disabled + _leds.runAnimation(LEDAnimation.RobotDisabled); + } else if (_shooter.isReady()) { // Run this animation when the shooter is ready _leds.runAnimation(LEDAnimation.ReadyToShoot); } else if (_leds.shouldRunHasNoteAnimation(_intake.hasNote())) { // Only run this animation one time, right when the intake first picks up a note + // blinks orange when we pick up a note _leds.runAnimation(LEDAnimation.PickedUpNote); - // } else if (_leds.getAmpReady()) { - // // Runs when human player should activate Amp - // _leds.runAnimation(LEDAnimation.AmpReady); - // _leds.setAmpReady(false); - } else if (DriverStation.isDisabled()) { - // Rainbows while robot is disabled - _leds.runAnimation(LEDAnimation.RobotDisabled); + } + else{ + // solid orange when we are doing nothing + _leds.runAnimation(LEDAnimation.RobotIdle); } // Increment the timer for another loop From 598e9bf744dd0be1f86deaf6c7333b03d3118f57 Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Thu, 28 Mar 2024 14:17:13 -0400 Subject: [PATCH 13/14] autoaim LEDS are working! --- .../drive/commands/CmdDriveAutoAim.java | 4 +-- .../java/frc/robot/subsystems/leds/LEDs.java | 25 +++++++++++++++++++ .../leds/commands/CmdLEDsRunLEDs.java | 16 +++++++++--- .../frc/robot/subsystems/shooter/Shooter.java | 3 ++- 4 files changed, 42 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java index d2efe4f3..bcd701ea 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java @@ -88,7 +88,7 @@ public void execute() { rotationVal, true)); Logger.recordOutput("Drive/autoaim/isReady", _angleController.atSetpoint()); - Logger.recordOutput("Drive/autoaim/enabled", true); + Logger.recordOutput("Drive/autoaim/isEnabled", true); } @@ -101,7 +101,7 @@ public void end(boolean interrupted) { _translationYSupplier.getAsDouble(), 0, false)); Logger.recordOutput("Drive/autoaim/isReady", false); - Logger.recordOutput("Drive/autoaim/enabled", false); + Logger.recordOutput("Drive/autoaim/isEnabled", false); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index 8fd3ffd6..dfc18d9b 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -11,6 +11,9 @@ import com.ctre.phoenix.led.StrobeAnimation; import com.ctre.phoenix.led.LarsonAnimation.BounceMode; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.HardwareConstants; public class LEDs { @@ -19,6 +22,9 @@ public class LEDs { LEDAnimation _currentAnimation = LEDAnimation.None; boolean _prevHasNote; boolean _ampReady = false; + NetworkTable _adkitNetworkTable; + BooleanSubscriber _isAutoAimEnabled; + BooleanSubscriber _isAutoAimReady; public enum LEDAnimation { @@ -26,6 +32,9 @@ public enum LEDAnimation { // RobotIdle(null, new FireAnimation(1.0, 0.02, _numLEDs, 0.01, 0.0), 0), RobotIdle(new LEDColor(255, 128, 0), null, 0), ReadyToShoot(new LEDColor(0, 255, 0), null, 3), + SolidRed(new LEDColor(255, 0, 0), null, 0), + SolidGreen(new LEDColor(0, 255, 0), null, 0), + BlinkingRed(null, new StrobeAnimation(255, 0, 0, 0, 0.5, _numLEDs), 3), PickedUpNote(null, new StrobeAnimation(255, 128, 0, 0, 0.5, _numLEDs), 3), ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3), AmpReady(null, new StrobeAnimation(204, 0, 255, 0, 0.5, _numLEDs), 3), @@ -56,12 +65,28 @@ public int getSecondsToRun() { public LEDs() { _candle = new CANdle(HardwareConstants.CanIds.CANDLE_ID); + _adkitNetworkTable = NetworkTableInstance.getDefault().getTable("AdvantageKit"); + _isAutoAimEnabled = _adkitNetworkTable.getBooleanTopic("RealOutputs/Drive/autoaim/isEnabled") + .subscribe(false); + _isAutoAimReady = _adkitNetworkTable.getBooleanTopic("RealOutputs/Drive/autoaim/isReady") + .subscribe(false); } public LEDAnimation getCurrentAnimation() { return _currentAnimation; } + public boolean _isAutoAimEnabled(){ + boolean enabled = _isAutoAimEnabled.get(); + Logger.recordOutput("LEDS/isAutoAimEnabled", enabled); + return enabled; + } + + public boolean _isAutoAimReady(){ + boolean enabled = _isAutoAimReady.get(); + Logger.recordOutput("LEDS/isAutoAimReady", enabled); + return enabled; } + public void runAnimation(LEDAnimation animation) { Logger.recordOutput("LEDS/currentAnimation", _currentAnimation); Logger.recordOutput("LEDS/requestedAnimation", animation); diff --git a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java index 43bbdc1b..a5d98124 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -36,9 +36,19 @@ public void execute() { if (DriverStation.isDisabled()) { // rainbow when DS is disabled _leds.runAnimation(LEDAnimation.RobotDisabled); - } else if (_shooter.isReady()) { - // Run this animation when the shooter is ready - _leds.runAnimation(LEDAnimation.ReadyToShoot); + } else if (_leds._isAutoAimEnabled()) { + // the drive wants to use autoaim + if(!_intake.hasNote()){ + // if we do not have a note, then blink red + _leds.runAnimation(LEDAnimation.BlinkingRed); + }else if(_shooter.shooterInPosition() && _leds._isAutoAimReady()){ + // if the shooter is in position and the autoaim code says its ready + // then signal to the drive we are ready + _leds.runAnimation(LEDAnimation.SolidGreen); + } else{ + // if we have a note but are not in position then we are not ready. + _leds.runAnimation(LEDAnimation.SolidRed); + } } else if (_leds.shouldRunHasNoteAnimation(_intake.hasNote())) { // Only run this animation one time, right when the intake first picks up a note // blinks orange when we pick up a note diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3de4fdbb..73f74671 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -271,7 +271,7 @@ public void setShooterPositionWithRadius(double radius) { } - private boolean shooterInPosition() { + public boolean shooterInPosition() { return Math.abs(_targetShooterAngle - getCurrentPositionInDegrees()) <= ShooterConstants.ANGLE_ENCODER_DEADBAND_DEGREES; } @@ -305,6 +305,7 @@ public void setFlywheelSpeed(double speedInRPM) { public boolean isReady() { return shooterInPosition() && areFlywheelsAtTargetSpeed(); } + public void runAnglePID() { double output = calcAnglePID(); From 18f950252561af6536210887dd9058ddfc70c8e5 Mon Sep 17 00:00:00 2001 From: area5188 Date: Fri, 29 Mar 2024 10:40:43 -0400 Subject: [PATCH 14/14] working and tested on the field --- .../shuffle-board-week-5.json | 8 ++--- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/HardwareConstants.java | 30 ++++++++++++++----- .../subsystems/drive/ModuleIOSparkFlex.java | 4 +-- .../java/frc/robot/subsystems/leds/LEDs.java | 7 +++-- .../leds/commands/CmdLEDsRunLEDs.java | 11 ++++--- 6 files changed, 40 insertions(+), 22 deletions(-) diff --git a/Shuffle-board-layouts/shuffle-board-week-5.json b/Shuffle-board-layouts/shuffle-board-week-5.json index 426dd590..602543f7 100644 --- a/Shuffle-board-layouts/shuffle-board-week-5.json +++ b/Shuffle-board-layouts/shuffle-board-week-5.json @@ -1014,9 +1014,9 @@ } ], "windowGeometry": { - "x": 55.0, - "y": 2.0, - "width": 1870.0, - "height": 1085.0 + "x": 0.800000011920929, + "y": 0.800000011920929, + "width": 1534.4000244140625, + "height": 814.4000244140625 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 71525b1e..8146afb3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,7 +23,7 @@ */ public final class Constants { public static final Mode CURRENT_MODE = Mode.REAL; - public static final boolean TUNING_MODE = true; + public static final boolean TUNING_MODE = false; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 83885dbf..c66ecc1f 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Translation3d; public class HardwareConstants { - public static final int NUMBER_OF_CAMERAS = 2; + public static final int NUMBER_OF_CAMERAS = 3; public class CanIds { // |====================== SWERVE MODULE CAN IDs ======================| public static int FL_DRIVE = 1; @@ -59,29 +59,43 @@ public class ComponentTransformations { * positive X, and where the left side of the robot is the negative Y. */ // How far foward/backward the camera is from robot center. - private static final double CAMERA_ONE_X_FROM_ROBOT_CENTER = -0.279; // Mechanical offset + 1/8" offset + private static final double CAMERA_ONE_X_FROM_ROBOT_CENTER = -0.032 + 0.279; // Mechanical offset + 1/8" offset // How far left/right the camera is from robot center. - private static final double CAMERA_ONE_Y_FROM_ROBOT_CENTER = 0.03 - 0.127; + private static final double CAMERA_ONE_Y_FROM_ROBOT_CENTER = 0.273; // How far up/down the camera is from center if we look at robot from side in 3D // space. private static final double CAMERA_ONE_Z_FROM_ROBOT_CENTER = 0.2583; private static final double CAMERA_ONE_ROLL = 0; private static final double CAMERA_ONE_PITCH = Math.toRadians(20.0); - private static final double CAMERA_ONE_YAW = Math.toRadians(180.0); + private static final double CAMERA_ONE_YAW = Math.toRadians(0.0); // Intake Camera private static Transform3d _cameraOnePosition = new Transform3d( new Translation3d(CAMERA_ONE_X_FROM_ROBOT_CENTER, CAMERA_ONE_Y_FROM_ROBOT_CENTER, CAMERA_ONE_Z_FROM_ROBOT_CENTER), new Rotation3d(CAMERA_ONE_ROLL, CAMERA_ONE_PITCH, CAMERA_ONE_YAW)); - private static final double CAMERA_TWO_X_FROM_ROBOT_CENTER = 0.0279 + 0.121; // Mechanical offset - 1/8" offset - private static final double CAMERA_TWO_Y_FROM_ROBOT_CENTER = 0.1586 + 0.1524; + private static final double CAMERA_TWO_X_FROM_ROBOT_CENTER = -0.16 - 0.254; // Mechanical offset - 1/8" offset + private static final double CAMERA_TWO_Y_FROM_ROBOT_CENTER = -0.273; private static final double CAMERA_TWO_Z_FROM_ROBOT_CENTER = 0.2583; private static final double CAMERA_TWO_ROLL = 0; private static final double CAMERA_TWO_PITCH = Math.toRadians(20.0); - private static final double CAMERA_TWO_YAW = Math.toRadians(0.0); + private static final double CAMERA_TWO_YAW = Math.toRadians(180.0); + + // METERS + private static final double CAMERA_THREE_X_FROM_ROBOT = -0.2508; + private static final double CAMERA_THREE_Y_FROM_ROBOT = 0.2508; + private static final double CAMERA_THREE_Z_FROM_ROBOT = 0.27; + + private static final double CAMERA_THREE_ROLL = 0.0; + private static final double CAMERA_THREE_PITCH = Math.toRadians(28.125); + private static final double CAMERA_THREE_YAW = Math.toRadians(135.0); + + private static final Transform3d _cameraThreePosition = new Transform3d( + new Translation3d(CAMERA_THREE_X_FROM_ROBOT, CAMERA_THREE_Y_FROM_ROBOT, CAMERA_THREE_Z_FROM_ROBOT), + new Rotation3d(CAMERA_THREE_ROLL, CAMERA_THREE_PITCH, CAMERA_THREE_YAW) + ); // Shooter Camera private static Transform3d _cameraTwoPosition = new Transform3d( @@ -89,7 +103,7 @@ public class ComponentTransformations { new Rotation3d(CAMERA_TWO_ROLL, CAMERA_TWO_PITCH, CAMERA_TWO_YAW)); // TODO: Swapped camera position. Rename after competition. - public static Transform3d[] _cameraPosition = new Transform3d[] {_cameraTwoPosition, _cameraOnePosition}; + public static Transform3d[] _cameraPosition = new Transform3d[] {_cameraOnePosition, _cameraTwoPosition, _cameraThreePosition}; // |====================== END VISION SUBSYSTEM TRANSFORMATIONS ======================| diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java index 49cbe2af..ae37970f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java @@ -102,8 +102,8 @@ public ModuleIOSparkFlex(int index) { _turnSparkFlex.setSmartCurrentLimit(30); _driveSparkFlex.enableVoltageCompensation(12.0); _turnSparkFlex.enableVoltageCompensation(12.0); - _driveSparkFlex.setClosedLoopRampRate(0.25); - _turnSparkFlex.setClosedLoopRampRate(0.25); + _driveSparkFlex.setClosedLoopRampRate(0.1); + _turnSparkFlex.setClosedLoopRampRate(0.1); _driveEncoder.setPosition(0.0); _driveEncoder.setMeasurementPeriod(10); diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index dfc18d9b..d1f3503e 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -18,7 +18,7 @@ public class LEDs { CANdle _candle; - static int _numLEDs = 8; + static int _numLEDs = 27; LEDAnimation _currentAnimation = LEDAnimation.None; boolean _prevHasNote; boolean _ampReady = false; @@ -35,6 +35,7 @@ public enum LEDAnimation { SolidRed(new LEDColor(255, 0, 0), null, 0), SolidGreen(new LEDColor(0, 255, 0), null, 0), BlinkingRed(null, new StrobeAnimation(255, 0, 0, 0, 0.5, _numLEDs), 3), + SolidYellow(new LEDColor(255, 128, 0), null, 0), PickedUpNote(null, new StrobeAnimation(255, 128, 0, 0, 0.5, _numLEDs), 3), ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3), AmpReady(null, new StrobeAnimation(204, 0, 255, 0, 0.5, _numLEDs), 3), @@ -101,12 +102,12 @@ public void runAnimation(LEDAnimation animation) { LEDColor color = animation.getColor(); _candle.clearAnimation(0); _candle.setLEDs(0,0,0); - _candle.setLEDs(color.getR(), color.getB(), color.getG()); + _candle.setLEDs(color.getR(), color.getG(), color.getB()); } else { _candle.clearAnimation(0); _candle.animate(animation.getAnimation()); LEDColor color = animation.getColor(); - _candle.setLEDs(color.getR(), color.getB(), color.getG()); + _candle.setLEDs(color.getR(), color.getG(), color.getB()); } } diff --git a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java index a5d98124..fb4319f0 100644 --- a/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/commands/CmdLEDsRunLEDs.java @@ -49,10 +49,13 @@ public void execute() { // if we have a note but are not in position then we are not ready. _leds.runAnimation(LEDAnimation.SolidRed); } - } else if (_leds.shouldRunHasNoteAnimation(_intake.hasNote())) { - // Only run this animation one time, right when the intake first picks up a note - // blinks orange when we pick up a note - _leds.runAnimation(LEDAnimation.PickedUpNote); + // } else if (_leds.shouldRunHasNoteAnimation(_intake.hasNote())) { + // // Only run this animation one time, right when the intake first picks up a note + // // blinks orange when we pick up a note + // _leds.runAnimation(LEDAnimation.PickedUpNote); + // } + } else if(_intake.hasNote()){ + _leds.runAnimation(LEDAnimation.SolidYellow); } else{ // solid orange when we are doing nothing