diff --git a/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto new file mode 100644 index 000000000..ce72f9f46 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A-Cycle1" + } + }, + { + "type": "path", + "data": { + "pathName": "A-Cycle2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto new file mode 100644 index 000000000..6961e215d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B-Cycle1" + } + }, + { + "type": "path", + "data": { + "pathName": "B-Cycle2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 9d22515a8..6c49fd076 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -3,73 +3,73 @@ "waypoints": [ { "anchor": { - "x": 4.295, - "y": 7.531 + "x": 4.424579606440071, + "y": 7.369320214669052 }, "prevControl": null, "nextControl": { - "x": 5.220865926213596, - "y": 7.587085598717599 + "x": 5.438196256563948, + "y": 7.412915747707522 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.588533094812165, - "y": 6.832881932021468 + "x": 7.783237924865832, + "y": 7.028586762075134 }, "prevControl": { - "x": 7.335202354903501, - "y": 7.21155347245828 + "x": 7.767012522361359, + "y": 7.531574239713775 }, "nextControl": { - "x": 7.834164231584512, - "y": 6.465719538012889 + "x": 7.817452345049281, + "y": 5.967939736388225 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.7021109123434695, - "y": 4.074563506261182 + "x": 7.783237924865832, + "y": 4.805706618962433 }, "prevControl": { - "x": 8.22271615942871, - "y": 4.362336125245133 + "x": 7.77903133903134, + "y": 5.171980056980058 }, "nextControl": { - "x": 7.200064179283715, - "y": 3.7970493935359855 + "x": 7.786250707881794, + "y": 4.543379297786823 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.965992844364937, - "y": 5.664652951699464 + "x": 6.797094017094016, + "y": 5.611267806267806 }, "prevControl": { - "x": 6.215916040758477, - "y": 5.658456508813674 + "x": 7.802016105528991, + "y": 5.5605141654377555 }, "nextControl": { - "x": 4.002719141323792, - "y": 5.713329159212882 + "x": 5.517991452991453, + "y": 5.675868945868947 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.256, - "y": 5.665 + "x": 2.533418803418803, + "y": 5.611267806267806 }, "prevControl": { - "x": 4.375552524919555, - "y": 5.729905885962339 + "x": 3.334472934472934, + "y": 5.675868945868947 }, "nextControl": null, "isLocked": false, @@ -83,41 +83,32 @@ }, { "waypointRelativePos": 1.171122994652407, - "rotationDegrees": -93.685 + "rotationDegrees": -90.0 }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": -136.0 + "rotationDegrees": -90.0 }, { - "waypointRelativePos": 3.0160427807486627, + "waypointRelativePos": 3.0, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 3.8502673796791456, + "waypointRelativePos": 3.45, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.7526652452025653, "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.1254237288135598, + "minWaypointRelativePos": 1.0508474576271185, "maxWaypointRelativePos": 2.1694915254237293, "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 6, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 3.118644067796608, - "maxWaypointRelativePos": 3.5254237288135593, - "constraints": { - "maxVelocity": 1.2, + "maxVelocity": 1.7, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, @@ -127,35 +118,22 @@ }, { "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0, + "minWaypointRelativePos": 3.14382174206618, + "maxWaypointRelativePos": 3.792032410533432, "constraints": { - "maxVelocity": 4, - "maxAcceleration": 6, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12, + "maxVelocity": 2.3, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, "unlimited": false } } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 0.5288135593220339, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "preloadBalls", - "waypointRelativePos": 2.535593220338976, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 8.0, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -164,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -51.09865544424842 + "rotation": -37.18470645323313 }, "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 976a35738..811891d20 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": 3.256350626118068, - "y": 5.664652951699464 + "x": 2.533418803418803, + "y": 5.611267806267806 }, "prevControl": null, "nextControl": { - "x": 3.6674363972882924, - "y": 5.6284350881968495 + "x": 2.267699809926946, + "y": 6.160705710211645 }, "isLocked": false, "linkedName": "A-Cycle1-End" }, { "anchor": { - "x": 3.256350626118068, - "y": 7.254742397137747 + "x": 2.5075783475783475, + "y": 7.447760964912282 }, "prevControl": { - "x": 2.1979715994142452, - "y": 6.347572517197855 + "x": 2.0694924799575967, + "y": 7.351408549885447 }, "nextControl": { - "x": 3.5736065913462336, - "y": 7.526672441691426 + "x": 2.7903904157463497, + "y": 7.509962528077773 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.4525328947368426, - "y": 7.254742397137747 + "x": 5.634273504273503, + "y": 7.447760964912282 }, "prevControl": { - "x": 5.8071316550071765, - "y": 7.778212679002352 + "x": 4.144916933849193, + "y": 7.530332248967907 }, "nextControl": { - "x": 7.57757028119644, - "y": 6.342250134115599 + "x": 6.494219837010534, + "y": 7.400084757398864 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.327521929824561, - "y": 4.409032894736844 + "x": 6.202763532763532, + "y": 4.060840455840457 }, "prevControl": { - "x": 6.863907746462161, - "y": 5.216361806741775 + "x": 6.215683760683759, + "y": 3.569871794871795 }, "nextControl": { - "x": 5.937085933334687, - "y": 3.8213770349498226 + "x": 6.1825608953181, + "y": 4.828540678766911 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 4.553276315789475 + "x": 5.866837606837606, + "y": 5.611 }, "prevControl": { - "x": 5.799882977639703, - "y": 4.304117085888362 + "x": 6.590370370370371, + "y": 5.624188034188035 }, "nextControl": { - "x": 5.6990813534677445, - "y": 5.5301053915928176 + "x": 5.351084085527113, + "y": 5.601599216214575 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 7.447760964912282 + "x": 4.755698005698005, + "y": 5.611 }, "prevControl": { - "x": 6.523025544615784, - "y": 6.993212709981212 + "x": 5.905598290598291, + "y": 5.598079772079772 }, "nextControl": { - "x": 5.527831315774364, - "y": 7.601532227331069 + "x": 4.148936797920834, + "y": 5.6178175416604175 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.1445504385964913, - "y": 7.447760964912282 + "x": 3.760840455840455, + "y": 5.611 }, "prevControl": { - "x": 3.619467665950238, - "y": 7.613424526324748 + "x": 4.6006552706552695, + "y": 5.598347578347578 }, "nextControl": { - "x": 2.9084995834559355, - "y": 7.3654202472362 + "x": 3.34446165048371, + "y": 5.61727304986715 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.548344298245614, - "y": 6.284197368421054 + "x": 2.533418803418803, + "y": 5.611267806267806 }, "prevControl": { - "x": 1.9265215510296607, - "y": 7.001825662162521 + "x": 3.476099715099715, + "y": 5.624455840455841 }, "nextControl": null, "isLocked": false, @@ -139,26 +139,26 @@ }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": -150.0 + "rotationDegrees": -90.0 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 80.0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": 90.0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 5.734276729559759, - "rotationDegrees": 90.0 + "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 2.526032315978456, - "maxWaypointRelativePos": 4.235188509874326, + "minWaypointRelativePos": 2.0644067796610166, + "maxWaypointRelativePos": 3.22711864406781, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 6, @@ -167,12 +167,38 @@ "nominalVoltage": 12, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.529372045914936, + "maxWaypointRelativePos": 1.380148548278184, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.442943956785955, + "maxWaypointRelativePos": 6.4280891289669135, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 8.0, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -181,13 +207,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -51.09865544424842 + "rotation": -37.195226690262515 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -51.09865544424842 + "rotation": -37.18470645323313 }, "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 14317bd18..632dc384e 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -3,77 +3,77 @@ "waypoints": [ { "anchor": { - "x": 4.295, - "y": 0.538 + "x": 4.424579606440071, + "y": 0.6996797853309484 }, "prevControl": null, "nextControl": { - "x": 5.220865926213596, - "y": 0.4819144012824006 + "x": 5.438196256563948, + "y": 0.6560842522924792 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.588533094812165, - "y": 1.236118067978532 + "x": 7.783237924865832, + "y": 1.0404132379248665 }, "prevControl": { - "x": 7.335202354903501, - "y": 0.8574465275417207 + "x": 7.767012522361359, + "y": 0.5374257602862258 }, "nextControl": { - "x": 7.834164231584512, - "y": 1.603280461987111 + "x": 7.817452345049281, + "y": 2.1010602636117754 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.7021109123434695, - "y": 3.994436493738819 + "x": 7.783237924865832, + "y": 3.2632933810375677 }, "prevControl": { - "x": 8.22271615942871, - "y": 3.7066638747548675 + "x": 7.77903133903134, + "y": 2.8970199430199433 }, "nextControl": { - "x": 7.200064179283715, - "y": 4.271950606464015 + "x": 7.786250707881794, + "y": 3.5256207022131782 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.965992844364937, - "y": 2.4043470483005365 + "x": 6.797094017094016, + "y": 2.4577321937321948 }, "prevControl": { - "x": 6.215916040758477, - "y": 2.410543491186327 + "x": 7.802016105528991, + "y": 2.5084858345622454 }, "nextControl": { - "x": 4.002719141323792, - "y": 2.355670840787119 + "x": 5.517991452991453, + "y": 2.3931310541310538 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.175, - "y": 2.404 + "x": 2.533418803418803, + "y": 2.4577321937321948 }, "prevControl": { - "x": 4.294552524919553, - "y": 2.3390941140376613 + "x": 3.334472934472934, + "y": 2.3931310541310538 }, "nextControl": null, "isLocked": false, - "linkedName": "B-Cycle1-End" + "linkedName": "A-Cycle1-End" } ], "rotationTargets": [ @@ -83,88 +83,66 @@ }, { "waypointRelativePos": 1.171122994652407, - "rotationDegrees": 86.315500893566 + "rotationDegrees": 90.0 }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": 115.5063543100694 + "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.0160427807486627, + "waypointRelativePos": 3, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 3.8502673796791456, + "waypointRelativePos": 3.45, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.7526652452025653, "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.1254237288135598, + "minWaypointRelativePos": 1.0508474576271185, "maxWaypointRelativePos": 2.1694915254237293, "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 3.118644067796608, - "maxWaypointRelativePos": 3.5254237288135593, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 1.7, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } }, { "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0, + "minWaypointRelativePos": 3.14382174206618, + "maxWaypointRelativePos": 3.792032410533432, "constraints": { - "maxVelocity": 4.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 2.3, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 0.5288135593220339, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "preloadBalls", - "waypointRelativePos": 2.535593220338976, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.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": 51.95295746817396 + "rotation": -37.18470645323313 }, "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 5ddf30eec..a28ef683d 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,121 +3,121 @@ "waypoints": [ { "anchor": { - "x": 3.1752236135957066, - "y": 2.4043470483005365 + "x": 2.533418803418803, + "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 3.586309384765931, - "y": 2.4405649118031514 + "x": 2.267699809926946, + "y": 1.9082942897883557 }, "isLocked": false, - "linkedName": "B-Cycle1-End" + "linkedName": "A-Cycle1-End" }, { "anchor": { - "x": 3.256350626118068, - "y": 0.8142576028622535 + "x": 2.5075783475783475, + "y": 0.6212390350877186 }, "prevControl": { - "x": 2.1979715994142452, - "y": 1.721427482802146 + "x": 2.0694924799575967, + "y": 0.717591450114554 }, "nextControl": { - "x": 3.5736065913462336, - "y": 0.5423275583085745 + "x": 2.7903904157463497, + "y": 0.5590374719222275 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.4525328947368426, - "y": 0.8142576028622535 + "x": 5.634273504273503, + "y": 0.6212390350877186 }, "prevControl": { - "x": 5.8071316550071765, - "y": 0.2907873209976489 + "x": 4.144916933849193, + "y": 0.5386677510320939 }, "nextControl": { - "x": 7.57757028119644, - "y": 1.7267498658844018 + "x": 6.494219837010534, + "y": 0.6689152426011367 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.327521929824561, - "y": 3.6599671052631573 + "x": 6.202763532763532, + "y": 4.008159544159544 }, "prevControl": { - "x": 6.863907746462161, - "y": 2.852638193258226 + "x": 6.215683760683759, + "y": 4.499128205128206 }, "nextControl": { - "x": 5.937085933334687, - "y": 4.247622965050178 + "x": 6.1825608953181, + "y": 3.2404593212330894 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 3.515723684210526 + "x": 5.866837606837606, + "y": 2.458000000000001 }, "prevControl": { - "x": 5.799882977639703, - "y": 3.764882914111639 + "x": 6.590370370370371, + "y": 2.444811965811966 }, "nextControl": { - "x": 5.6990813534677445, - "y": 2.538894608407183 + "x": 5.351084085527113, + "y": 2.4674007837854255 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 0.6212390350877184 + "x": 4.755698005698005, + "y": 2.458000000000001 }, "prevControl": { - "x": 6.523025544615784, - "y": 1.0757872900187888 + "x": 5.905598290598291, + "y": 2.470920227920229 }, "nextControl": { - "x": 5.527831315774364, - "y": 0.46746777266893197 + "x": 4.148936797920834, + "y": 2.4511824583395834 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.1445504385964913, - "y": 0.6212390350877184 + "x": 3.760840455840455, + "y": 2.458000000000001 }, "prevControl": { - "x": 3.619467665950238, - "y": 0.4555754736752522 + "x": 4.6006552706552695, + "y": 2.4706524216524226 }, "nextControl": { - "x": 2.9084995834559355, - "y": 0.7035797527638007 + "x": 3.34446165048371, + "y": 2.4517269501328505 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.548344298245614, - "y": 1.784802631578947 + "x": 2.533418803418803, + "y": 2.4577321937321948 }, "prevControl": { - "x": 1.9265215510296607, - "y": 1.0671743378374803 + "x": 3.476099715099715, + "y": 2.4445441595441597 }, "nextControl": null, "isLocked": false, @@ -139,32 +139,58 @@ }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": 119.99999999999999 + "rotationDegrees": 90.0 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": -100.0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": -90.0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 5.734276729559759, - "rotationDegrees": -90.0 + "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 2.526032315978456, - "maxWaypointRelativePos": 4.235188509874326, + "minWaypointRelativePos": 2.0644067796610166, + "maxWaypointRelativePos": 3.22711864406781, "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 + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.529372045914936, + "maxWaypointRelativePos": 1.380148548278184, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.442943956785955, + "maxWaypointRelativePos": 6.4280891289669135, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } } @@ -172,22 +198,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.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": 55.84030545433046 + "rotation": 52.804773309737485 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 51.95295746817396 + "rotation": -37.18470645323313 }, "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 new file mode 100644 index 000000000..90ed3dfca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot.path @@ -0,0 +1,113 @@ +{ + "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/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index d5a531bdd..2e5cfd4e0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,10 +1,10 @@ { - "robotWidth": 0.702, - "robotLength": 0.702, + "robotWidth": 0.9017, + "robotLength": 0.8128, "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 4.0, + "defaultMaxVel": 8.0, "defaultMaxAccel": 6.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, @@ -19,13 +19,13 @@ "driveCurrentLimit": 46.0, "wheelCOF": 1.2, "flModuleX": 0.273, - "flModuleY": 0.244, + "flModuleY": 0.299, "frModuleX": 0.273, - "frModuleY": -0.244, + "frModuleY": -0.299, "blModuleX": -0.273, - "blModuleY": 0.244, + "blModuleY": 0.299, "brModuleX": -0.273, - "brModuleY": -0.244, + "brModuleY": -0.299, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 5c9ed0a49..37135b86b 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -10,6 +10,7 @@ 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; @@ -26,9 +27,11 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants; 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; @@ -51,7 +54,7 @@ public enum ZoneId { ZONE_4 } - private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0); + private final double FRONT_HUB_OFFSET = Units.inchesToMeters(58.0); private final Drive drive; private final Shooter shooter; private final MagicCarpet magicCarpet; @@ -139,6 +142,47 @@ public ZoneId getCurrentZone() { return ZoneId.NONE; } + public Command zoneBasedShooter() { + DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY()); + + return new SelectCommand<>( + Map.ofEntries( + Map.entry(ZoneId.ZONE_1, spinUpShooterDistance(getHubDistance())), + Map.entry( + ZoneId.ZONE_2, + new ConditionalCommand( + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(BBumpClosePose) + .getTranslation() + .getDistance((drive.getPose().getTranslation())))), + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpClosePose)) + .getTranslation() + .getDistance(drive.getPose().getTranslation()))), + () -> allianceY.getAsDouble() > FieldConstants.fieldWidth / 2)), + Map.entry( + ZoneId.ZONE_3, + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(BBumpFarPose) + .getTranslation() + .getDistance(drive.getPose().getTranslation())))), + Map.entry( + ZoneId.ZONE_4, + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpFarPose)) + .getTranslation() + .getDistance(drive.getPose().getTranslation()))))), + this::getCurrentZone); + } + public Command zoneBasedAim() { DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY()); @@ -186,7 +230,11 @@ public Supplier getHubDistance() { return () -> Meters.of( AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) - .getDistance(drive.getPose().getTranslation())); + .getDistance( + drive + .getPose() + .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) + .getTranslation())); } private Rotation2d filteredHubAngle(Rotation2d raw) { @@ -234,8 +282,7 @@ public Command driveToHub() { public Command feedUp() { return Commands.repeatingSequence( - Commands.parallel(magicCarpet.run(), indexer.run()) - .until(() -> !RobotState.getInstance().shooterAtSpeed)) + Commands.parallel(indexer.run()).until(() -> !RobotState.getInstance().shooterAtSpeed)) .onlyIf(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0))) .onlyIf(() -> RobotState.getInstance().shooterAtSpeed) .onlyWhile(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0))) @@ -262,6 +309,20 @@ public Command spinUpShooter(double velocityRPM) { return shooter.setTargetVelocity(Rotations.per(Minute).of(velocityRPM)); } + public Command shootWhileRetractingIntake(Command shooterCommand) { + return Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), shooterCommand, feedUp()) + .withName("shootWhileRetractingIntake"); + } + + public Command shootWhileRetractingIntakeHub() { + return shootWhileRetractingIntake(spinUpShooterHub()); + } + + public Command shootWhileRetractingIntakeDistance(Supplier targetDistance) { + return shootWhileRetractingIntake(spinUpShooterDistance(targetDistance)); + } + public Command driveShootAtAngle() { return Commands.parallel( Commands.run( @@ -292,7 +353,16 @@ public Command aimToHub() { drive.getPose().getX(), drive.getPose().getY(), AllianceFlipUtil.apply(Hub.blueCenter) - .minus(drive.getPose().getTranslation()) + .plus(new Translation2d(-0.4, 0)) + .minus( + drive + .getPose() + .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) + .getTranslation()) .getAngle())); } + + public Command stopShootingAuto() { + return Commands.parallel(indexer.stop(), shooter.stop()); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 151a6d629..a205ec02e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,11 +6,9 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.ppConfig; -import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM; import static frc.robot.subsystems.vision.VisionConstants.*; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; @@ -19,6 +17,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -29,7 +28,6 @@ import frc.lib.utils.LocalADStarAK; import frc.robot.RobotState.IntakePosition; import frc.robot.commands.auto.Autos; -import frc.robot.commands.auto.DriveToPose; import frc.robot.commands.drive.DriveCommands; import frc.robot.commands.drive.DriveWithDpad; import frc.robot.subsystems.drive.*; @@ -183,44 +181,12 @@ public RobotContainer() { new Orchestrator( drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController); Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter); - NamedCommands.registerCommand( - "startIntake", intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(10.0)); - NamedCommands.registerCommand( - "endIntake", - Commands.parallel( - intakeRollers.stopIntakeCommand().withTimeout(0.05), - indexer.stop().withTimeout(0.05)) - .withTimeout(0.05)); - NamedCommands.registerCommand( - "spinUp", - shooter.setTargetVelocityRepeatedly(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM))); - NamedCommands.registerCommand("feedShooter", orchestrator.feedUp()); - NamedCommands.registerCommand("bringInIntake", intake.extendToAngleAndIntake(0)); - NamedCommands.registerCommand("driveToHub", orchestrator.driveToHub()); - NamedCommands.registerCommand("driveToHubAuto", orchestrator.driveToHub().withTimeout(3.0)); - NamedCommands.registerCommand( - "shootAuto", - Commands.sequence( - Commands.parallel( - shooter - .setTargetVelocityRepeatedly( - Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)) - .withTimeout(0.8), - intakeRollers.stopIntakeCommand(), - indexer.stop()) - .withTimeout(0.8), - Commands.deadline( - Commands.waitSeconds(3.2), - shooter.setTargetVelocityRepeatedly( - Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)), - orchestrator.feedUp()), - Commands.parallel(indexer.stop().withTimeout(0.05)).withTimeout(0.05))); AutoBuilder.configure( drive::getPose, drive::setPose, drive::getChassisSpeeds, drive::runVelocity, - new PPHolonomicDriveController(new PIDConstants(12, 0, 0), new PIDConstants(9, 0, 0)), + new PPHolonomicDriveController(new PIDConstants(14.0, 0, 0), new PIDConstants(7, 0, 0)), ppConfig, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, drive); @@ -235,12 +201,12 @@ public RobotContainer() { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); }); - NamedCommands.registerCommand( - "Drive Back Left", - new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180)))); - NamedCommands.registerCommand( - "Drive Over Left", - new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180)))); + // NamedCommands.registerCommand( + // "Drive Back Left", + // new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180)))); + // NamedCommands.registerCommand( + // "Drive Over Left", + // new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180)))); // // NamedCommands.registerCommand("startShooter",Commands.parallel(orchestrator.preloadBalls(),orchestrator.prepShooter())); @@ -256,7 +222,8 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); autoChooser.addDefaultOption("Do Nothing", Commands.none()); - + autoChooser.addOption( + "RunIntakeOut", intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)); autoChooser.addOption("Manual A-CC", autos.ACCManualAuto()); autoChooser.addOption("Manual A-CC-Improved", autos.ACCManuelAutoAlt()); autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManualAutoOverBump()); @@ -308,7 +275,6 @@ public RobotContainer() { private void configureButtonBindings() { indexer.setDefaultCommand(indexer.stop()); shooter.setDefaultCommand(shooter.stop()); - // shooter.setDefaultCommand(shooter.stop()); drive.setDefaultCommand( DriveCommands.joystickDrive( @@ -318,7 +284,11 @@ private void configureButtonBindings() { () -> -driverController.getRightX())); // Lock to 0 degrees when A button is held - + Trigger intaking = new Trigger(() -> RobotState.getInstance().intaking); + intaking.onTrue( + Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.2))); + intaking.onFalse( + Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.0))); driverController .start() .onTrue( @@ -330,7 +300,7 @@ private void configureButtonBindings() { .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - driverController.x().toggleOnTrue(orchestrator.aimToHub()); + driverController.x().toggleOnTrue(orchestrator.zoneBasedAim()); driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); driverController .leftBumper() @@ -346,12 +316,13 @@ private void configureButtonBindings() { driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED) .and(() -> !operatorController.pov(180).getAsBoolean()) - .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)); + .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS)); - // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE + // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE + // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); + + driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); - // driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - driverController.rightTrigger(0.1).toggleOnTrue(indexer.run()); driverController .a() @@ -366,19 +337,17 @@ private void configureButtonBindings() { .and(operatorController.pov(180)) .whileTrue(intakeExtend.runIntakeExtendVolts(4)) .onFalse(intakeExtend.stopExtendingCommand()); - // operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); + operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) - .toggleOnTrue( - orchestrator.spinUpShooterDistance(orchestrator.getShootWhileDrivingResultDistance())); + .toggleOnTrue(orchestrator.zoneBasedShooter()); operatorController .rightTrigger(0.1) .and(operatorController.pov(0)) .toggleOnTrue(orchestrator.spinUpShooterHub()); - operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(4)); operatorController.y().whileTrue(indexer.reverse()); - // operatorController.x().whileTrue(intake.outtake()); + operatorController.x().whileTrue(intakeRollers.outtake()); } /** @@ -394,4 +363,12 @@ 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 c145ea0e5..06eb4f0a5 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -12,6 +12,7 @@ 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 */ @@ -98,14 +99,8 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000))); private static final Supplier startBside = () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); - private static final Supplier connectBside = - () -> - AllianceFlipUtil.apply( - new Pose2d(3.175, 2.404, new Rotation2d(Units.degreesToRadians(-118.29 + 180)))); - private static final Supplier connectAside = - () -> - AllianceFlipUtil.apply( - new Pose2d(3.256, 5.665, new Rotation2d(Units.degreesToRadians(115.83 + 180)))); + private static final Supplier startDepot = + () -> AllianceFlipUtil.apply(new Pose2d(3.630, 5.844, new Rotation2d(0.000))); /** * Helper function for a single cycle auto with a constant shooting speed @@ -130,6 +125,21 @@ private Command ppCycle(String path, Supplier startPose) { orchestrator.feedUp())); } + private Command ppCycleConnect(String path) { + return Commands.sequence( + Commands.deadline( + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + orchestrator.spinUpShooterHub()) + .withTimeout(14.5), + orchestrator.aimToHub().withTimeout(2.5), + Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), + Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooter(1214), + orchestrator.feedUp())); + } + /** * Helper function for a single cycle auto that uses shooter regression to shoot into the hub * @@ -142,19 +152,60 @@ private Command ppCycleRegression(String path, Supplier startPose) { Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(5) + .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + .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()), + 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) { + 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))) + .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()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) + .withTimeout(1.6))); + } + + 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))) .withTimeout(14.5), Commands.deadline( - orchestrator.aimToHub().withTimeout(2.5), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), - Commands.parallel( - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp()) - .withTimeout(2.5), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp())); + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())))); } /** @@ -167,12 +218,18 @@ private Command ppCycleRegression(String path, Supplier startPose) { * up in after the path * @return A command that follows the paths and shoots */ - private Command pp2CycleRegression( - String path1, String path2, Supplier startPose, Supplier startPose2) { + private Command pp2CycleRegression(String path1, String path2, Supplier startPose) { return ppCycleRegression(path1, startPose) - .withTimeout(10.5) - .andThen(ppCycleRegression(path2, startPose2)); + .withTimeout(14.5) + .andThen(orchestrator.stopShootingAuto().withTimeout(0.1)) + .andThen(ppCycleRegressionConnect(path2)); + } + + /** Depot Auto */ + public Command ppDepot() { + return ppCycleRegression("A-Cycle-LeftSweep", startDepot); } + ; /** * Left side pathplanner auto for a single cycle with fixed shooting distance @@ -198,7 +255,7 @@ public Command ppBCycleRight() { * @return A command that executes the auto */ public Command ppB2CycleRightRegression() { - return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside, connectBside); + return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside); } /** @@ -207,7 +264,7 @@ public Command ppB2CycleRightRegression() { * @return A comand that executes the auto */ public Command ppA2CycleRightRegression() { - return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside, connectAside); + return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside); } /** diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java index 9f51e5912..4b9eafb20 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java @@ -64,7 +64,7 @@ public class DriveToPose extends Command { thetaMaxVelocity.initDefault(Units.degreesToRadians(360.0)); thetaMaxAcceleration.initDefault(Units.degreesToRadians(720.0)); driveTolerance.initDefault(0.01); - thetaTolerance.initDefault(Units.degreesToRadians(1.0)); + thetaTolerance.initDefault(Units.degreesToRadians(0.2)); case ROBOT_2025_COMP: driveKp.initDefault(2.5); driveKd.initDefault(0.0); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 09d8613f1..4d405d38d 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 ce75e28ba..a469e5c73 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(1.905); + public static final double wheelRadiusMeters = Units.inchesToMeters(2); public static final double driveMotorReduction = 8.16; public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1); @@ -96,7 +96,7 @@ public class DriveConstants { (2 * PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec // Turn PID configuration - public static final double turnKp = 3.5; + public static final double turnKp = 12.0; public static final double turnKd = 0.0; public static final double turnSimP = 9.0; public static final double turnSimD = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 468aaec25..4d41fba74 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -25,16 +25,13 @@ public Command extendToAngle(double angle) { return extend.extendToAngle(angle); } - public Command holdAngleAndIntake(double angle) { - return Commands.parallel(extend.holdAngle(angle), runIntakeMotor()) - .withName("holdAngleAndIntake"); + public Command runIntakeMotor() { + return rollers.intake(); } - public Command runIntakeMotor() { - return rollers - .intake() - .onlyWhile(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE) - .onlyIf(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE); + public Command slowlyBringInIntake() { + return Commands.parallel(rollers.intake(), extend.runIntakeExtendVolts(SLOW_VOLTS)) + .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly()); } // Jack's Chugga Chugga mode diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index ee3abab23..263ce3b4d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -10,28 +10,30 @@ public class IntakeConstants { public static final double STALL_TIME = 0.1; public static final double STALL_VELOCITY = 0.1; - public static final double INTAKE_VOLTS = 12; - public static final double OUTTAKE_VOLTS = -10; + public static final double INTAKE_VOLTS = 8; + public static final double OUTTAKE_VOLTS = -12; public static final double EXTEND_VOLTS = 0.01; public static final double COLLAPSE_VOLTS = -0.01; public static final double INTAKE_MAX_VELOCITY = 200; - public static final int EXTEND_LIMIT_ID = 1; + public static final int EXTEND_LIMIT_ID = 0; - public static final double PID_P = 0.023; // arbritrary values + public static final double PID_P = 0.046; // arbritrary values public static final double PID_I = 0.000004; public static final double PID_D = 0.00001; public static final double EXTEND_POS = -13; // TODO: change the actual values - public static final double FUNNEL_POS = -7; + public static final double FUNNEL_POS = -10; // shake around the funnel pos by this much 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; // 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; - public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 20; + public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 30; public static final double INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT = 45; } 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 2b0621a3b..bbf147252 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -2,12 +2,14 @@ import static frc.robot.subsystems.intake.IntakeConstants.COLLAPSE_POS; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POS; +import static frc.robot.subsystems.intake.IntakeConstants.HOME_VOLTAGE; import static frc.robot.subsystems.intake.IntakeConstants.POSITION_TOLERANCE; import static frc.robot.subsystems.intake.IntakeConstants.STALL_VELOCITY; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import frc.robot.RobotState.IntakePosition; @@ -25,6 +27,7 @@ public class IntakeExtend extends SubsystemBase { private boolean stalledCollapse = false; private boolean isStowed = false; private boolean isExtended = false; + private boolean hasPose = false; private double targetExtendPosition = 0; @@ -53,8 +56,14 @@ public void periodic() { RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED; } if (inputs.isCollapsed) { + if (!hasPose) { + Logger.recordOutput("Intake/IntakeExtend/HomeCompleted", true); + } io.resetExtendEncoder(0.0); + hasPose = true; } + inputs.hasPose = hasPose; + Logger.recordOutput("Intake/IntakeExtend/HasPose", hasPose); } public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) { @@ -70,6 +79,10 @@ public DoubleSupplier getAngle() { return () -> inputs.getExtendPos; } + public void setIdleMode(boolean idleMode) { + io.setIdleMode(idleMode); + } + public Command setPose(double pose) { return Commands.runOnce(() -> io.resetExtendEncoder(pose), this); } @@ -124,26 +137,30 @@ public Command stopExtendingCommand() { return Commands.run(this::stopExtend, this); } - public Command extendToAngle(double angle) { + public Command homeExtend() { return Commands.run( () -> { - io.setPIDEnabled(true); - io.goToPos(angle); + io.setPIDEnabled(false); + io.setVoltageExtend(HOME_VOLTAGE); }, this) - .until(() -> inputs.atSetpoint) + .beforeStarting(() -> Logger.recordOutput("Intake/IntakeExtend/HomeStarted", true)) + .until(() -> inputs.isCollapsed) .finallyDo(this::stopExtend) - .withName("extendToAngle"); + .withName("homeExtend"); } - public Command holdAngle(double angle) { - return Commands.run( - () -> { - io.setPIDEnabled(true); - io.goToPos(angle); - }, - this) - .finallyDo(this::stopExtend) - .withName("holdAngle"); + public Command extendToAngle(double angle) { + return new ConditionalCommand(homeExtend(), Commands.none(), () -> !hasPose) + .andThen( + Commands.run( + () -> { + io.setPIDEnabled(true); + io.goToPos(angle); + }, + this) + .until(() -> inputs.atSetpoint) + .finallyDo(this::stopExtend) + .withName("extendToAngle")); } } 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 082317d7c..6a8695012 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java @@ -21,6 +21,7 @@ class IntakeExtendIOInputs { public double stallExtendTimer = 0.0; public double stallCollapseTimer = 0.0; public boolean stowed = false; + public boolean hasPose = false; } default void updateInputs(IntakeExtendIOInputs inputs) {} @@ -35,6 +36,9 @@ default void goToPos(double pos) {} default void setPIDEnabled(boolean enabled) {} + default void setIdleMode(boolean cost) {} + ; + default boolean getPIDEnabled() { return false; } 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 ed688a961..c21bc77d9 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -33,6 +33,16 @@ public class IntakeExtendIOSparkMax implements IntakeExtendIO { public IntakeExtendIOSparkMax() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); + + pidController = extendMotor.getClosedLoopController(); + extendMotorEncoder = extendMotor.getEncoder(); + + extendMotor.configure( + getSparkMaxConfig(), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID); + } + + public SparkMaxConfig getSparkMaxConfig() { EncoderConfig extendEnc = new EncoderConfig(); extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION); extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION); @@ -46,13 +56,7 @@ public IntakeExtendIOSparkMax() { .pid(PID_P, PID_I, PID_D) .feedbackSensor(FeedbackSensor.kPrimaryEncoder); extendConfig.encoder.apply(extendEnc); - - pidController = extendMotor.getClosedLoopController(); - extendMotorEncoder = extendMotor.getEncoder(); - - extendMotor.configure( - extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID); + return extendConfig; } @Override @@ -61,7 +65,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendVelocity = extendMotorEncoder.getVelocity(); inputs.extendVolts = extendMotor.getAppliedOutput(); inputs.extendAmps = extendMotor.getOutputCurrent(); - inputs.isCollapsed = false; + inputs.isCollapsed = isCollapsed(); inputs.getExtendPos = extendMotorEncoder.getPosition(); inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID; inputs.hasSetpoint = usingPID; @@ -100,6 +104,21 @@ public void setPIDEnabled(boolean enabled) { this.usingPID = enabled; } + @Override + public void setIdleMode(boolean coast) { + if (coast) { + extendMotor.configure( + getSparkMaxConfig().idleMode(IdleMode.kCoast), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + } else { + extendMotor.configure( + getSparkMaxConfig().idleMode(IdleMode.kBrake), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + } + } + @Override public void resetExtendEncoder(double position) { extendMotorEncoder.setPosition(position); diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java index 66590c588..b37a506a3 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java +++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java @@ -7,7 +7,7 @@ public class LedConstants { public static final int LED_COUNT; /* num LEDs - change the first two numbers based on actual led strip */ - public static final int FULL_LENGTH = 42; // 2026 robot LED count + public static final int FULL_LENGTH = 43; // 2026 robot LED count public static final int BAR_LENGTH = Math.min(10, FULL_LENGTH); public static final int BASE_LENGTH = (FULL_LENGTH - BAR_LENGTH) / 2; @@ -46,6 +46,10 @@ public class LedConstants { LED_CHANNEL = 0; LED_COUNT = 14; } + case ROBOT_2026_COMP -> { + LED_CHANNEL = 0; + LED_COUNT = FULL_LENGTH; + } default -> { LED_CHANNEL = 0; LED_COUNT = 0; diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java index dff198558..8d715dfe1 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java @@ -4,7 +4,7 @@ public final class MagicCarpetConstants { public static final boolean MOTOR_INVERTED = false; - public static final int CURRENT_LIMIT = 30; + public static final int CURRENT_LIMIT = 20; public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed private MagicCarpetConstants() {} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3d18d1c9a..80c857bb0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,10 @@ 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.FF_SCALAR; +import static frc.robot.subsystems.shooter.ShooterConstants.KA; +import static frc.robot.subsystems.shooter.ShooterConstants.KS; +import static frc.robot.subsystems.shooter.ShooterConstants.KV; import static frc.robot.subsystems.shooter.ShooterConstants.TOLERANCE; import edu.wpi.first.math.controller.PIDController; @@ -12,6 +16,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -32,8 +37,7 @@ public class Shooter extends SubsystemBase { private double rampedTarget = 0.0; // Feedforward: handles steady-state voltage (V = KS + KV * velocity) - private final SimpleMotorFeedforward feedforward = - new SimpleMotorFeedforward(ShooterConstants.KS, ShooterConstants.KV); + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV, KA); // PID: handles error correction on top of feedforward // P only — no I term (causes integral windup oscillation) private final PIDController pid = new PIDController(0.001, 0.1, 0.1, 0.02); @@ -41,6 +45,7 @@ public class Shooter extends SubsystemBase { // Slew rate limiter: ramps the target velocity gradually (rad/s per second) // This prevents current spikes that cause oscillation with a 20A limit private final SlewRateLimiter targetRamper = new SlewRateLimiter(800); + private double feedForwardScalar; /** * Initializes the shooter with a Shooter IO @@ -48,6 +53,11 @@ public class Shooter extends SubsystemBase { * @param io A ShooterIO implementing instance */ public Shooter(ShooterIO io) { + SmartDashboard.putNumber("Shooter/KS", KS * FF_SCALAR); + SmartDashboard.putNumber("Shooter/KA", KA * FF_SCALAR); + SmartDashboard.putNumber("Shooter/KV", KV * FF_SCALAR); + SmartDashboard.putNumber("Shooter/FeedForward_Scalar", FF_SCALAR); + feedForwardScalar = FF_SCALAR; this.io = io; sysId = new SysIdRoutine( @@ -61,6 +71,37 @@ public Shooter(ShooterIO io) { @Override public void periodic() { + + if (SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedForwardScalar + || SmartDashboard.getNumber( + "Shooter/KS", + feedforward.getKs() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedforward.getKs() + || SmartDashboard.getNumber("Shooter/KA", feedforward.getKa()) != feedforward.getKa() + || SmartDashboard.getNumber( + "Shooter/KV", + feedforward.getKv() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedforward.getKv()) { + feedForwardScalar = SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar); + feedforward.setKa(SmartDashboard.getNumber("Shooter/KA", feedforward.getKa())); + feedforward.setKv( + SmartDashboard.getNumber( + "Shooter/KV", + feedforward.getKv() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)); + feedforward.setKa( + SmartDashboard.getNumber( + "Shooter/KS", + feedforward.getKs() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)); + } io.updateInputs(inputs); RobotState.getInstance().shooterAtSpeed = isAtSetpoint() && targetSpeed.gt(RadiansPerSecond.of(0)); @@ -165,6 +206,7 @@ public Command setTargetVelocity(Supplier velocity) { controllerEnabled = true; }, this) + .finallyDo(io::stop) .withName("setTargetVelocity"); } @@ -219,11 +261,15 @@ public Time getAirTimeSeconds(Distance distance) { * @return Setpoint in radians per second */ public Supplier calculateSetpoint(Supplier distance) { - // calculate rad/s depending on distance + // calculate rpm depending on distance return () -> { - AngularVelocity velocity = RadiansPerSecond.of(183.35 * distance.get().in(Meters) + 750.93); - if (velocity.gt(RadiansPerSecond.of(5000))) { - return RadiansPerSecond.of(5000); + AngularVelocity velocity = + RPM.of( + 25.30184 * Math.pow(distance.get().in(Meters), 2) + - 47.12642 * distance.get().in(Meters) + + 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 7db7ddb98..f784e5bea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -4,26 +4,33 @@ 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; 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 VOLTAGE_COMPENSATION = 9.0; - public static final int CURRENT_LIMIT = 40; + public static final double VOLTAGE_COMPENSATION = 12.0; + public static final int CURRENT_LIMIT = 35; public static final IdleMode IDLE_MODE = IdleMode.kCoast; - public static final double KV = 0.0465; - public static final double KA = 0.031259; - public static final double KS = 0.25; + public static final double KV = 0.0662; + public static final double KA = 0.028853; + public static final double KS = 0.2723; - public static final double SHOOTER_WHEEL_GEAR_RATIO = 2.5; - public static final double CLOSE_HUB_SHOOTER_RPM = 1085; + // for 3.7647 gear ratio + // public static final double KV = 0.0662; + // public static final double KA = 0.028853; + // public static final double KS = 0.2723; + public static final double FF_SCALAR = 1; + public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5; + public static final double CLOSE_HUB_SHOOTER_RPM = 1100; public static final double MAX_VOLTAGE = 12.0; - public static final double TOLERANCE = 50; // measured in radians + public static final double TOLERANCE = + Units.rotationsPerMinuteToRadiansPerSecond(100); // measured in radians public static Transform2d kShooterOffsetFromRobotCenter = - new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0)); + new Transform2d(new Translation2d(-0.1585, 0.0), new Rotation2d(0.0)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index b019e9229..17cb653a8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -44,26 +44,26 @@ public ShooterIOSparkMax(boolean independentMode) { .inverted(false) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); - bottomLeftMotorConfig.follow(shooterTopRightMotorCanId, false); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); bottomLeftMotorConfig.apply(enc); var topLeftMotorConfig = new SparkMaxConfig(); topLeftMotorConfig - .inverted(false) + .inverted(true) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); - topLeftMotorConfig.follow(shooterTopRightMotorCanId, true); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); topLeftMotorConfig.apply(enc); var bottomRightMotorConfig = new SparkMaxConfig(); bottomRightMotorConfig - .inverted(false) + .inverted(true) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); - bottomRightMotorConfig.follow(shooterTopRightMotorCanId, true); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); bottomRightMotorConfig.apply(enc); var topRightMotorConfig = new SparkMaxConfig(); @@ -71,7 +71,8 @@ public ShooterIOSparkMax(boolean independentMode) { .inverted(false) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); topRightMotorConfig.apply(enc); bottomLeftMotor.configure( @@ -125,6 +126,9 @@ public void updateInputs(ShooterIOInputs inputs) { @Override public void setVoltage(double volts) { topRightMotor.setVoltage(volts); + topLeftMotor.setVoltage(volts); + bottomRightMotor.setVoltage(volts); + bottomLeftMotor.setVoltage(volts); } @Override diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 109cc6887..463e6ce91 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,7 +24,7 @@ public class VisionConstants { private static Rotation3d RotationCorrection = new Rotation3d(0, 0, Math.PI / 2); // 90 degree roation around z-axis public static Transform3d robotToCamera0 = - new Transform3d( // front camera + new Transform3d( new Translation3d( Units.inchesToMeters(-8.931), Units.inchesToMeters(12.349), @@ -35,7 +35,7 @@ public class VisionConstants { Units.degreesToRadians(0.0), Units.degreesToRadians(-158.771))); public static Transform3d robotToCamera1 = - new Transform3d( // rear left camera + new Transform3d( new Translation3d( Units.inchesToMeters(8.931), // x Units.inchesToMeters(12.349), // y @@ -44,7 +44,7 @@ public class VisionConstants { new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), - Units.degreesToRadians(-201.229))); + Units.degreesToRadians(158.771))); public static Transform3d robotToCamera2 = new Transform3d( // rear right camera