diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 6c49fd076..88d2f4567 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 7.783237924865832, - "y": 7.028586762075134 + "x": 6.797094017094016, + "y": 7.077262969588551 }, "prevControl": { - "x": 7.767012522361359, - "y": 7.531574239713775 + "x": 6.780868614589544, + "y": 7.580250447227192 }, "nextControl": { - "x": 7.817452345049281, - "y": 5.967939736388225 + "x": 6.831308437277466, + "y": 6.016615943901642 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.783237924865832, - "y": 4.805706618962433 + "x": 6.797094017094016, + "y": 4.49742397137746 }, "prevControl": { - "x": 7.77903133903134, - "y": 5.171980056980058 + "x": 6.792887431259524, + "y": 4.863697409395084 }, "nextControl": { - "x": 7.786250707881794, - "y": 4.543379297786823 + "x": 6.800106800109979, + "y": 4.235096650201849 }, "isLocked": false, "linkedName": null @@ -52,28 +52,28 @@ "y": 5.611267806267806 }, "prevControl": { - "x": 7.802016105528991, - "y": 5.5605141654377555 + "x": 7.069320214665453, + "y": 5.616976744193628 }, "nextControl": { - "x": 5.517991452991453, - "y": 5.675868945868947 + "x": 5.516642687480301, + "y": 5.584415074153799 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.533418803418803, - "y": 5.611267806267806 + "x": 2.35, + "y": 5.611 }, "prevControl": { - "x": 3.334472934472934, - "y": 5.675868945868947 + "x": 3.151054131054131, + "y": 5.675601139601141 }, "nextControl": null, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": "ACycleEnd" } ], "rotationTargets": [ @@ -91,15 +91,15 @@ }, { "waypointRelativePos": 3.0, - "rotationDegrees": 0.0 + "rotationDegrees": -139.08745511543628 }, { - "waypointRelativePos": 3.45, - "rotationDegrees": 0.0 + "waypointRelativePos": 3.2406417112299466, + "rotationDegrees": 180.0 }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": 0.0 + "rotationDegrees": 180.0 } ], "constraintZones": [ @@ -142,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": -38.82347239526609 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 811891d20..6869a3fda 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -3,121 +3,121 @@ "waypoints": [ { "anchor": { - "x": 2.533418803418803, - "y": 5.611267806267806 + "x": 2.1, + "y": 5.555967806267805 }, "prevControl": null, "nextControl": { - "x": 2.267699809926946, - "y": 6.160705710211645 + "x": 1.8342810065081427, + "y": 6.105405710211644 }, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": null }, { "anchor": { - "x": 2.5075783475783475, - "y": 7.447760964912282 + "x": 3.094096601073345, + "y": 7.392460964912281 }, "prevControl": { - "x": 2.0694924799575967, - "y": 7.351408549885447 + "x": 2.656010733452594, + "y": 7.296108549885446 }, "nextControl": { - "x": 2.7903904157463497, - "y": 7.509962528077773 + "x": 3.376908669241347, + "y": 7.4546625280777725 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.634273504273503, - "y": 7.447760964912282 + "x": 6.0146690518783545, + "y": 7.392460964912281 }, "prevControl": { - "x": 4.144916933849193, - "y": 7.530332248967907 + "x": 4.525312481454044, + "y": 7.475032248967906 }, "nextControl": { - "x": 6.494219837010534, - "y": 7.400084757398864 + "x": 6.874615384615385, + "y": 7.344784757398863 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.202763532763532, - "y": 4.060840455840457 + "x": 6.566332737030411, + "y": 4.692128801431127 }, "prevControl": { - "x": 6.215683760683759, - "y": 3.569871794871795 + "x": 6.579252964950638, + "y": 4.201160140462465 }, "nextControl": { - "x": 6.1825608953181, - "y": 4.828540678766911 + "x": 6.546130099584979, + "y": 5.459829024357583 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.866837606837606, - "y": 5.611 + "x": 6.566332737030411, + "y": 5.438497316636852 }, "prevControl": { - "x": 6.590370370370371, - "y": 5.624188034188035 + "x": 6.825939177101969, + "y": 5.42227191413238 }, "nextControl": { - "x": 5.351084085527113, - "y": 5.601599216214575 + "x": 6.051498103813295, + "y": 5.470674481212921 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.755698005698005, - "y": 5.611 + "x": 4.667960644007155, + "y": 5.438497316636852 }, "prevControl": { - "x": 5.905598290598291, - "y": 5.598079772079772 + "x": 5.706386404293381, + "y": 5.438497316636852 }, "nextControl": { - "x": 4.148936797920834, - "y": 5.6178175416604175 + "x": 4.061161136642901, + "y": 5.438497316636852 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.760840455840455, - "y": 5.611 + "x": 3.5646332737030413, + "y": 5.438497316636852 }, "prevControl": { - "x": 4.6006552706552695, - "y": 5.598347578347578 + "x": 4.404448088517856, + "y": 5.42584489498443 }, "nextControl": { - "x": 3.34446165048371, - "y": 5.61727304986715 + "x": 3.148254468346296, + "y": 5.444770366504002 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.533418803418803, - "y": 5.611267806267806 + "x": 2.396404293381037, + "y": 5.438497316636852 }, "prevControl": { - "x": 3.476099715099715, - "y": 5.624455840455841 + "x": 3.3140409461339693, + "y": 5.434604644537204 }, "nextControl": null, "isLocked": false, @@ -143,15 +143,15 @@ }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 0.0 + "rotationDegrees": -134.71789215346988 }, { - "waypointRelativePos": 4.688679245283029, - "rotationDegrees": 0.0 + "waypointRelativePos": 4.398395721925132, + "rotationDegrees": 180.0 }, { - "waypointRelativePos": 5.734276729559759, - "rotationDegrees": 0.0 + "waypointRelativePos": 6.344919786096259, + "rotationDegrees": 180.0 } ], "constraintZones": [ @@ -160,7 +160,7 @@ "minWaypointRelativePos": 2.0644067796610166, "maxWaypointRelativePos": 3.22711864406781, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.3, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, @@ -174,10 +174,10 @@ "maxWaypointRelativePos": 1.380148548278184, "constraints": { "maxVelocity": 1.5, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } }, @@ -187,10 +187,10 @@ "maxWaypointRelativePos": 6.4280891289669135, "constraints": { "maxVelocity": 2.3, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } } @@ -198,22 +198,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 8, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -37.195226690262515 + "rotation": -41.308614013548784 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": 30.68666362424798 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 632dc384e..98ffcdb1e 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 4.424579606440071, - "y": 0.6996797853309484 + "x": 4.425, + "y": 0.6443797853309476 }, "prevControl": null, "nextControl": { - "x": 5.438196256563948, - "y": 0.6560842522924792 + "x": 5.438616650123877, + "y": 0.6007842522924784 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.783237924865832, - "y": 1.0404132379248665 + "x": 6.797094017094016, + "y": 0.9364370304114491 }, "prevControl": { - "x": 7.767012522361359, - "y": 0.5374257602862258 + "x": 6.780868614589544, + "y": 0.4334495527728084 }, "nextControl": { - "x": 7.817452345049281, - "y": 2.1010602636117754 + "x": 6.831308437277466, + "y": 1.997084056098358 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.783237924865832, - "y": 3.2632933810375677 + "x": 6.797094017094016, + "y": 3.5162760286225403 }, "prevControl": { - "x": 7.77903133903134, - "y": 2.8970199430199433 + "x": 6.792887431259524, + "y": 3.150002590604916 }, "nextControl": { - "x": 7.786250707881794, - "y": 3.5256207022131782 + "x": 6.800106800109979, + "y": 3.778603349798151 }, "isLocked": false, "linkedName": null @@ -49,31 +49,31 @@ { "anchor": { "x": 6.797094017094016, - "y": 2.4577321937321948 + "y": 2.402432193732194 }, "prevControl": { - "x": 7.802016105528991, - "y": 2.5084858345622454 + "x": 7.069320214665453, + "y": 2.3967232558063722 }, "nextControl": { - "x": 5.517991452991453, - "y": 2.3931310541310538 + "x": 5.516642687480301, + "y": 2.4292849258462015 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.533418803418803, + "x": 2.35, "y": 2.4577321937321948 }, "prevControl": { - "x": 3.334472934472934, + "x": 3.151054131054131, "y": 2.3931310541310538 }, "nextControl": null, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": "BCycleEnd" } ], "rotationTargets": [ @@ -91,15 +91,15 @@ }, { "waypointRelativePos": 3, - "rotationDegrees": 0.0 + "rotationDegrees": 139.08745511543628 }, { - "waypointRelativePos": 3.45, - "rotationDegrees": 0.0 + "waypointRelativePos": 3.2406417112299466, + "rotationDegrees": 180.0 }, { "waypointRelativePos": 3.7526652452025653, - "rotationDegrees": 0.0 + "rotationDegrees": 180.0 } ], "constraintZones": [ @@ -142,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": 30.68666362424798 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index a28ef683d..d010fc67e 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,28 +3,28 @@ "waypoints": [ { "anchor": { - "x": 2.533418803418803, + "x": 2.1, "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 2.267699809926946, + "x": 1.8342810065081427, "y": 1.9082942897883557 }, "isLocked": false, - "linkedName": "A-Cycle1-End" + "linkedName": null }, { "anchor": { - "x": 2.5075783475783475, + "x": 3.094096601073345, "y": 0.6212390350877186 }, "prevControl": { - "x": 2.0694924799575967, + "x": 2.656010733452594, "y": 0.717591450114554 }, "nextControl": { - "x": 2.7903904157463497, + "x": 3.376908669241347, "y": 0.5590374719222275 }, "isLocked": false, @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 5.634273504273503, + "x": 6.0146690518783545, "y": 0.6212390350877186 }, "prevControl": { - "x": 4.144916933849193, + "x": 4.525312481454044, "y": 0.5386677510320939 }, "nextControl": { - "x": 6.494219837010534, + "x": 6.874615384615385, "y": 0.6689152426011367 }, "isLocked": false, @@ -48,76 +48,76 @@ }, { "anchor": { - "x": 6.202763532763532, - "y": 4.008159544159544 + "x": 6.566332737030411, + "y": 3.321571198568873 }, "prevControl": { - "x": 6.215683760683759, - "y": 4.499128205128206 + "x": 6.579252964950638, + "y": 3.8125398595375346 }, "nextControl": { - "x": 6.1825608953181, - "y": 3.2404593212330894 + "x": 6.546130099584979, + "y": 2.5538709756424174 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.866837606837606, - "y": 2.458000000000001 + "x": 6.566332737030411, + "y": 2.5752026833631483 }, "prevControl": { - "x": 6.590370370370371, - "y": 2.444811965811966 + "x": 6.825939177101969, + "y": 2.59142808586762 }, "nextControl": { - "x": 5.351084085527113, - "y": 2.4674007837854255 + "x": 6.051498103813295, + "y": 2.543025518787079 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.755698005698005, - "y": 2.458000000000001 + "x": 4.667960644007155, + "y": 2.5752026833631483 }, "prevControl": { - "x": 5.905598290598291, - "y": 2.470920227920229 + "x": 5.706386404293381, + "y": 2.5752026833631483 }, "nextControl": { - "x": 4.148936797920834, - "y": 2.4511824583395834 + "x": 4.061161136642901, + "y": 2.5752026833631483 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.760840455840455, - "y": 2.458000000000001 + "x": 3.5646332737030413, + "y": 2.5752026833631483 }, "prevControl": { - "x": 4.6006552706552695, - "y": 2.4706524216524226 + "x": 4.404448088517856, + "y": 2.58785510501557 }, "nextControl": { - "x": 3.34446165048371, - "y": 2.4517269501328505 + "x": 3.148254468346296, + "y": 2.5689296334959977 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.533418803418803, - "y": 2.4577321937321948 + "x": 2.396404293381037, + "y": 2.5752026833631483 }, "prevControl": { - "x": 3.476099715099715, - "y": 2.4445441595441597 + "x": 3.3140409461339693, + "y": 2.579095355462796 }, "nextControl": null, "isLocked": false, @@ -143,15 +143,15 @@ }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 0.0 + "rotationDegrees": 134.71789215346988 }, { - "waypointRelativePos": 4.688679245283029, - "rotationDegrees": 0.0 + "waypointRelativePos": 4.398395721925132, + "rotationDegrees": -180.0 }, { - "waypointRelativePos": 5.734276729559759, - "rotationDegrees": 0.0 + "waypointRelativePos": 6.344919786096259, + "rotationDegrees": -180.0 } ], "constraintZones": [ @@ -160,7 +160,7 @@ "minWaypointRelativePos": 2.0644067796610166, "maxWaypointRelativePos": 3.22711864406781, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.3, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, @@ -207,13 +207,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 52.804773309737485 + "rotation": 41.308614013548784 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -37.18470645323313 + "rotation": 30.68666362424798 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot+Outpost.path b/src/main/deploy/pathplanner/paths/Depot+Outpost.path new file mode 100644 index 000000000..192420be5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot+Outpost.path @@ -0,0 +1,165 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.2, + "y": 4.027 + }, + "prevControl": null, + "nextControl": { + "x": 1.6987119856887296, + "y": 3.9133094812164577 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7311627906976743, + "y": 5.9577101967799635 + }, + "prevControl": { + "x": 2.039445438282647, + "y": 5.9739355992844345 + }, + "nextControl": { + "x": 0.5617388688105667, + "y": 5.896161569312226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8549910554561715, + "y": 5.9577101967799635 + }, + "prevControl": { + "x": 2.0233327067511238, + "y": 5.95771019680967 + }, + "nextControl": { + "x": 0.6049910554561715, + "y": 5.957710196773607 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2606261180679783, + "y": 5.9577101967799635 + }, + "prevControl": { + "x": 0.8761262902266167, + "y": 5.947029646006592 + }, + "nextControl": { + "x": 2.428855098389982, + "y": 5.990161001788908 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.893416815742397, + "y": 4.027 + }, + "prevControl": { + "x": 1.8771914132379246, + "y": 4.465085867620752 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1310160427807487, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.02139037433155, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.1256684491978635, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 4.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 4.75, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 4.75, + "rotationDegrees": 29.42136089373414 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1389830508474574, + "maxWaypointRelativePos": 2.3728813559322037, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4, + "maxWaypointRelativePos": 4.75, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.62, + "y": 4.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.013559322033898305, + "maxWaypointRelativePos": 0.6237288135593221, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path deleted file mode 100644 index 90ed3dfca..000000000 --- a/src/main/deploy/pathplanner/paths/Depot.path +++ /dev/null @@ -1,113 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.63, - "y": 5.844 - }, - "prevControl": null, - "nextControl": { - "x": 2.9647584973166365, - "y": 5.90890161001789 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.282826475849731, - "y": 5.9577101967799635 - }, - "prevControl": { - "x": 3.2798809613513087, - "y": 5.881013697895228 - }, - "nextControl": { - "x": 1.8609660107334522, - "y": 5.990161001788907 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.685826210826211, - "y": 5.9577101967799635 - }, - "prevControl": { - "x": 1.7065242165242167, - "y": 5.947193732193732 - }, - "nextControl": { - "x": 0.3447247623997972, - "y": 5.9612246361956025 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9480679785330945, - "y": 4.611001788908765 - }, - "prevControl": { - "x": 2.365455840455841, - "y": 6.218518518518518 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0160427807486627, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 1.9358288770053491, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 2.660980810234532, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.799999999999997, - "maxWaypointRelativePos": 2.450847457627116, - "constraints": { - "maxVelocity": 3.58, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -27.897271030947596 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 37135b86b..895d17556 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -5,12 +5,13 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Rotations; import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM; +import static frc.robot.subsystems.shooter.ShooterConstants.kShooterOffsetFromRobotCenter; +import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -31,7 +32,6 @@ import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.magicCarpet.MagicCarpet; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.ShooterLeadCompensator; import frc.robot.util.Zone; import frc.robot.util.Zone.Tuple2d; @@ -54,7 +54,7 @@ public enum ZoneId { ZONE_4 } - private final double FRONT_HUB_OFFSET = Units.inchesToMeters(58.0); + private final double FRONT_HUB_OFFSET = Units.inchesToMeters(70.0); private final Drive drive; private final Shooter shooter; private final MagicCarpet magicCarpet; @@ -231,10 +231,7 @@ public Supplier getHubDistance() { Meters.of( AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) .getDistance( - drive - .getPose() - .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) - .getTranslation())); + drive.getPose().transformBy(kShooterOffsetFromRobotCenter).getTranslation())); } private Rotation2d filteredHubAngle(Rotation2d raw) { @@ -352,13 +349,10 @@ public Command aimToHub() { new Pose2d( drive.getPose().getX(), drive.getPose().getY(), - AllianceFlipUtil.apply(Hub.blueCenter) - .plus(new Translation2d(-0.4, 0)) + AllianceFlipUtil.apply(Hub.innerCenterPoint) + .toTranslation2d() .minus( - drive - .getPose() - .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) - .getTranslation()) + drive.getPose().transformBy(kShooterOffsetFromRobotCenter).getTranslation()) .getAngle())); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a205ec02e..62fa60ceb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,8 +74,6 @@ public class RobotContainer { private Intake intake; private IntakeRollers intakeRollers; private IntakeExtend intakeExtend; - private RobotState robotState = RobotState.getInstance(); - private boolean isRobotOriented = true; // Workaround, change if needed // Controller private final CommandXboxController driverController = new CommandXboxController(0); @@ -100,8 +98,6 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1), new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); } @@ -116,16 +112,13 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1), new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); shooter = new Shooter(new ShooterIOSparkMax()); magicCarpet = new MagicCarpet(new MagicCarpetSparkMax()); indexer = new Indexer(new IndexerIOSparkMax()); intakeRollers = new IntakeRollers(new IntakeRollersIOKraken()); - intakeExtend = - new IntakeExtend(new IntakeExtendIOSparkMax(), operatorController.rightTrigger()); + intakeExtend = new IntakeExtend(new IntakeExtendIOSparkMax()); // climber = new Climber(new ClimberIOPhysical()); } @@ -164,7 +157,7 @@ public RobotContainer() { intakeRollers = new IntakeRollers(new IntakeRollersIO() {}); } if (intakeExtend == null) { - intakeExtend = new IntakeExtend(new IntakeExtendIO() {}, () -> false); + intakeExtend = new IntakeExtend(new IntakeExtendIO() {}); } intake = new Intake(intakeRollers, intakeExtend); if (magicCarpet == null) { @@ -262,6 +255,8 @@ public RobotContainer() { autoChooser.addOption("A-side", autos.ppA2CycleRightRegression()); autoChooser.addOption("B-side", autos.ppB2CycleRightRegression()); + autoChooser.addOption("Depot+Outpost", autos.ppDepot()); + // Configure the button bindings configureButtonBindings(); } @@ -311,7 +306,14 @@ private void configureButtonBindings() { driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED) .and(() -> !operatorController.pov(180).getAsBoolean()) + .and(() -> !RobotState.getInstance().shooterAtSpeed) .toggleOnTrue(intakeExtend.extendToAngle(IntakeConstants.COLLAPSE_POS)); + CustomTriggers.toggleIntakeUp( + driverController.leftBumper(), + () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED) + .and(() -> !operatorController.pov(180).getAsBoolean()) + .and(() -> RobotState.getInstance().shooterAtSpeed) + .toggleOnTrue(intake.slowlyBringInIntakeWithoutRollers()); CustomTriggers.toggleIntakeDown( driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED) @@ -337,7 +339,9 @@ private void configureButtonBindings() { .and(operatorController.pov(180)) .whileTrue(intakeExtend.runIntakeExtendVolts(4)) .onFalse(intakeExtend.stopExtendingCommand()); - operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); + operatorController + .leftTrigger(0.1) + .toggleOnTrue(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())); operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) @@ -363,12 +367,4 @@ public void robotPeriodic() { RobotState.getInstance().updateLEDState(); orchestrator.orchestratorPeriodic(); } - - private Command rumblePulse(double intensity, double seconds) { - return Commands.sequence( - Commands.runOnce( - () -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), - Commands.waitSeconds(seconds), - Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))); - } } diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 06eb4f0a5..641e5a99d 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -1,5 +1,7 @@ package frc.robot.commands.auto; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -12,7 +14,6 @@ import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import java.util.function.Supplier; /** Contains all autos */ @@ -87,7 +88,6 @@ public Autos( this.shooter = shooter; } - private static final Supplier center = () -> new Pose2d(3.504, 4.019, new Rotation2d(0)); private static final Supplier depotPoseStopPoint = () -> new Pose2d(1.7998201847076416, 5.983600616455078, Rotation2d.fromDegrees(180)); private static final Supplier depotPose = @@ -101,6 +101,8 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); private static final Supplier startDepot = () -> AllianceFlipUtil.apply(new Pose2d(3.630, 5.844, new Rotation2d(0.000))); + private static final Supplier center = + () -> AllianceFlipUtil.apply(new Pose2d(3.581, 4.027, Rotation2d.k180deg)); /** * Helper function for a single cycle auto with a constant shooting speed @@ -153,49 +155,71 @@ private Command ppCycleRegression(String path, Supplier startPose) { Commands.deadline( drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), - Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + Commands.waitSeconds(5.7) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.769)))) .withTimeout(15.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) .andThen( Commands.parallel( - Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + Commands.waitSeconds(0.4).andThen(intake.slowlyBringInIntake()), orchestrator.aimToHub().withTimeout(1).repeatedly(), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) .withTimeout(1.6))); } - private Command ppCycleRegressionDepo(String path, Supplier startPose) { + private Command ppCycleRegressionCustom( + String path, Supplier startPose, double endDistance) { return Commands.sequence( Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.FUNNEL_POS).withTimeout(5.5), - Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + intake + .extendToAngleAndIntake(IntakeConstants.FUNNEL_POS) + .withTimeout(4.5) + .andThen(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)), + Commands.waitSeconds(5.7) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(endDistance)))) .withTimeout(15.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) .andThen( Commands.parallel( - Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + Commands.waitSeconds(0.4).andThen(intake.slowlyBringInIntake()), orchestrator.aimToHub().withTimeout(1).repeatedly(), orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) .withTimeout(1.6))); } + private Command ppCycleRegressionDepo(String path) { + return Commands.sequence( + Commands.deadline( + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS), + orchestrator.spinUpShooterDistance(() -> Meters.of(3.185))) + .withTimeout(14.5), + Commands.deadline( + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.9).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.9).andThen(orchestrator.feedUp())))); + } + private Command ppCycleRegressionConnect(String path) { return Commands.sequence( Commands.deadline( drive.getAutonomousCommand(path), intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), - Commands.waitSeconds(5) - .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + Commands.waitSeconds(5.5) + .andThen(orchestrator.spinUpShooterDistance(() -> Meters.of(2.869)))) .withTimeout(14.5), Commands.deadline( orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), @@ -227,7 +251,7 @@ private Command pp2CycleRegression(String path1, String path2, Supplier /** Depot Auto */ public Command ppDepot() { - return ppCycleRegression("A-Cycle-LeftSweep", startDepot); + return Commands.sequence(EightBalls().withTimeout(8.2), ppCycleRegressionDepo("Depot+Outpost")); } ; @@ -285,6 +309,10 @@ public Command ppACycleLeftRegression() { return ppCycleRegression("A-Cycle-LeftSweep", startAside); } + public Command ppDepotOutpost() { + return ppCycleRegressionCustom("Depot+Outpost", center, 4.285); + } + /** * Gets relevant poses for the autos * @@ -452,7 +480,11 @@ public Command BCCManualAutoOverBump() { public Command EightBalls() { return Commands.sequence( Commands.deadline( - orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()), - Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp())); + orchestrator.driveToHub().withTimeout(3.0), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), + Commands.parallel( + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(1).andThen(orchestrator.feedUp())) + .withTimeout(5.2)); } } diff --git a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java index 62d1f7a2a..6a23da9a6 100644 --- a/src/main/java/frc/robot/commands/drive/DriveWithDpad.java +++ b/src/main/java/frc/robot/commands/drive/DriveWithDpad.java @@ -7,16 +7,19 @@ import java.util.function.Supplier; public class DriveWithDpad extends Command { + public static final double SLOW_SPEED = Units.inchesToMeters(20); + private final Drive drive; private final Supplier povSupplier; - public static final double SLOW_SPEED = Units.inchesToMeters(20); + /** Creates a slow cardinal-direction drive command controlled by the driver's D-pad angle. */ public DriveWithDpad(Drive drive, Supplier povSupplier) { this.drive = drive; this.povSupplier = povSupplier; addRequirements(drive); } + /** Converts the current POV angle into a low-speed chassis movement. */ @Override public void execute() { int pov = povSupplier.get(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4d405d38d..09d8613f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { - logCameraPositions(); // uncomment to show camera positions in advantage scope + // logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index a469e5c73..219a39371 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -61,7 +61,7 @@ public class DriveConstants { public static final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(2); + public static final double wheelRadiusMeters = Units.inchesToMeters(1.98); public static final double driveMotorReduction = 8.16; public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4d41fba74..51254c99e 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -11,30 +11,47 @@ public class Intake { private final IntakeRollers rollers; private final IntakeExtend extend; + /** Creates the intake facade from the roller and extension subsystems. */ public Intake(IntakeRollers rollers, IntakeExtend extend) { this.rollers = rollers; this.extend = extend; } + /** Extends to the requested angle while running the rollers inward. */ public Command extendToAngleAndIntake(double angle) { return Commands.parallel(extend.extendToAngle(angle), runIntakeMotor()) .withName("extendToAngleAndIntake"); } + /** Moves the intake extension to the requested angle. */ public Command extendToAngle(double angle) { return extend.extendToAngle(angle); } + /** Runs the intake rollers inward. */ public Command runIntakeMotor() { return rollers.intake(); } + /** Collapses the intake slowly while keeping the rollers running. */ public Command slowlyBringInIntake() { - return Commands.parallel(rollers.intake(), extend.runIntakeExtendVolts(SLOW_VOLTS)) + return Commands.parallel( + rollers.intake(), + extend + .runIntakeExtendVolts(SLOW_VOLTS) + .until(extend::isHopperCollapsed) + .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly())); + } + + /** Collapses the intake slowly without moving the rollers. */ + public Command slowlyBringInIntakeWithoutRollers() { + return extend + .runIntakeExtendVolts(SLOW_VOLTS) + .until(extend::isHopperCollapsed) .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly()); } - // Jack's Chugga Chugga mode + /** Alternates the extension around the funnel position while intaking game pieces. */ public Command shakeAndIntake() { return Commands.repeatingSequence( Commands.deadline( @@ -44,6 +61,7 @@ public Command shakeAndIntake() { .withName("shakeAndIntake"); } + /** Alternates the extension around the funnel position without running the rollers. */ public Command shake() { return Commands.repeatingSequence( extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 263ce3b4d..22d24bb90 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -29,7 +29,7 @@ public class IntakeConstants { public static final double SHAKE_POS_OFFSET = 1; public static final double HOME_VOLTAGE = 3; public static final double COLLAPSE_POS = 4.0; - public static final double SLOW_VOLTS = 2; + public static final double SLOW_VOLTS = 2.1; // How close we need to be to collapse position to stop the intake rollers public static final double SAFETY_TOLERANCE = 2; public static final double POSITION_TOLERANCE = 7; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index bbf147252..5b9880701 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import frc.robot.RobotState.IntakePosition; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -26,17 +25,10 @@ public class IntakeExtend extends SubsystemBase { private boolean stalledExtend = false; private boolean stalledCollapse = false; private boolean isStowed = false; - private boolean isExtended = false; private boolean hasPose = false; - private double targetExtendPosition = 0; - - /** - * @param limitSwitchDisabled A supplier to return whether the limit switch is currently disabled - * or not. If disabled, uses slipping to control intake. - */ - private final BooleanSupplier limitSwitchDisabled; - + /** Updates logged inputs and keeps robot-wide intake state in sync with the extension encoder. */ + @Override public void periodic() { inputs.stalledExtended = stalledExtend; inputs.stalledCollapsed = stalledCollapse; @@ -49,10 +41,10 @@ public void periodic() { Logger.recordOutput("Intake/IntakeExtended/StalledCollapse", stalledCollapse); Logger.recordOutput("Intake/stallingExtend", isStallingExtend()); Logger.recordOutput("Intake/stallingCollapse", isStallingCollapse()); - if (inputs.getExtendPos > EXTEND_POS / 2) { + if (inputs.extendPosition > EXTEND_POS / 2) { RobotState.getInstance().intakePosition = IntakePosition.STOWED; } - if (inputs.getExtendPos <= EXTEND_POS / 2) { + if (inputs.extendPosition <= EXTEND_POS / 2) { RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED; } if (inputs.isCollapsed) { @@ -66,64 +58,71 @@ public void periodic() { Logger.recordOutput("Intake/IntakeExtend/HasPose", hasPose); } - public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) { + /** Creates an intake extension subsystem using the selected hardware abstraction. */ + public IntakeExtend(IntakeExtendIO io) { this.io = io; - this.limitSwitchDisabled = limitSwitchDisabled; } + /** Returns whether the extension is currently on the collapsed limit switch. */ public boolean isHopperCollapsed() { return io.isCollapsed(); } + /** Supplies the current extension position for commands that need a live sensor reading. */ public DoubleSupplier getAngle() { - return () -> inputs.getExtendPos; + return () -> inputs.extendPosition; } + /** Sets the motor idle behavior through the IO layer. */ public void setIdleMode(boolean idleMode) { io.setIdleMode(idleMode); } + /** Resets the extension encoder to a known pose. */ public Command setPose(double pose) { return Commands.runOnce(() -> io.resetExtendEncoder(pose), this); } + /** Returns true when the extension is being driven outward but the encoder is barely moving. */ private boolean isStallingExtend() { - // For our volts, we are not getting the right velocity return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < -0.01; } + /** Returns true when the extension is being collapsed but the encoder is barely moving. */ private boolean isStallingCollapse() { return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts > 0.01; } - private void setPercentExtend(double extendPercent) { - io.setPercentExtend(extendPercent); - } - + /** Drives the extension motor with open-loop voltage. */ public void setVoltageExtend(double extendVolts) { io.setVoltageExtend(extendVolts); } + /** Stops the extension motor immediately. */ public void stopExtend() { io.setVoltageExtend(0); } + /** Creates a one-shot command that zeros the extension encoder. */ public Command resetExtendPosition() { return Commands.runOnce(() -> io.resetExtendEncoder(0), this); } + /** Moves the extension to the configured deployed position and finishes at tolerance. */ public Command moveToExtendedPosition() { return Commands.run(() -> io.extendToPosition(EXTEND_POS)) - .until(() -> Math.abs(inputs.getExtendPos - EXTEND_POS) <= POSITION_TOLERANCE) + .until(() -> Math.abs(inputs.extendPosition - EXTEND_POS) <= POSITION_TOLERANCE) .withName("moveToExtendedPosition"); } + /** Moves the extension to the configured collapsed position and finishes at tolerance. */ public Command moveToCollapsedPosition() { return Commands.run(() -> io.extendToPosition(COLLAPSE_POS)) - .until(() -> Math.abs(inputs.getExtendPos - COLLAPSE_POS) <= POSITION_TOLERANCE) + .until(() -> Math.abs(inputs.extendPosition - COLLAPSE_POS) <= POSITION_TOLERANCE) .withName("moveToCollapsedPosition"); } + /** Creates a manual voltage command for operator-controlled extension movement. */ public Command runIntakeExtendVolts(double volts) { return Commands.run( () -> { @@ -133,10 +132,12 @@ public Command runIntakeExtendVolts(double volts) { this); } + /** Creates a command that keeps the extension stopped while scheduled. */ public Command stopExtendingCommand() { return Commands.run(this::stopExtend, this); } + /** Homes the extension by driving until the collapsed limit switch is reached. */ public Command homeExtend() { return Commands.run( () -> { @@ -150,6 +151,7 @@ public Command homeExtend() { .withName("homeExtend"); } + /** Homes first if needed, then moves the extension to the requested encoder angle. */ public Command extendToAngle(double angle) { return new ConditionalCommand(homeExtend(), Commands.none(), () -> !hasPose) .andThen( diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java index 6a8695012..5f2697d3f 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java @@ -11,11 +11,10 @@ class IntakeExtendIOInputs { public double extendVelocity = 0.0; public double extendAmps = 0.0; public boolean isCollapsed = false; - // public boolean manualModeInput; public boolean atSetpoint = false; public boolean hasSetpoint = false; public double setpointValue = 0.0; - public double getExtendPos = 0.0; + public double extendPosition = 0.0; public boolean stalledExtended = false; public boolean stalledCollapsed = false; public double stallExtendTimer = 0.0; @@ -24,30 +23,40 @@ class IntakeExtendIOInputs { public boolean hasPose = false; } + /** Refreshes telemetry from the active extension hardware. */ default void updateInputs(IntakeExtendIOInputs inputs) {} + /** Drives the extension by percent output. */ default void setPercentExtend(double extendPercent) {} + /** Drives the extension by voltage. */ default void setVoltageExtend(double extendVolts) {} + /** Stops the extension if the hardware implementation has a dedicated stop path. */ default void stop() {} + /** Requests a closed-loop extension position. */ default void goToPos(double pos) {} + /** Enables or disables closed-loop extension control. */ default void setPIDEnabled(boolean enabled) {} - default void setIdleMode(boolean cost) {} - ; + /** Sets the extension motor idle behavior. */ + default void setIdleMode(boolean coast) {} + /** Returns whether closed-loop extension control is currently enabled. */ default boolean getPIDEnabled() { return false; } + /** Resets the extension encoder to the provided position. */ default void resetExtendEncoder(double position) {} + /** Returns whether the extension is on the collapsed limit switch. */ default boolean isCollapsed() { return false; } + /** Requests a closed-loop extension position without changing PID enabled state. */ default void extendToPosition(double position) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java index 814cfc72f..59b608544 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java @@ -29,9 +29,9 @@ public class IntakeExtendIOKraken implements IntakeExtendIO { private final DigitalInput collapsedLimitSwitch; private final RelativeEncoder extendMotorEncoder; private final SparkClosedLoopController pidController; - private double setPos = 0; private boolean usingPID = false; + /** Configures the extension motor, encoder, PID, and limit switch for the Kraken variant. */ public IntakeExtendIOKraken() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); @@ -70,7 +70,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendVolts = extendMotor.getAppliedOutput(); inputs.extendAmps = extendMotor.getOutputCurrent(); inputs.isCollapsed = !collapsedLimitSwitch.get(); - inputs.getExtendPos = extendMotorEncoder.getPosition(); + inputs.extendPosition = extendMotorEncoder.getPosition(); inputs.hasSetpoint = usingPID; inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint(); inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint(); @@ -92,10 +92,9 @@ public void setVoltageExtend(double extendVolts) { } @Override - public void goToPos(double setPos) { - this.setPos = setPos; - if (usingPID && !((isCollapsed() && (setPos > 0)))) { - pidController.setSetpoint(setPos, ControlType.kPosition); + public void goToPos(double targetPosition) { + if (usingPID && !(isCollapsed() && targetPosition > 0)) { + pidController.setSetpoint(targetPosition, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java index c21bc77d9..9fb6f075a 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -10,6 +10,7 @@ import static frc.robot.subsystems.intake.IntakeConstants.PID_P; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -28,9 +29,9 @@ public class IntakeExtendIOSparkMax implements IntakeExtendIO { private final DigitalInput collapsedLimitSwitch; private final RelativeEncoder extendMotorEncoder; private final SparkClosedLoopController pidController; - private double setPos = 0; private boolean usingPID = false; + /** Configures the Spark Max-backed extension motor, encoder, PID, and limit switch. */ public IntakeExtendIOSparkMax() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); @@ -42,6 +43,7 @@ public IntakeExtendIOSparkMax() { collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID); } + /** Builds the Spark Max configuration shared by startup and idle-mode changes. */ public SparkMaxConfig getSparkMaxConfig() { EncoderConfig extendEnc = new EncoderConfig(); extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION); @@ -54,7 +56,8 @@ public SparkMaxConfig getSparkMaxConfig() { .smartCurrentLimit((int) Math.round(INTAKE_EXTEND_MOTOR_CURRENT_LIMIT)) .closedLoop .pid(PID_P, PID_I, PID_D) - .feedbackSensor(FeedbackSensor.kPrimaryEncoder); + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .allowedClosedLoopError(0.5, ClosedLoopSlot.kSlot0); extendConfig.encoder.apply(extendEnc); return extendConfig; } @@ -66,7 +69,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendVolts = extendMotor.getAppliedOutput(); inputs.extendAmps = extendMotor.getOutputCurrent(); inputs.isCollapsed = isCollapsed(); - inputs.getExtendPos = extendMotorEncoder.getPosition(); + inputs.extendPosition = extendMotorEncoder.getPosition(); inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID; inputs.hasSetpoint = usingPID; inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint(); @@ -93,10 +96,9 @@ public void setVoltageExtend(double extendVolts) { } @Override - public void goToPos(double setPos) { - this.setPos = setPos; + public void goToPos(double targetPosition) { if (usingPID) { - pidController.setSetpoint(setPos, ControlType.kPosition); + pidController.setSetpoint(targetPosition, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java index 0cec68ba6..d5942dcb6 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java @@ -17,36 +17,39 @@ public IntakeRollers(IntakeRollersIO io) { this.io = io; } + /** Updates roller telemetry and exposes the active intake state to robot-wide logic. */ + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake/IntakeRollers", inputs); RobotState.getInstance().intaking = inputs.intakeVolts > 0; } - private void setPercentIntake(double intakePercent) { - io.setPercentIntake(intakePercent); - } - + /** Drives the intake rollers with open-loop voltage. */ public void setVoltageIntake(double intakeVolts) { io.setVoltageIntake(intakeVolts); } + /** Stops the intake rollers immediately. */ private void stopIntake() { io.setVoltageIntake(0); } + /** Runs the rollers inward until the command ends. */ public Command intake() { return Commands.run(() -> setVoltageIntake(INTAKE_VOLTS), this) .finallyDo(this::stopIntake) .withName("intake"); } + /** Runs the rollers outward until the command ends. */ public Command outtake() { return Commands.run(() -> setVoltageIntake(OUTTAKE_VOLTS), this) .finallyDo(this::stopIntake) .withName("outtake"); } + /** Creates a command that keeps the rollers stopped while scheduled. */ public Command stopIntakeCommand() { return Commands.run(this::stopIntake).withName("stopIntakeCommand"); } diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java index 305898aa5..00c4670cd 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java @@ -9,14 +9,17 @@ class IntakeRollersIOInputs { public double intakeVolts = 0.0; public double intakeAmps = 0.0; public double intakeRPM = 0.0; - // public boolean manualModeInput; } + /** Refreshes telemetry from the active roller hardware. */ default void updateInputs(IntakeRollersIOInputs inputs) {} + /** Drives the rollers by percent output. */ default void setPercentIntake(double intakePercent) {} + /** Drives the rollers by voltage. */ default void setVoltageIntake(double intakeVolts) {} + /** Stops the rollers if the hardware implementation has a dedicated stop path. */ default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java index cbe116e01..50d83e8b9 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java @@ -16,11 +16,11 @@ public class IntakeRollersIOKraken implements IntakeRollersIO { private final TalonFX intakeMotor; - private double setPos = 0; private final StatusSignal intakeAppliedVolts; private final StatusSignal intakeCurrent; + /** Configures the TalonFX-backed intake roller motor and cached status signals. */ public IntakeRollersIOKraken() { intakeMotor = new TalonFX(intakeMotorCanId); diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java index 39b4ded2e..ea75ede72 100644 --- a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java @@ -13,9 +13,8 @@ public class IntakeRollersIOSparkMax implements IntakeRollersIO { private final SparkMax intakeMotor; - private double setPos = 0; - private boolean usingPID = false; + /** Configures the Spark Max-backed intake roller motor. */ public IntakeRollersIOSparkMax() { intakeMotor = new SparkMax(intakeMotorCanId, MotorType.kBrushless); @@ -49,8 +48,4 @@ public void setPercentIntake(double intakePercent) { public void setVoltageIntake(double intakeVolts) { intakeMotor.setVoltage(intakeVolts); } - - public void setPIDEnabled(boolean enabled) { - this.usingPID = enabled; - } } diff --git a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java index 21b4f6f7b..77e51b9d1 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedPatterns.java +++ b/src/main/java/frc/robot/subsystems/leds/LedPatterns.java @@ -65,43 +65,59 @@ public enum LedPatterns { private final LEDPattern colorPattern; + /** Returns the base LED pattern without adding animation. */ public LEDPattern colorPatternOnly() { return colorPattern; } + /** Blinks the pattern at the default strobe timing. */ public LEDPattern blink() { return colorPattern.blink(Seconds.of(STROBE_TIME)); } + /** + * Blinks the pattern at the requested timing, falling back to default timing for invalid input. + */ public LEDPattern blink(double time) { return colorPattern.blink(Seconds.of(time > 0.0 ? time : STROBE_TIME)); } + /** Breathes the pattern at the default timing. */ public LEDPattern breathe() { return colorPattern.breathe(Seconds.of(BREATH_TIME)); } + /** + * Breathes the pattern at the requested timing, falling back to default timing for invalid input. + */ public LEDPattern breathe(double time) { return colorPattern.breathe(Seconds.of(time > 0.0 ? time : BREATH_TIME)); } + /** Scrolls the pattern at the default speed. */ public LEDPattern scroll() { return colorPattern.scrollAtAbsoluteSpeed(MetersPerSecond.of(1), LED_SPACING); } + /** + * Scrolls the pattern at the requested speed, falling back to default speed for invalid input. + */ public LEDPattern scroll(double metersPerSecond) { return colorPattern.scrollAtAbsoluteSpeed( MetersPerSecond.of(metersPerSecond > 0.0 ? metersPerSecond : 1), LED_SPACING); } + /** Overlays this pattern on top of another pattern. */ public LEDPattern overlayOn(LEDPattern base) { return colorPattern.overlayOn(base); } + /** Blends this pattern with another pattern. */ public LEDPattern blend(LEDPattern other) { return colorPattern.blend(other); } + /** Stores the underlying WPILib LED pattern for this enum value. */ private LedPatterns(LEDPattern colorPattern) { this.colorPattern = colorPattern; } diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java index c4f7d94e1..3899393d8 100644 --- a/src/main/java/frc/robot/subsystems/leds/Leds.java +++ b/src/main/java/frc/robot/subsystems/leds/Leds.java @@ -18,10 +18,10 @@ import frc.robot.RobotState.Mode; public class Leds extends SubsystemBase { - private static final boolean debug = false; - private RobotState state = RobotState.getInstance(); + private static final boolean DEBUG = false; + private final RobotState state = RobotState.getInstance(); private final AddressableLED leds; - public static final AddressableLEDBuffer buffer = + private static final AddressableLEDBuffer buffer = new AddressableLEDBuffer(LedConstants.FULL_LENGTH); public enum Animations { @@ -71,42 +71,45 @@ public enum Sections { LedConstants.BASE2_FOURTH_QUARTER_START, LedConstants.BASE2_FOURTH_QUARTER_END); - private final AddressableLEDBufferView buf_view_1; - private final AddressableLEDBufferView buf_view_2; + private final AddressableLEDBufferView primaryBufferView; + private final AddressableLEDBufferView secondaryBufferView; - private Sections(int start_1, int end_1, int start_2, int end_2) { - if (debug) { + /** Creates a section with mirrored LED ranges. */ + private Sections(int primaryStart, int primaryEnd, int secondaryStart, int secondaryEnd) { + if (DEBUG) { System.out.println( "create section " + toString() - + " start_1: " - + start_1 - + " end_1: " - + end_1 - + " start_2: " - + start_2 - + " end_2:" - + end_2); + + " primaryStart: " + + primaryStart + + " primaryEnd: " + + primaryEnd + + " secondaryStart: " + + secondaryStart + + " secondaryEnd:" + + secondaryEnd); } - this.buf_view_1 = buffer.createView(start_1, end_1); - this.buf_view_2 = buffer.createView(start_2, end_2); + primaryBufferView = buffer.createView(primaryStart, primaryEnd); + secondaryBufferView = buffer.createView(secondaryStart, secondaryEnd); } - private Sections(int start_1, int end_1) { - if (debug) { - System.out.println( - "create section " + toString() + " start_1: " + start_1 + " end_1: " + end_1); + /** Creates a section with one contiguous LED range. */ + private Sections(int start, int end) { + if (DEBUG) { + System.out.println("create section " + toString() + " start: " + start + " end: " + end); } - this.buf_view_1 = buffer.createView(start_1, end_1); - this.buf_view_2 = null; + primaryBufferView = buffer.createView(start, end); + secondaryBufferView = null; } - public AddressableLEDBufferView getBufferView_1() { - return buf_view_1; + /** Returns the first LED range for this section. */ + public AddressableLEDBufferView getPrimaryBufferView() { + return primaryBufferView; } - public AddressableLEDBufferView getBufferView_2() { - return buf_view_2; + /** Returns the mirrored LED range, or null when this section only has one range. */ + public AddressableLEDBufferView getSecondaryBufferView() { + return secondaryBufferView; } } @@ -122,10 +125,10 @@ public AddressableLEDBufferView getBufferView_2() { private LEDPattern currentPattern = LedPatterns.BLACK.colorPatternOnly(); private Sections applySection = Sections.FULL; - private Boolean isReversed = false; + private boolean isReversed = false; + /** Creates and starts the addressable LED strip and Shuffleboard controls. */ public Leds() { - leds = new AddressableLED(LedConstants.LED_CHANNEL); leds.setLength(LedConstants.FULL_LENGTH); leds.setColorOrder(ColorOrder.kRGB); @@ -135,15 +138,14 @@ public Leds() { initLedModeTabInShuffleboard(); } + /** Selects the current LED pattern from test controls or the robot state, then applies it. */ @Override public void periodic() { - if (enableTestEntry.getBoolean(false)) { processLedPatternTestInputs(); } else if (enableLedModeTestEntry.getBoolean(false)) { processLedModeTestInputs(); } else { - /* get from robot state */ Mode ledMode = state.getMode(); currentPattern = ledMode.ledPattern; applySection = ledMode.ledSection; @@ -154,11 +156,9 @@ public void periodic() { loadLedPatterns(); leds.setData(buffer); } - // else no op for simulation - } - // tests led patterns from shuffleboard gui + /** Reads Shuffleboard controls that directly test LED colors and animations. */ private void processLedPatternTestInputs() { try { LedPatterns pattern = testPattern.getSelected(); @@ -178,36 +178,37 @@ private void processLedPatternTestInputs() { case BREATHE_OVERLAY -> currentPattern = pattern.breathe().overlayOn(pattern2.breathe()); default -> currentPattern = pattern.colorPatternOnly(); } - } catch (IllegalArgumentException E) { + } catch (IllegalArgumentException exception) { currentPattern = LedPatterns.RED.blink(); } } - // process test inputs for led mode based on robot state + /** Reads Shuffleboard controls that preview a full robot LED mode. */ private void processLedModeTestInputs() { try { Mode mode = testMode.getSelected(); applySection = mode.ledSection; currentPattern = mode.ledPattern; - } catch (IllegalArgumentException E) { + } catch (IllegalArgumentException exception) { currentPattern = LedPatterns.RED.blink(); } } + /** Clears the strip and applies the selected pattern to the selected section. */ private void loadLedPatterns() { LedPatterns.BLACK.colorPatternOnly().applyTo(buffer); if (isReversed) { - currentPattern.applyTo(applySection.getBufferView_1().reversed()); + currentPattern.applyTo(applySection.getPrimaryBufferView().reversed()); } else { - currentPattern.applyTo(applySection.getBufferView_1()); + currentPattern.applyTo(applySection.getPrimaryBufferView()); } - if (applySection.getBufferView_2() != null) { - currentPattern.applyTo(applySection.getBufferView_2().reversed()); + if (applySection.getSecondaryBufferView() != null) { + currentPattern.applyTo(applySection.getSecondaryBufferView().reversed()); } } + /** Builds Shuffleboard controls for manually testing LED patterns. */ private void initLedTabInShuffleboard() { - ShuffleboardTab ledTab = Shuffleboard.getTab("LEDs"); ShuffleboardLayout ledTestingLayout = ledTab @@ -277,6 +278,7 @@ private void initLedTabInShuffleboard() { .getEntry(); } + /** Builds Shuffleboard controls for previewing robot LED modes. */ private void initLedModeTabInShuffleboard() { ShuffleboardTab ledModeTab = Shuffleboard.getTab("LED Modes"); ShuffleboardLayout ledModeTestingLayout = diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 80c857bb0..c5ae95899 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,9 @@ import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; +import static frc.robot.subsystems.shooter.ShooterConstants.ACONST; +import static frc.robot.subsystems.shooter.ShooterConstants.BCONST; +import static frc.robot.subsystems.shooter.ShooterConstants.CCONST; import static frc.robot.subsystems.shooter.ShooterConstants.FF_SCALAR; import static frc.robot.subsystems.shooter.ShooterConstants.KA; import static frc.robot.subsystems.shooter.ShooterConstants.KS; @@ -57,6 +60,9 @@ public Shooter(ShooterIO io) { SmartDashboard.putNumber("Shooter/KA", KA * FF_SCALAR); SmartDashboard.putNumber("Shooter/KV", KV * FF_SCALAR); SmartDashboard.putNumber("Shooter/FeedForward_Scalar", FF_SCALAR); + SmartDashboard.putNumber("Shooter/AConst", ACONST); + SmartDashboard.putNumber("Shooter/BCONST", BCONST); + SmartDashboard.putNumber("Shooter/CCONST", CCONST); feedForwardScalar = FF_SCALAR; this.io = io; sysId = @@ -245,7 +251,8 @@ public Command setTargetVelocityRepeatedly(AngularVelocity velocity) { */ public boolean isAtSetpoint() { return Math.abs(inputs.shooterWheelVelocityRadPerSec - (targetSpeed.in(RadiansPerSecond))) - < TOLERANCE; + < TOLERANCE + && targetSpeed.gt(RadiansPerSecond.of(0)); } // TODO: empirically determine the relationship between distance and air time @@ -265,9 +272,11 @@ public Supplier calculateSetpoint(Supplier distance) return () -> { AngularVelocity velocity = RPM.of( - 25.30184 * Math.pow(distance.get().in(Meters), 2) - - 47.12642 * distance.get().in(Meters) - + 1001.70713); + SmartDashboard.getNumber("Shooter/ACONST", 25.30184) + * Math.pow(distance.get().in(Meters), 2) + + SmartDashboard.getNumber("Shooter/BCONST", -65.12642) + * distance.get().in(Meters) + + SmartDashboard.getNumber("Shooter/CCONST", 1001.70713)); if (velocity.gt(RPM.of(1550))) { return RPM.of(1550); } else return velocity; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index f784e5bea..da40c170b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,6 +9,9 @@ public class ShooterConstants { public static final double ENCODER_POSITION_CONVERSION = 1; public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60; + public static final double ACONST = 25.30184; + public static final double BCONST = -65.12642; + public static final double CCONST = 1001.70713; public static final double VOLTAGE_COMPENSATION = 12.0; public static final int CURRENT_LIMIT = 35; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 463e6ce91..1454e21dc 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -33,7 +33,7 @@ public class VisionConstants { new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), - Units.degreesToRadians(-158.771))); + Units.degreesToRadians(201.229))); public static Transform3d robotToCamera1 = new Transform3d( new Translation3d(