From 5bac3c896c6872edb6bba197dee54cad0425507c Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Mon, 22 Jan 2024 11:33:56 -0500 Subject: [PATCH 01/42] [#64] Added test named Command --- src/main/java/frc/robot/RobotContainer.java | 5 +++++ src/main/java/frc/thunder | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6cfc9e32..d6b859a3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -76,6 +76,11 @@ protected void initializeSubsystems() { collision = new Collision(drivetrain); } + @Override + protected void initializeNamedCommands() { + NamedCommands.registerCommand("test", new InstantCommand(() -> System.out.println("Hello World!"))); + } + @Override protected void configureButtonBindings() { new Trigger(driver::getLeftBumper).onTrue(drivetrain.runOnce(drivetrain::seedFieldRelative)); diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 18c97aaf..cc1afe9d 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 18c97aaf1f29dacc7e36d265ee0c94bf4b33d118 +Subproject commit cc1afe9d9e9419b3dda674293ffb5b7480b2bd1a From 5546e1427c97d6fcb8f0ff1f486d1ddb5e771b95 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Mon, 22 Jan 2024 20:49:38 -0500 Subject: [PATCH 02/42] [#64] Path to test named command --- .../deploy/pathplanner/autos/TestNAmes.auto | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/TestNAmes.auto diff --git a/src/main/deploy/pathplanner/autos/TestNAmes.auto b/src/main/deploy/pathplanner/autos/TestNAmes.auto new file mode 100644 index 00000000..73e68898 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TestNAmes.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.0, + "y": 0.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1MeterSquare" + } + }, + { + "type": "named", + "data": { + "name": "test" + } + }, + { + "type": "path", + "data": { + "pathName": "4MeterSquare" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file From 68433acac393fa87cb79f21de143d16198e38177 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Wed, 24 Jan 2024 20:33:34 -0500 Subject: [PATCH 03/42] [#88] basic system tests --- src/main/java/frc/robot/RobotContainer.java | 14 ++++++ .../frc/robot/command/tests/DriveTest.java | 46 +++++++++++++++++++ .../command/tests/DrivetrainSystemTest.java | 26 +++++++++++ .../robot/command/tests/TurnSystemTest.java | 43 +++++++++++++++++ 4 files changed, 129 insertions(+) create mode 100644 src/main/java/frc/robot/command/tests/DriveTest.java create mode 100644 src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java create mode 100644 src/main/java/frc/robot/command/tests/TurnSystemTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 080404fe..77ca2b0a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,12 +10,16 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ControllerConstants; import frc.robot.Constants.DrivetrAinConstants; import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; +import frc.robot.command.tests.DrivetrainSystemTest; +import frc.robot.command.tests.TurnSystemTest; import frc.robot.subsystems.Collector; import frc.robot.subsystems.Collision; import frc.robot.subsystems.Flywheel; @@ -24,7 +28,9 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; import frc.thunder.LightningContainer; +import frc.thunder.command.TimedCommand; import frc.thunder.shuffleboard.LightningShuffleboard; +import frc.thunder.testing.SystemTest; public class RobotContainer extends LightningContainer { XboxController driver; @@ -136,5 +142,13 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { + SystemTest.registerTest("Drive All Directions", new DrivetrainSystemTest(drivetrain, 0.25)); + + SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup( + new TimedCommand(new TurnSystemTest(drivetrain, () -> 0.25), 2), + new WaitCommand(0.5), + new TimedCommand(new TurnSystemTest(drivetrain, () -> -0.25), 2) + + )); } } diff --git a/src/main/java/frc/robot/command/tests/DriveTest.java b/src/main/java/frc/robot/command/tests/DriveTest.java new file mode 100644 index 00000000..b3f33ef5 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/DriveTest.java @@ -0,0 +1,46 @@ +// 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.robot.command.tests; + +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Swerve; + +public class DriveTest extends Command { + + private Swerve drivetrain; + private DoubleSupplier speedX; + private DoubleSupplier speedY; + + public DriveTest(Swerve drivetrain, DoubleSupplier speedX, DoubleSupplier speedY) { + this.drivetrain = drivetrain; + this.speedX = speedX; + this.speedY = speedY; + + addRequirements(drivetrain); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(speedX.getAsDouble()).withVelocityY(speedY.getAsDouble())); + } + + @Override + public void end(boolean interrupted) { + drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java new file mode 100644 index 00000000..d89f8122 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java @@ -0,0 +1,26 @@ +// 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.robot.command.tests; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.Swerve; +import frc.thunder.command.TimedCommand; + +public class DrivetrainSystemTest extends SequentialCommandGroup { + + public DrivetrainSystemTest(Swerve drivetrain, double speed) { + addCommands( + new WaitCommand(0.5), + new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 2), // Forward + new WaitCommand(1), + new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 2), // Backward + new WaitCommand(1), + new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0), 2), // Left + new WaitCommand(1), + new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0), 2) // Right + ); + } +} diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java new file mode 100644 index 00000000..e7ea4d06 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -0,0 +1,43 @@ +// 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.robot.command.tests; + +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Swerve; + +public class TurnSystemTest extends Command { + + private Swerve drivetrain; + private DoubleSupplier speed; + + public TurnSystemTest(Swerve drivetrain, DoubleSupplier speed) { + this.drivetrain = drivetrain; + this.speed = speed; + + addRequirements(drivetrain); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(speed.getAsDouble())); + } + + @Override + public void end(boolean interrupted) { + drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + } + + @Override + public boolean isFinished() { + return false; + } +} From e7da1c447ff0c97c0ffd4d8c5195fe5d3a9e4c10 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 25 Jan 2024 19:51:04 -0500 Subject: [PATCH 04/42] [#64] New path --- src/main/deploy/pathplanner/autos/SM-3.auto | 25 +++++++++++++++++++++ src/main/java/frc/thunder | 2 +- 2 files changed, 26 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/autos/SM-3.auto diff --git a/src/main/deploy/pathplanner/autos/SM-3.auto b/src/main/deploy/pathplanner/autos/SM-3.auto new file mode 100644 index 00000000..1414e5f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SM-3.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SM-3" + } + }, + { + "type": "named", + "data": { + "name": "test" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index cc1afe9d..bc17e326 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit cc1afe9d9e9419b3dda674293ffb5b7480b2bd1a +Subproject commit bc17e3266077838fd43d669470efdb727182e6b6 From 0b831e4966d5190389fa9c996c86ed6ceb358c58 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 25 Jan 2024 19:53:54 -0500 Subject: [PATCH 05/42] [#64] Fix imports --- src/main/java/frc/robot/RobotContainer.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a013f84..ce5d2eab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,7 +3,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; - +import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.XboxController; @@ -114,10 +114,6 @@ protected void configureButtonBindings() { new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED)); } - @Override - protected void initializeNamedCommands() { - } - @Override protected void configureDefaultCommands() { drivetrain.registerTelemetry(logger::telemeterize); From 20e9b3bf6ccd389bd20540493a3484534fd77d56 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 25 Jan 2024 22:33:55 -0500 Subject: [PATCH 06/42] [#64] More Pos logging --- .../pathplanner/autos/1MeterSquareLL.auto | 25 +++++ src/main/deploy/pathplanner/autos/SM-3.auto | 2 +- src/main/deploy/pathplanner/autos/Spin.auto | 2 +- .../pathplanner/autos/Through_Pieces.auto | 24 +++++ .../pathplanner/paths/1MeterSquareLL.path | 97 +++++++++++++++++++ .../pathplanner/paths/Run_Through_Pieces.path | 12 +-- .../paths/{Spin.path => Sipnslow.path} | 0 .../deploy/pathplanner/paths/StraightOut.path | 65 +++++++++++++ src/main/java/frc/robot/RobotContainer.java | 11 ++- .../java/frc/robot/subsystems/Swerve.java | 22 ++++- vendordeps/PathplannerLib.json | 6 +- 11 files changed, 247 insertions(+), 19 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/1MeterSquareLL.auto create mode 100644 src/main/deploy/pathplanner/paths/1MeterSquareLL.path rename src/main/deploy/pathplanner/paths/{Spin.path => Sipnslow.path} (100%) create mode 100644 src/main/deploy/pathplanner/paths/StraightOut.path diff --git a/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto b/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto new file mode 100644 index 00000000..48410087 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "test" + } + }, + { + "type": "path", + "data": { + "pathName": "1MeterSquareLL" + } + } + ] + } + }, + "folder": "Test Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SM-3.auto b/src/main/deploy/pathplanner/autos/SM-3.auto index 1414e5f7..97fb9a92 100644 --- a/src/main/deploy/pathplanner/autos/SM-3.auto +++ b/src/main/deploy/pathplanner/autos/SM-3.auto @@ -8,7 +8,7 @@ { "type": "path", "data": { - "pathName": "SM-3" + "pathName": "Sipnslow" } }, { diff --git a/src/main/deploy/pathplanner/autos/Spin.auto b/src/main/deploy/pathplanner/autos/Spin.auto index 5987af6a..66d3398b 100644 --- a/src/main/deploy/pathplanner/autos/Spin.auto +++ b/src/main/deploy/pathplanner/autos/Spin.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "Spin" + "pathName": "Sipnslow" } } ] diff --git a/src/main/deploy/pathplanner/autos/Through_Pieces.auto b/src/main/deploy/pathplanner/autos/Through_Pieces.auto index 9cbe39a5..dac2c604 100644 --- a/src/main/deploy/pathplanner/autos/Through_Pieces.auto +++ b/src/main/deploy/pathplanner/autos/Through_Pieces.auto @@ -11,11 +11,35 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "disable-Vision" + } + }, { "type": "path", "data": { "pathName": "Run_Through_Pieces" } + }, + { + "type": "named", + "data": { + "name": "test" + } + }, + { + "type": "path", + "data": { + "pathName": "StraightOut" + } + }, + { + "type": "named", + "data": { + "name": "test" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/1MeterSquareLL.path b/src/main/deploy/pathplanner/paths/1MeterSquareLL.path new file mode 100644 index 00000000..8473de26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1MeterSquareLL.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.32, + "y": 5.525487974052871 + }, + "prevControl": null, + "nextControl": { + "x": 1.82, + "y": 5.525487974052871 + }, + "isLocked": false, + "linkedName": "SubwooferFront" + }, + { + "anchor": { + "x": 2.32, + "y": 5.53 + }, + "prevControl": { + "x": 1.8199999999999998, + "y": 5.53 + }, + "nextControl": { + "x": 2.38, + "y": 5.53 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.32, + "y": 6.53 + }, + "prevControl": { + "x": 2.32, + "y": 6.03 + }, + "nextControl": { + "x": 2.32, + "y": 6.59 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.32, + "y": 6.53 + }, + "prevControl": { + "x": 1.82, + "y": 6.53 + }, + "nextControl": { + "x": 1.26, + "y": 6.53 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.32, + "y": 5.525487974052871 + }, + "prevControl": { + "x": 1.32, + "y": 6.025487974052871 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SubwooferFront" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Test paths", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path b/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path index bef3c2aa..e8c3abdb 100644 --- a/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path +++ b/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path @@ -64,16 +64,16 @@ }, { "anchor": { - "x": 0.7319122330552369, - "y": 4.459858150485532 + "x": 1.32, + "y": 5.525487974052871 }, "prevControl": { - "x": 2.616498309919698, - "y": 4.09478126648561 + "x": 3.204586076864461, + "y": 5.160411090052949 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "SubwooferFront" } ], "rotationTargets": [ @@ -103,7 +103,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 118.35496178532406, + "rotation": 180.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Spin.path b/src/main/deploy/pathplanner/paths/Sipnslow.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Spin.path rename to src/main/deploy/pathplanner/paths/Sipnslow.path diff --git a/src/main/deploy/pathplanner/paths/StraightOut.path b/src/main/deploy/pathplanner/paths/StraightOut.path new file mode 100644 index 00000000..980c607b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StraightOut.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.32, + "y": 5.525487974052871 + }, + "prevControl": null, + "nextControl": { + "x": 2.4380339887498943, + "y": 5.525487974052871 + }, + "isLocked": false, + "linkedName": "SubwooferFront" + }, + { + "anchor": { + "x": 2.8631718801899155, + "y": 5.525487974052871 + }, + "prevControl": { + "x": 1.4489583178168204, + "y": 5.525487974052871 + }, + "nextControl": { + "x": 4.27738544256301, + "y": 5.525487974052871 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.458177839432603, + "y": 5.525487974052871 + }, + "prevControl": { + "x": 3.937487206858049, + "y": 5.525487974052871 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ 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 ce5d2eab..c789dc1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer extends LightningContainer { // Shooter shooter; // Collision collision; // Indexer indexer; - Climber climber; + // Climber climber; private SendableChooser autoChooser; // TODO I want field-centric driving in open loop WE NEED TO FIGURE OUT WHAT @@ -79,7 +79,7 @@ protected void initializeSubsystems() { // pivot = new Pivot(); // shooter = new Shooter(pivot, flywheel, indexer); // collision = new Collision(drivetrain); - climber = new Climber(); + // climber = new Climber(); drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop @@ -93,6 +93,7 @@ protected void initializeSubsystems() { @Override protected void initializeNamedCommands() { NamedCommands.registerCommand("test", new InstantCommand(() -> System.out.println("Hello World!"))); + NamedCommands.registerCommand("disable-Vision", new InstantCommand(() -> drivetrain.disableVision())); } @Override @@ -109,9 +110,9 @@ protected void configureButtonBindings() { new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", false)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); - new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", false)); - new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED)); + new Trigger(driver::getYButton).onTrue(new InstantCommand(() -> drivetrain.disableVision()).alongWith(new InstantCommand(() -> System.out.println("Vision Disabled")))); + // new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED)); } @Override @@ -124,7 +125,7 @@ protected void configureDefaultCommands() { .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_MULT) // Drive counterclockwise with negative X (left) )); // climber.setDefaultCommand(new ManualClimb(() -> (coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), climber)); - climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED)); + // climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED)); // shooter.setDefaultCommand(new Shoot(shooter, indexer, drivetrain, () -> coPilot.getAButton())); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f202139a..d2133ef3 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -33,6 +33,7 @@ public class Swerve extends SwerveDrivetrain implements Subsystem { private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); private Limelight[] limelights; private boolean slowMode = false; + private boolean disableVision = false; public Swerve(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { @@ -66,9 +67,16 @@ public void simulationPeriodic() { public void periodic() { for (Limelight limelight : Limelight.filterLimelights(limelights)) { Pose4d pose = limelight.getAlliancePose(); - addVisionMeasurement(pose.toPose2d(), - Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) - - VisionConstants.PROCESS_LATENCY); + // LightningShuffleboard.set("Swerve", "Vision pose", pose.toPose2d()); + LightningShuffleboard.setDouble("Swerve", "Vision pose X", pose.toPose2d().getX()); + LightningShuffleboard.setDouble("Swerve", "Vision pose Y", pose.toPose2d().getY()); + LightningShuffleboard.setDouble("Swerve", "Vision time offset", Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) + - VisionConstants.PROCESS_LATENCY); + if(!disableVision) { + addVisionMeasurement(pose.toPose2d(), + Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) + - VisionConstants.PROCESS_LATENCY); + } } LightningShuffleboard.setDouble("Swerve", "yaw", m_yawGetter.getValueAsDouble()); @@ -98,6 +106,14 @@ AutonomousConstants.DRIVE_BASE_RADIUS, new ReplanningConfig(), }, this); // Subsystem for requirements } + public void disableVision() { + disableVision = true; + } + + public void enableVision() { + disableVision = false; + } + public Supplier getPose() { return () -> getState().Pose; } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 0bf11fbf..6ddd312f 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.4", + "version": "2024.1.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.4" + "version": "2024.1.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.4", + "version": "2024.1.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From d6571008a3d6f648e1b8e33f6fd820f3b42b072c Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Fri, 26 Jan 2024 13:16:46 -0500 Subject: [PATCH 07/42] Use the timestamp now stored in Pose4d (be sure and update Thunder first) --- src/main/java/frc/robot/subsystems/Swerve.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f202139a..69d4aa91 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -66,9 +66,7 @@ public void simulationPeriodic() { public void periodic() { for (Limelight limelight : Limelight.filterLimelights(limelights)) { Pose4d pose = limelight.getAlliancePose(); - addVisionMeasurement(pose.toPose2d(), - Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) - - VisionConstants.PROCESS_LATENCY); + addVisionMeasurement(pose.toPose2d(), pose.getFPGATimestamp()); } LightningShuffleboard.setDouble("Swerve", "yaw", m_yawGetter.getValueAsDouble()); From 120eddc1193afd19fc229d1a309d4c410b53e4b1 Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Fri, 26 Jan 2024 13:31:06 -0500 Subject: [PATCH 08/42] Include atomic method to filter and get pose --- src/main/java/frc/thunder | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index cc1afe9d..62e9221e 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit cc1afe9d9e9419b3dda674293ffb5b7480b2bd1a +Subproject commit 62e9221ef55b37e8c31e03e5357505e998eaf9a3 From 962bf42299b53b328c2a766d23838aedac9732e8 Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Fri, 26 Jan 2024 13:35:19 -0500 Subject: [PATCH 09/42] Use new filteredPoses method to close race condition --- src/main/java/frc/robot/subsystems/Swerve.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 69d4aa91..0aa53e2d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -64,8 +64,7 @@ public void simulationPeriodic() { @Override public void periodic() { - for (Limelight limelight : Limelight.filterLimelights(limelights)) { - Pose4d pose = limelight.getAlliancePose(); + for (Pose4d pose : Limelight.filteredPoses(limelights)) { addVisionMeasurement(pose.toPose2d(), pose.getFPGATimestamp()); } From 560fdf37618db239e717e0f384bbfb1420b1029d Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 26 Jan 2024 22:55:15 -0500 Subject: [PATCH 10/42] [#64] Merged Main --- src/main/java/frc/robot/RobotContainer.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5246c1c1..be17da64 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -116,10 +116,6 @@ protected void configureButtonBindings() { // new Trigger(driver::getYButton).whileTrue(new Climb(climber, drivetrain)); } - @Override - protected void initializeNamedCommands() { - } - @Override protected void configureDefaultCommands() { drivetrain.registerTelemetry(logger::telemeterize); From 5de987cb5367e92576b85cf4cce6cd5eb44015ba Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 27 Jan 2024 08:56:01 -0500 Subject: [PATCH 11/42] [#3] Submodule update --- src/main/java/frc/thunder | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index bc17e326..3f195436 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit bc17e3266077838fd43d669470efdb727182e6b6 +Subproject commit 3f19543610c88f98dbb98247268eb159862f00f0 From 7aa9607cdbfcd5d3aeddc335148a0981f211cd78 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 27 Jan 2024 09:04:16 -0500 Subject: [PATCH 12/42] [#3] Update Submodule --- src/main/java/frc/thunder | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 3f195436..9605b432 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 3f19543610c88f98dbb98247268eb159862f00f0 +Subproject commit 9605b43298b268d6f709309df307e35b3e234261 From bc4efe7f6d70f52a3da9a9c784eade10db8672fe Mon Sep 17 00:00:00 2001 From: Joey456788 Date: Sat, 27 Jan 2024 09:39:08 -0500 Subject: [PATCH 13/42] [#108] - added ontarget check --- .../java/frc/robot/command/chasePieces.java | 115 ++++++++++++++++++ src/main/java/frc/thunder | 2 +- 2 files changed, 116 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/command/chasePieces.java diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java new file mode 100644 index 00000000..84cc7dd2 --- /dev/null +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -0,0 +1,115 @@ +package frc.robot.command; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SwerveDriveBrake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.ControllerConstants; +import frc.robot.Constants.DrivetrAinConstants; +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.Swerve; +import frc.thunder.shuffleboard.LightningShuffleboard; +import frc.thunder.vision.Limelight; + +public class chasePieces extends Command { + + private Swerve drivetrain; + private Limelight limelight; + + private XboxController driver; + private FieldCentric slow; + private FieldCentric drive; + + private SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private Limelight[] limelights; + + private int limelightId = 0; + private double pidOutput; + private double lockedOnHeading; + private double targetHeading; + private boolean useLimelights; + private boolean onTarget; + private PIDController headingController = VisionConstants.HEADING_CONTROLLER; + + /** + * Creates a new PointAtTag. + * @param drivetrain to request movement + * @param driver the driver's controller, used for drive input + * @param limelight_name the name of the limelight to use + * @param useLimelights to get if we want to use vision data or not + * @param noteDetection to get if we want to use this for note detection or april tag detection + */ + public chasePieces(Swerve drivetrain, XboxController driver, String limelight_name, boolean useLimelights, boolean noteDetection) { + this.drivetrain = drivetrain; + this.driver = driver; + this.useLimelights = useLimelights; + + //TODO Figure out which of these is the right one to use + for (var l : drivetrain.getLimelights()) { + if (l.getName().equals(limelight_name)) { + limelight = l; + } + } + + limelight = drivetrain.getLimelights()[0]; + + limelightId = limelight.getPipeline(); + + limelight.setPipeline(VisionConstants.NOTE_PIPELINE); + + drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop + slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + if(Math.abs(limelight.getTargetX()) < VisionConstants.ALIGNMENT_TOLERANCE){ + + onTarget = true; + } + else{ + onTarget = false; + } + + if (onTarget){ + if (driver.getRightBumper()) { + drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) + .withRotationalRate(-pidOutput) // Drive counterclockwise with negative X (left) + ); + } else { + drivetrain.setControl(drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-pidOutput) // Rotate toward the desired direction + ); // Drive counterclockwise with negative X (left) + } + } else { + + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + limelight.setPipeline(limelightId); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index bc17e326..9605b432 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit bc17e3266077838fd43d669470efdb727182e6b6 +Subproject commit 9605b43298b268d6f709309df307e35b3e234261 From e512a1650f98ce5e35433b1b6e659cb9402ef80d Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 27 Jan 2024 10:54:50 -0500 Subject: [PATCH 14/42] [#104 created LED subsystem, added swirl LEDs, created basic LED command' --- src/main/java/frc/robot/Constants.java | 5 +++++ src/main/java/frc/robot/RobotContainer.java | 8 ++++++++ 2 files changed, 13 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e5954665..32c7b690 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -374,4 +374,9 @@ public class ClimbConstants { public static final double CLIMB_PID_SETPOINT_EXTENDED = 10; //TODO: find real values public static final double CLIMB_PID_SETPOINT_RETRACTED = 0; } + + public class LEDsConstants { + public static final int LED_PWM_PORT = 0; + public static final int LED_BUFFER_TIME = 60; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 484a837d..bb6dd5f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc.robot.subsystems.Collector; import frc.robot.command.TipDetection; import frc.robot.command.PointAtTag; +import frc.robot.command.SetLED; import frc.robot.command.Collect; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ClimbConstants; @@ -37,6 +38,7 @@ import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.LEDs; import frc.thunder.LightningContainer; import frc.thunder.shuffleboard.LightningShuffleboard; @@ -53,6 +55,7 @@ public class RobotContainer extends LightningContainer { // Collision collision; // Indexer indexer; Climber climber; + LEDs leds; private SendableChooser autoChooser; // TODO I want field-centric driving in open loop WE NEED TO FIGURE OUT WHAT @@ -87,6 +90,7 @@ protected void initializeSubsystems() { brake = new SwerveRequest.SwerveDriveBrake(); point = new SwerveRequest.PointWheelsAt(); logger = new Telemetry(DrivetrAinConstants.MaxSpeed); + leds = new LEDs(); } @@ -125,6 +129,10 @@ protected void configureDefaultCommands() { // climber.setDefaultCommand(new ManualClimb(() -> (coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), climber)); climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED)); + leds.setDefaultCommand(new SetLED(leds)); + + + // shooter.setDefaultCommand(new Shoot(shooter, indexer, drivetrain, () -> coPilot.getAButton())); // collector.setDefaultCommand(new Collect(() -> (coPilot.getRightTriggerAxis() From a7ec45f2fa2241ef40fe6015f4e50d47d31887a0 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 11:50:04 -0500 Subject: [PATCH 15/42] [#108] - actually chases pieces now (pid needs tuning) --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/command/PointAtTag.java | 3 + .../java/frc/robot/command/chasePieces.java | 71 ++++++++----------- 4 files changed, 35 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 62d50248..0ce43e48 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -283,6 +283,7 @@ public class VisionConstants { public static final double COLLISION_DEADZONE = 2d; public static final double ALIGNMENT_TOLERANCE = 4d; // TODO: make this an actual value public static final PIDController HEADING_CONTROLLER = new PIDController(0.05, 0, 0); + public static final PIDController CHASE_CONTROLLER = new PIDController(0.12, 0, 0.05); public static final int TAG_PIPELINE = 0; public static final int NOTE_PIPELINE = 2; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c0931bd8..32754737 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; +import frc.robot.command.ChasePieces; import frc.robot.command.Climb; import frc.robot.command.ManualClimb; import frc.robot.subsystems.Climber; @@ -102,9 +103,10 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", true, true)); + new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); + // new Trigger(driver::getYButton).whileTrue(new Climb(climber, drivetrain)); } diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index dfa5b0ff..9cb03e7e 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -90,6 +90,9 @@ public void execute() { LightningShuffleboard.setDouble("PointAtTag", "Target Heading", targetHeading); LightningShuffleboard.setDouble("PointAtTag", "Pid Output", pidOutput); + + pidOutput = headingController.calculate(0, targetHeading); + if (driver.getRightBumper()) { drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java index 84cc7dd2..606c03c5 100644 --- a/src/main/java/frc/robot/command/chasePieces.java +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -3,67 +3,50 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SwerveDriveBrake; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.ControllerConstants; -import frc.robot.Constants.DrivetrAinConstants; + import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.Swerve; import frc.thunder.shuffleboard.LightningShuffleboard; import frc.thunder.vision.Limelight; -public class chasePieces extends Command { +public class ChasePieces extends Command { private Swerve drivetrain; private Limelight limelight; - private XboxController driver; - private FieldCentric slow; private FieldCentric drive; - - private SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private Limelight[] limelights; + private RobotCentric noteChase; private int limelightId = 0; private double pidOutput; - private double lockedOnHeading; private double targetHeading; - private boolean useLimelights; private boolean onTarget; - private PIDController headingController = VisionConstants.HEADING_CONTROLLER; + private PIDController headingController = VisionConstants.CHASE_CONTROLLER; /** - * Creates a new PointAtTag. + * Creates a new ChasePieces. * @param drivetrain to request movement * @param driver the driver's controller, used for drive input * @param limelight_name the name of the limelight to use * @param useLimelights to get if we want to use vision data or not * @param noteDetection to get if we want to use this for note detection or april tag detection */ - public chasePieces(Swerve drivetrain, XboxController driver, String limelight_name, boolean useLimelights, boolean noteDetection) { + public ChasePieces(Swerve drivetrain) { this.drivetrain = drivetrain; - this.driver = driver; - this.useLimelights = useLimelights; - - //TODO Figure out which of these is the right one to use - for (var l : drivetrain.getLimelights()) { - if (l.getName().equals(limelight_name)) { - limelight = l; - } - } limelight = drivetrain.getLimelights()[0]; - limelightId = limelight.getPipeline(); limelight.setPipeline(VisionConstants.NOTE_PIPELINE); drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop - slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + } // Called when the command is initially scheduled. @@ -76,29 +59,31 @@ public void initialize() { @Override public void execute() { - if(Math.abs(limelight.getTargetX()) < VisionConstants.ALIGNMENT_TOLERANCE){ + targetHeading = limelight.getTargetX(); + if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ onTarget = true; } else{ onTarget = false; } + + LightningShuffleboard.setBool("ChasePieces", "On Target", onTarget); + LightningShuffleboard.setDouble("ChasePieces", "Drivetrain Angle", drivetrain.getPigeon2().getAngle()); + LightningShuffleboard.setDouble("ChasePieces", "Target Heading", targetHeading); + LightningShuffleboard.setDouble("ChasePieces", "Pid Output", pidOutput); + + headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", 0.1)); + headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", 0)); + headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", 0)); + + + pidOutput = headingController.calculate(0, targetHeading); - if (onTarget){ - if (driver.getRightBumper()) { - drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) - .withRotationalRate(-pidOutput) // Drive counterclockwise with negative X (left) - ); - } else { - drivetrain.setControl(drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-pidOutput) // Rotate toward the desired direction - ); // Drive counterclockwise with negative X (left) - } - } else { - - } + drivetrain.setControl(noteChase.withRotationalRate(-pidOutput) + .withVelocityX(1.5) + ); + } // Called once the command ends or is interrupted. From 329e743169b3606bffdd347333e7616e8403fb0a Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 11:51:56 -0500 Subject: [PATCH 16/42] [#108] - cleaned up the code a little bit --- src/main/java/frc/robot/command/chasePieces.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java index 606c03c5..5adc2ac2 100644 --- a/src/main/java/frc/robot/command/chasePieces.java +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -18,7 +18,6 @@ public class ChasePieces extends Command { private Swerve drivetrain; private Limelight limelight; - private FieldCentric drive; private RobotCentric noteChase; private int limelightId = 0; @@ -30,10 +29,6 @@ public class ChasePieces extends Command { /** * Creates a new ChasePieces. * @param drivetrain to request movement - * @param driver the driver's controller, used for drive input - * @param limelight_name the name of the limelight to use - * @param useLimelights to get if we want to use vision data or not - * @param noteDetection to get if we want to use this for note detection or april tag detection */ public ChasePieces(Swerve drivetrain) { this.drivetrain = drivetrain; @@ -43,8 +38,6 @@ public ChasePieces(Swerve drivetrain) { limelight.setPipeline(VisionConstants.NOTE_PIPELINE); - drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop - noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); } From 80ef18c1efc1a1ee14e208d406739f1c3bcb1082 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 27 Jan 2024 11:55:46 -0500 Subject: [PATCH 17/42] [#3] Submodule Update --- src/main/java/frc/thunder | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 9605b432..1bf13634 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 9605b43298b268d6f709309df307e35b3e234261 +Subproject commit 1bf13634e93d412076ee270ebb26137b5b69cf1f From ec52d358426ca09fe4f71f8a892520deb4dbfc08 Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 27 Jan 2024 12:02:37 -0500 Subject: [PATCH 18/42] [#104] fixed linter erros --- src/main/java/frc/robot/command/SetLED.java | 7 ++++++- src/main/java/frc/robot/command/SwirlLEDs.java | 12 ++++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/command/SetLED.java b/src/main/java/frc/robot/command/SetLED.java index ddda28f7..1f87f96b 100644 --- a/src/main/java/frc/robot/command/SetLED.java +++ b/src/main/java/frc/robot/command/SetLED.java @@ -11,7 +11,12 @@ public class SetLED extends Command { private LEDs leds; private Collector collector; - /** Creates a new LEDs. */ + /** + * Sets the angle of the pivot + * + * @param leds set the leds + * @param collector find if collector has peice + */ public SetLED(LEDs leds, Collector collector) { this.leds = leds; this.collector = collector; diff --git a/src/main/java/frc/robot/command/SwirlLEDs.java b/src/main/java/frc/robot/command/SwirlLEDs.java index 47cfb733..ab558522 100644 --- a/src/main/java/frc/robot/command/SwirlLEDs.java +++ b/src/main/java/frc/robot/command/SwirlLEDs.java @@ -10,7 +10,11 @@ public class SwirlLEDs extends Command { private LEDs leds; private int index = 0; - /** Creates a new LEDs. */ + /** Creates a new LEDs. *//** + * Sets the angle of the pivot + * + * @param leds set the leds + */ public SwirlLEDs(LEDs leds) { this.leds = leds; // Use addRequirements() here to declare subsystem dependencies. @@ -24,13 +28,13 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - for (int i = 0; i < 55; i += 5) { + for (int i = 0; i < 55; i+= 5) { if (i % 10 == 0) { - for (int x = 0; x < 5; x ++) { + for (int x = 0; x < 5; x++) { leds.setIndexHSV(i + x + index, 30, 255, 255); } } else { - for (int x = 0; x < 5; x ++) { + for (int x = 0; x < 5; x++) { leds.setIndexHSV(i + x + index, 240, 255, 255); } } From 886ef1c94fedf62b8e3a7846ad0bb4973ffb8d42 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 12:57:48 -0500 Subject: [PATCH 19/42] [#108] - fixed naming issue --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/command/chasePieces.java | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 32754737..e7207efa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Collector; import frc.robot.command.TipDetection; +import frc.robot.command.chasePieces; import frc.robot.command.PointAtTag; import frc.robot.command.Collect; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -103,7 +104,7 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); + new Trigger(driver::getXButton).whileTrue(new chasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java index 5adc2ac2..3107946d 100644 --- a/src/main/java/frc/robot/command/chasePieces.java +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -13,7 +13,7 @@ import frc.thunder.shuffleboard.LightningShuffleboard; import frc.thunder.vision.Limelight; -public class ChasePieces extends Command { +public class chasePieces extends Command { private Swerve drivetrain; private Limelight limelight; @@ -30,7 +30,7 @@ public class ChasePieces extends Command { * Creates a new ChasePieces. * @param drivetrain to request movement */ - public ChasePieces(Swerve drivetrain) { + public chasePieces(Swerve drivetrain) { this.drivetrain = drivetrain; limelight = drivetrain.getLimelights()[0]; From c989c556f1fa398d831588021dedf709fb6bed70 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:16:28 -0500 Subject: [PATCH 20/42] [#108] - fixed wrong import --- 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 e7207efa..627dec20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,7 +29,6 @@ import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; -import frc.robot.command.ChasePieces; import frc.robot.command.Climb; import frc.robot.command.ManualClimb; import frc.robot.subsystems.Climber; From 51b5f2d387a2dab02b3961ad1f204b4863f3922d Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:21:42 -0500 Subject: [PATCH 21/42] [#108] - fixing naming conventions --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/command/chasePieces.java | 93 ------------------- 2 files changed, 1 insertion(+), 95 deletions(-) delete mode 100644 src/main/java/frc/robot/command/chasePieces.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 627dec20..6f4660e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Collector; import frc.robot.command.TipDetection; -import frc.robot.command.chasePieces; import frc.robot.command.PointAtTag; import frc.robot.command.Collect; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -103,7 +102,7 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - new Trigger(driver::getXButton).whileTrue(new chasePieces(drivetrain)); + // new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java deleted file mode 100644 index 3107946d..00000000 --- a/src/main/java/frc/robot/command/chasePieces.java +++ /dev/null @@ -1,93 +0,0 @@ -package frc.robot.command; - -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.Command; - -import frc.robot.Constants.VisionConstants; -import frc.robot.subsystems.Swerve; -import frc.thunder.shuffleboard.LightningShuffleboard; -import frc.thunder.vision.Limelight; - -public class chasePieces extends Command { - - private Swerve drivetrain; - private Limelight limelight; - - private RobotCentric noteChase; - - private int limelightId = 0; - private double pidOutput; - private double targetHeading; - private boolean onTarget; - private PIDController headingController = VisionConstants.CHASE_CONTROLLER; - - /** - * Creates a new ChasePieces. - * @param drivetrain to request movement - */ - public chasePieces(Swerve drivetrain) { - this.drivetrain = drivetrain; - - limelight = drivetrain.getLimelights()[0]; - limelightId = limelight.getPipeline(); - - limelight.setPipeline(VisionConstants.NOTE_PIPELINE); - - noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - targetHeading = limelight.getTargetX(); - - if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ - onTarget = true; - } - else{ - onTarget = false; - } - - LightningShuffleboard.setBool("ChasePieces", "On Target", onTarget); - LightningShuffleboard.setDouble("ChasePieces", "Drivetrain Angle", drivetrain.getPigeon2().getAngle()); - LightningShuffleboard.setDouble("ChasePieces", "Target Heading", targetHeading); - LightningShuffleboard.setDouble("ChasePieces", "Pid Output", pidOutput); - - headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", 0.1)); - headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", 0)); - headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", 0)); - - - pidOutput = headingController.calculate(0, targetHeading); - - drivetrain.setControl(noteChase.withRotationalRate(-pidOutput) - .withVelocityX(1.5) - ); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - limelight.setPipeline(limelightId); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file From 4f1a598d97d17c98916b2d996ed8fc6b7ba357a8 Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 27 Jan 2024 13:21:43 -0500 Subject: [PATCH 22/42] [#104] fixed linter issues --- src/main/java/frc/robot/command/SwirlLEDs.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/command/SwirlLEDs.java b/src/main/java/frc/robot/command/SwirlLEDs.java index ab558522..dae7a145 100644 --- a/src/main/java/frc/robot/command/SwirlLEDs.java +++ b/src/main/java/frc/robot/command/SwirlLEDs.java @@ -10,6 +10,7 @@ public class SwirlLEDs extends Command { private LEDs leds; private int index = 0; + /** Creates a new LEDs. *//** * Sets the angle of the pivot * @@ -45,7 +46,7 @@ public void execute() { for (int i = 0; i < index; i++) { leds.setIndexHSV(i, 240, 255, 255); } - index ++; + index++; index %= 6; } // Called once the command ends or is interrupted. From 5d00f60e67aade7084d97b95afa74e2a988322b5 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:23:02 -0500 Subject: [PATCH 23/42] [#108] - (hopefully) fixed the naming --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/command/ChasePieces.java | 93 +++++++++++++++++++ 2 files changed, 95 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/command/ChasePieces.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f4660e4..32754737 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; +import frc.robot.command.ChasePieces; import frc.robot.command.Climb; import frc.robot.command.ManualClimb; import frc.robot.subsystems.Climber; @@ -102,7 +103,7 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - // new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); + new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java new file mode 100644 index 00000000..5adc2ac2 --- /dev/null +++ b/src/main/java/frc/robot/command/ChasePieces.java @@ -0,0 +1,93 @@ +package frc.robot.command; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; + +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.Swerve; +import frc.thunder.shuffleboard.LightningShuffleboard; +import frc.thunder.vision.Limelight; + +public class ChasePieces extends Command { + + private Swerve drivetrain; + private Limelight limelight; + + private RobotCentric noteChase; + + private int limelightId = 0; + private double pidOutput; + private double targetHeading; + private boolean onTarget; + private PIDController headingController = VisionConstants.CHASE_CONTROLLER; + + /** + * Creates a new ChasePieces. + * @param drivetrain to request movement + */ + public ChasePieces(Swerve drivetrain) { + this.drivetrain = drivetrain; + + limelight = drivetrain.getLimelights()[0]; + limelightId = limelight.getPipeline(); + + limelight.setPipeline(VisionConstants.NOTE_PIPELINE); + + noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + targetHeading = limelight.getTargetX(); + + if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ + onTarget = true; + } + else{ + onTarget = false; + } + + LightningShuffleboard.setBool("ChasePieces", "On Target", onTarget); + LightningShuffleboard.setDouble("ChasePieces", "Drivetrain Angle", drivetrain.getPigeon2().getAngle()); + LightningShuffleboard.setDouble("ChasePieces", "Target Heading", targetHeading); + LightningShuffleboard.setDouble("ChasePieces", "Pid Output", pidOutput); + + headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", 0.1)); + headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", 0)); + headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", 0)); + + + pidOutput = headingController.calculate(0, targetHeading); + + drivetrain.setControl(noteChase.withRotationalRate(-pidOutput) + .withVelocityX(1.5) + ); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + limelight.setPipeline(limelightId); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file From 52a7e32b49ba805c3202213b5f9c1872ee6e8bb9 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:25:50 -0500 Subject: [PATCH 24/42] [#108] - fixed linter issues --- src/main/java/frc/robot/command/ChasePieces.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java index 5adc2ac2..39041f8d 100644 --- a/src/main/java/frc/robot/command/ChasePieces.java +++ b/src/main/java/frc/robot/command/ChasePieces.java @@ -56,8 +56,7 @@ public void execute() { if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ onTarget = true; - } - else{ + } else{ onTarget = false; } From 0cd55d51e0b73c2047ab537223133971fd2415cc Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 27 Jan 2024 13:27:06 -0500 Subject: [PATCH 25/42] [#104] fixed linter issues (again) --- src/main/java/frc/robot/command/SwirlLEDs.java | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/command/SwirlLEDs.java b/src/main/java/frc/robot/command/SwirlLEDs.java index dae7a145..c386cee0 100644 --- a/src/main/java/frc/robot/command/SwirlLEDs.java +++ b/src/main/java/frc/robot/command/SwirlLEDs.java @@ -11,22 +11,19 @@ public class SwirlLEDs extends Command { private LEDs leds; private int index = 0; - /** Creates a new LEDs. *//** - * Sets the angle of the pivot - * - * @param leds set the leds - */ + /** + * Creats a new SwirlLEDs command + * @param leds set the leds + */ public SwirlLEDs(LEDs leds) { this.leds = leds; - // Use addRequirements() here to declare subsystem dependencies. + addRequirements(leds); } - // Called when the command is initially scheduled. @Override public void initialize() {} - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { for (int i = 0; i < 55; i+= 5) { @@ -49,11 +46,10 @@ public void execute() { index++; index %= 6; } - // Called once the command ends or is interrupted. + @Override public void end(boolean interrupted) {} - // Returns true when the command should end. @Override public boolean isFinished() { return false; From 7349c622d3992e38c1069ef6f356039b58ed99f8 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 27 Jan 2024 13:31:45 -0500 Subject: [PATCH 26/42] [#64] changed the order of initialization so named commands worked --- .../autos/{TestNAmes.auto => TestNamedCommands.auto} | 12 ++++++++++++ src/main/java/frc/robot/RobotContainer.java | 10 ++++++---- 2 files changed, 18 insertions(+), 4 deletions(-) rename src/main/deploy/pathplanner/autos/{TestNAmes.auto => TestNamedCommands.auto} (73%) diff --git a/src/main/deploy/pathplanner/autos/TestNAmes.auto b/src/main/deploy/pathplanner/autos/TestNamedCommands.auto similarity index 73% rename from src/main/deploy/pathplanner/autos/TestNAmes.auto rename to src/main/deploy/pathplanner/autos/TestNamedCommands.auto index 73e68898..c0898b40 100644 --- a/src/main/deploy/pathplanner/autos/TestNAmes.auto +++ b/src/main/deploy/pathplanner/autos/TestNamedCommands.auto @@ -17,12 +17,24 @@ "pathName": "1MeterSquare" } }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "named", "data": { "name": "test" } }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be17da64..6680abdd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,7 @@ package frc.robot; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.fasterxml.jackson.databind.util.Named; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -70,9 +71,6 @@ protected void initializeSubsystems() { drivetrain = TunerConstants.getDrivetrain(); // My drivetrain - autoChooser = AutoBuilder.buildAutoChooser(); - LightningShuffleboard.set("Auton", "Auto Chooser", autoChooser); - // indexer = new Indexer(); // collector = new Collector(); // flywheel = new Flywheel(); @@ -91,8 +89,12 @@ protected void initializeSubsystems() { @Override protected void initializeNamedCommands() { - NamedCommands.registerCommand("test", new InstantCommand(() -> System.out.println("Hello World!"))); + NamedCommands.registerCommand("test", new InstantCommand(() -> System.err.println("HELLO WORLD AHHHHHHHHHHHHH!!!!"))); NamedCommands.registerCommand("disable-Vision", new InstantCommand(() -> drivetrain.disableVision())); + + // make sure named commands is initialized before autobuilder! + autoChooser = AutoBuilder.buildAutoChooser(); + LightningShuffleboard.set("Auton", "Auto Chooser", autoChooser); } @Override From 7a4afc5ecada468d9934d48ce324b4809a121681 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 13:49:10 -0500 Subject: [PATCH 27/42] [#88] comments n stuff --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/command/tests/DriveTest.java | 7 ++++++- src/main/java/frc/robot/command/tests/TurnSystemTest.java | 5 +++++ 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be995dd2..57ea49b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -165,7 +165,6 @@ protected void configureSystemTests() { new TimedCommand(new TurnSystemTest(drivetrain, () -> 0.25), 2), new WaitCommand(0.5), new TimedCommand(new TurnSystemTest(drivetrain, () -> -0.25), 2) - )); } } diff --git a/src/main/java/frc/robot/command/tests/DriveTest.java b/src/main/java/frc/robot/command/tests/DriveTest.java index b3f33ef5..9cecdfb8 100644 --- a/src/main/java/frc/robot/command/tests/DriveTest.java +++ b/src/main/java/frc/robot/command/tests/DriveTest.java @@ -6,7 +6,6 @@ import java.util.function.DoubleSupplier; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import edu.wpi.first.wpilibj2.command.Command; @@ -18,6 +17,12 @@ public class DriveTest extends Command { private DoubleSupplier speedX; private DoubleSupplier speedY; + /** + * Creates a new drive test + * @param drivetrain swerve subsystem + * @param speedX X velocity + * @param speedY Y velocity + */ public DriveTest(Swerve drivetrain, DoubleSupplier speedX, DoubleSupplier speedY) { this.drivetrain = drivetrain; this.speedX = speedX; diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index e7ea4d06..6820bca1 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -16,6 +16,11 @@ public class TurnSystemTest extends Command { private Swerve drivetrain; private DoubleSupplier speed; + /** + * System test for testing azimuth motors + * @param drivetrain swerve subsystem + * @param speed rotational rate + */ public TurnSystemTest(Swerve drivetrain, DoubleSupplier speed) { this.drivetrain = drivetrain; this.speed = speed; From ecf3d1eb059e24bb67e5b0e5169223c0b13d8b3e Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 27 Jan 2024 13:50:12 -0500 Subject: [PATCH 28/42] [#64] got rid of useless named commands and renamed path to Spinslow --- .../deploy/pathplanner/paths/{Sipnslow.path => Spinslow.path} | 0 src/main/java/frc/robot/RobotContainer.java | 1 - 2 files changed, 1 deletion(-) rename src/main/deploy/pathplanner/paths/{Sipnslow.path => Spinslow.path} (100%) diff --git a/src/main/deploy/pathplanner/paths/Sipnslow.path b/src/main/deploy/pathplanner/paths/Spinslow.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Sipnslow.path rename to src/main/deploy/pathplanner/paths/Spinslow.path diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8d7049e3..3e87b466 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,7 +90,6 @@ protected void initializeSubsystems() { @Override protected void initializeNamedCommands() { - NamedCommands.registerCommand("test", new InstantCommand(() -> System.err.println("HELLO WORLD AHHHHHHHHHHHHH!!!!"))); NamedCommands.registerCommand("disable-Vision", new InstantCommand(() -> drivetrain.disableVision())); // make sure named commands is initialized before autobuilder! From 158c03cd1ca3c4ac834b6278c4ce7f12f83cb97a Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 27 Jan 2024 13:50:35 -0500 Subject: [PATCH 29/42] [#64] oopsie forgot to add this --- src/main/deploy/pathplanner/autos/Spin.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/Spin.auto b/src/main/deploy/pathplanner/autos/Spin.auto index 66d3398b..cd5aed22 100644 --- a/src/main/deploy/pathplanner/autos/Spin.auto +++ b/src/main/deploy/pathplanner/autos/Spin.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "Sipnslow" + "pathName": "Spinslow" } } ] From 047b485b9699418a6845791dcb8c9f9f70e7dce3 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 14:07:26 -0500 Subject: [PATCH 30/42] added back (unused) imports to robotcontainer --- src/main/java/frc/robot/RobotContainer.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5aa94da5..3af8614f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.Collector; +import frc.robot.command.TipDetection; +import frc.robot.command.PointAtTag; +import frc.robot.command.Collect; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ControllerConstants; import frc.robot.Constants.DrivetrAinConstants; @@ -24,6 +32,10 @@ import frc.robot.subsystems.Climber; import frc.robot.subsystems.Collector; import frc.robot.subsystems.LEDs; +import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; import frc.thunder.LightningContainer; import frc.thunder.shuffleboard.LightningShuffleboard; From 8cbe57eafd3515fc2e353c14d5551de51506fd1c Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 14:35:17 -0500 Subject: [PATCH 31/42] [#118] robot centric + fixed constant --- src/main/java/frc/robot/Constants.java | 4 +++- src/main/java/frc/robot/RobotContainer.java | 22 +++++++++---------- .../java/frc/robot/command/PointAtTag.java | 10 ++++----- .../frc/robot/command/tests/DriveTest.java | 4 ++-- .../robot/command/tests/TurnSystemTest.java | 4 ++-- 5 files changed, 23 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b8c7f2ff..64709ecb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ public static final boolean isMercury() { return MERCURY_PATH.toFile().exists(); } - public class DrivetrAinConstants { // TODO Get new for new robot + public class DrivetrainConstants { // TODO Get new for new robot public static final double MaxSpeed = 6; // 6 meters per second desired top speed private static final double WHEELBASE = TunerConstants.kFrontLeftXPosInches * 2; // 2 * x distance from center of robot to wheel public static final double MaxAngularRate = 2 * Math.PI * ( // convert to radians per second @@ -37,6 +37,8 @@ public class DrivetrAinConstants { // TODO Get new for new robot public static final double SLOW_ROT_MULT = 0.007; // TODO Tune for Driver public static final double SLOW_SPEED_MULT = 0.4; // TODO Tune for Driver + + public static final double SYS_TEST_SPEED = 0.25; } public class RobotMap { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73d3e42b..5d37a1fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,7 +24,7 @@ import frc.robot.command.Collect; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ControllerConstants; -import frc.robot.Constants.DrivetrAinConstants; +import frc.robot.Constants.DrivetrainConstants; import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.SetLED; @@ -93,7 +93,7 @@ protected void initializeSubsystems() { brake = new SwerveRequest.SwerveDriveBrake(); point = new SwerveRequest.PointWheelsAt(); - logger = new Telemetry(DrivetrAinConstants.MaxSpeed); + logger = new Telemetry(DrivetrainConstants.MaxSpeed); } @@ -113,9 +113,9 @@ protected void configureButtonBindings() { new Trigger(driver::getAButton).whileTrue(drivetrain.applyRequest(() -> brake)); new Trigger(driver::getBButton).whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection(new Rotation2d(-driver.getLeftY(), -driver.getLeftX())))); new Trigger(driver::getRightBumper).whileTrue( - drivetrain.applyRequest(() -> slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) - .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.SLOW_ROT_MULT))); // Drive counterclockwise with negative X (left) + drivetrain.applyRequest(() -> slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) + .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate * DrivetrainConstants.SLOW_ROT_MULT))); // Drive counterclockwise with negative X (left) new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); @@ -130,9 +130,9 @@ protected void configureDefaultCommands() { drivetrain.registerTelemetry(logger::telemeterize); drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_MULT) // Drive counterclockwise with negative X (left) + drivetrain.applyRequest(() -> drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxAngularRate * DrivetrainConstants.ROT_MULT) // Drive counterclockwise with negative X (left) )); // climber.setDefaultCommand(new ManualClimb(() -> (coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), climber)); // climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED)); @@ -169,12 +169,12 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { - SystemTest.registerTest("Drive All Directions", new DrivetrainSystemTest(drivetrain, 0.25)); + SystemTest.registerTest("Drive All Directions", new DrivetrainSystemTest(drivetrain, Constants.DrivetrainConstants.SYS_TEST_SPEED)); SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup( - new TimedCommand(new TurnSystemTest(drivetrain, () -> 0.25), 2), + new TimedCommand(new TurnSystemTest(drivetrain, () -> Constants.DrivetrainConstants.SYS_TEST_SPEED), 2), new WaitCommand(0.5), - new TimedCommand(new TurnSystemTest(drivetrain, () -> -0.25), 2) + new TimedCommand(new TurnSystemTest(drivetrain, () -> -Constants.DrivetrainConstants.SYS_TEST_SPEED), 2) )); } } diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index 9cb03e7e..907aecce 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ControllerConstants; -import frc.robot.Constants.DrivetrAinConstants; +import frc.robot.Constants.DrivetrainConstants; import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.Swerve; import frc.thunder.shuffleboard.LightningShuffleboard; @@ -94,13 +94,13 @@ public void execute() { pidOutput = headingController.calculate(0, targetHeading); if (driver.getRightBumper()) { - drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) + drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) .withRotationalRate(-pidOutput) // Drive counterclockwise with negative X (left) ); } else { - drivetrain.setControl(drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive left with negative X (left) + drivetrain.setControl(drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrainConstants.MaxSpeed) // Drive left with negative X (left) .withRotationalRate(-pidOutput) // Rotate toward the desired direction ); // Drive counterclockwise with negative X (left) } diff --git a/src/main/java/frc/robot/command/tests/DriveTest.java b/src/main/java/frc/robot/command/tests/DriveTest.java index 9cecdfb8..0e2bd4bd 100644 --- a/src/main/java/frc/robot/command/tests/DriveTest.java +++ b/src/main/java/frc/robot/command/tests/DriveTest.java @@ -36,12 +36,12 @@ public void initialize() {} @Override public void execute() { - drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(speedX.getAsDouble()).withVelocityY(speedY.getAsDouble())); + drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(speedX.getAsDouble()).withVelocityY(speedY.getAsDouble())); } @Override public void end(boolean interrupted) { - drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); } @Override diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 6820bca1..32b6b732 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -33,12 +33,12 @@ public void initialize() {} @Override public void execute() { - drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(speed.getAsDouble())); + drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(speed.getAsDouble())); } @Override public void end(boolean interrupted) { - drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); } @Override From 634d71309a09087cfe90999c5c162b21b1f1f46d Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 14:48:03 -0500 Subject: [PATCH 32/42] [#118] Tested sys tests and modified them --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 7 ++++--- .../robot/command/tests/DrivetrainSystemTest.java | 15 ++++++++++----- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 64709ecb..e729dec2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,7 +38,8 @@ public class DrivetrainConstants { // TODO Get new for new robot public static final double SLOW_ROT_MULT = 0.007; // TODO Tune for Driver public static final double SLOW_SPEED_MULT = 0.4; // TODO Tune for Driver - public static final double SYS_TEST_SPEED = 0.25; + public static final double SYS_TEST_SPEED_DRIVE = 0.5; + public static final double SYS_TEST_SPEED_TURN = 1d; } public class RobotMap { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d37a1fa..cddf1867 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -169,12 +169,13 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { - SystemTest.registerTest("Drive All Directions", new DrivetrainSystemTest(drivetrain, Constants.DrivetrainConstants.SYS_TEST_SPEED)); + SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, Constants.DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup( - new TimedCommand(new TurnSystemTest(drivetrain, () -> Constants.DrivetrainConstants.SYS_TEST_SPEED), 2), + new TimedCommand(new TurnSystemTest(drivetrain, () -> Constants.DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), new WaitCommand(0.5), - new TimedCommand(new TurnSystemTest(drivetrain, () -> -Constants.DrivetrainConstants.SYS_TEST_SPEED), 2) + new TimedCommand(new TurnSystemTest(drivetrain, () -> -Constants.DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), + drivetrain.applyRequest(() -> brake) )); } } diff --git a/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java index d89f8122..822a28e7 100644 --- a/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java +++ b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java @@ -4,6 +4,9 @@ package frc.robot.command.tests; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.Swerve; @@ -11,16 +14,18 @@ public class DrivetrainSystemTest extends SequentialCommandGroup { - public DrivetrainSystemTest(Swerve drivetrain, double speed) { + public DrivetrainSystemTest(Swerve drivetrain, SwerveRequest brake, double speed) { addCommands( new WaitCommand(0.5), - new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 2), // Forward + new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0d), 1), // Forward new WaitCommand(1), - new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 2), // Backward + new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0d), 1), // Backward new WaitCommand(1), - new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0), 2), // Left + new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 1), // Left new WaitCommand(1), - new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0), 2) // Right + new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 1), // Right + new WaitCommand(0.5), + drivetrain.applyRequest(() -> brake) // Brake ); } } From 3195e73d6944b3ae41f9d79dc86692fd11ae2370 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 14:49:51 -0500 Subject: [PATCH 33/42] [#118] fix constants ref --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cddf1867..585f501c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -169,12 +169,12 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { - SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, Constants.DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); + SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup( - new TimedCommand(new TurnSystemTest(drivetrain, () -> Constants.DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), + new TimedCommand(new TurnSystemTest(drivetrain, () -> DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), new WaitCommand(0.5), - new TimedCommand(new TurnSystemTest(drivetrain, () -> -Constants.DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), + new TimedCommand(new TurnSystemTest(drivetrain, () -> -DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), drivetrain.applyRequest(() -> brake) )); } From 4cf1499b45e2c52dfeed1e134a39d63f8643eb3a Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 18:34:57 -0500 Subject: [PATCH 34/42] [#118] cleaner way to create system tests --- src/main/java/frc/robot/RobotContainer.java | 8 +-- .../command/tests/DrivetrainSystemTest.java | 32 ++++++----- .../robot/command/tests/TurnSystemTest.java | 53 ++++++------------- .../tests/{ => testCommands}/DriveTest.java | 4 +- .../command/tests/testCommands/TurnTest.java | 48 +++++++++++++++++ src/main/java/frc/thunder | 2 +- 6 files changed, 88 insertions(+), 59 deletions(-) rename src/main/java/frc/robot/command/tests/{ => testCommands}/DriveTest.java (93%) create mode 100644 src/main/java/frc/robot/command/tests/testCommands/TurnTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 585f501c..8a4c5399 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,7 @@ import frc.robot.command.Shoot; import frc.robot.command.tests.DrivetrainSystemTest; import frc.robot.command.tests.TurnSystemTest; +import frc.robot.command.tests.testCommands.TurnTest; import frc.robot.command.ChasePieces; import frc.robot.command.Climb; import frc.robot.command.ManualClimb; @@ -171,11 +172,6 @@ protected void configureFaultMonitors() { protected void configureSystemTests() { SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); - SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup( - new TimedCommand(new TurnSystemTest(drivetrain, () -> DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), - new WaitCommand(0.5), - new TimedCommand(new TurnSystemTest(drivetrain, () -> -DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), - drivetrain.applyRequest(() -> brake) - )); + SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, brake, DrivetrainConstants.SYS_TEST_SPEED_TURN)); } } diff --git a/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java index 822a28e7..31adea81 100644 --- a/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java +++ b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java @@ -6,26 +6,30 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.command.tests.testCommands.DriveTest; import frc.robot.subsystems.Swerve; import frc.thunder.command.TimedCommand; +import frc.thunder.testing.SystemTestCommandGroup; -public class DrivetrainSystemTest extends SequentialCommandGroup { - +public class DrivetrainSystemTest extends SystemTestCommandGroup { + public DrivetrainSystemTest(Swerve drivetrain, SwerveRequest brake, double speed) { - addCommands( - new WaitCommand(0.5), - new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0d), 1), // Forward - new WaitCommand(1), - new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0d), 1), // Backward - new WaitCommand(1), - new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 1), // Left - new WaitCommand(1), - new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 1), // Right - new WaitCommand(0.5), - drivetrain.applyRequest(() -> brake) // Brake + super( + new SequentialCommandGroup( + new WaitCommand(0.5), + new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0d), 1), // Forward + new WaitCommand(1), + new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0d), 1), // Backward + new WaitCommand(1), + new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 1), // Left + new WaitCommand(1), + new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 1), // Right + new WaitCommand(0.5), + drivetrain.applyRequest(() -> brake) // Brake + ) ); } + } diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 32b6b732..6a433a3e 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -4,45 +4,26 @@ package frc.robot.command.tests; -import java.util.function.DoubleSupplier; - import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.command.tests.testCommands.TurnTest; import frc.robot.subsystems.Swerve; - -public class TurnSystemTest extends Command { - - private Swerve drivetrain; - private DoubleSupplier speed; - - /** - * System test for testing azimuth motors - * @param drivetrain swerve subsystem - * @param speed rotational rate - */ - public TurnSystemTest(Swerve drivetrain, DoubleSupplier speed) { - this.drivetrain = drivetrain; - this.speed = speed; - - addRequirements(drivetrain); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(speed.getAsDouble())); +import frc.thunder.command.TimedCommand; +import frc.thunder.testing.SystemTestCommandGroup; + +public class TurnSystemTest extends SystemTestCommandGroup { + + public TurnSystemTest(Swerve drivetrain, SwerveRequest brake, double speed) { + super( + new SequentialCommandGroup( + new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), + new WaitCommand(0.5), + new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1), + drivetrain.applyRequest(() -> brake) + ) + ); } - @Override - public void end(boolean interrupted) { - drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); - } - - @Override - public boolean isFinished() { - return false; - } } diff --git a/src/main/java/frc/robot/command/tests/DriveTest.java b/src/main/java/frc/robot/command/tests/testCommands/DriveTest.java similarity index 93% rename from src/main/java/frc/robot/command/tests/DriveTest.java rename to src/main/java/frc/robot/command/tests/testCommands/DriveTest.java index 0e2bd4bd..f783e2f6 100644 --- a/src/main/java/frc/robot/command/tests/DriveTest.java +++ b/src/main/java/frc/robot/command/tests/testCommands/DriveTest.java @@ -2,7 +2,7 @@ // 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.robot.command.tests; +package frc.robot.command.tests.testCommands; import java.util.function.DoubleSupplier; @@ -18,7 +18,7 @@ public class DriveTest extends Command { private DoubleSupplier speedY; /** - * Creates a new drive test + * System test command for testing drive motors * @param drivetrain swerve subsystem * @param speedX X velocity * @param speedY Y velocity diff --git a/src/main/java/frc/robot/command/tests/testCommands/TurnTest.java b/src/main/java/frc/robot/command/tests/testCommands/TurnTest.java new file mode 100644 index 00000000..f59548f8 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/testCommands/TurnTest.java @@ -0,0 +1,48 @@ +// 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.robot.command.tests.testCommands; + +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Swerve; + +public class TurnTest extends Command { + + private Swerve drivetrain; + private DoubleSupplier speed; + + /** + * System test command for testing azimuth motors + * @param drivetrain swerve subsystem + * @param speed rotational rate + */ + public TurnTest(Swerve drivetrain, DoubleSupplier speed) { + this.drivetrain = drivetrain; + this.speed = speed; + + addRequirements(drivetrain); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(speed.getAsDouble())); + } + + @Override + public void end(boolean interrupted) { + drivetrain.setControl(new SwerveRequest.RobotCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0)); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 1bf13634..e887f7d6 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 1bf13634e93d412076ee270ebb26137b5b69cf1f +Subproject commit e887f7d6c264725820be98556253b22f4eaa82ad From d421c10d07b5878da4cdde4d20435cf1c770eba8 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 19:05:19 -0500 Subject: [PATCH 35/42] [#117] Changed dependabot interval --- .github/dependabot.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index cabd49b8..6157decb 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -5,7 +5,7 @@ updates: reviewers: - "frc-862/reviewers" schedule: - interval: "weekly" + interval: "daily" commit-message: prefix: "[#3] " labels: From 2e691f4075a2078f8275de642a6681094fb99241 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 20:50:21 -0500 Subject: [PATCH 36/42] [#121] Test workflow dispatch --- .github/workflows/linter.yml | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/.github/workflows/linter.yml b/.github/workflows/linter.yml index 20c20fae..efd5acff 100644 --- a/.github/workflows/linter.yml +++ b/.github/workflows/linter.yml @@ -1,5 +1,14 @@ name: Code Linter -on: [pull_request] +on: + pull_request: + workflow_dispatch: + inputs: + showSupressions: + description: 'Show Supressions' + required: false + default: false + type: boolean + jobs: Build: runs-on: ubuntu-latest @@ -13,6 +22,11 @@ jobs: uses: actions/checkout@v4 with: fetch-depth: 0 + - name: Check Supressions + run: | + if [ ${{ github.event.inputs.showSupressions }} == true ]; then + rm .github/linter_supressions.xml + fi - name: Java Code Linter uses: super-linter/super-linter/slim@v5.7.2 env: From 1f5e8a4298586bd50c11a8e69027c25ab795d7e8 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 20:51:15 -0500 Subject: [PATCH 37/42] test linter on push quickly --- .github/workflows/linter.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/linter.yml b/.github/workflows/linter.yml index efd5acff..78b608b2 100644 --- a/.github/workflows/linter.yml +++ b/.github/workflows/linter.yml @@ -1,5 +1,6 @@ name: Code Linter on: + push: # test on push pull_request: workflow_dispatch: inputs: From 0f8a69af2c90192bea21e08c5836c8b5e14238a7 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 21:05:13 -0500 Subject: [PATCH 38/42] lint lint lint lint lint lint lint --- .github/workflows/linter.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/linter.yml b/.github/workflows/linter.yml index 78b608b2..8908c9f9 100644 --- a/.github/workflows/linter.yml +++ b/.github/workflows/linter.yml @@ -25,9 +25,11 @@ jobs: fetch-depth: 0 - name: Check Supressions run: | - if [ ${{ github.event.inputs.showSupressions }} == true ]; then + if [ $SUPPRESSIONS == true ]; then rm .github/linter_supressions.xml fi + env: + SUPRESSIONS: ${{ github.event.inputs.showSupressions }} - name: Java Code Linter uses: super-linter/super-linter/slim@v5.7.2 env: From 5a15828298def92a53a2a073a4bd664217cfa57a Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 27 Jan 2024 23:29:51 -0500 Subject: [PATCH 39/42] [#123] update Vendor deps + made build --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++-------- vendordeps/PathplannerLib.json | 6 +++--- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 585f501c..15b5b227 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -169,13 +169,13 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { - SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); - - SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup( - new TimedCommand(new TurnSystemTest(drivetrain, () -> DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), - new WaitCommand(0.5), - new TimedCommand(new TurnSystemTest(drivetrain, () -> -DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), - drivetrain.applyRequest(() -> brake) - )); + // SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); + + // SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup( + // new TimedCommand(new TurnSystemTest(drivetrain, () -> DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), + // new WaitCommand(0.5), + // new TimedCommand(new TurnSystemTest(drivetrain, () -> -DrivetrainConstants.SYS_TEST_SPEED_TURN), 1), + // drivetrain.applyRequest(() -> brake) + // )); } } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 6ddd312f..cae13633 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.5", + "version": "2024.1.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.5" + "version": "2024.1.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.5", + "version": "2024.1.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 4614102389c3653485c966a7b4fa5a1fa6742253 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sun, 28 Jan 2024 00:40:42 -0500 Subject: [PATCH 40/42] [#120] Update thunder --- src/main/java/frc/thunder | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index e887f7d6..cd540794 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit e887f7d6c264725820be98556253b22f4eaa82ad +Subproject commit cd54079489d55b374a3d96cf4a81e697bae7e236 From bded2813ac56b89b97e11cc5ff3103308493f727 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sun, 28 Jan 2024 00:44:45 -0500 Subject: [PATCH 41/42] [#120] adjust timings --- src/main/java/frc/robot/command/tests/TurnSystemTest.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 6a433a3e..7ae235c5 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -18,9 +18,11 @@ public class TurnSystemTest extends SystemTestCommandGroup { public TurnSystemTest(Swerve drivetrain, SwerveRequest brake, double speed) { super( new SequentialCommandGroup( - new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), new WaitCommand(0.5), + new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), + new WaitCommand(1), new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1), + new WaitCommand(0.5), drivetrain.applyRequest(() -> brake) ) ); From 7c5a071f28131bab6d3d1f614262e904828cecc2 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sun, 28 Jan 2024 21:12:05 -0500 Subject: [PATCH 42/42] [#121] Modify suppressions and add workflow_dispatch --- .github/linter_suppressions.xml | 3 ++ .github/linter_suppressions_limited.xml | 14 ++++++ .github/sun_checks.xml | 11 ++-- .github/workflows/gradle.yml | 8 ++- .github/workflows/linter.yml | 67 +++++++++++++++++++------ 5 files changed, 80 insertions(+), 23 deletions(-) create mode 100644 .github/linter_suppressions_limited.xml diff --git a/.github/linter_suppressions.xml b/.github/linter_suppressions.xml index b5cbef8b..1e0ae578 100644 --- a/.github/linter_suppressions.xml +++ b/.github/linter_suppressions.xml @@ -5,6 +5,7 @@ "https://checkstyle.org/dtds/suppressions_1_2.dtd"> + @@ -23,7 +24,9 @@ + + \ No newline at end of file diff --git a/.github/linter_suppressions_limited.xml b/.github/linter_suppressions_limited.xml new file mode 100644 index 00000000..4fcc73ca --- /dev/null +++ b/.github/linter_suppressions_limited.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/.github/sun_checks.xml b/.github/sun_checks.xml index 2cfd55d6..7fd23ed9 100644 --- a/.github/sun_checks.xml +++ b/.github/sun_checks.xml @@ -62,7 +62,9 @@ - + + + @@ -130,6 +132,7 @@ + @@ -188,11 +191,11 @@ - + + + - - diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 2cae0cf9..9cec5b76 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -1,5 +1,7 @@ name: Gradle Build + on: [push, pull_request] + jobs: Build: runs-on: ubuntu-latest @@ -16,8 +18,4 @@ jobs: - name: Set Execution Permissions run: chmod +x ./gradlew - name: Gradle build Rhapsody - run: ./gradlew build - # ENV may not be needed when using thunder, will test later - # env: - # USERNAME: ${{ github.actor }} - # TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file + run: ./gradlew build \ No newline at end of file diff --git a/.github/workflows/linter.yml b/.github/workflows/linter.yml index 8908c9f9..481ae0b9 100644 --- a/.github/workflows/linter.yml +++ b/.github/workflows/linter.yml @@ -1,14 +1,45 @@ name: Code Linter + on: - push: # test on push pull_request: workflow_dispatch: - inputs: - showSupressions: - description: 'Show Supressions' - required: false - default: false - type: boolean + inputs: + suppressionLevel: + description: 'Suppressions Level' + required: true + default: 'Normal' + type: choice + options: + - Normal + - More + - Show All + lintActions: + description: 'Lint Github Actions' + required: true + default: true + type: boolean + lintMD: + description: 'Lint Markdown' + required: true + default: false + type: boolean + lintXML: + description: 'Lint XML' + required: true + default: false + type: boolean + lintYML: + description: 'Lint YML' + required: true + default: false + type: boolean + +env: + suppressionLevel: ${{ github.event_name != 'workflow_dispatch' && 'Normal' || inputs.suppressionLevel }} + lintActions: ${{ github.event_name != 'workflow_dispatch' && 'true' || inputs.lintActions }} + lintMD: ${{ github.event_name != 'workflow_dispatch' && 'false' || inputs.lintMD }} + lintXML: ${{ github.event_name != 'workflow_dispatch' && 'false' || inputs.lintXML }} + lintYML: ${{ github.event_name != 'workflow_dispatch' && 'false' || inputs.lintYML }} jobs: Build: @@ -23,13 +54,20 @@ jobs: uses: actions/checkout@v4 with: fetch-depth: 0 - - name: Check Supressions + - name: Check Suppressions Level run: | - if [ $SUPPRESSIONS == true ]; then - rm .github/linter_supressions.xml + if [ "$SUPPRESSIONS" == "Show All" ]; then + echo "Showing ALL Suppressions (Use at your own risk)" + rm .github/linter_suppressions.xml + elif [ "$SUPPRESSIONS" == "More" ]; then + echo "Showing More Suppressions" + rm .github/linter_suppressions.xml + mv .github/linter_suppressions_limited.xml .github/linter_suppressions.xml + else + echo "Using Normal Suppressions" fi env: - SUPRESSIONS: ${{ github.event.inputs.showSupressions }} + SUPPRESSIONS: ${{ env.suppressionLevel }} - name: Java Code Linter uses: super-linter/super-linter/slim@v5.7.2 env: @@ -39,12 +77,13 @@ jobs: JAVA_FILE_NAME: 'sun_checks.xml' VALIDATE_BASH: false VALIDATE_BASH_EXEC: false + VALIDATE_GITHUB_ACTIONS: ${{ env.lintActions != 'true' && 'false' || '' }} VALIDATE_GITLEAKS: false VALIDATE_GOOGLE_JAVA_FORMAT: false VALIDATE_GROOVY: false VALIDATE_JSCPD: false VALIDATE_JSON: false - VALIDATE_MARKDOWN: false + VALIDATE_MARKDOWN: ${{ env.lintMD != 'true' && 'false' || '' }} VALIDATE_NATURAL_LANGUAGE: false - VALIDATE_XML: false - VALIDATE_YAML: false \ No newline at end of file + VALIDATE_XML: ${{ env.lintXML != 'true' && 'false' || '' }} + VALIDATE_YAML: ${{ env.lintYML != 'true' && 'false' || '' }} \ No newline at end of file