From 93138fee638807cb408a09b393898d1ce5b4bf9f Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Tue, 10 Sep 2024 20:40:18 +0800 Subject: [PATCH] migrated six notes auto from 2024 code --- .../paths/grab fifth and shoot fast.path | 58 +++++++++ .../paths/grab fourth and shoot fast.path | 52 ++++++++ .../paths/grab sixth and shoot fast.path | 58 +++++++++ .../paths/move to second and grab fast.path | 123 ++++++++++++++++++ .../paths/move to shoot fourth slow.path | 64 +++++++++ .../paths/move to shoot third fast.path | 107 +++++++++++++++ .../paths/move to third and grab slow.path | 75 +++++++++++ .../paths/move to third fast alter.path | 74 +++++++++++ .../pathplanner/paths/move to third fast.path | 86 ++++++++++++ .../deploy/pathplanner/paths/park slow.path | 64 +++++++++ .../paths/retry grab second fast.path | 86 ++++++++++++ .../pathplanner/paths/shoot second fast.path | 64 +++++++++ .../main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/autos/AmpSideSixNotesFast.java | 109 ++++++++++++++++ .../main/java/frc/robot/autos/AutoUtils.java | 16 +++ .../java/frc/robot/autos/ExampleAuto.java | 20 --- 16 files changed, 1037 insertions(+), 21 deletions(-) create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fifth and shoot fast.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fourth and shoot fast.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab sixth and shoot fast.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to second and grab fast.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot fourth slow.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot third fast.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third and grab slow.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast alter.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/park slow.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/retry grab second fast.path create mode 100644 example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/shoot second fast.path create mode 100644 example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java create mode 100644 example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java delete mode 100644 example/5516-2024-OffSeason/src/main/java/frc/robot/autos/ExampleAuto.java diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fifth and shoot fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fifth and shoot fast.path new file mode 100644 index 0000000..7735602 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fifth and shoot fast.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.796456677516902, + "y": 6.987605867505556 + }, + "prevControl": null, + "nextControl": { + "x": 1.6268793085931452, + "y": 6.56091098935013 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6869572379813143, + "y": 5.533450842404673 + }, + "prevControl": { + "x": 0.9860884253203863, + "y": 5.570246205735591 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": 180.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 179.59013950824868, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": -151.83829108404882, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fourth and shoot fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fourth and shoot fast.path new file mode 100644 index 0000000..3381f67 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab fourth and shoot fast.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9342664209477884, + "y": 6.2625328158468205 + }, + "prevControl": null, + "nextControl": { + "x": 2.282538086404351, + "y": 6.56166505577594 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8457612546136124, + "y": 6.967577964964403 + }, + "prevControl": { + "x": 2.4347810023679513, + "y": 6.642875365955014 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -144.43334096477494, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": -144.86413746252984, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab sixth and shoot fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab sixth and shoot fast.path new file mode 100644 index 0000000..3d0cf66 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/grab sixth and shoot fast.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6869572379813143, + "y": 5.533450842404673 + }, + "prevControl": null, + "nextControl": { + "x": 1.702819405638812, + "y": 5.186561414645751 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6869572379813143, + "y": 4.259485472712135 + }, + "prevControl": { + "x": 1.5236366605592058, + "y": 4.937263682361081 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8, + "rotationDegrees": 150.83036216050985, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 150.0130531320794, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": 178.5593878174664, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to second and grab fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to second and grab fast.path new file mode 100644 index 0000000..7185bae --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to second and grab fast.path @@ -0,0 +1,123 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.474717716270295, + "y": 6.319729668395012 + }, + "prevControl": null, + "nextControl": { + "x": 2.4747177162702965, + "y": 6.319729668395012 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 6.32 + }, + "prevControl": { + "x": 2.6300482226283646, + "y": 6.310840030134598 + }, + "nextControl": { + "x": 4.433134030426526, + "y": 6.329825233790444 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.215497887698029, + "y": 7.069927876510557 + }, + "prevControl": { + "x": 5.681053224314813, + "y": 6.951162395758732 + }, + "nextControl": { + "x": 6.7413602917360045, + "y": 7.1867861885189965 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.533187742532068, + "y": 7.566575702546424 + }, + "prevControl": { + "x": 7.500939319790858, + "y": 7.371811849199027 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 0.25, + "rotationDegrees": 180.0, + "rotateFast": true + }, + { + "waypointRelativePos": 2.8, + "rotationDegrees": -171.25383773744474, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "slow down to turn", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.85, + "constraints": { + "maxVelocity": 1.6, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + }, + { + "name": "slow down to grab", + "minWaypointRelativePos": 2.7, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -170.5376777919743, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": -151.29115678153866, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot fourth slow.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot fourth slow.path new file mode 100644 index 0000000..cf28f7d --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot fourth slow.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.270256540517947, + "y": 5.765010059087858 + }, + "prevControl": null, + "nextControl": { + "x": 6.248790036803855, + "y": 6.836359700795371 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 5.8 + }, + "prevControl": { + "x": 5.84586259180776, + "y": 6.8841646518966035 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "slow down for accuracy", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -157.1521759318685, + "rotateFast": true + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": 163.94001269283933, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot third fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot third fast.path new file mode 100644 index 0000000..e71d781 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to shoot third fast.path @@ -0,0 +1,107 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.270256540517947, + "y": 5.765010059087858 + }, + "prevControl": null, + "nextControl": { + "x": 7.257484503104291, + "y": 6.09610860977815 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.718850061659844, + "y": 6.495374509140316 + }, + "prevControl": { + "x": 6.357734877103481, + "y": 6.586643768489407 + }, + "nextControl": { + "x": 5.173511272287129, + "y": 6.417468967801356 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.00492815220506, + "y": 6.294767740187914 + }, + "prevControl": { + "x": 4.604909256650227, + "y": 6.2995294949859115 + }, + "nextControl": { + "x": 3.704937599982476, + "y": 6.292386862788915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9, + "y": 6.3 + }, + "prevControl": { + "x": 2.484291560042195, + "y": 6.307727735776344 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.0, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2.8, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "slow down for accuracy", + "minWaypointRelativePos": 2.0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 1.3, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -157.1521759318685, + "rotateFast": true + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": 163.94001269283933, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third and grab slow.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third and grab slow.path new file mode 100644 index 0000000..82f4d8a --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third and grab slow.path @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8947741881248894, + "y": 7.069927876510557 + }, + "prevControl": null, + "nextControl": { + "x": 4.258121161556677, + "y": 6.85568763782842 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.44554400852574, + "y": 7.517884739209573 + }, + "prevControl": { + "x": 7.41329558578453, + "y": 7.323120885862176 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "slow down to turn", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.1, + "constraints": { + "maxVelocity": 1.4, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + }, + { + "name": "slow down to grab", + "minWaypointRelativePos": 0.85, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -170.5376777919743, + "rotateFast": true + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": -151.59475356652922, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast alter.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast alter.path new file mode 100644 index 0000000..4535d34 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast alter.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 5.8 + }, + "prevControl": null, + "nextControl": { + "x": 4.550266941577775, + "y": 4.53799778299438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.640944520324009, + "y": 4.119255498297474 + }, + "prevControl": { + "x": 4.98094452032401, + "y": 4.119255498297474 + }, + "nextControl": { + "x": 6.867956796412619, + "y": 4.119255498297474 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.038487555029677, + "y": 4.119255498297474 + }, + "prevControl": { + "x": 7.415243224318003, + "y": 4.148470076299584 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 178.74554837731847, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 2.5, + "rotation": 179.44374777291935, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": -179.08304115818086, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast.path new file mode 100644 index 0000000..624bce1 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/move to third fast.path @@ -0,0 +1,86 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 5.8 + }, + "prevControl": null, + "nextControl": { + "x": 4.589219712240718, + "y": 6.802232821137257 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.040210419678835, + "y": 6.787520289149915 + }, + "prevControl": { + "x": 5.114877124407482, + "y": 6.787520289149915 + }, + "nextControl": { + "x": 6.897171374407386, + "y": 6.787520289149915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.50397316452996, + "y": 5.61 + }, + "prevControl": { + "x": 7.802823292472789, + "y": 6.24298252337213 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 140.64824737373522, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "slow down", + "minWaypointRelativePos": 1.45, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 2.5, + "rotation": 141.4993462796546, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": -179.08304115818086, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/park slow.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/park slow.path new file mode 100644 index 0000000..23b1a77 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/park slow.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 5.8 + }, + "prevControl": null, + "nextControl": { + "x": 5.183422555119264, + "y": 6.993433111556562 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.631229645613709, + "y": 6.720261962406666 + }, + "prevControl": { + "x": 5.197081112576758, + "y": 7.007091669014056 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "slow down for accuracy", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": 179.13194855025444, + "rotateFast": true + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": -156.08972329432788, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/retry grab second fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/retry grab second fast.path new file mode 100644 index 0000000..343b8f6 --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/retry grab second fast.path @@ -0,0 +1,86 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.007325338494093, + "y": 7.459455583205354 + }, + "prevControl": null, + "nextControl": { + "x": 6.663454750397046, + "y": 7.235477151855846 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.578844861129818, + "y": 6.086370417106196 + }, + "prevControl": { + "x": 6.895186408263061, + "y": 6.45749929151958 + }, + "nextControl": { + "x": 7.919681604487764, + "y": 5.901344756426168 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.552664127866809, + "y": 5.64815174707455 + }, + "prevControl": { + "x": 8.124183650502532, + "y": 5.794224637085098 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.8, + "rotationDegrees": 156.90755197239994, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "slow down to turn", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.2, + "constraints": { + "maxVelocity": 1.4, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 158.96248897457835, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": 177.31622484053108, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/shoot second fast.path b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/shoot second fast.path new file mode 100644 index 0000000..bfa540d --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/deploy/pathplanner/paths/shoot second fast.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.01, + "y": 7.449717390537984 + }, + "prevControl": null, + "nextControl": { + "x": 5.426704281641067, + "y": 7.313382693194805 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 5.8 + }, + "prevControl": { + "x": 4.803459950929391, + "y": 7.206262573853738 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "slow down to shoot", + "minWaypointRelativePos": 0.8, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 7.5, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 7.0, + "maxAngularVelocity": 670.0, + "maxAngularAcceleration": 1080.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -178.34366447921664, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side Six Note", + "previewStartingState": { + "rotation": 179.20404091066868, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java index 1e30544..ce07c44 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java @@ -258,7 +258,7 @@ private void configureAutoNamedCommands() { private static LoggedDashboardChooser buildAutoChooser() { final LoggedDashboardChooser autoSendableChooser = new LoggedDashboardChooser<>("Select Auto"); autoSendableChooser.addDefaultOption("None", Auto.none()); - autoSendableChooser.addOption("Example Custom Auto", new ExampleAuto()); + autoSendableChooser.addOption("Amp Side Six Notes Fast", new AmpSideSixNotesFast()); autoSendableChooser.addOption("Example Pathplanner Auto", new PathPlannerAuto("Example Auto", new Pose2d(1.3, 7.2, new Rotation2d()))); SmartDashboard.putData("Select Auto", autoSendableChooser.getSendableChooser()); diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java new file mode 100644 index 0000000..19f082b --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AmpSideSixNotesFast.java @@ -0,0 +1,109 @@ +package frc.robot.autos; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotContainer; +import frc.robot.commands.CommandOnFly; +import frc.robot.commands.shooter.AimAtSpeakerFactory; +import frc.robot.commands.shooter.FollowPathGrabAndShootStill; + + +public class AmpSideSixNotesFast implements Auto { + @Override + public Command getAutoCommand(RobotContainer robot) { + final boolean[] noteFightFailed = new boolean[2]; + final SequentialCommandGroup commands = new SequentialCommandGroup(); + + /* shoot preloaded */ + commands.addCommands(AimAtSpeakerFactory.shootAtSpeakerStill( + robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, + robot.visualizerForShooter + )); + commands.addCommands(AutoUtils.setIdleForSuperStructureCommand(robot)); + + /* grab second */ + final Command chassisMoveToSecond = AutoBuilder.followPath(PathPlannerPath.fromPathFile("move to second and grab fast")) + .andThen(Commands.runOnce(robot.drive::stop, robot.drive)); + final Command grabSecondWhenClose = Commands.waitSeconds(1.5).andThen( + robot.intake.suckNoteUntilTouching().withTimeout(2) + .finallyDo(() -> noteFightFailed[0] = !robot.intake.isNotePresent()) + ); + commands.addCommands(grabSecondWhenClose.deadlineWith( + chassisMoveToSecond.alongWith(robot.pitch.getPitchDefaultCommand()) + )); + + /* retry grab second */ + final Command chassisMoveToRetrySecond = AutoBuilder.followPath(PathPlannerPath.fromPathFile("retry grab second fast")) + .andThen(Commands.runOnce(robot.drive::stop, robot.drive)); + final Command retryGrabSecond = Commands.waitSeconds(0.5).onlyIf(() -> noteFightFailed[0]).andThen( + robot.intake.suckNoteUntilTouching().withTimeout(2) + ); + commands.addCommands(retryGrabSecond + .deadlineWith(chassisMoveToRetrySecond) + ); + + /* move and shoot second */ + commands.addCommands(new FollowPathGrabAndShootStill( + PathPlannerPath.fromPathFile("shoot second fast"), + 1.5, + robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, + robot.visualizerForShooter + )); + commands.addCommands(AutoUtils.setIdleForSuperStructureCommand(robot)); + + + /* move to third and grab */ + final Command moveToThirdNormal = AutoBuilder.followPath(PathPlannerPath.fromPathFile("move to third fast")) + .andThen(Commands.runOnce(robot.drive::stop, robot.drive)); + final Command moveToThirdAlter = AutoBuilder.followPath(PathPlannerPath.fromPathFile("move to third fast alter")) + .andThen(Commands.runOnce(robot.drive::stop, robot.drive)); + final Command moveToThirdDecisive = new CommandOnFly(() -> noteFightFailed[0] ? moveToThirdAlter : moveToThirdNormal) + .deadlineWith(robot.pitch.getPitchDefaultCommand()); + final Command intakeThird = Commands.waitSeconds(1) + .andThen(robot.intake.suckNoteUntilTouching().withTimeout(4)); + commands.addCommands(intakeThird.deadlineWith(moveToThirdDecisive)); + + /* shoot third */ + commands.addCommands(new FollowPathGrabAndShootStill( + PathPlannerPath.fromPathFile("move to shoot third fast"), + 1.2, + robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, + robot.visualizerForShooter + )); + + /* grab fourth and shoot */ + commands.addCommands(new FollowPathGrabAndShootStill( + PathPlannerPath.fromPathFile("grab fourth and shoot fast"), + 0.8, + robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, + robot.visualizerForShooter + )); + + /* grab fifth and shoot */ + commands.addCommands(new FollowPathGrabAndShootStill( + PathPlannerPath.fromPathFile("grab fifth and shoot fast"), + 0.8, + robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, + robot.visualizerForShooter + )); + + /* grab sixth and shoot */ + commands.addCommands(new FollowPathGrabAndShootStill( + PathPlannerPath.fromPathFile("grab sixth and shoot fast"), + 0.8, + robot.drive, robot.intake, robot.pitch, robot.flyWheels, robot.shooterOptimization, robot.ledStatusLight, + robot.visualizerForShooter + )); + + return commands; + } + @Override + public Pose2d getStartingPoseAtBlueAlliance() { + return new Pose2d(1.47, 6.32, Rotation2d.fromDegrees(-150)); + } +} diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java new file mode 100644 index 0000000..c4bba7e --- /dev/null +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/AutoUtils.java @@ -0,0 +1,16 @@ +package frc.robot.autos; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.RobotContainer; + +public class AutoUtils { + public static Command setIdleForSuperStructureCommand(RobotContainer robot) { + return Commands.runOnce(() -> setIdleForSuperStructuer(robot), robot.intake, robot.pitch, robot.flyWheels); + } + public static void setIdleForSuperStructuer(RobotContainer robot) { + robot.intake.runIdle(); + robot.flyWheels.runIdle(); + robot.pitch.setIdle(); + } +} diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/ExampleAuto.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/ExampleAuto.java deleted file mode 100644 index fa83ffb..0000000 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/autos/ExampleAuto.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.autos; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotContainer; - -public class ExampleAuto implements Auto { - @Override - public Command getAutoCommand(RobotContainer robot) { - return AutoBuilder.followPath(PathPlannerPath.fromPathFile("Example Path")); - } - - @Override - public Pose2d getStartingPoseAtBlueAlliance() { - return new Pose2d(1.3, 7.2, Rotation2d.fromDegrees(180)); - } -}