From f68f768059aec953ae705dc7d74c4850a1385d16 Mon Sep 17 00:00:00 2001 From: James Ding Date: Wed, 3 Apr 2024 22:45:28 -0700 Subject: [PATCH] Add changes from 4/3/2024 --- .pathplanner/settings.json | 6 +- .../autos/6 Note Auto from Amp Side.auto | 6 + .../pathplanner/autos/6 Note from Center.auto | 332 ++++++++++++++++++ .../autos/Penta Note from Center.auto | 43 --- .../paths/#1 Note from Amp Side.path | 6 +- .../paths/#1 Note from Speaker.path | 18 +- .../deploy/pathplanner/paths/#2 Note.path | 6 +- .../deploy/pathplanner/paths/#3 Note.path | 6 +- .../deploy/pathplanner/paths/#4 Note.path | 6 +- .../deploy/pathplanner/paths/#5 Note.path | 6 +- .../deploy/pathplanner/paths/#7 Note.path | 68 ++++ .../deploy/pathplanner/paths/1m Test.path | 6 +- .../deploy/pathplanner/paths/2m Test.path | 6 +- .../deploy/pathplanner/paths/Circle Test.path | 6 +- .../paths/Long Shot Speaker Amp Side.path | 6 +- .../deploy/pathplanner/paths/Park Right.path | 6 +- .../deploy/pathplanner/paths/Playground.path | 6 +- .../pathplanner/paths/Source Direct Left.path | 6 +- .../paths/Source Direct Middle.path | 6 +- .../paths/Source Direct Right.path | 6 +- src/main/java/org/rambots/RobotContainer.kt | 27 +- .../java/org/rambots/auto/AutoConstants.kt | 2 +- .../actions/AmpScoreCommandGroup.kt | 2 +- .../actions/AmpScoreHomeCommandGroup.kt | 4 +- .../actions/AutoShootCommand.kt | 9 +- .../actions/BackAmpCommandGroup.kt | 14 + .../actions/ExtendClimberCommandGroup.kt | 4 +- .../actions/IntakeCommandGroup.kt | 2 +- .../actions/RetractClimberCommand.kt | 17 + .../primitive/ReverseSlowIntakeCommand.kt | 20 ++ .../superstructure/primitive/ShootCommand.kt | 20 ++ .../primitive/StopShooterCommand.kt | 20 ++ .../primitive/WristPositionCommand.kt | 4 +- .../org/rambots/config/ElevatorConstants.kt | 3 +- .../subsystems/drive/DriveConstants.java | 2 +- .../superstructure/ElevatorSubsystem.kt | 6 + .../superstructure/ShooterSubsystem.kt | 4 + src/main/java/org/rambots/util/MathUtil.kt | 9 + 38 files changed, 604 insertions(+), 122 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/6 Note from Center.auto delete mode 100644 src/main/deploy/pathplanner/autos/Penta Note from Center.auto create mode 100644 src/main/deploy/pathplanner/paths/#7 Note.path create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/BackAmpCommandGroup.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/primitive/ReverseSlowIntakeCommand.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/primitive/ShootCommand.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/primitive/StopShooterCommand.kt create mode 100644 src/main/java/org/rambots/util/MathUtil.kt diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 4518688..a003c26 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -11,8 +11,8 @@ "Troll" ], "defaultMaxVel": 3.0, - "defaultMaxAccel": 4.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxAccel": 1.0, + "defaultMaxAngVel": 360.0, + "defaultMaxAngAccel": 360.0, "maxModuleSpeed": 4.331208 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/6 Note Auto from Amp Side.auto b/src/main/deploy/pathplanner/autos/6 Note Auto from Amp Side.auto index 4f95655..6826a8b 100644 --- a/src/main/deploy/pathplanner/autos/6 Note Auto from Amp Side.auto +++ b/src/main/deploy/pathplanner/autos/6 Note Auto from Amp Side.auto @@ -46,6 +46,12 @@ "data": { "pathName": "#5 Note" } + }, + { + "type": "path", + "data": { + "pathName": "Long Shot Speaker Amp Side" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/6 Note from Center.auto b/src/main/deploy/pathplanner/autos/6 Note from Center.auto new file mode 100644 index 0000000..e7e0d4b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/6 Note from Center.auto @@ -0,0 +1,332 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.588271083518912, + "y": 5.624616329733554 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "ConfirmShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "#1 Note from Speaker" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeHome" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "ConfirmShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "#2 Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeHome" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "ConfirmShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "#3 Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeHome" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "named", + "data": { + "name": "ConfirmShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Penta Note from Center.auto b/src/main/deploy/pathplanner/autos/Penta Note from Center.auto deleted file mode 100644 index 45bf7bb..0000000 --- a/src/main/deploy/pathplanner/autos/Penta Note from Center.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.323928801703758, - "y": 5.595244965087426 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "#1 Note from Speaker" - } - }, - { - "type": "path", - "data": { - "pathName": "#2 Note" - } - }, - { - "type": "path", - "data": { - "pathName": "#3 Note" - } - }, - { - "type": "path", - "data": { - "pathName": "#4 Note" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path b/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path index 7201429..10c74c1 100644 --- a/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path +++ b/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path b/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path index 139296d..0c54c25 100644 --- a/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path +++ b/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.309243119380694, - "y": 5.624616329733554 + "x": 1.602956765841976, + "y": 6.212043622656119 }, "prevControl": null, "nextControl": { - "x": 1.4707856249343993, - "y": 6.447014539825145 + "x": 1.8232420006879377, + "y": 6.505757269117401 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 6.7994709155786826 }, "prevControl": { - "x": 1.86729904765713, - "y": 6.579185680732721 + "x": 2.014155870887771, + "y": 6.623242727701913 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/#2 Note.path b/src/main/deploy/pathplanner/paths/#2 Note.path index 09a1914..254f347 100644 --- a/src/main/deploy/pathplanner/paths/#2 Note.path +++ b/src/main/deploy/pathplanner/paths/#2 Note.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/#3 Note.path b/src/main/deploy/pathplanner/paths/#3 Note.path index 4b3b134..a0c8389 100644 --- a/src/main/deploy/pathplanner/paths/#3 Note.path +++ b/src/main/deploy/pathplanner/paths/#3 Note.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/#4 Note.path b/src/main/deploy/pathplanner/paths/#4 Note.path index 02fc797..89f4554 100644 --- a/src/main/deploy/pathplanner/paths/#4 Note.path +++ b/src/main/deploy/pathplanner/paths/#4 Note.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/#5 Note.path b/src/main/deploy/pathplanner/paths/#5 Note.path index 4c57c77..74277d8 100644 --- a/src/main/deploy/pathplanner/paths/#5 Note.path +++ b/src/main/deploy/pathplanner/paths/#5 Note.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/#7 Note.path b/src/main/deploy/pathplanner/paths/#7 Note.path new file mode 100644 index 0000000..53a30fa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/#7 Note.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.202322537024323, + "y": 4.846275166611156 + }, + "prevControl": null, + "nextControl": { + "x": 4.760378465300759, + "y": 4.479133108534554 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.714947816299927, + "y": 4.185419462073272 + }, + "prevControl": { + "x": 5.155997971525456, + "y": 4.3303323847925785 + }, + "nextControl": { + "x": 6.111461239022656, + "y": 4.0826196858118236 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.668143565267453, + "y": 4.082619685811823 + }, + "prevControl": { + "x": 6.684202849622158, + "y": 4.009191274196502 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.14490260373321, + "rotateFast": false + }, + "reversed": false, + "folder": "Autonomous", + "previewStartingState": { + "rotation": 159.64677085193117, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1m Test.path b/src/main/deploy/pathplanner/paths/1m Test.path index 016c6de..b3882fd 100644 --- a/src/main/deploy/pathplanner/paths/1m Test.path +++ b/src/main/deploy/pathplanner/paths/1m Test.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/2m Test.path b/src/main/deploy/pathplanner/paths/2m Test.path index 91a3465..9c2b925 100644 --- a/src/main/deploy/pathplanner/paths/2m Test.path +++ b/src/main/deploy/pathplanner/paths/2m Test.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Circle Test.path b/src/main/deploy/pathplanner/paths/Circle Test.path index 0f000e8..d6c79b2 100644 --- a/src/main/deploy/pathplanner/paths/Circle Test.path +++ b/src/main/deploy/pathplanner/paths/Circle Test.path @@ -87,9 +87,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path b/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path index 33b4c86..87455d3 100644 --- a/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path +++ b/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Park Right.path b/src/main/deploy/pathplanner/paths/Park Right.path index 085132c..3aa8b31 100644 --- a/src/main/deploy/pathplanner/paths/Park Right.path +++ b/src/main/deploy/pathplanner/paths/Park Right.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Playground.path b/src/main/deploy/pathplanner/paths/Playground.path index 71d7ebe..d5c2100 100644 --- a/src/main/deploy/pathplanner/paths/Playground.path +++ b/src/main/deploy/pathplanner/paths/Playground.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Source Direct Left.path b/src/main/deploy/pathplanner/paths/Source Direct Left.path index 3fe104a..c16fb8e 100644 --- a/src/main/deploy/pathplanner/paths/Source Direct Left.path +++ b/src/main/deploy/pathplanner/paths/Source Direct Left.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Source Direct Middle.path b/src/main/deploy/pathplanner/paths/Source Direct Middle.path index 9bc3fb2..ce194fe 100644 --- a/src/main/deploy/pathplanner/paths/Source Direct Middle.path +++ b/src/main/deploy/pathplanner/paths/Source Direct Middle.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Source Direct Right.path b/src/main/deploy/pathplanner/paths/Source Direct Right.path index c82a333..080ccfb 100644 --- a/src/main/deploy/pathplanner/paths/Source Direct Right.path +++ b/src/main/deploy/pathplanner/paths/Source Direct Right.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAcceleration": 1.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index 1a123ad..85c5ff6 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -12,7 +12,9 @@ // GNU General Public License for more details. package org.rambots +import com.fasterxml.jackson.databind.util.Named import com.pathplanner.lib.auto.AutoBuilder +import com.pathplanner.lib.auto.NamedCommands import edu.wpi.first.math.geometry.* import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands @@ -110,6 +112,12 @@ object RobotContainer { aprilTagVision = AprilTagVision(object : AprilTagVisionIO {}) } } + + NamedCommands.registerCommand("Intake", IntakeCommandGroup()) + NamedCommands.registerCommand("IntakeHome", IntakeHomeCommandGroup()) + NamedCommands.registerCommand("Stop", StopShooterCommand()) + NamedCommands.registerCommand("Shoot", AutoShootCommand(driveController) { drive.pose }) + NamedCommands.registerCommand("ConfirmShoot", IntakeCommand()) // // // Registering named commands // NamedCommands.registerCommand("aim", AutoAimCommand(driveController) {drive.pose} ) @@ -171,14 +179,12 @@ object RobotContainer { drive.setBrakeMode(false) ArmSubsystem.setBrakeMode(false) - ElevatorSubsystem.setBrakeMode(false) } fun lockAllMotors() { drive.setBrakeMode(true) ArmSubsystem.setBrakeMode(true) - ElevatorSubsystem.setBrakeMode(true) } /** @@ -194,29 +200,28 @@ object RobotContainer { { (-controller.rightX).pow(1.0) } ) - /* default commands are continuously scheduled after ending, and will continuously move to desired position */ - ArmSubsystem.defaultCommand = ArmPositionCommand { ArmSubsystem.desiredPosition } - ElevatorSubsystem.defaultCommand = ElevatorPositionCommand { ElevatorSubsystem.desiredPosition } - WristSubsystem.defaultCommand = WristPositionCommand { WristSubsystem.desiredPosition } - controller.L1().onTrue(AmpScoreCommandGroup()) controller.L1().onFalse(AmpScoreHomeCommandGroup()) controller.L2().onTrue(IntakeCommandGroup()) controller.L2().onFalse(IntakeHomeCommandGroup()) + controller.povUp().onTrue(BackAmpCommandGroup()) + controller.R2().whileTrue(AutoShootCommand(driveController) { drive.pose }) controller.square().whileTrue(IntakeCommand()) controller.triangle().whileTrue(ReverseIntakeCommand()) + controller.circle().whileTrue(ReverseSlowIntakeCommand()) - - - controller.povUp().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition--})) - controller.povDown().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition++})) + xbox.povUp().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition--})) + xbox.povDown().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition++})) xbox.a().onTrue(FullHomeCommandGroup()) xbox.x().onTrue(ExtendClimberCommandGroup()) + xbox.y().whileTrue(RetractClimberCommand()) + + xbox.rightTrigger().whileTrue(ShootCommand()) // // controller.cross().onTrue(SuperStructure.ampCommand) diff --git a/src/main/java/org/rambots/auto/AutoConstants.kt b/src/main/java/org/rambots/auto/AutoConstants.kt index cbe8670..36cf3e2 100644 --- a/src/main/java/org/rambots/auto/AutoConstants.kt +++ b/src/main/java/org/rambots/auto/AutoConstants.kt @@ -25,7 +25,7 @@ object AutoConstants { AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0))) ) - const val CLOSEST_AUTOAIM_DISTANCE = 1.6 + const val CLOSEST_AUTOAIM_DISTANCE = 1.68 const val FURTHEST_AUTOAIM_DISTANCE = 6.0 } \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt index 585690d..2214e86 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt @@ -11,6 +11,6 @@ import org.rambots.subsystems.superstructure.ShooterSubsystem class AmpScoreCommandGroup : SequentialCommandGroup( ArmPositionCommand( { 50.0 }, { it > 35.0}), ElevatorPositionCommand({ 68.0 }, { it > 5.0}), - WristPositionCommand({ 165.0 }, { it > 155.0 }), + WristPositionCommand({ 170.0 }, { it > 165.0 }), Commands.runOnce({ ShooterSubsystem.reverseIntake() }, ShooterSubsystem) ) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt index 3f4dba3..48de682 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt @@ -10,7 +10,7 @@ import org.rambots.subsystems.superstructure.ShooterSubsystem class AmpScoreHomeCommandGroup : SequentialCommandGroup( Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem), - WristPositionCommand( { 0.0 }, { it < 10.0 }), - ElevatorPositionCommand ({ 0.0 }, { it < 10.0 }), + WristPositionCommand( { 0.0 }, { it < 70.0 }), + ElevatorPositionCommand ({ 0.0 }, { it < 30.0 }), ArmPositionCommand ({ 0.0 }, { true }), ) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt b/src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt index e28a202..e8afc31 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt @@ -18,10 +18,11 @@ class AutoShootCommand(private val controller: DriveController, private val pose } private val wristAngle = InterpolatingDoubleTreeMap().apply { - put(AutoConstants.CLOSEST_AUTOAIM_DISTANCE, 68.0) - put(1.9, 66.5) - put(2.5, 63.14) - put(3.98, 42.4) + put(AutoConstants.CLOSEST_AUTOAIM_DISTANCE, 79.0) + put(1.9, 73.5) + put(2.5, 67.14) + put(3.35, 53.0) + put(4.5, 45.4) put(AutoConstants.FURTHEST_AUTOAIM_DISTANCE, 0.0) } diff --git a/src/main/java/org/rambots/commands/superstructure/actions/BackAmpCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/BackAmpCommandGroup.kt new file mode 100644 index 0000000..b39638c --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/BackAmpCommandGroup.kt @@ -0,0 +1,14 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.rambots.commands.superstructure.primitive.ArmPositionCommand +import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand +import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class BackAmpCommandGroup : SequentialCommandGroup( + ArmPositionCommand { 70.0 }, +) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt index faef4fb..8c15502 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt @@ -5,10 +5,12 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import org.rambots.commands.superstructure.primitive.ArmPositionCommand import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.config.ElevatorConstants +import org.rambots.subsystems.superstructure.ElevatorSubsystem class ExtendClimberCommandGroup : SequentialCommandGroup( WristPositionCommand { 0.0 }, ElevatorPositionCommand({ 0.0 }, { it < 5.0 }), ArmPositionCommand({ 80.0 }, { it > 75.0 }), - ElevatorPositionCommand({ 65.0 }, { it > 60.0 }), + ElevatorPositionCommand({ ElevatorConstants.ELEVATOR_MAX_HEIGHT }, { it > ElevatorConstants.ELEVATOR_MAX_HEIGHT - 5.0 }) ) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt index 7a8b82b..25f08a5 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt @@ -14,7 +14,7 @@ import org.rambots.util.LimelightHelpers class IntakeCommandGroup : SequentialCommandGroup( ArmPositionCommand ({ 0.0 }, { it < 5.0 }), ElevatorPositionCommand ({ 55.0 }, { it > 20.0 }), - WristPositionCommand ({ 125.0 }, { true }), + WristPositionCommand ({ 130.0 }, { true }), Commands.runOnce ({ ShooterSubsystem.intake() }, ShooterSubsystem), diff --git a/src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt b/src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt new file mode 100644 index 0000000..41cf982 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt @@ -0,0 +1,17 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.superstructure.ElevatorSubsystem + +class RetractClimberCommand : Command() { + + override fun execute() { + ElevatorSubsystem.desiredPosition -= 0.5 + } + + override fun isFinished(): Boolean { + return false + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/ReverseSlowIntakeCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/ReverseSlowIntakeCommand.kt new file mode 100644 index 0000000..a2c14ed --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/primitive/ReverseSlowIntakeCommand.kt @@ -0,0 +1,20 @@ +package org.rambots.commands.superstructure.primitive + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class ReverseSlowIntakeCommand : Command() { + + override fun initialize() { + ShooterSubsystem.reverseIntakeSlow() + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + ShooterSubsystem.stopIntake() + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/ShootCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/ShootCommand.kt new file mode 100644 index 0000000..152e544 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/primitive/ShootCommand.kt @@ -0,0 +1,20 @@ +package org.rambots.commands.superstructure.primitive + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class ShootCommand : Command() { + + override fun initialize() { + ShooterSubsystem.shoot() + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + ShooterSubsystem.stopShooter() + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/StopShooterCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/StopShooterCommand.kt new file mode 100644 index 0000000..46bdcb6 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/primitive/StopShooterCommand.kt @@ -0,0 +1,20 @@ +package org.rambots.commands.superstructure.primitive + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class StopShooterCommand : Command() { + + override fun initialize() { + ShooterSubsystem.stopShooter() + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + ShooterSubsystem.stopShooter() + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt index b6d958a..773b0db 100644 --- a/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt +++ b/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt @@ -23,9 +23,9 @@ class WristPositionCommand( override fun end(interrupted: Boolean) { if (!interrupted && angle.invoke() == 0.0) { - SequentialCommandGroup(WaitCommand(2.0), Commands.runOnce({ + SequentialCommandGroup(WaitCommand(1.0), Commands.runOnce({ if (WristSubsystem.desiredPosition == 0.0) WristSubsystem.resetEncoder() - })) + })).schedule() } } diff --git a/src/main/java/org/rambots/config/ElevatorConstants.kt b/src/main/java/org/rambots/config/ElevatorConstants.kt index 51e8384..ef6f9cb 100644 --- a/src/main/java/org/rambots/config/ElevatorConstants.kt +++ b/src/main/java/org/rambots/config/ElevatorConstants.kt @@ -9,6 +9,7 @@ object ElevatorConstants { val ELEVATOR_PID = PIDConstants(0.2, 0.0, 0.0) - const val ELEVATOR_MAX_HEIGHT = 0.0 + const val ELEVATOR_MIN_HEIGHT = 0.0 + const val ELEVATOR_MAX_HEIGHT = 69.0 } \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java index d706165..6f27e87 100644 --- a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java +++ b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java @@ -27,7 +27,7 @@ public final class DriveConstants { }; public static final double xyStdDevCoefficient = switch (Constants.getRobot()) { - default -> 0.005; + default -> 0.065; }; public static final double thetaStdDevCoefficient = switch (Constants.getRobot()) { diff --git a/src/main/java/org/rambots/subsystems/superstructure/ElevatorSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/ElevatorSubsystem.kt index a2d04a9..388da38 100644 --- a/src/main/java/org/rambots/subsystems/superstructure/ElevatorSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/ElevatorSubsystem.kt @@ -7,13 +7,19 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger import org.rambots.config.ElevatorConstants.ELEVATOR_FOLLOWER_ID import org.rambots.config.ElevatorConstants.ELEVATOR_LEADER_ID +import org.rambots.config.ElevatorConstants.ELEVATOR_MAX_HEIGHT +import org.rambots.config.ElevatorConstants.ELEVATOR_MIN_HEIGHT import org.rambots.config.ElevatorConstants.ELEVATOR_PID +import org.rambots.util.MathUtil import org.rambots.util.SparkMaxUtil object ElevatorSubsystem : SubsystemBase() { var offset = 0.0 var desiredPosition = 0.0 + set(value) { + field = MathUtil.bound(value, ELEVATOR_MIN_HEIGHT, ELEVATOR_MAX_HEIGHT) + } val position get() = leader.encoder.position diff --git a/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt index 6a45cb1..cbc492d 100644 --- a/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt @@ -59,6 +59,10 @@ object ShooterSubsystem : SubsystemBase() { intakeTopLead.set(-INTAKE_OUTPUT) } + fun reverseIntakeSlow() { + intakeTopLead.set(-INTAKE_OUTPUT * 0.5) + } + fun stopIntake() { intakeTopLead.stopMotor() } diff --git a/src/main/java/org/rambots/util/MathUtil.kt b/src/main/java/org/rambots/util/MathUtil.kt new file mode 100644 index 0000000..3e15408 --- /dev/null +++ b/src/main/java/org/rambots/util/MathUtil.kt @@ -0,0 +1,9 @@ +package org.rambots.util + +object MathUtil { + + fun bound(value: Double, min: Double, max: Double): Double { + return min.coerceAtLeast(max.coerceAtMost(value)) + } + +} \ No newline at end of file