diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index f000c633..02fcb8d5 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -5,12 +5,18 @@ "x": 0.6842044526319189, "y": 6.690138362483131 }, - "rotation": -125.0 + "rotation": -125.5 }, "command": { "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "initialShot" + } + }, { "type": "named", "data": { @@ -55,7 +61,7 @@ { "type": "named", "data": { - "name": "autoAimAtSpeaker" + "name": "midAmpSideShooterAngle" } }, { @@ -67,12 +73,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "shootNote" - } - }, { "type": "path", "data": { @@ -98,7 +98,7 @@ { "type": "named", "data": { - "name": "autoAimAtSpeaker" + "name": "midAmpSideShooterAngle" } }, { @@ -109,12 +109,6 @@ } ] } - }, - { - "type": "named", - "data": { - "name": "shootNote" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto index 19d6d2f3..6aef7d9e 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto @@ -92,6 +92,12 @@ "name": "shootNote" } }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto index b0c27942..502986ae 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7607347530216694, - "y": 4.480646518544946 + "x": 0.7523718013035084, + "y": 4.450354048988052 }, - "rotation": 116.0 + "rotation": 119.3 }, "command": { "type": "sequential", @@ -24,9 +24,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "Blue5AutoPickupFromStart" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue5AutoPickupFromStart" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + } + ] } }, { @@ -36,15 +49,34 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "BlueSourceShoot5" + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueSourceShoot5" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] } }, { "type": "named", "data": { - "name": "autoShootNote" + "name": "sourceSideLongShot" } }, { @@ -60,15 +92,28 @@ } }, { - "type": "path", - "data": { - "pathName": "BlueSourceShoot4" - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "autoShootNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueSourceShoot4UnderStage" + } + }, + { + "type": "named", + "data": { + "name": "lowAmpSideShooterAngle" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/AmpShotPathTest.auto b/src/main/deploy/pathplanner/autos/AmpShotPathTest.auto new file mode 100644 index 00000000..9cbcab7b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AmpShotPathTest.auto @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 4.696339831588324, + "y": 7.478931968540094 + }, + "rotation": 178.8308606720925 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue1AutoPickupFromStart" + } + }, + { + "type": "named", + "data": { + "name": "autoPickup" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueAmpShoot1" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Blue2AutoPickup" + } + }, + { + "type": "named", + "data": { + "name": "autoPickup" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path index 91fa5b84..45841f1b 100644 --- a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.0581473014797882, - "y": 7.409038027952599 + "x": 1.1126849299961952, + "y": 7.108880647180037 }, "prevControl": null, "nextControl": { - "x": 1.263190077007704, - "y": 7.416781454542863 + "x": 1.9209549213878978, + "y": 7.352335463864286 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.2091145416858735, - "y": 7.761753795112528 + "x": 2.79739226145119, + "y": 7.741863170559082 }, "prevControl": { - "x": 1.4065244211409311, - "y": 7.7560004609150734 + "x": 2.057289618731077, + "y": 7.741863170559082 }, "nextControl": { - "x": 4.798790832149566, - "y": 7.780317782857788 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.852150914266242, - "y": 7.45 - }, - "prevControl": { - "x": 4.719085501741837, - "y": 7.479677359878633 - }, - "nextControl": { - "x": 5.202725850291558, - "y": 7.371811849199025 + "x": 4.170511959714229, + "y": 7.741863170559082 }, "isLocked": false, "linkedName": null @@ -52,8 +36,8 @@ "y": 7.225738959188476 }, "prevControl": { - "x": 4.986149852656257, - "y": 7.181901774728098 + "x": 5.212464042958929, + "y": 6.953069564502119 }, "nextControl": null, "isLocked": false, @@ -68,25 +52,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 1.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shootNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.5, "maxAcceleration": 2.3, @@ -94,7 +60,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 2.0, "rotation": 180.0, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index f11c588c..ac990131 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 5.318582489016831, - "y": 6.582940573288553 + "x": 5.475395244977917, + "y": 6.310348848455705 }, "prevControl": null, "nextControl": { - "x": 5.447041047159915, - "y": 6.546428133560659 + "x": 5.603853803121001, + "y": 6.273836408727811 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.9361241450834035, - "y": 5.62867536173981 + "x": 6.19602150236329, + "y": 6.281134270453595 }, "prevControl": { - "x": 6.790051255072855, - "y": 6.115584995108306 + "x": 5.942217077535311, + "y": 6.352516764936465 + }, + "nextControl": { + "x": 6.507643667719127, + "y": 6.193490536447265 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.052982457091843, + "y": 5.72605728841351 + }, + "prevControl": { + "x": 6.449214511714907, + "y": 5.618937169072441 }, "nextControl": null, "isLocked": false, @@ -49,12 +65,15 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 163.54, "rotateFast": false }, "reversed": false, "folder": null, - "previewStartingState": null, + "previewStartingState": { + "rotation": 174.05471649527038, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path index 813f53bc..ab529493 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.608696097581994, + "y": 6.144799573110416 }, "prevControl": null, "nextControl": { - "x": 6.7450028218176925, - "y": 5.271930425780205 + "x": 7.04688679450616, + "y": 5.388788737788644 }, "isLocked": false, "linkedName": "SourceSideShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path index 4d8d9a63..030968b9 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": null, "nextControl": { - "x": 4.590121955578446, - "y": 3.1185271540507364 + "x": 5.0847896467283356, + "y": 3.167402918073519 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path index 7b298687..59740a4f 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.608696097581994, + "y": 6.144799573110416 }, "prevControl": null, "nextControl": { - "x": 1.3950925173499225, - "y": 2.8922432222088648 + "x": 1.6969764900383901, + "y": 3.009101534217304 }, "isLocked": false, "linkedName": "SourceSideShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path index d61cb3bf..18a3a384 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": null, "nextControl": { - "x": 0.8809484408652919, - "y": 5.3558211663675195 + "x": 1.3756161320151823, + "y": 5.404696930390301 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path index 9c8d1a43..460dc70a 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.608696097581994, - "y": 1.8210420287981743 + "x": 5.981781263681151, + "y": 1.4217761294360087 }, "prevControl": null, "nextControl": { - "x": 5.942828493011672, - "y": 1.606801790116036 + "x": 6.682931135731786, + "y": 2.4735009375119583 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.9848151084202525, - "y": 2.113187808819271 + "x": 7.072458842426581, + "y": 2.8825050295414942 }, "prevControl": { - "x": 5.8746611443400845, - "y": 1.674969138787625 + "x": 6.751098484403375, + "y": 2.619573827522507 }, "nextControl": null, "isLocked": false, @@ -44,7 +44,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": -165.06858282186255, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path index 9758ae01..a542966a 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.608696097581994, + "y": 6.144799573110416 }, "prevControl": null, "nextControl": { - "x": 3.6446150235123724, - "y": 3.807633332941637 + "x": 3.94649899620084, + "y": 3.924491644950076 }, "isLocked": false, "linkedName": "SourceSideShoot" @@ -44,7 +44,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 150.02, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path index 65ff1d5c..7758b384 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.7748820313773495, - "y": 3.7492041769374174 + "x": 2.622104793438532, + "y": 2.999363341549934 }, "prevControl": null, "nextControl": { - "x": 2.7779158761164506, - "y": 2.34690443283615 + "x": 5.7578028323316435, + "y": 1.4120379367686373 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.60877963731236, - "y": 1.412869986233799 + "x": 7.208793539769761, + "y": 1.5483726341118162 }, "prevControl": { - "x": 4.783983565594652, - "y": 1.645754560785515 + "x": 6.225236080365399, + "y": 2.142402386821381 }, "nextControl": null, "isLocked": false, @@ -44,7 +44,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 162.4744316262771, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path index 4daafbed..a4b71d0c 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": null, "nextControl": { - "x": 4.334691138518079, - "y": 1.2179481149085758 + "x": 4.829358829667969, + "y": 1.2668238789313584 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path index ab0164f1..163a87f0 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.302473839515581, - "y": 6.852118395594816 + "x": 7.9683725678246144, + "y": 7.459455583205354 }, "prevControl": null, "nextControl": { - "x": 6.078981161244491, - "y": 6.774673444449585 + "x": 4.326288510228267, + "y": 7.751601363226451 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.845200801512714, - "y": 6.3787367080907 + "x": 4.725554409590433, + "y": 6.436945353131514 }, "prevControl": { - "x": 5.207198562545273, - "y": 6.70360649363274 + "x": 4.71302813700922, + "y": 6.874943512199754 }, "nextControl": null, "isLocked": false, @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -44,7 +62,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 0.8, "rotation": -177.7974018382342, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path index fd058514..cd206d61 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.73, + "y": 6.144799573110416 }, "prevControl": { - "x": 5.864922951672713, - "y": 7.167309803184257 + "x": 4.369686871307314, + "y": 7.86845967523489 }, "nextControl": null, "isLocked": false, - "linkedName": "SourceSideShoot" + "linkedName": null } ], "rotationTargets": [ @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.95, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -50,6 +68,9 @@ }, "reversed": false, "folder": null, - "previewStartingState": null, + "previewStartingState": { + "rotation": 173.91416260815873, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path index bedeb531..cde3eccb 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": { - "x": 4.454865149790057, - "y": 2.3759342101523275 + "x": 2.8460832247880394, + "y": 2.2592606988298214 }, "nextControl": null, "isLocked": false, @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -44,7 +62,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 0.8, "rotation": 148.60298946245044, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path index e34b8194..831841ba 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.029894257986713, - "y": 2.4960698570045796 + "x": 7.0140296864223615, + "y": 3.3402000849078815 }, "prevControl": null, "nextControl": { - "x": 6.634240172394935, - "y": 4.48930681965753 + "x": 5.280631391630518, + "y": 5.424173315725042 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.511314170908295, - "y": 4.7814525996786275 + "x": 4.043880922874538, + "y": 6.096108609773567 }, "prevControl": { - "x": 5.7772792176663845, - "y": 3.7784187549395263 + "x": 3.9172844181987303, + "y": 3.963444415619555 }, "nextControl": null, "isLocked": false, - "linkedName": "ShootUnderStage" + "linkedName": null } ], "rotationTargets": [ @@ -35,8 +35,38 @@ "rotateFast": false } ], - "constraintZones": [], - "eventMarkers": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.8, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.8, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -50,6 +80,9 @@ }, "reversed": false, "folder": null, - "previewStartingState": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path index a7fea243..8e000727 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 6.107551968190356, - "y": 1.3850240046159097 + "x": 7.734655943807735, + "y": 0.9056519180654026 }, "prevControl": null, "nextControl": { - "x": 4.415771287331697, - "y": 1.9027273823650077 + "x": 6.877694989079185, + "y": 1.1491067347496495 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 5.913613915009562, + "y": 1.460728900105488 }, "prevControl": { - "x": 4.367682945957438, - "y": 2.350923222652966 + "x": 6.390785355710689, + "y": 1.2367504687559792 }, "nextControl": null, "isLocked": false, - "linkedName": "SourceSideBlueShoot" + "linkedName": null } ], "rotationTargets": [ @@ -44,7 +44,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": 150.19409724626237, "rotateFast": false }, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3cb8978e..1b3dcc27 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -61,6 +61,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + m_robotContainer.autonomousInit(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f18e4687..920f11a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -312,8 +312,18 @@ private void configureBindings() { FarStageAngleSup, SubwooferAngleSup, OverStagePassSup, - OppositeStageShotSup + OppositeStageShotSup, + falseSup )); + // new Trigger(()->driverController.getL3Button()&&driverController.getR3Button()).onTrue( + // new SequentialCommandGroup( + // new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(22.5), angleShooterSubsystem), + // new ParallelRaceGroup( + // new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, ()->true), + // new ShootNote(indexer, 0.65, 0.4, intake) + // ) + // ) + // ); if(Preferences.getBoolean("Detector Limelight", false)) { Command AutoGroundIntake = new SequentialCommandGroup( @@ -391,10 +401,9 @@ private void configureBindings() { new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter), new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), new InstantCommand(driveTrain::pointWheelsInward, driveTrain), - new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(15), angleShooterSubsystem), - new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<1)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)), + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem), + new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<0.3)), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), - new WaitCommand(0.1), new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), new WaitCommand(0.5), new InstantCommand(()->intake.setNoteHeld(false)) @@ -487,6 +496,9 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } + public void autonomousInit() { + SmartDashboard.putNumber("autos ran", SmartDashboard.getNumber("autos ran", 0)+1); + } private double modifyAxis(double value, double deadband) { // Deadband value = MathUtil.applyDeadband(value, deadband); @@ -539,7 +551,7 @@ private void registerNamedCommands() { } if(intakeExists&&!indexerExists&&!angleShooterExists) { NamedCommands.registerCommand("groundIntake", new InstantCommand(()->intake.intakeYes(IntakeConstants.INTAKE_SPEED, IntakeConstants.INTAKE_SIDE_SPEED))); - NamedCommands.registerCommand("autoShootNote", new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup).withTimeout(1)); + NamedCommands.registerCommand("autoShootNote", new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, falseSup).withTimeout(1)); NamedCommands.registerCommand("autoPickup", new SequentialCommandGroup( new CollectNote(driveTrain, limelight), new DriveTimeCommand(-1, 0, 0, 1, driveTrain) @@ -550,7 +562,24 @@ private void registerNamedCommands() { SmartDashboard.putData("shooterOn", new InstantCommand(()->shooter.shootRPS(LONG_SHOOTING_RPS), shooter)); } if(intakeExists&&indexerExists&&angleShooterExists) { - NamedCommands.registerCommand("shootNote", new ShootNote(indexer, 0.25, 0, intake)); + NamedCommands.registerCommand("shootNote", new ShootNote(indexer, 0.2, 0, intake)); + NamedCommands.registerCommand("sourceSideLongShot", new SequentialCommandGroup( + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(22.5), angleShooterSubsystem), + new ParallelRaceGroup( + new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, ()->true), + new ShootNote(indexer, 0.5, 0.4, intake) + ) + )); + NamedCommands.registerCommand("midAmpSideShooterAngle", new OverrideCommand(angleShooterSubsystem) { + public void execute() { + angleShooterSubsystem.setDesiredShooterAngle(27); + }; + }); + NamedCommands.registerCommand("lowAmpSideShooterAngle", new OverrideCommand(angleShooterSubsystem) { + public void execute() { + angleShooterSubsystem.setDesiredShooterAngle(29.5); + }; + }); } if(indexerExists) { // NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED), indexer)); @@ -561,14 +590,14 @@ private void registerNamedCommands() { if(sideWheelsExists){ NamedCommands.registerCommand("intakeSideWheels", new InstantCommand(()-> intake.intakeSideWheels(1))); } - NamedCommands.registerCommand("note isn't held", new WaitUntilCommand(()->!intake.isNoteSeen())); + NamedCommands.registerCommand("note isn't held", new WaitUntil(()->!intake.isNoteSeen())); } if(indexerExists&&shooterExists) { - NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 1.0, 1.25, angleShooterSubsystem)); + NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 0.9, 0.1, angleShooterSubsystem)); //the following command will both aim the robot at the speaker (with the AimRobotMoving), and shoot a note while aiming the shooter (with shootNote). As a race group, it ends //when either command finishes. the AimRobotMoving command will never finish, but the shootNote finishes when shootTime is reached. NamedCommands.registerCommand("autoShootNote", new ParallelRaceGroup( - new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup), + new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, falseSup), new SequentialCommandGroup( new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle()), angleShooterSubsystem), new ShootNote(indexer, 1.5, 0.5,intake) diff --git a/src/main/java/frc/robot/commands/AimRobotMoving.java b/src/main/java/frc/robot/commands/AimRobotMoving.java index c8c2b27f..d22dbbca 100644 --- a/src/main/java/frc/robot/commands/AimRobotMoving.java +++ b/src/main/java/frc/robot/commands/AimRobotMoving.java @@ -36,6 +36,7 @@ public class AimRobotMoving extends Command { BooleanSupplier SubwooferAngleSup; BooleanSupplier OverStagePassSup; BooleanSupplier OppositeStageShotSup; + BooleanSupplier AutonomousLongShotSup; /** * a command to automatically aim the robot at the speaker if odometry is correct. This command also controls aiming from setpoints incase the odometry isn't working. * @param drivetrain the swerve drive subsystem @@ -49,7 +50,7 @@ public class AimRobotMoving extends Command { * @param OverStagePassSup button to press to aim at the speaker from the opposite side of the stage (from the speaker) * @param OppositeStageShotSup button to press to aim at the speaker from the opposite side of the stage (from the speaker) */ - public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier PodiumAngleSup, BooleanSupplier FarStageAngleSup, BooleanSupplier SubwooferAngleSup, BooleanSupplier OverStagePassSup, BooleanSupplier OppositeStageShotSup){ + public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier PodiumAngleSup, BooleanSupplier FarStageAngleSup, BooleanSupplier SubwooferAngleSup, BooleanSupplier OverStagePassSup, BooleanSupplier OppositeStageShotSup, BooleanSupplier AutonomousLongShotSup){ m_drivetrain = drivetrain; speedController = new PIDController( AUTO_AIM_ROBOT_kP, @@ -66,6 +67,7 @@ public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSup this.OverStagePassSup = OverStagePassSup; this.OppositeStageShotSup = OppositeStageShotSup; this.run = run; + this.AutonomousLongShotSup = AutonomousLongShotSup; addRequirements(drivetrain); } @@ -84,16 +86,19 @@ public void execute() { double farStageRobotAngle; double OverStagePassAngle; double OppositeStageShotAngle; + double AutonomousLongShotAngle; if(DriverStation.getAlliance().get() == Alliance.Red) { podiumRobotAngle = Field.RED_PODIUM_ROBOT_ANGLE; farStageRobotAngle = Field.RED_FAR_STAGE_ROBOT_ANGLE; OverStagePassAngle = Field.RED_OVER_STAGE_PASS_ANGLE; OppositeStageShotAngle = Field.RED_OPPOSITE_STAGE_ROBOT_ANGLE; + AutonomousLongShotAngle = 35; } else { podiumRobotAngle = Field.BLUE_PODIUM_ROBOT_ANGLE; farStageRobotAngle = Field.BLUE_FAR_STAGE_ROBOT_ANGLE; OverStagePassAngle = Field.BLUE_OVER_STAGE_PASS_ANGLE; OppositeStageShotAngle = Field.BLUE_OPPOSITE_STAGE_ROBOT_ANGLE; + AutonomousLongShotAngle = 146.3; } if(PodiumAngleSup.getAsBoolean()) { speedController.setSetpoint(podiumRobotAngle); @@ -103,6 +108,8 @@ public void execute() { speedController.setSetpoint(OverStagePassAngle); } else if(OppositeStageShotSup.getAsBoolean()) { speedController.setSetpoint(OppositeStageShotAngle); + } else if (AutonomousLongShotSup.getAsBoolean()) { + speedController.setSetpoint(AutonomousLongShotAngle); } else { speedController.setSetpoint(desiredRobotAngle); } diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index eefd876e..4a29c587 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -50,7 +50,7 @@ public void initialize() { 0.6,//Vision.K_DETECTOR_TY_P, Vision.K_DETECTOR_TY_I, Vision.K_DETECTOR_TY_D); - tyLimiter = new SlewRateLimiter(2, -2, 0); + tyLimiter = new SlewRateLimiter(20, -20, 0); txController.setSetpoint(0); tyController.setSetpoint(0); txController.setTolerance(3.5, 0.25); @@ -114,7 +114,7 @@ public void execute() { } SmartDashboard.putNumber("CollectNote/calculated radians per second", txController.calculate(tx)); - SmartDashboard.putNumber("CollectNote/calculated forward meters per second", tyController.calculate(ty)); + SmartDashboard.putNumber("CollectNote/calculated forward meters per second", tyLimiter.calculate(-20/Math.abs(tx))); SmartDashboard.putBoolean("CollectNote/closeNote", closeNote); SmartDashboard.putNumber("CollectNote/runsInvalid", runsInvalid); // drives the robot forward faster if the object is higher up on the screen, and turns it more based on how far away the object is from x=0 diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 1ce45630..72896d9e 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -65,7 +65,8 @@ public void end(boolean interrupted) { intake.intakeOff(); // indexer.magicRPS(0); if(DriverStation.isAutonomous()) { - indexer.forwardInches(-1); + // indexer.forwardInches(-1); + indexer.off(); } else { indexer.off(); } diff --git a/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java b/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java index 3942d93b..a3cd5dbb 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java +++ b/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java @@ -5,6 +5,7 @@ package frc.robot.commands.NamedCommands; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.Field; import frc.robot.settings.Constants.ShooterConstants; @@ -19,6 +20,7 @@ public class InitialShot extends Command { double revTime; double shootTime; AngleShooterSubsystem angleShooter; + boolean shot; /** Creates a new shootThing. */ public InitialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double revTime, double shootTime, AngleShooterSubsystem angleShooter) { this.indexer = indexer; @@ -30,22 +32,26 @@ public InitialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double re addRequirements(shooter, indexer, angleShooter); // Use addRequirements() here to declare subsystem dependencies. } - + // Called when the command is initially scheduled. @Override public void initialize() { + shot = false; timer.reset(); timer.start(); indexer.off(); - shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS); + shooter.setTargetVelocity(65, 50, 600, 600); + SmartDashboard.putNumber("shooter resets", SmartDashboard.getNumber("shooter resets", 0)+1); angleShooter.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(timer.get()>revTime) { + if(shooter.getRSpeed()>35&&shooter.getLSpeed()>60) { indexer.on(); + timer.reset(); + shot = true; } } @@ -58,6 +64,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get()>shootTime; + return timer.get()>shootTime&&shot; + } } diff --git a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java index 1cb53ab0..1e30e68d 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java +++ b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java @@ -43,7 +43,7 @@ public void initialize() { @Override public void execute() { if(timer.get()>revTime) { - indexer.set(IndexerConstants.INDEXER_SHOOTING_POWER); + indexer.setVoltage(12); } SmartDashboard.putNumber("auto timer", timer.get()); } diff --git a/src/main/java/frc/robot/commands/WaitUntil.java b/src/main/java/frc/robot/commands/WaitUntil.java index f9a7750e..ba9a0268 100644 --- a/src/main/java/frc/robot/commands/WaitUntil.java +++ b/src/main/java/frc/robot/commands/WaitUntil.java @@ -6,6 +6,7 @@ import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class WaitUntil extends Command { @@ -31,6 +32,8 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return condition.getAsBoolean(); + boolean finish = condition.getAsBoolean(); + SmartDashboard.putBoolean("wait until end condition", finish); + return finish; } } diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 794f34bd..22d76b37 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -406,9 +406,9 @@ public CTREConfigs() { driveMotorConfig.MotorOutput.Inverted = DriveConstants.DRIVETRAIN_DRIVE_INVERTED; driveMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.0; driveMotorConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = DriveConstants.DRIVE_MOTOR_RAMP; - driveMotorConfig.Slot0.kP = DriveConstants.k_DRIVE_P; - driveMotorConfig.Slot0.kI = DriveConstants.k_DRIVE_I; - driveMotorConfig.Slot0.kD = DriveConstants.k_DRIVE_D; + driveMotorConfig.Slot0.kP = DriveConstants.k_DRIVE_P*12; + driveMotorConfig.Slot0.kI = DriveConstants.k_DRIVE_I*12; + driveMotorConfig.Slot0.kD = DriveConstants.k_DRIVE_D*12; driveMotorConfig.Slot0.kS = DriveConstants.k_DRIVE_FF_S; driveMotorConfig.Slot0.kV = DriveConstants.k_DRIVE_FF_V; driveMotorConfig.Voltage.PeakForwardVoltage = 12; @@ -466,15 +466,15 @@ public final class Field{ public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263; - public static final double RED_SPEAKER_Y = 5.613; - public static final double SHOOTER_RED_SPEAKER_X = 16.582;//changed so that shots from the side wil aim to the opposite side, and bank in + public static final double RED_SPEAKER_Y = 5.613;//home field: 5.613 HCPA: 5.68 + public static final double SHOOTER_RED_SPEAKER_X = 16.582;//home field: 16.582 HCPA: 16.55 public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_BLUE_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263; - public static final double BLUE_SPEAKER_Y = 5.39; - public static final double SHOOTER_BLUE_SPEAKER_X = 0; - public static final double ROBOT_BLUE_SPEAKER_X =SHOOTER_BLUE_SPEAKER_X+0.6; + public static final double BLUE_SPEAKER_Y = 5.348;//home field: HCPA: 5.348 + public static final double SHOOTER_BLUE_SPEAKER_X = 0.13; //HCPA: 0.13 + public static final double ROBOT_BLUE_SPEAKER_X = SHOOTER_BLUE_SPEAKER_X+0.2; public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index babdfb51..b28bdf9a 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -185,7 +185,7 @@ public DrivetrainSubsystem() { getGyroscopeRotation(), getModulePositions(), DRIVE_ODOMETRY_ORIGIN); - odometer.setVisionMeasurementStdDevs(VecBuilder.fill(0.9, 0.9, 99999999)); + odometer.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 99999999)); } /** * Sets the gyroscope angle to zero. This can be used to set the direction the robot is currently facing to the diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index f82207ff..430a7152 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -13,6 +13,7 @@ import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -94,6 +95,9 @@ public void reverse() { public void set(double speed) { m_IndexerMotor.set(speed); } + public void setVoltage(double voltage) { + m_IndexerMotor.setControl(new VoltageOut(voltage)); + } /** * uses the indexer motor's onboard Motion Magic control to move the indexer forward. To move backwards, use negative inches. * @param inches the inches to move forward. diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 70ca4c4f..9a03cb0b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -41,8 +41,8 @@ public class ShooterSubsystem extends SubsystemBase { double m_DesiredShooterAngle; double targetVelocityL; double targetVelocityR; - boolean isRevingL; - boolean isRevingR; + int revStateL; + int revStateR; CurrentLimitsConfigs currentLimitConfigs; Slot0Configs PIDLeftconfigs; @@ -66,8 +66,8 @@ public ShooterSubsystem(double runSpeed) { PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.CompRightkP).withKV(ShooterConstants.CompRightkFF).withKI(0.004); PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.CompLeftkP).withKV(ShooterConstants.CompLeftkFF).withKI(0.004); } else { - PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.PracRightkP).withKV(ShooterConstants.PracRightkFF).withKI(0.004); - PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.PracLeftkP).withKV(ShooterConstants.PracLeftkFF).withKI(0.004); + PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.PracRightkP).withKV(ShooterConstants.PracRightkFF).withKI(0); + PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.PracLeftkP).withKV(ShooterConstants.PracLeftkFF).withKI(0); } runsValid = 0; shooterR = new TalonFX(ShooterConstants.SHOOTER_R_MOTORID); @@ -128,6 +128,9 @@ public void shootRPS(double RPS) { public void shootSameRPS(double RPS) { setTargetVelocity(RPS, RPS, ShooterConstants.CURRENT_LIMIT, 100); } + public void shootSameRPSWithCurrent(double RPS, double supplyLimit, double statorLimit) { + setTargetVelocity(RPS, RPS, supplyLimit, statorLimit); + } /** * allows you to set the shooter's speed using a supplier. this way you can use a value on SmartDashboard to tune * the shooter's speed. @@ -194,25 +197,33 @@ public void turnOff(){ * @param isReving Old isReving * @return Updated isReving */ - private static boolean updateIsReving(boolean isReving, double targetSpeed, double speed){ - double revUpEn = -6; + private static int updateIsReving(int revState, double targetSpeed, double speed){ + double revUpEn = -10; double revUpDis = -2; + double revDownEn = 10; + double revDownDis = 2; targetSpeed = Math.abs(targetSpeed); speed = Math.abs(speed); if (speed < targetSpeed + revUpEn){ - isReving = true; - } else if (speed > targetSpeed + revUpDis){ - isReving = false; - } - return isReving; + revState = 1; + } else if (speed > targetSpeed + revDownEn && targetSpeed<10){ + revState = -1; + } else if((revState == 1 && speed > targetSpeed + revUpDis) || (revState == -1 && speed < targetSpeed + revDownDis)) { + revState = 0; + } + return revState; } /** * If the motor is Reving, set speed to full, if not Reving, use PID Mode */ - private static void updateMotor(TalonFX shooterMotor, boolean revState, double targetSpeed){ + private static void updateMotor(TalonFX shooterMotor, int revState, double targetSpeed, CurrentLimitsConfigs currentLimits){ + currentLimits.StatorCurrentLimitEnable = revState != -1; + shooterMotor.getConfigurator().apply(currentLimits); if(targetSpeed == 0) { shooterMotor.set(0); - } else if (revState){ + } else if (revState == -1) { + shooterMotor.set(-Math.signum(targetSpeed)); + } else if(revState == 1) { shooterMotor.set(Math.signum(targetSpeed)); } else { shooterMotor.setControl(new VelocityDutyCycle(targetSpeed).withSlot(0)); @@ -222,10 +233,10 @@ private static void updateMotor(TalonFX shooterMotor, boolean revState, double t * Revs left and right motors while below target velocity, if above target velocity, the motors are put to PID Mode */ private void updateMotors(){ - isRevingL = updateIsReving(isRevingL, targetVelocityL, shooterL.getVelocity().getValueAsDouble()); - isRevingR = updateIsReving(isRevingR, targetVelocityR, shooterR.getVelocity().getValueAsDouble()); - updateMotor(shooterL, isRevingL, targetVelocityL); - updateMotor(shooterR, isRevingR, targetVelocityR); + revStateL = updateIsReving(revStateL, targetVelocityL, shooterL.getVelocity().getValueAsDouble()); + revStateR = updateIsReving(revStateR, targetVelocityR, shooterR.getVelocity().getValueAsDouble()); + updateMotor(shooterL, revStateL, targetVelocityL, currentLimitConfigs); + updateMotor(shooterR, revStateR, targetVelocityR, currentLimitConfigs); } public double getRSpeed() { return shooterR.getVelocity().getValueAsDouble(); @@ -246,6 +257,9 @@ public void periodic() { SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Right shooter speed", shooterR.getVelocity().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Left shooter speed", shooterL.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Shooter/Left shooter revState", revStateL); + SmartDashboard.putNumber("Shooter/right shooter revState", revStateR); + SmartDashboard.putNumber("Shooter/right integral value", shooterR.getClosedLoopIntegratedOutput().getValueAsDouble()); double error = getSignedError(); RobotState.getInstance().ShooterError = error; if(Math.abs(error)