From 429055826dae778d1b38438b20823bcbf3da7691 Mon Sep 17 00:00:00 2001 From: BenG49 <64862590+BenG49@users.noreply.github.com> Date: Mon, 8 Apr 2024 16:24:36 -0400 Subject: [PATCH] Changes from NYC Regional (#102) * pr * Shuffleboard config * Add logging + calibrated amp distance * Remove kA term * HP attention on far side of field * Add advantage scope config * Tune 5 piece * Tune 5 piece * Robot relative swerve drive bind, operator intake forever bind * Change lift height handoff checking * Revert B to A * Halve trap score speed * 5 Piece * Clamp drive speed while ferry shooting * Robot relative drive also intakes * 5 piece * Remove some auton chooser * Lift coast * Lift idle mode * Red HGF, Blue CBAED * Move ferry target * Fix typo * red 5 piece * 20% trap speed * 5 piece first shot * Debounce ferry * Re zero front right * Increase shoot timeout on second hgf * pull out G to shoot * adjust blue hgf --------- Co-authored-by: Prog694 --- advantagescope-config.json | 449 ++++++++++++++++++ shuffleboard.json | 17 +- .../autos/{4 HGF.auto => 4 HGF Blue.auto} | 0 .../deploy/pathplanner/autos/4 HGF Red.auto | 49 ++ .../autos/{6 CBAED.auto => 6 CBAED Blue.auto} | 0 .../deploy/pathplanner/autos/6 CBAED Red.auto | 61 +++ src/main/deploy/pathplanner/paths/A to D.path | 4 +- src/main/deploy/pathplanner/paths/A to E.path | 10 +- .../deploy/pathplanner/paths/B to A Red.path | 55 +++ src/main/deploy/pathplanner/paths/B to A.path | 12 +- .../pathplanner/paths/C to B Close.path | 8 +- .../deploy/pathplanner/paths/C to B Red.path | 58 +++ src/main/deploy/pathplanner/paths/C to B.path | 14 +- .../deploy/pathplanner/paths/E to Shoot.path | 4 +- .../pathplanner/paths/Ferry Shot to E.path | 4 +- .../paths/G to Shoot (HGF) Red.path | 67 +++ .../pathplanner/paths/G to Shoot (HGF).path | 12 +- .../paths/H to HShoot (HGF) Red.path | 55 +++ .../paths/HShoot to G (HGF) Red.path | 49 ++ .../paths/Preload to C Close Red.path | 52 ++ .../deploy/pathplanner/paths/Shoot to E.path | 4 +- .../paths/Start to H (HGF) Red.path | 55 +++ src/main/java/com/stuypulse/robot/Robot.java | 4 + .../com/stuypulse/robot/RobotContainer.java | 46 +- .../FastAlignShootSpeakerRelative.java | 10 +- .../robot/commands/amper/AmperToHeight.java | 5 + .../commands/auton/HGF/FourPieceHGF.java | 2 +- .../commands/conveyor/ConveyorToAmp.java | 4 +- .../robot/commands/leds/LEDDefaultMode.java | 9 +- .../commands/swerve/SwerveDriveAutoFerry.java | 34 +- .../commands/swerve/SwerveDriveDrive.java | 17 +- .../stuypulse/robot/constants/Settings.java | 10 +- .../robot/subsystems/amper/Amper.java | 14 +- .../robot/subsystems/amper/AmperImpl.java | 7 + .../robot/subsystems/amper/AmperSim.java | 5 +- .../swerve/modules/KrakenSwerveModule.java | 4 +- 36 files changed, 1116 insertions(+), 94 deletions(-) create mode 100644 advantagescope-config.json rename src/main/deploy/pathplanner/autos/{4 HGF.auto => 4 HGF Blue.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/4 HGF Red.auto rename src/main/deploy/pathplanner/autos/{6 CBAED.auto => 6 CBAED Blue.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/6 CBAED Red.auto create mode 100644 src/main/deploy/pathplanner/paths/B to A Red.path create mode 100644 src/main/deploy/pathplanner/paths/C to B Red.path create mode 100644 src/main/deploy/pathplanner/paths/G to Shoot (HGF) Red.path create mode 100644 src/main/deploy/pathplanner/paths/H to HShoot (HGF) Red.path create mode 100644 src/main/deploy/pathplanner/paths/HShoot to G (HGF) Red.path create mode 100644 src/main/deploy/pathplanner/paths/Preload to C Close Red.path create mode 100644 src/main/deploy/pathplanner/paths/Start to H (HGF) Red.path diff --git a/advantagescope-config.json b/advantagescope-config.json new file mode 100644 index 00000000..18530837 --- /dev/null +++ b/advantagescope-config.json @@ -0,0 +1,449 @@ +{ + "hubs": [ + { + "x": -8, + "y": -8, + "width": 1552, + "height": 840, + "state": { + "sidebar": { + "width": 364, + "expanded": [ + "/SmartDashboard", + "/SmartDashboard/Amper/Lift", + "/Vision/shooter_camera", + "/Vision/climber_camera", + "/SmartDashboard/Odometry/Estimator", + "/SmartDashboard/Swerve/Modules/Back Right", + "/SmartDashboard/Swerve/Modules/Back Left", + "/SmartDashboard/Swerve/Modules/Front Left", + "/SmartDashboard/Swerve/Modules/Front Right" + ] + }, + "tabs": { + "selected": 2, + "tabs": [ + { + "type": 0, + "path": "../docs/INDEX.md" + }, + { + "type": 1, + "legendHeight": 0.3007186644671743, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Left/Target Velocity", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Left/Velocity", + "color": "#e5b31b", + "show": true + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Right/Target Velocity", + "color": "#af2437", + "show": false + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Right/Velocity", + "color": "#aacaee", + "show": false + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Front Left/Target Velocity", + "color": "#c0b487", + "show": false + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Front Left/Velocity", + "color": "#858584", + "show": false + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Front Right/Target Velocity", + "color": "#3b875a", + "show": false + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Front Right/Velocity", + "color": "#d993aa", + "show": false + } + ] + }, + "discrete": { + "fields": [ + { + "key": "NT:/SmartDashboard/Robot State", + "color": "#eb987e", + "show": true + } + ] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Left/Drive Current", + "color": "#80588e", + "show": false + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Left/Drive Voltage", + "color": "#e48b32", + "show": false + } + ] + } + }, + "title": "Swerve" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:SmartDashboard/Shooter/Feeder Target RPM", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Feeder RPM", + "color": "#e5b31b", + "show": true + }, + { + "key": "NT:SmartDashboard/Shooter/Right Target RPM", + "color": "#af2437", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Right RPM", + "color": "#80588e", + "show": true + }, + { + "key": "NT:SmartDashboard/Shooter/Left Target RPM", + "color": "#e48b32", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Left RPM", + "color": "#aacaee", + "show": true + } + ] + }, + "discrete": { + "fields": [ + { + "key": "NT:/SmartDashboard/Robot State", + "color": "#a64b6b", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Note Shot", + "color": "#dbd345", + "show": true + }, + { + "key": "NT:/SmartDashboard/Is DS Attached", + "color": "#7e331f", + "show": false + } + ] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Shooter/Feeder Current", + "color": "#c0b487", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Right Current", + "color": "#858584", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Left Current", + "color": "#3b875a", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Feeder Requested Voltage", + "color": "#d993aa", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Left Requested Voltage", + "color": "#eb987e", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Right Requested Voltage", + "color": "#5d4f92", + "show": true + } + ] + } + }, + "title": "Shooter" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Conveyor/Gandalf Motor Current", + "color": "#e48b32", + "show": true + }, + { + "key": "NT:/SmartDashboard/Intake/Current", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/SmartDashboard/PDP/Total Current (amps)", + "color": "#af2437", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Feeder Current", + "color": "#80588e", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Left Current", + "color": "#aacaee", + "show": true + }, + { + "key": "NT:/SmartDashboard/Shooter/Right Current", + "color": "#c0b487", + "show": true + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Left/Drive Current", + "color": "#858584", + "show": true + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Back Right/Drive Current", + "color": "#3b875a", + "show": true + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Front Left/Drive Current", + "color": "#d993aa", + "show": true + }, + { + "key": "NT:/SmartDashboard/Swerve/Modules/Front Right/Drive Current", + "color": "#eb987e", + "show": true + } + ] + }, + "discrete": { + "fields": [ + { + "key": "NT:/SmartDashboard/Robot State", + "color": "#5d4f92", + "show": true + } + ] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/PDP/Battery Voltage (volts)", + "color": "#e5b31b", + "show": true + } + ] + } + }, + "title": "Current" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Amper/Lift Height", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:SmartDashboard/Amper/Target Height", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [ + { + "key": "NT:/SmartDashboard/Robot State", + "color": "#80588e", + "show": true + } + ] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Amper/Voltage", + "color": "#af2437", + "show": true + } + ] + } + }, + "title": "Lift" + }, + { + "type": 5, + "uuid": "uy1sf3yn3frhpjct2dslc7qpkcoh7sf8", + "fields": [], + "listFields": [ + [ + { + "type": "Robot", + "key": "NT:/SmartDashboard/Field/Estimator Pose", + "sourceTypeIndex": 0, + "sourceType": 5 + }, + { + "type": "Ghost", + "key": "NT:/SmartDashboard/Field/Odometry Pose", + "sourceTypeIndex": 0, + "sourceType": 5 + } + ] + ], + "options": { + "game": "2024 Field", + "unitDistance": "meters", + "unitRotation": "degrees", + "origin": "right", + "size": 0.75, + "allianceBumpers": "auto", + "allianceOrigin": "auto", + "orientation": "blue left, red right" + }, + "configHidden": false, + "visualizer": null, + "title": "Odometry" + }, + { + "type": 6, + "uuid": "p364dbqgrnnpxy6wmo55un543xpdc4dx", + "fields": [], + "listFields": [ + [ + { + "type": "Yellow Ghost", + "key": "NT:Vision/climber_camera/Pose3d", + "sourceTypeIndex": 1, + "sourceType": "Pose3d" + }, + { + "type": "Green Ghost", + "key": "NT:Vision/shooter_camera/Pose3d", + "sourceTypeIndex": 1, + "sourceType": "Pose3d" + } + ], + [ + { + "type": "Robot", + "key": "NT:/SmartDashboard/Field/Estimator Pose", + "sourceTypeIndex": 0, + "sourceType": 5 + } + ] + ], + "options": { + "field": "2024 Field", + "alliance": "auto", + "robot": "2024 KitBot", + "unitDistance": "meters", + "unitRotation": "degrees" + }, + "configHidden": false, + "visualizer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + -6.732997560240504, + 10.099499096206074, + 0.43926360402853587 + ], + "cameraTarget": [ + -5.5755802131592755, + -0.5897574748872353, + 0.4047789098522105 + ] + }, + "title": "3D Field" + } + ] + } + } + } + ], + "satellites": [], + "version": "3.2.0" +} diff --git a/shuffleboard.json b/shuffleboard.json index 9c7ffe39..ae53c04c 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -91,21 +91,6 @@ "_showGlyph": false } }, - "6,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/Note Detection/Has Note Data", - "_title": "Sees Note", - "_glyph": 148, - "_showGlyph": false, - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - } - }, "6,0": { "size": [ 1, @@ -168,7 +153,7 @@ "content": { "_type": "Camera Stream", "_source0": "camera_server://USB Camera 0", - "_title": "USB Camera 0", + "_title": "Driver Camera", "_glyph": 148, "_showGlyph": false, "Crosshair/Show crosshair": true, diff --git a/src/main/deploy/pathplanner/autos/4 HGF.auto b/src/main/deploy/pathplanner/autos/4 HGF Blue.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/4 HGF.auto rename to src/main/deploy/pathplanner/autos/4 HGF Blue.auto diff --git a/src/main/deploy/pathplanner/autos/4 HGF Red.auto b/src/main/deploy/pathplanner/autos/4 HGF Red.auto new file mode 100644 index 00000000..e8a21d0d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4 HGF Red.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to H (HGF) Red" + } + }, + { + "type": "path", + "data": { + "pathName": "H to HShoot (HGF) Red" + } + }, + { + "type": "path", + "data": { + "pathName": "HShoot to G (HGF) Red" + } + }, + { + "type": "path", + "data": { + "pathName": "G to Shoot (HGF) Red" + } + }, + { + "type": "path", + "data": { + "pathName": "GShoot to F (HGF)" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot (HGF)" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/6 CBAED.auto b/src/main/deploy/pathplanner/autos/6 CBAED Blue.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/6 CBAED.auto rename to src/main/deploy/pathplanner/autos/6 CBAED Blue.auto diff --git a/src/main/deploy/pathplanner/autos/6 CBAED Red.auto b/src/main/deploy/pathplanner/autos/6 CBAED Red.auto new file mode 100644 index 00000000..df604240 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/6 CBAED Red.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Preload to C Close Red" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Preload to C" + } + }, + { + "type": "path", + "data": { + "pathName": "C to B Red" + } + }, + { + "type": "path", + "data": { + "pathName": "B to A Red" + } + }, + { + "type": "path", + "data": { + "pathName": "A to E" + } + }, + { + "type": "path", + "data": { + "pathName": "E to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Shoot to D (CBAED)" + } + }, + { + "type": "path", + "data": { + "pathName": "D to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path index ca53b80e..6cc3538e 100644 --- a/src/main/deploy/pathplanner/paths/A to D.path +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.70383902591167, + "x": 2.9000000000000004, "y": 6.887856966597124 }, "prevControl": null, "nextControl": { - "x": 3.6941070946532406, + "x": 3.890268068741571, "y": 7.027030067557189 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A to E.path b/src/main/deploy/pathplanner/paths/A to E.path index bf4678ec..32e0f73c 100644 --- a/src/main/deploy/pathplanner/paths/A to E.path +++ b/src/main/deploy/pathplanner/paths/A to E.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.70383902591167, + "x": 2.9000000000000004, "y": 6.887856966597124 }, "prevControl": null, "nextControl": { - "x": 4.82368893835003, + "x": 5.01984991243836, "y": 6.971014628418452 }, "isLocked": false, @@ -17,11 +17,11 @@ { "anchor": { "x": 8.17, - "y": 6.042894923752811 + "y": 5.84 }, "prevControl": { "x": 7.01740860685425, - "y": 6.665124436730354 + "y": 6.462229512977543 }, "nextControl": null, "isLocked": false, @@ -40,7 +40,7 @@ "goalEndState": { "velocity": 0, "rotation": 0.0, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": "ABCDE", diff --git a/src/main/deploy/pathplanner/paths/B to A Red.path b/src/main/deploy/pathplanner/paths/B to A Red.path new file mode 100644 index 00000000..19d0be09 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B to A Red.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.7, + "y": 5.27 + }, + "prevControl": null, + "nextControl": { + "x": 2.5306942929129037, + "y": 5.892387378210238 + }, + "isLocked": false, + "linkedName": "B Red" + }, + { + "anchor": { + "x": 2.70383902591167, + "y": 6.887856966597124 + }, + "prevControl": { + "x": 2.5507512734398694, + "y": 6.452168507627614 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "A Red" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 52.37639263407269, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 50.90614111377048, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index d4928521..f8febcf0 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 2.9260279603180286, - "y": 5.309146116867734 + "x": 2.9, + "y": 5.27 }, "prevControl": null, "nextControl": { - "x": 2.756722253230932, - "y": 5.931533495077972 + "x": 2.7306942929129034, + "y": 5.892387378210238 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 2.70383902591167, + "x": 2.9000000000000004, "y": 6.887856966597124 }, "prevControl": { - "x": 2.5507512734398694, + "x": 2.7469122475281997, "y": 6.452168507627614 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/C to B Close.path b/src/main/deploy/pathplanner/paths/C to B Close.path index 8efaae76..a1916338 100644 --- a/src/main/deploy/pathplanner/paths/C to B Close.path +++ b/src/main/deploy/pathplanner/paths/C to B Close.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9260279603180286, - "y": 5.309146116867734 + "x": 2.9, + "y": 5.27 }, "prevControl": { - "x": 2.6241439876295614, - "y": 5.133858648855076 + "x": 2.5981160273115327, + "y": 5.094712531987342 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C to B Red.path b/src/main/deploy/pathplanner/paths/C to B Red.path new file mode 100644 index 00000000..5be0b6ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C to B Red.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.265328388308022 + }, + "prevControl": null, + "nextControl": { + "x": 2.388096855965791, + "y": 4.841379939170136 + }, + "isLocked": false, + "linkedName": "C" + }, + { + "anchor": { + "x": 2.7, + "y": 5.27 + }, + "prevControl": { + "x": 2.376402701523351, + "y": 4.817991630285256 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "B Red" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 49.9380466176507, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 43.876697285924436, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": -20.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 61ff0ab2..44480865 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.62197994481459, - "y": 4.794603321400375 + "x": 2.5401208637175103, + "y": 4.864768248055015 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 2.9260279603180286, - "y": 5.309146116867734 + "x": 2.9, + "y": 5.27 }, "prevControl": { - "x": 2.692311336301151, - "y": 4.870927446836089 + "x": 2.576402701523351, + "y": 4.817991630285256 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 34.799293349232, + "rotationDegrees": 49.9380466176507, "rotateFast": true } ], diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index 135d3fe9..14bfc8d3 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -4,12 +4,12 @@ { "anchor": { "x": 8.17, - "y": 6.042894923752811 + "y": 5.84 }, "prevControl": null, "nextControl": { "x": 6.661102338686158, - "y": 6.851472614982266 + "y": 6.648577691229455 }, "isLocked": false, "linkedName": "E" diff --git a/src/main/deploy/pathplanner/paths/Ferry Shot to E.path b/src/main/deploy/pathplanner/paths/Ferry Shot to E.path index d3ffe808..40d4906c 100644 --- a/src/main/deploy/pathplanner/paths/Ferry Shot to E.path +++ b/src/main/deploy/pathplanner/paths/Ferry Shot to E.path @@ -17,11 +17,11 @@ { "anchor": { "x": 8.17, - "y": 6.042894923752811 + "y": 5.84 }, "prevControl": { "x": 7.574271630024742, - "y": 6.131140109502442 + "y": 5.928245185749631 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/G to Shoot (HGF) Red.path b/src/main/deploy/pathplanner/paths/G to Shoot (HGF) Red.path new file mode 100644 index 00000000..72b3a8fb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G to Shoot (HGF) Red.path @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.13, + "y": 2.32 + }, + "prevControl": null, + "nextControl": { + "x": 6.305711906979371, + "y": 0.9634780846769682 + }, + "isLocked": false, + "linkedName": "G Red" + }, + { + "anchor": { + "x": 2.5050384003901907, + "y": 3.180810008343664 + }, + "prevControl": { + "x": 4.095443404568156, + "y": 1.3097452975592547 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HShoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -54.462322208206466, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Slow Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.25, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -52.06624772942949, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path b/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path index f27190e0..e49c95d7 100644 --- a/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 8.1299266872038, - "y": 2.1166419540816297 + "x": 8.430000000000001, + "y": 2.12 }, "prevControl": null, "nextControl": { - "x": 6.305638594183171, - "y": 0.760120038758598 + "x": 6.032698339299814, + "y": 0.2723235980944895 }, "isLocked": false, - "linkedName": "G" + "linkedName": null }, { "anchor": { @@ -39,7 +39,7 @@ { "name": "Slow Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.25, + "maxWaypointRelativePos": 0.4, "constraints": { "maxVelocity": 3.0, "maxAcceleration": 2.5, diff --git a/src/main/deploy/pathplanner/paths/H to HShoot (HGF) Red.path b/src/main/deploy/pathplanner/paths/H to HShoot (HGF) Red.path new file mode 100644 index 00000000..70e9d752 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H to HShoot (HGF) Red.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.07, + "y": 0.8800000000000001 + }, + "prevControl": null, + "nextControl": { + "x": 5.860497457486886, + "y": 1.268665081258114 + }, + "isLocked": false, + "linkedName": "H Red" + }, + { + "anchor": { + "x": 2.5050384003901907, + "y": 3.180810008343664 + }, + "prevControl": { + "x": 3.7500926544825335, + "y": 1.924076107885846 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HShoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": -54.90055879498553, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -47.64254529406478, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HShoot to G (HGF) Red.path b/src/main/deploy/pathplanner/paths/HShoot to G (HGF) Red.path new file mode 100644 index 00000000..cb079915 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HShoot to G (HGF) Red.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5050384003901907, + "y": 3.180810008343664 + }, + "prevControl": null, + "nextControl": { + "x": 4.8555634433206185, + "y": 0.7133434209888375 + }, + "isLocked": false, + "linkedName": "HShoot" + }, + { + "anchor": { + "x": 8.13, + "y": 2.32 + }, + "prevControl": { + "x": 6.63314823136769, + "y": 0.7997599224828073 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "G Red" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 42.79740183823393, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Preload to C Close Red.path b/src/main/deploy/pathplanner/paths/Preload to C Close Red.path new file mode 100644 index 00000000..971caea0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Preload to C Close Red.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3473171105886368, + "y": 5.402699352407253 + }, + "prevControl": null, + "nextControl": { + "x": 1.8501657516135543, + "y": 4.9934039469218545 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.33, + "y": 4.48 + }, + "prevControl": { + "x": 1.7335981234355633, + "y": 5.169955112103956 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.970000000000027, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to E.path b/src/main/deploy/pathplanner/paths/Shoot to E.path index 0794fae6..51e904f4 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/Shoot to E.path @@ -17,11 +17,11 @@ { "anchor": { "x": 8.17, - "y": 6.042894923752811 + "y": 5.84 }, "prevControl": { "x": 6.389445717754282, - "y": 6.433901618128758 + "y": 6.231006694375947 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Start to H (HGF) Red.path b/src/main/deploy/pathplanner/paths/Start to H (HGF) Red.path new file mode 100644 index 00000000..8864898f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start to H (HGF) Red.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.87, + "y": 3.264181107065918 + }, + "prevControl": null, + "nextControl": { + "x": 3.661009849273592, + "y": 1.8329545447689473 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.07, + "y": 0.8800000000000001 + }, + "prevControl": { + "x": 5.048463833761794, + "y": 0.8800000000000002 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "H Red" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -10.418493398400832, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -5.675672575285497, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index f382b993..6d0eae44 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -96,6 +96,7 @@ public static boolean isBlue() { public void disabledInit() { robot.intake.setIdleMode(IdleMode.kCoast); robot.conveyor.setIdleMode(IdleMode.kCoast); + robot.amper.setLiftIdleMode(IdleMode.kCoast); scheduler.schedule(new LEDSet(new LED694())); @@ -136,6 +137,7 @@ public void autonomousInit() { robot.intake.setIdleMode(IdleMode.kBrake); robot.conveyor.setIdleMode(IdleMode.kBrake); + robot.amper.setLiftIdleMode(IdleMode.kBrake); SmartDashboard.putString("Robot State", "AUTON"); } @@ -165,6 +167,7 @@ public void teleopInit() { robot.intake.setIdleMode(IdleMode.kBrake); robot.conveyor.setIdleMode(IdleMode.kBrake); + robot.amper.setLiftIdleMode(IdleMode.kBrake); SmartDashboard.putString("Robot State", "TELEOP"); } @@ -191,6 +194,7 @@ public void testInit() { robot.intake.setIdleMode(IdleMode.kBrake); robot.conveyor.setIdleMode(IdleMode.kBrake); + robot.amper.setLiftIdleMode(IdleMode.kBrake); SmartDashboard.putString("Robot State", "TEST"); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 940a9467..3880e553 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -44,7 +44,6 @@ import com.stuypulse.robot.util.PathUtil.AutonConfig; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.PixelFormat; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -53,6 +52,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; public class RobotContainer { @@ -131,7 +131,15 @@ private void configureDriverBindings() { .alongWith(new LEDSet(LEDInstructions.PICKUP) .withTimeout(3.0)))); + // intaking (also robot relative swerve) driver.getLeftTriggerButton() + .whileTrue(new IntakeAcquire() + .deadlineWith(new LEDSet(LEDInstructions.INTAKE)) + .andThen(new BuzzController(driver) + .alongWith(new LEDSet(LEDInstructions.PICKUP) + .withTimeout(3.0)))); + + driver.getDPadLeft() .onTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); @@ -244,11 +252,13 @@ private void configureOperatorBindings() { .onTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); operator.getRightTriggerButton() - .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.INTAKE)) - .andThen(new BuzzController(driver) + .onTrue(new IntakeAcquireForever()) + .whileTrue(new WaitUntilCommand(Intake.getInstance()::hasNote) + .deadlineWith(new LEDSet(LEDInstructions.INTAKE)) + .andThen(new BuzzController(driver) .alongWith(new LEDSet(LEDInstructions.PICKUP) - .withTimeout(3.0)))); + .withTimeout(3.0)))) + .onFalse(new IntakeStop()); operator.getLeftBumper() .onTrue(new ConveyorToAmp()) @@ -304,19 +314,23 @@ public void configureAutons() { autonChooser.addOption("Mobility", new Mobility()); AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new, - "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)"); + "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)"); + AutonConfig HGF_RED = new AutonConfig("4 HGF", FourPieceHGF::new, + "Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)"); AutonConfig TrackingCBAE = new AutonConfig("Tracking 5 CBAE Podium", FivePieceTrackingCBAE::new, "Preload to C", "C to B", "B to A", "A to E", "E to Shoot"); AutonConfig CBAED = new AutonConfig("5 CBAE", SixPieceCBAED::new, - "Preload to C Close", "Close Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); + "Preload to C Close", "Close Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); + AutonConfig CBAED_RED = new AutonConfig("5 CBAE", SixPieceCBAED::new, + "Preload to C Close Red", "Close Preload to C", "C to B Red", "B to A Red","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); AutonConfig CBAED_OLD = new AutonConfig("5 CBAE Old", SixPieceCBAEDOld::new, - "Preload to C", "C to B", "B to A", "A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); + "Preload to C", "C to B", "B to A", "A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); AutonConfig CHGF = new AutonConfig("4.5 Piece CHGF", FivePieceCHGF::new, - "Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)"); + "Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)"); // AutonConfig ADEF = new AutonConfig("4.5 Piece ADEF", FourPieceADEF::new, // "Preload to A", "A to D", "D to Shoot", "Shoot to E", "E to Shoot", "Shoot To F (ADEF)", "F To Shoot (ADEF)"); @@ -330,16 +344,12 @@ public void configureAutons() { // AutonConfig PodiumCloseCBAE = new AutonConfig("Podium Close 5 Piece CBAE", FivePiecePodiumForwardCBAE::new, // "Forward First Piece to C", "C to B 2", "B to A","A to E", "E to Shoot"); - HGF.registerDefaultBlue(autonChooser) - .registerRed(autonChooser); - - CBAED - .registerBlue(autonChooser) - .registerRed(autonChooser); + // TODO: automatically choose red/blue + HGF.registerDefaultBlue(autonChooser); + HGF_RED.registerRed(autonChooser); - CHGF - .registerBlue(autonChooser) - .registerRed(autonChooser); + CBAED.registerBlue(autonChooser); + CBAED_RED.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/FastAlignShootSpeakerRelative.java b/src/main/java/com/stuypulse/robot/commands/FastAlignShootSpeakerRelative.java index f2241368..e86f179c 100644 --- a/src/main/java/com/stuypulse/robot/commands/FastAlignShootSpeakerRelative.java +++ b/src/main/java/com/stuypulse/robot/commands/FastAlignShootSpeakerRelative.java @@ -60,10 +60,14 @@ public class FastAlignShootSpeakerRelative extends Command { private Pose2d targetPose; public FastAlignShootSpeakerRelative(double angleToSpeaker) { - this(angleToSpeaker, Alignment.PODIUM_SHOT_DISTANCE.get()); + this(angleToSpeaker, Alignment.PODIUM_SHOT_DISTANCE.get(), 0.75); } - public FastAlignShootSpeakerRelative(double angleToSpeaker, double distanceToSpeaker) { + public FastAlignShootSpeakerRelative(double angleToSpeaker, double shootTimeout) { + this(angleToSpeaker, Alignment.PODIUM_SHOT_DISTANCE.get(), shootTimeout); + } + + public FastAlignShootSpeakerRelative(double angleToSpeaker, double distanceToSpeaker, double shootTimeout) { swerve = SwerveDrive.getInstance(); odometry = Odometry.getInstance(); conveyor = Conveyor.getInstance(); @@ -92,7 +96,7 @@ public FastAlignShootSpeakerRelative(double angleToSpeaker, double distanceToSpe .filtered(new BDebounce.Falling(2.0)); isFinished = isAligned - .filtered(new BDebounce.Rising(0.75)) + .filtered(new BDebounce.Rising(shootTimeout)) .or(Shooter.getInstance()::noteShot); velocityError = IStream.create(() -> { diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java index 21714eb2..bf9b1e1f 100644 --- a/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/amper/AmperToHeight.java @@ -24,6 +24,11 @@ public static Command untilDone(double height, double epsilon) { .andThen(new WaitUntilCommand(() -> Amper.getInstance().isAtTargetHeight(epsilon))); } + public static Command untilBelow(double height, double epsilon) { + return new AmperToHeight(height) + .andThen(new WaitUntilCommand(() -> Amper.getInstance().isAtBelowTargetHeight(epsilon))); + } + private final Amper amper; private final double height; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index a568d775..ddfdf88e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -30,7 +30,7 @@ public FourPieceHGF(PathPlannerPath... paths) { ConveyorShootRoutine.untilNoteShot(0.75), new FollowPathAndIntake(paths[0]), - new FollowPathAlignAndShootFast(paths[1], new FastAlignShootSpeakerRelative(-45)), + new FollowPathAlignAndShootFast(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), new FollowPathAndIntake(paths[2]), new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), new FollowPathAndIntake(paths[4]), diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmp.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmp.java index 667ad86c..1c2bbdd5 100644 --- a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmp.java +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmp.java @@ -8,17 +8,17 @@ import com.stuypulse.robot.commands.amper.AmperToHeight; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Amper.Lift; import com.stuypulse.robot.subsystems.amper.Amper; import com.stuypulse.robot.subsystems.conveyor.Conveyor; import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.robot.util.ShooterSpeeds; import edu.wpi.first.wpilibj2.command.Command; public class ConveyorToAmp extends Command { public static Command withCheckLift() { - return AmperToHeight.untilDone(Settings.Amper.Lift.MIN_HEIGHT) + return AmperToHeight.untilBelow(Lift.MIN_HEIGHT, Lift.MAX_HEIGHT_ERROR) .andThen(new ConveyorToAmp()); } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index e68a7576..e2554870 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -6,6 +6,7 @@ package com.stuypulse.robot.commands.leds; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.LEDInstructions; import com.stuypulse.robot.constants.Settings.Amper.Lift; import com.stuypulse.robot.subsystems.amper.Amper; @@ -13,7 +14,7 @@ import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction; -import com.stuypulse.stuylib.util.StopWatch; +import com.stuypulse.robot.subsystems.odometry.Odometry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -31,11 +32,13 @@ public class LEDDefaultMode extends Command { private final LEDController leds; + private final Odometry odometry; private final Intake intake; private final Amper amper; public LEDDefaultMode() { leds = LEDController.getInstance(); + odometry = Odometry.getInstance(); intake = Intake.getInstance(); amper = Amper.getInstance(); @@ -58,6 +61,10 @@ private LEDInstruction getInstruction() { if (intake.hasNote()) { return LEDInstructions.CONTAINS_NOTE; } + + if (odometry.getPose().getX() > Field.LENGTH / 2.0) { + return LEDInstructions.ATTENTION; + } return LEDInstructions.DEFAULT; } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java index dcab5158..42b4794e 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java @@ -11,6 +11,9 @@ import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; @@ -22,6 +25,7 @@ import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -42,6 +46,8 @@ public class SwerveDriveAutoFerry extends Command { private final AngleController controller; private final IStream angleVelocity; + private final BStream shoot; + public SwerveDriveAutoFerry(Gamepad driver) { swerve = SwerveDrive.getInstance(); shooter = Shooter.getInstance(); @@ -70,15 +76,27 @@ public SwerveDriveAutoFerry(Gamepad driver) { .filtered(x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST)) .filtered(x -> -x); + shoot = BStream.create(this::canShoot) + .and(() -> Math.abs(controller.getError().toDegrees()) < getAngleTolerance()) + .filtered(new BDebounce.Falling(0.4)); addRequirements(swerve, shooter, odometry, conveyor, intake); } // returns pose of close amp corner private Translation2d getTargetPose() { - return Robot.isBlue() - ? new Translation2d(0, Field.WIDTH - 0.75) - : new Translation2d(0, 0.5); + final double NOTE_TRAVEL_TIME = 2.5; + + Translation2d pose = Robot.isBlue() + ? new Translation2d(0, Field.WIDTH - 1.5) + : new Translation2d(0, 1.5); + + return pose; + + // Translation2d offset = odometry.getRobotVelocity() + // .times(NOTE_TRAVEL_TIME); + + // return pose.minus(offset); } private Rotation2d getTargetAngle() { @@ -134,15 +152,21 @@ public void execute() { conveyor.stop(); } else { SmartDashboard.putNumber("Ferry/Angle Tolerance", getAngleTolerance()); - if (canShoot() && Math.abs(controller.getError().toDegrees()) < getAngleTolerance()) { + if (shoot.get()) { intake.acquire(); conveyor.toShooter(); + + Vector2D vel = drive.get(); + double mag = MathUtil.clamp(vel.magnitude(), 0.0, 2.0); + + swerve.drive(vel.normalize().mul(mag), omega); } else { intake.stop(); conveyor.stop(); + + swerve.drive(drive.get(), omega); } - swerve.drive(drive.get(), omega); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index a2ddeba8..99f42a54 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -19,14 +19,17 @@ import com.stuypulse.robot.constants.Settings.Driver.Turn; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveDrive extends Command { - private SwerveDrive swerve; + private final SwerveDrive swerve; - private VStream speed; - private IStream turn; + private final Gamepad driver; + + private final VStream speed; + private final IStream turn; public SwerveDriveDrive(Gamepad driver) { swerve = SwerveDrive.getInstance(); @@ -47,11 +50,17 @@ public SwerveDriveDrive(Gamepad driver) { x -> x * Turn.MAX_TELEOP_TURNING.get(), new LowPassFilter(Turn.RC.get())); + this.driver = driver; + addRequirements(swerve); } @Override public void execute() { - swerve.drive(speed.get(), turn.get()); + if (driver.getLeftTriggerPressed()) { + swerve.setChassisSpeeds(new ChassisSpeeds(speed.get().y, -speed.get().x, -turn.get())); + } else { + swerve.drive(speed.get(), turn.get()); + } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 5773db76..6ae03e51 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -88,7 +88,7 @@ public interface Amper { public interface Score { double AMP_SPEED = 1.0; - double TRAP_SPEED = 0.3; + double TRAP_SPEED = 0.2; double FROM_CONVEYOR_SPEED = 0.35; double TO_CONVEYOR_SPEED = 0.3; @@ -223,7 +223,7 @@ public interface Drive { public interface FrontRight { String ID = "Front Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(42.714844); + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(40.4); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5); } @@ -399,7 +399,7 @@ public interface Alignment { double PODIUM_SHOT_MAX_ANGLE = 80; SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5)); - SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(21.5)); // was 23.5 at comp + SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5)); SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(22.0)); SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0)); @@ -413,7 +413,7 @@ public interface Alignment { public interface Translation { SmartNumber kP = new SmartNumber("Alignment/Translation/kP", 6.0); SmartNumber kI = new SmartNumber("Alignment/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("Alignment/Translation/kD", 0.6); + SmartNumber kD = new SmartNumber("Alignment/Translation/kD", 0.6); // 0.0 with kA term } public interface Rotation { @@ -426,7 +426,7 @@ public interface Shoot { public interface Translation { SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.8); + SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.8); // 0.2 with kA term } public interface Rotation { diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java index f42ce6bb..5519ab04 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/Amper.java @@ -8,12 +8,13 @@ import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; - +import com.revrobotics.CANSparkBase.IdleMode; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.RobotType; import com.stuypulse.robot.constants.Settings.Amper.Lift; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,6 +22,9 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.odometry.Odometry; + /* AMP: 1 motor @@ -87,6 +91,10 @@ public void setTargetHeight(double height) { public final double getTargetHeight() { return targetHeight.get(); } + + public final boolean isAtBelowTargetHeight(double epsilonMeters) { + return getLiftHeight() <= getTargetHeight() + epsilonMeters; + } public final boolean isAtTargetHeight(double epsilonMeters) { return Math.abs(getTargetHeight() - getLiftHeight()) < epsilonMeters; @@ -107,6 +115,8 @@ public final double getMinHeight() { public abstract double getLiftHeight(); public abstract void stopLift(); + + public abstract void setLiftIdleMode(IdleMode mode); /*** IR SENSOR ***/ @@ -150,6 +160,8 @@ public final void resetConstraints() { @Override public void periodic() { + SmartDashboard.putNumber("Amper/Amp Distance", Units.metersToInches(Field.WIDTH - Odometry.getInstance().getPose().getY())); + lift2d.setLength(Settings.Amper.Lift.VISUALIZATION_MIN_LENGTH + getLiftHeight()); if (targetHeight.get() > Settings.Amper.Lift.MAX_HEIGHT) diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java index 507dc03c..38050952 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java @@ -27,6 +27,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkBase.IdleMode; import java.util.Optional; @@ -112,6 +113,12 @@ public void setTargetHeight(double height) { voltageOverride = Optional.empty(); } + @Override + public void setLiftIdleMode(IdleMode mode) { + liftMotor.setIdleMode(mode); + liftMotor.burnFlash(); + } + /*** IR SENSOR ***/ @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java index 814e355a..cbe19444 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperSim.java @@ -17,7 +17,7 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; - +import com.revrobotics.CANSparkBase.IdleMode; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Amper.Lift; @@ -95,6 +95,9 @@ public void stopLift() { sim.setInputVoltage(0.0); } + @Override + public void setLiftIdleMode(IdleMode mode) {} + /*** IR SENSOR ***/ @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index 4124401c..d55d740f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -166,7 +166,7 @@ private double convertDriveVel(double speedMetersPerSecond) { public void periodic() { super.periodic(); - final boolean USE_ACCEL = true; + final boolean USE_ACCEL = false; final boolean USE_ACCEL_IN_AUTON = false; final boolean USE_FOC_IN_AUTON = false; @@ -202,11 +202,13 @@ public void periodic() { pivotMotor.setVoltage(pivotController.getOutput()); } + SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Target Acceleration", acceleration * Encoder.Drive.POSITION_CONVERSION); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Drive Current", driveMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Drive Position", getPosition()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Velocity", getVelocity()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Drive Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Turn Voltage", pivotController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Turn Current", pivotMotor.getOutputCurrent()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Angle Error", pivotController.getError().toDegrees()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Raw Encoder Angle", Units.rotationsToDegrees(pivotEncoder.getAbsolutePosition().getValueAsDouble())); }