diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f2b442cd..e6660ec6 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.84, - "robotLength": 0.84, + "robotWidth": 0.8382, + "robotLength": 0.8382, "holonomicMode": true, "pathFolders": [ "Auto Logic Paths" diff --git a/src/main/deploy/bonkswerve/modules/physicalproperties.json b/src/main/deploy/bonkswerve/modules/physicalproperties.json index a4e443a1..c3f4500f 100644 --- a/src/main/deploy/bonkswerve/modules/physicalproperties.json +++ b/src/main/deploy/bonkswerve/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { + "statorLimit": { "drive": 40, "angle": 20 }, diff --git a/src/main/deploy/bonkswerve/modules/pidfproperties.json b/src/main/deploy/bonkswerve/modules/pidfproperties.json index 4d615439..42df985c 100644 --- a/src/main/deploy/bonkswerve/modules/pidfproperties.json +++ b/src/main/deploy/bonkswerve/modules/pidfproperties.json @@ -3,14 +3,14 @@ "p": 20.0, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 }, "angle": { "p": 35.0, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 } } \ No newline at end of file diff --git a/src/main/deploy/craneswerve/modules/physicalproperties.json b/src/main/deploy/craneswerve/modules/physicalproperties.json index 9be10f3d..d4f63e09 100644 --- a/src/main/deploy/craneswerve/modules/physicalproperties.json +++ b/src/main/deploy/craneswerve/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { + "statorLimit": { "drive": 40, "angle": 20 }, diff --git a/src/main/deploy/craneswerve/modules/pidfproperties.json b/src/main/deploy/craneswerve/modules/pidfproperties.json index 48abf219..0bbe6b00 100644 --- a/src/main/deploy/craneswerve/modules/pidfproperties.json +++ b/src/main/deploy/craneswerve/modules/pidfproperties.json @@ -3,14 +3,14 @@ "p": 0.1, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 }, "angle": { "p": 0.01, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 } } \ No newline at end of file diff --git a/src/main/deploy/launcher_data.csv b/src/main/deploy/launcher_data_lamprey.csv similarity index 58% rename from src/main/deploy/launcher_data.csv rename to src/main/deploy/launcher_data_lamprey.csv index 526a3b0d..5c09ec9c 100644 --- a/src/main/deploy/launcher_data.csv +++ b/src/main/deploy/launcher_data_lamprey.csv @@ -5,19 +5,38 @@ # These RPMs are target RPMs, actual was much lower # Front subwoofer -1.30, 254, 3500 +1.30, 257, 3500 # Front bumper at autoline -2.13, 250, 3500 +2.13, 236, 3500 + +# dcmps + +# On Autoline N2 +2.81, 225, 3600 + +#chain +# 3.84, 213, 3600 + +# wing line amp side +5.93, 207, 4000 + +3.82, 217, 4000 +4.32, 213, 4000 +4.10, 212, 4000 + +# bad values? + # Near podium -5.66, 246, 3600 +# 5.66, 225, 3600 # Auto point 1 (X 2.870, Y 2.196, theta 130.6) -4.46, 242, 3600 +# 4.46, 230, 3600 # After this, these are setpoint angles # Auto point 2 (X 5.699, Y 7.501, theta -162.8) -5.55, 240.3, 4400 +# 5.55, 226, 4400 # Auto point 3 (X 4.299, Y 5.134, theta 176.222) -4.32, 242, 4300 # Might be a little low, and could probably lower RPM- Ran out of time to fully test +# 4.32, 232, 4300 # Might be a little low, and could probably lower RPM- Ran out of time to fully test + # Pre 3/28 values: # 1.397, 254, 4500 diff --git a/src/main/deploy/launcher_data_throughbore.csv b/src/main/deploy/launcher_data_throughbore.csv new file mode 100644 index 00000000..774c1c2d --- /dev/null +++ b/src/main/deploy/launcher_data_throughbore.csv @@ -0,0 +1,8 @@ +# DISTANCE (meters), ANGLE (degrees without PIVOT_OFFSET), RPM (velocity) + +1.30, 254, 3500 +2.38, 237, 3500 +3.00, 232, 3750 +4.00, 228, 4000 +5.00, 224.3, 4500 +6.00, 220.5, 5500 \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MasterPIDTest.auto b/src/main/deploy/pathplanner/autos/MasterPIDTest.auto new file mode 100644 index 00000000..42cad18d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MasterPIDTest.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.3, + "y": 5.496252587946771 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MasterPIDTest" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto index 7b1d693d..260b9e9c 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score2.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score2.auto new file mode 100644 index 00000000..50b0e905 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score2.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerCenterLineNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerCenterlineN2Return" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetMidAutoline3Score2.auto b/src/main/deploy/pathplanner/autos/PresetMidAutoline3Score2.auto new file mode 100644 index 00000000..4269fdc8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetMidAutoline3Score2.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLowNote" + } + }, + { + "type": "path", + "data": { + "pathName": "LowNoteToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "MidSpeakerPassAutoLine" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto index c19c5ecd..6e2e3634 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto @@ -56,7 +56,7 @@ { "type": "path", "data": { - "pathName": "SpeakerToMidNote" + "pathName": "launchSecondNoteLaunch2" } } ] diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto index 7afeebe1..36b47fef 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto @@ -44,25 +44,19 @@ { "type": "path", "data": { - "pathName": "launchSecondNoteLaunch" - } - }, - { - "type": "named", - "data": { - "name": "VisionLaunch" + "pathName": "launchSecondNoteCenterLineLaunch" } }, { "type": "path", "data": { - "pathName": "MidNoteToBottomNote" + "pathName": "launchSecondNoteLaunch2" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "BottomNoteLaunch" + "name": "VisionLaunch" } }, { diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline6Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline6Score.auto new file mode 100644 index 00000000..eff9c4f8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline6Score.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerToHighNoteLaunch2" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighNoteLaunch2ToFarHighNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteCenterLineLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteLaunch2" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "MidNoteToBottomNote" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide4ScoreUnderStage.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide4ScoreUnderStage.auto new file mode 100644 index 00000000..f6a052ba --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide4ScoreUnderStage.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.47 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowSpeakerToMiddleLowNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MiddleLowNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowLaunchNoteFourCenterLineUnderStage" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowLaunchNoteThreeCenterLineUnderStage" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path b/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path index ddb02157..5d0cd8b8 100644 --- a/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path @@ -46,8 +46,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7, - "rotationDegrees": 155.83202134477403, + "waypointRelativePos": 0.8, + "rotationDegrees": 158.64, + "rotateFast": false + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 158.64, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path index 40507852..3eb62dad 100644 --- a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 1.6972725837343565, - "y": 3.0877308601464057 + "x": 1.6947995088736767, + "y": 3.084897857158463 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path index e0f6ecab..25b9a887 100644 --- a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.459228700001152, - "y": 7.268235228706192 + "x": 6.48105091081977, + "y": 7.320540680967402 }, "isLocked": false, "linkedName": "LaunchHighNote" @@ -17,27 +17,27 @@ { "anchor": { "x": 8.150075399249896, - "y": 7.395762539272827 + "y": 7.45 }, "prevControl": { - "x": 8.405377986562273, - "y": 7.426708307431903 + "x": 9.311036285890234, + "y": 7.444943068349215 }, "nextControl": { - "x": 7.630253917365975, - "y": 7.332753874802049 + "x": 7.58026817713618, + "y": 7.452481975240951 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0824060026540545, - "y": 7.552922493229885 + "x": 5.413643259718952, + "y": 7.183134366268733 }, "prevControl": { - "x": 4.562069053099387, - "y": 7.653478859199186 + "x": 5.893306310164284, + "y": 7.283690732238034 }, "nextControl": null, "isLocked": false, @@ -46,7 +46,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.75, + "waypointRelativePos": 0.55, "rotationDegrees": -172.5750748946377, "rotateFast": false } diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path b/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path index 3409018e..a1a59c2f 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path @@ -17,15 +17,15 @@ { "anchor": { "x": 8.37, - "y": 7.45 + "y": 7.46 }, "prevControl": { "x": 8.73873589093028, - "y": 7.43760330109946 + "y": 7.44760330109946 }, "nextControl": { "x": 8.00126410906971, - "y": 7.46239669890054 + "y": 7.47239669890054 }, "isLocked": false, "linkedName": null @@ -47,12 +47,12 @@ "rotationTargets": [ { "waypointRelativePos": 0.75, - "rotationDegrees": -172.64, + "rotationDegrees": -174.0, "rotateFast": false }, { "waypointRelativePos": 1.2, - "rotationDegrees": -172.0, + "rotationDegrees": -174.0, "rotateFast": false }, { @@ -61,8 +61,8 @@ "rotateFast": false }, { - "waypointRelativePos": 0.6, - "rotationDegrees": 180.0, + "waypointRelativePos": 0.35, + "rotationDegrees": -161.01300595815806, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerCenterlineN2Return.path b/src/main/deploy/pathplanner/paths/HighSpeakerCenterlineN2Return.path new file mode 100644 index 00000000..34780f55 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HighSpeakerCenterlineN2Return.path @@ -0,0 +1,141 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": null, + "nextControl": { + "x": 4.048666786792263, + "y": 6.7241388044029655 + }, + "isLocked": false, + "linkedName": "TopSpeaker" + }, + { + "anchor": { + "x": 8.293644849397962, + "y": 5.800300603450209 + }, + "prevControl": { + "x": 9.030376579271676, + "y": 5.613194132371172 + }, + "nextControl": { + "x": 7.9360527619599806, + "y": 5.8911176415296955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": { + "x": 1.2057927218565005, + "y": 7.331049617052808 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopSpeaker" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.05, + "rotationDegrees": -130.0, + "rotateFast": false + }, + { + "waypointRelativePos": 0.35, + "rotationDegrees": 170.2291447191198, + "rotateFast": true + }, + { + "waypointRelativePos": 0.85, + "rotationDegrees": 171.51986105015604, + "rotateFast": false + }, + { + "waypointRelativePos": 1.5, + "rotationDegrees": -173.34649448732242, + "rotateFast": false + }, + { + "waypointRelativePos": 1.85, + "rotationDegrees": -130.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.15, + "maxWaypointRelativePos": 1.05, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "rev launcher", + "waypointRelativePos": 1.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RevLauncher" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.8, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path b/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path index 3c528ed9..6a28feb8 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.35, - "rotationDegrees": -166.19572126875954, + "rotationDegrees": -169.16565991676583, "rotateFast": false }, { @@ -40,7 +40,19 @@ "rotateFast": false } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.45, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [ { "name": "intake", diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path index 91c897e3..c392d2dd 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path @@ -20,12 +20,12 @@ "y": 7.68155593454044 }, "prevControl": { - "x": 1.96668964766501, - "y": 7.7108748503211295 + "x": 1.9554131415955136, + "y": 7.636282850913281 }, "nextControl": { - "x": 3.734277569039197, - "y": 7.657311579976456 + "x": 3.7337899746634324, + "y": 7.718539229435451 }, "isLocked": false, "linkedName": null @@ -54,6 +54,11 @@ "waypointRelativePos": 0.05, "rotationDegrees": -120.0, "rotateFast": false + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 180.0, + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path index 186bbffd..18cfec5e 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.3644632694670795, - "y": 6.924282677394184 + "x": 1.4057878828008366, + "y": 6.876162812154685 }, "isLocked": false, "linkedName": "TopSpeaker" @@ -31,13 +31,18 @@ "rotationTargets": [ { "waypointRelativePos": 0.4, - "rotationDegrees": -178.6167416994117, - "rotateFast": false + "rotationDegrees": -172.45241554385072, + "rotateFast": true }, { "waypointRelativePos": 0.1, "rotationDegrees": -126.93135629519831, "rotateFast": false + }, + { + "waypointRelativePos": 0.25, + "rotationDegrees": -148.60777368547065, + "rotateFast": false } ], "constraintZones": [ @@ -46,8 +51,8 @@ "minWaypointRelativePos": 0.0, "maxWaypointRelativePos": 0.3, "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.0, + "maxAcceleration": 2.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 } diff --git a/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path b/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path index 5ae1d3d7..f12d1959 100644 --- a/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path @@ -48,28 +48,28 @@ }, { "anchor": { - "x": 6.726391110439878, - "y": 3.4297848775877737 + "x": 6.525781051562102, + "y": 3.1158011406931907 }, "prevControl": { - "x": 7.484456770715343, - "y": 2.838123874445947 + "x": 7.057519569584594, + "y": 2.378830212205879 }, "nextControl": { - "x": 5.7597547108329605, - "y": 4.1842327992321975 + "x": 5.878621542262904, + "y": 4.012741513230675 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.405970613743029, - "y": 5.306459621928253 + "x": 4.212252060867758, + "y": 5.158796499411181 }, "prevControl": { - "x": 4.858961069273489, - "y": 3.9197541458145984 + "x": 5.2664003509825195, + "y": 4.3751818412727745 }, "nextControl": null, "isLocked": false, @@ -86,6 +86,16 @@ "waypointRelativePos": 1.2, "rotationDegrees": 169.04986019290004, "rotateFast": false + }, + { + "waypointRelativePos": 2.1, + "rotationDegrees": 173.2949618109403, + "rotateFast": false + }, + { + "waypointRelativePos": 2.7, + "rotationDegrees": 127.35178648180019, + "rotateFast": false } ], "constraintZones": [ @@ -99,6 +109,17 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 } + }, + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 2.3, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } } ], "eventMarkers": [ diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path index 455f25eb..c993acd6 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path @@ -52,7 +52,7 @@ }, { "waypointRelativePos": 1.0, - "rotationDegrees": 162.59131699825656, + "rotationDegrees": 149.60543545481846, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path index 6afe0186..96e1878b 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path @@ -20,12 +20,12 @@ "y": 2.4481294676875076 }, "prevControl": { - "x": 8.337533809215634, - "y": 1.7307971969497182 + "x": 8.057894836551087, + "y": 1.8070758038267916 }, "nextControl": { - "x": 8.240937624732393, - "y": 3.5769077252440225 + "x": 8.534598822328235, + "y": 3.0693076055203994 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 5.321383214195 }, "prevControl": { - "x": 5.059835262527669, - "y": 3.9580536303929756 + "x": 5.641519404680388, + "y": 4.3469619676204285 }, "nextControl": null, "isLocked": false, @@ -46,18 +46,33 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.75, - "rotationDegrees": 146.44882381880868, + "waypointRelativePos": 0.7, + "rotationDegrees": 137.84146867175556, "rotateFast": false }, { - "waypointRelativePos": 1.65, + "waypointRelativePos": 1.6, "rotationDegrees": 176.0, "rotateFast": false }, { "waypointRelativePos": 1.0, - "rotationDegrees": 162.59131699825656, + "rotationDegrees": 145.1820620122512, + "rotateFast": false + }, + { + "waypointRelativePos": 0.8, + "rotationDegrees": 135.96440825571213, + "rotateFast": false + }, + { + "waypointRelativePos": 1.3, + "rotationDegrees": 157.65879132111158, + "rotateFast": false + }, + { + "waypointRelativePos": 1.45, + "rotationDegrees": 164.3979661370285, "rotateFast": false } ], @@ -98,8 +113,8 @@ } }, { - "name": "retract", - "waypointRelativePos": 0.45, + "name": "under stage", + "waypointRelativePos": 1.5, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteThreeCenterLineUnderStage.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteThreeCenterLineUnderStage.path new file mode 100644 index 00000000..1734d8c1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteThreeCenterLineUnderStage.path @@ -0,0 +1,126 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.9896948365325304, + "y": 5.321383214195 + }, + "prevControl": null, + "nextControl": { + "x": 5.260103863424924, + "y": 4.362135811002534 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.349955083898307, + "y": 4.1046482092964185 + }, + "prevControl": { + "x": 7.631641521812732, + "y": 4.1046482092964185 + }, + "nextControl": { + "x": 8.44804559883397, + "y": 4.1046482092964185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9896948365325304, + "y": 5.321383214195 + }, + "prevControl": { + "x": 5.266234520608404, + "y": 4.165954781131208 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "rev launcher", + "waypointRelativePos": 1.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RevLauncher" + } + } + ] + } + } + }, + { + "name": "retract", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RetractPivot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 176.97, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 176.97, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path index f354569c..b2e1e6b1 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.770693659609494, - "y": 5.1295050435105765 + "x": 2.7776195372993056, + "y": 4.946521435183591 }, "isLocked": false, "linkedName": "PodiumLaunch" @@ -20,8 +20,8 @@ "y": 5.55 }, "prevControl": { - "x": 2.843883094194073, - "y": 5.224806376319017 + "x": 2.877482177096141, + "y": 5.287229265078679 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path index 466702ce..43172183 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 1.1980586123334407, - "y": 3.974467068033235 + "x": 1.1955855374727609, + "y": 3.9716340650452926 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path index c8cd71d7..5a8a4831 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.6785413734247925, - "y": 4.938025867025607 + "y": 4.9380258670256065 }, "isLocked": false, "linkedName": "Podium" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path index 15be5d2c..80cb7c5c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.689169744135214, - "y": 2.3304424556117516 + "x": 1.686696669274534, + "y": 2.3276094526238094 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path index ab8d22c8..0ff0eb2d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 2.288933586876183, - "y": 2.1355264332150297 + "x": 2.286460512015503, + "y": 2.1326934302270875 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path index b6baab31..2cda12a4 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.689169744135214, - "y": 3.8609042538750162 + "x": 1.686696669274534, + "y": 3.8580712508870736 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 1.6794199859183583, - "y": 3.948902319609865 + "x": 1.6769469110576778, + "y": 3.946069316621923 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path index 21e23e74..a5e7dc9d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.2211813497261212, - "y": 3.0616743218759597 + "x": 1.2187082748654414, + "y": 3.0588413188880175 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path index 634af2cb..f56e0458 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.2649659988419504, - "y": 3.9350340011580496 + "x": 1.2624929239812706, + "y": 3.9322009981701074 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path index c2920a61..d170d21c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.7933352100034745, - "y": 2.218463140171392 + "x": 1.7908621351427945, + "y": 2.21563013718345 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path index 6bda5110..868ed51c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.148516485067158, - "y": 3.4029989427500227 + "x": 1.1460434102064783, + "y": 3.400165939762081 }, "isLocked": false, "linkedName": "LowSpeaker" }, { "anchor": { - "x": 3.78, - "y": 1.8944530196752711 + "x": 3.8, + "y": 1.9 }, "prevControl": { - "x": 2.6615614666991383, - "y": 2.126231436115029 + "x": 2.092354686019913, + "y": 1.9233883088848789 }, "nextControl": { - "x": 5.394077866540532, - "y": 1.559961314696977 + "x": 4.572240225750198, + "y": 1.8894232175819474 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 2.4960698570045796 }, "prevControl": { - "x": 8.02042238010854, - "y": 2.264505651461778 + "x": 7.976085063097927, + "y": 2.378058890871132 }, "nextControl": { - "x": 8.834401077526127, - "y": 2.7075036084231523 + "x": 9.030376579271676, + "y": 2.642878903991426 }, "isLocked": false, "linkedName": null @@ -49,27 +49,27 @@ { "anchor": { "x": 3.78, - "y": 1.7744559954678114 + "y": 1.8827588652328318 }, "prevControl": { "x": 5.181058023487222, - "y": 1.549460899884071 + "y": 1.6577637696490914 }, "nextControl": { "x": 2.3229040282308606, - "y": 2.0084501926723575 + "y": 2.116753062437377 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 1.5429233708823726, - "y": 2.778681091803989 + "x": 1.4968680434796764, + "y": 3.3066127042225606 }, "nextControl": null, "isLocked": false, @@ -84,12 +84,12 @@ }, { "waypointRelativePos": 1.0, - "rotationDegrees": -171.68, + "rotationDegrees": -171.13373619481098, "rotateFast": false }, { "waypointRelativePos": 1.75, - "rotationDegrees": -158.14409893935215, + "rotationDegrees": -167.33444776423735, "rotateFast": false }, { @@ -101,6 +101,11 @@ "waypointRelativePos": 2.6, "rotationDegrees": -164.3286881943862, "rotateFast": false + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 159.12457737382582, + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path index 2c532257..0fd6c18d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.6794199859183583, - "y": 3.80240570457388 + "x": 1.6769469110576782, + "y": 3.7995727015859373 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 2.079160072809459, - "y": 3.3149177937310745 + "x": 2.0766869979487783, + "y": 3.312084790743132 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path index ebc7459b..dd0fac3d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 1.6986020361229914, - "y": 4.097669396600055 + "x": 1.6961289612623114, + "y": 4.094836393612113 }, "isLocked": false, "linkedName": "LowSpeaker" }, { "anchor": { - "x": 2.77, + "x": 2.7713929062062417, "y": 4.07539893464585 }, "prevControl": { - "x": 2.4513806186170926, + "x": 2.452773524823334, "y": 4.070802440768179 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path index b89a2a3c..093abf34 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 2.0485713043498155, - "y": 1.848940634818207 + "x": 2.046098229489136, + "y": 1.8461076318302645 }, "isLocked": false, "linkedName": "LowSpeaker" }, { "anchor": { - "x": 6.97, + "x": 6.4, "y": 0.8 }, "prevControl": { - "x": 4.923673991492127, - "y": 0.785799769797738 + "x": 4.100008967931012, + "y": 0.8064228032996081 }, "nextControl": { - "x": 7.0774886027857535, - "y": 0.8007459040726312 + "x": 6.509999571098952, + "y": 0.7996928224508884 }, "isLocked": false, "linkedName": null @@ -36,7 +36,7 @@ "y": 0.77 }, "prevControl": { - "x": 7.848466239624848, + "x": 7.8505863938223595, "y": 0.7700000000000005 }, "nextControl": null, @@ -47,23 +47,23 @@ "rotationTargets": [ { "waypointRelativePos": 0.05, - "rotationDegrees": 120.0, - "rotateFast": false - }, - { - "waypointRelativePos": 0.25, - "rotationDegrees": 150.98859902992157, + "rotationDegrees": 130.0, "rotateFast": false }, { "waypointRelativePos": 0.5, "rotationDegrees": 175.80321002729156, - "rotateFast": false + "rotateFast": true }, { "waypointRelativePos": 0.65, "rotationDegrees": 180.0, "rotateFast": true + }, + { + "waypointRelativePos": 0.3, + "rotationDegrees": 174.13621568699799, + "rotateFast": false } ], "constraintZones": [], @@ -87,7 +87,7 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/MasterPIDTest.path b/src/main/deploy/pathplanner/paths/MasterPIDTest.path new file mode 100644 index 00000000..a5c08fd1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MasterPIDTest.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7275269251393202, + "y": 4.4671669970120575 + }, + "prevControl": null, + "nextControl": { + "x": 2.2009903848867527, + "y": 6.151125236723411 + }, + "isLocked": false, + "linkedName": "LowSpeaker" + }, + { + "anchor": { + "x": 6.223779513086091, + "y": 7.063269283233724 + }, + "prevControl": { + "x": 5.346717929903097, + "y": 7.215293290985443 + }, + "nextControl": { + "x": 7.617215478650247, + "y": 6.821740382535936 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4522649102278256, + "y": 6.0692661556263285 + }, + "prevControl": { + "x": 4.762010207781099, + "y": 6.3616200166873265 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.05, + "rotationDegrees": 34.76, + "rotateFast": false + }, + { + "waypointRelativePos": 0.1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.332219853869633, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path b/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path index 83f8c4da..3fac1700 100644 --- a/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path @@ -20,15 +20,21 @@ "y": 4.07539893464585 }, "prevControl": { - "x": 1.8241337089851442, - "y": 4.2187176915536355 + "x": 1.8408648194756145, + "y": 4.097305368134887 }, "nextControl": null, "isLocked": false, "linkedName": "Podium" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 163.6034850413123, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { diff --git a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path index 40c7b046..10d0f373 100644 --- a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 3.9067566423421134, - "y": 5.445130169539619 + "x": 3.907019136132327, + "y": 5.033384050796496 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.059835262527669, - "y": 4.265902246090207 + "x": 5.589292620026682, + "y": 4.487068849914097 }, "prevControl": { - "x": 4.106065843242836, - "y": 4.566441586006087 + "x": 4.615676309458881, + "y": 4.715260172703426 }, "nextControl": { - "x": 6.137894028707098, - "y": 3.926198473430942 + "x": 6.5216705628659755, + "y": 4.2685427695611375 }, "isLocked": false, "linkedName": null @@ -36,28 +36,28 @@ "y": 4.104648209296419 }, "prevControl": { - "x": 8.202270809898764, - "y": 4.4595947557966555 + "x": 8.110859453742629, + "y": 4.264045296066871 }, "nextControl": { - "x": 8.397729190101238, - "y": 3.749701662796183 + "x": 8.5815171861562, + "y": 3.867401250687913 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.589292620026682, - "y": 4.265902246090207 + "x": 5.785884780858597, + "y": 4.3346788339914255 }, "prevControl": { - "x": 6.35984666440237, - "y": 3.9957190898956214 + "x": 6.538462814530829, + "y": 4.0178527359044205 }, "nextControl": { - "x": 4.757269334358488, - "y": 4.55763866482688 + "x": 5.045760238335085, + "y": 4.646262165439392 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 5.78 }, "prevControl": { - "x": 4.4337047232653015, - "y": 5.001384417183249 + "x": 4.405272674669961, + "y": 5.16154399760424 }, "nextControl": null, "isLocked": false, @@ -86,6 +86,16 @@ "waypointRelativePos": 1.2, "rotationDegrees": 180.0, "rotateFast": false + }, + { + "waypointRelativePos": 2.4, + "rotationDegrees": 171.2669358966649, + "rotateFast": false + }, + { + "waypointRelativePos": 2.85, + "rotationDegrees": 180.0, + "rotateFast": false } ], "constraintZones": [ @@ -104,7 +114,7 @@ "eventMarkers": [ { "name": "intake", - "waypointRelativePos": 0, + "waypointRelativePos": 1.25, "command": { "type": "parallel", "data": { @@ -118,6 +128,23 @@ ] } } + }, + { + "name": "understage", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RetractPivot" + } + } + ] + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path index 498e7c2b..eb62451b 100644 --- a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path +++ b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path @@ -81,11 +81,6 @@ "waypointRelativePos": 2.75, "rotationDegrees": 180.0, "rotateFast": false - }, - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0, - "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path index ca3f8eff..caa86ac2 100644 --- a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path @@ -108,7 +108,7 @@ "eventMarkers": [ { "name": "retract", - "waypointRelativePos": 0.2, + "waypointRelativePos": 0.25, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path index e9d716d7..ac23a341 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path @@ -30,9 +30,14 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.35, "rotationDegrees": 180.0, "rotateFast": false + }, + { + "waypointRelativePos": 0.75, + "rotationDegrees": 147.9081562884914, + "rotateFast": true } ], "constraintZones": [], @@ -56,7 +61,7 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -72,5 +77,5 @@ "rotation": 180.0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path index 0102b3da..e40e7ec4 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path @@ -8,36 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.01058639382236, - "y": 0.7700000000000005 + "x": 5.156891888026078, + "y": 1.4098255030081461 }, "isLocked": false, "linkedName": "CenterLineN5" }, { "anchor": { - "x": 3.0808084473311808, - "y": 1.363329583802025 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 3.9310570049711533, - "y": 0.7769512681882514 - }, - "nextControl": { - "x": 1.8459892684582195, - "y": 2.2149290175075143 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.73, - "y": 4.47 - }, - "prevControl": { - "x": 1.4899471997783693, - "y": 3.081925867364013 + "x": 1.7057565421060124, + "y": 2.2909664423919924 }, "nextControl": null, "isLocked": false, @@ -46,13 +30,23 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2, - "rotationDegrees": 180.0, + "waypointRelativePos": 0.25, + "rotationDegrees": -173.50545807332855, "rotateFast": false }, { - "waypointRelativePos": 1.9, + "waypointRelativePos": 0.6, + "rotationDegrees": 117.19405608697582, + "rotateFast": false + }, + { + "waypointRelativePos": 0.95, "rotationDegrees": 120.0, + "rotateFast": true + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": -161.29004592647703, "rotateFast": false } ], @@ -60,7 +54,7 @@ "eventMarkers": [ { "name": "rev launcher", - "waypointRelativePos": 1.0, + "waypointRelativePos": 0.5, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path index 3a93eb82..75aa19ac 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path @@ -48,7 +48,7 @@ { "waypointRelativePos": 0.65, "rotationDegrees": -131.3418666211863, - "rotateFast": false + "rotateFast": true }, { "waypointRelativePos": 1.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path index b2630521..95b72437 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.6148626582967456, - "y": 4.529772488116405 + "x": 1.382399573915957, + "y": 4.7595208580730555 }, "isLocked": false, "linkedName": "MidSpeaker" @@ -20,8 +20,8 @@ "y": 4.07539893464585 }, "prevControl": { - "x": 1.9520301897746657, - "y": 4.089988751406074 + "x": 1.3005404928188775, + "y": 4.1046482092964185 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/launchCenterN2LaunchMidNote.path b/src/main/deploy/pathplanner/paths/launchCenterN2LaunchMidNote.path new file mode 100644 index 00000000..3943b529 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/launchCenterN2LaunchMidNote.path @@ -0,0 +1,87 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.41, + "y": 6.347545266519105 + }, + "prevControl": null, + "nextControl": { + "x": 4.942163397840309, + "y": 6.1662170171187185 + }, + "isLocked": false, + "linkedName": "LaunchCenterlineN2" + }, + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": { + "x": 3.6201708439452913, + "y": 5.583800717150984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.3, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "rev launcher", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RevLauncher" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -170.62, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path index 594f0ab9..dfff0d38 100644 --- a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path @@ -8,40 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 4.572375270876667, - "y": 6.35111108060054 + "x": 5.885430896139777, + "y": 6.6979161979080075 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.02379366920896, - "y": 6.434313409167359 + "x": 8.399000341366138, + "y": 5.67 }, "prevControl": { - "x": 5.422887962893042, - "y": 6.434313409167359 + "x": 8.121465209600638, + "y": 6.028469660415459 }, "nextControl": { - "x": 6.67043999635063, - "y": 6.434313409167359 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.270256540513081, - "y": 5.78 - }, - "prevControl": { - "x": 8.239036732334611, - "y": 6.2371314046481 - }, - "nextControl": { - "x": 8.299575456293768, - "y": 5.350702129975688 + "x": 8.595181371237466, + "y": 5.41660884656091 }, "isLocked": false, "linkedName": null @@ -62,8 +46,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.7, - "rotationDegrees": -132.10466146576135, + "waypointRelativePos": 0.85, + "rotationDegrees": 156.2896135289697, + "rotateFast": false + }, + { + "waypointRelativePos": 0.2, + "rotationDegrees": 155.22824361973127, "rotateFast": false } ], @@ -71,7 +60,7 @@ "eventMarkers": [ { "name": "intake", - "waypointRelativePos": 1.2000000000000002, + "waypointRelativePos": 0.6, "command": { "type": "parallel", "data": { @@ -88,7 +77,7 @@ }, { "name": "rev launcher", - "waypointRelativePos": 2.55, + "waypointRelativePos": 1.5499999999999998, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path index c9224c11..08d8d18f 100644 --- a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.0824060026540545, - "y": 7.552922493229885 + "x": 5.413643259718952, + "y": 7.183134366268733 }, "prevControl": null, "nextControl": { - "x": 6.0634581056947585, - "y": 7.21302167675837 + "x": 7.394695362759656, + "y": 6.843233549797217 }, "isLocked": false, "linkedName": "LaunchCenterlineN1" @@ -46,8 +46,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.75, - "rotationDegrees": -148.64916364014846, + "waypointRelativePos": 0.65, + "rotationDegrees": -148.65, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path b/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path index f4f6c87c..e060d9ad 100644 --- a/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path @@ -20,28 +20,28 @@ "y": 4.890447979094157 }, "prevControl": { - "x": 2.3609477505216754, - "y": 4.877599864575197 + "x": 2.360805106514846, + "y": 4.897019030363906 }, "nextControl": { - "x": 4.017693080431205, - "y": 4.927426791790521 + "x": 3.9143033388107593, + "y": 4.873131591870992 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.886695178795763, + "x": 5.334722861104995, "y": 4.317276382300513 }, "prevControl": { - "x": 3.9622248613866597, - "y": 5.121565558446433 + "x": 4.4485644760238685, + "y": 4.49361633451813 }, "nextControl": { - "x": 5.326316485345794, - "y": 3.934805845601985 + "x": 6.1287009530540795, + "y": 4.159279729384658 }, "isLocked": false, "linkedName": null @@ -64,16 +64,16 @@ }, { "anchor": { - "x": 5.136302164496221, - "y": 4.317276382300513 + "x": 5.334722861104995, + "y": 4.450647836521938 }, "prevControl": { - "x": 5.727963167638047, - "y": 3.9752223648591443 + "x": 5.80717681627009, + "y": 4.269540487041985 }, "nextControl": { - "x": 4.5376988828435225, - "y": 4.663343904505979 + "x": 4.897670700399076, + "y": 4.618184498125873 }, "isLocked": false, "linkedName": null @@ -84,8 +84,8 @@ "y": 5.775826408795675 }, "prevControl": { - "x": 4.370800467689787, - "y": 5.2187723096530965 + "x": 4.577165782548069, + "y": 5.215489117757295 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json index 901c1a2b..ffb1caf4 100644 --- a/src/main/deploy/swerve/controllerproperties.json +++ b/src/main/deploy/swerve/controllerproperties.json @@ -1,8 +1,8 @@ { "angleJoystickRadiusDeadband": 0.5, "heading": { - "p": 0.5, + "p": 0.4, "i": 0, - "d": 0.05 + "d": 0.15 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index c2735a21..1d0d6978 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.0036753, + "p": 0.61249, "i": 0, "d": 0, - "kS": 0.18532, - "kV": 1.6909, - "kA": 0.28447 + "kS": 0.30249, + "kV": 1.7619, + "kA": 0.058255 }, "angleTuning": { "p": 68.706, diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 37bf4195..cb7fec7b 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.055241, + "p": 1.0788, "i": 0, "d": 0, - "kS": 0.15508, - "kV": 1.7507, - "kA": 0.26534 + "kS": 0.14653, + "kV": 1.7027, + "kA": 0.28842 }, "angleTuning": { "p": 68.04, diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index e83987de..8cf926a3 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.087084, + "p": 0.080223, "i": 0, "d": 0, - "kS": 0.026162, - "kV": 1.8747, - "kA": 0.39611 + "kS": 0.20631, + "kV": 1.8319, + "kA": 0.19741 }, "angleTuning": { "p": 69.289, diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index fff7a39b..33a1b835 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.76249, + "p": 0.00097084, "i": 0, "d": 0, - "kS": 0.12121, - "kV": 1.7955, - "kA": 0.33215 + "kS": 0.2071, + "kV": 1.8453, + "kA": 0.13646 }, "angleTuning": { "p": 67.31, diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index d2bb71ac..8b88ec01 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -1,9 +1,13 @@ { "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { - "drive": 60, - "angle": 24 + "statorLimit": { + "drive": 120, + "angle": 50 + }, + "supplyLimit": { + "drive": 80, + "angle": 30 }, "conversionFactor": { "angle": 19.2, diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index f95a39a7..c137e3d9 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -8,7 +8,6 @@ import static frc.team2412.robot.Subsystems.SubsystemConstants.LED_ENABLED; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -31,7 +30,6 @@ import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.util.AmpAlign; -import frc.team2412.robot.util.TrapAlign; public class Controls { public static class ControlConstants { @@ -60,7 +58,7 @@ public static class ControlConstants { private final Trigger launcherSubwooferPresetButton; private final Trigger launcherLowerPresetButton; // private final Trigger launcherPodiumPresetButton; - private final Trigger launcherTrapPresetButton; + // private final Trigger launcherTrapPresetButton; private final Trigger launcherAmpAlignPresetButton; private final Trigger launcherLaunchButton; @@ -78,7 +76,7 @@ public Controls(Subsystems s) { launcherSubwooferPresetButton = codriveController.a(); launcherLowerPresetButton = codriveController.y(); // launcherPodiumPresetButton = codriveController.povLeft(); - launcherTrapPresetButton = codriveController.start(); + // launcherTrapPresetButton = codriveController.start(); launcherAmpAlignPresetButton = driveController.y(); launcherLaunchButton = codriveController.rightBumper(); // intake controls (confirmed with driveteam) @@ -113,22 +111,6 @@ public Controls(Subsystems s) { if (LED_ENABLED) { bindLEDControls(); } - Pose2d SPEAKER_POSE = - Robot.isBlue() - ? new Pose2d(0.0, 5.55, Rotation2d.fromRotations(0)) - : new Pose2d(16.5, 5.55, Rotation2d.fromRotations(0)); - if (DRIVEBASE_ENABLED && LAUNCHER_ENABLED) { - Commands.run( - () -> { - Pose2d robotPose = s.drivebaseSubsystem.getPose(); - Pose2d relativeSpeaker = robotPose.relativeTo(SPEAKER_POSE); - double distance = relativeSpeaker.getTranslation().getNorm(); - s.launcherSubsystem.updateDistanceEntry(distance); - }) - .withName("Update distance") - .ignoringDisable(true) - .schedule(); - } if (DRIVEBASE_ENABLED && LAUNCHER_ENABLED && INTAKE_ENABLED) { // temporary controls, not sure what drive team wants driveController @@ -172,7 +154,8 @@ private void bindDrivebaseControls() { s.drivebaseSubsystem.driveJoystick( driveController::getLeftY, driveController::getLeftX, - () -> Rotation2d.fromRotations(driveController.getRightX()))); + () -> Rotation2d.fromRotations(driveController.getRightX()), + driveController.rightTrigger())); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); driveController .start() @@ -221,7 +204,9 @@ private void bindLauncherControls() { s.launcherSubsystem, () -> MathUtil.applyDeadband(codriveController.getLeftY(), 0.1) - * LauncherSubsystem.ANGLE_MAX_SPEED)); + * LauncherSubsystem.ANGLE_MAX_SPEED, + codriveController.leftBumper(), + codriveController.back())); launcherLowerPresetButton.onTrue( s.launcherSubsystem @@ -242,11 +227,12 @@ private void bindLauncherControls() { s.launcherSubsystem, LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.AMP_AIM_ANGLE)); - launcherTrapPresetButton.onTrue( - TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); + + // launcherTrapPresetButton.onTrue( + // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); launcherAmpAlignPresetButton.onTrue( Commands.either( - AmpAlign.ampPreset(s.drivebaseSubsystem), + AmpAlign.ampPreset(s.drivebaseSubsystem, s.launcherSubsystem), Commands.none(), () -> s.drivebaseSubsystem.getPose().getY() > 5.0)); @@ -258,7 +244,7 @@ private void bindLauncherControls() { // s.launcherSubsystem.runEnd( // s.launcherSubsystem::launch, s.launcherSubsystem::stopLauncher)); - driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(4000))); + driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(6500))); } private void bindSysIdControls() { @@ -277,10 +263,11 @@ private void bindSysIdControls() { // .rightTrigger() // .whileTrue(s.launcherSubsystem.flywheelSysIdDynamic(Direction.kReverse)); // switch these between angle and drive tests in code when tuning - driveController.x().whileTrue(s.drivebaseSubsystem.angleSysIdQuasistatic(Direction.kForward)); - driveController.y().whileTrue(s.drivebaseSubsystem.angleSysIdQuasistatic(Direction.kReverse)); - driveController.a().whileTrue(s.drivebaseSubsystem.angleSysIdDynamic(Direction.kForward)); - driveController.b().whileTrue(s.drivebaseSubsystem.angleSysIdDynamic(Direction.kReverse)); + driveController.x().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kForward)); + driveController.y().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kReverse)); + driveController.a().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kForward)); + driveController.b().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kReverse)); + driveController.back().whileTrue(s.drivebaseSubsystem.debugDriveFullPower()); } public void vibrateDriveController(double vibration) { diff --git a/src/main/java/frc/team2412/robot/Hardware.java b/src/main/java/frc/team2412/robot/Hardware.java index f5af0645..20b7ca90 100644 --- a/src/main/java/frc/team2412/robot/Hardware.java +++ b/src/main/java/frc/team2412/robot/Hardware.java @@ -21,9 +21,9 @@ public class Hardware { public static final int LAUNCHER_PIVOT_TWO_MOTOR_ID = 32; // intake [40 - 49] - public static final int INTAKE_MOTOR_FRONT = 40; - public static final int INTAKE_MOTOR_BACK = 41; - public static final int INTAKE_MOTOR_LEFT = 42; + public static final int INTAKE_MOTOR_FRONT = 41; + // public static final int INTAKE_MOTOR_BACK = 42; + public static final int INTAKE_MOTOR_LEFT = 40; public static final int INTAKE_MOTOR_RIGHT = 43; public static final int INGEST_MOTOR = 44; @@ -31,8 +31,8 @@ public class Hardware { public static final int INDEX_MOTOR_UPPER = 45; public static final int FEEDER_MOTOR = 46; - // LED strip is PWM port 8 - public static final int BLINKIN_LED = 8; + // LED strip is PWM port 3 + public static final int BLINKIN_LED = 3; // intake sensors (Digital IO) public static final int INDEX_SENSOR = 1; diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index df649ad1..789ec924 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,6 +1,7 @@ package frc.team2412.robot; import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -19,6 +20,7 @@ import frc.team2412.robot.Subsystems.SubsystemConstants; import frc.team2412.robot.commands.diagnostic.IntakeDiagnosticCommand; import frc.team2412.robot.commands.diagnostic.LauncherDiagnosticCommand; +import frc.team2412.robot.commands.launcher.SyncPivotRelativeEncoderCommand; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.MatchDashboard; import frc.team2412.robot.util.auto.AutoLogic; @@ -121,6 +123,12 @@ public void robotInit() { dashboard = new MatchDashboard(subsystems); RobotController.setBrownoutVoltage(5.75); + + CommandScheduler.getInstance() + .schedule( + new SyncPivotRelativeEncoderCommand(subsystems.launcherSubsystem) + .ignoringDisable(true) + .withName("Sync Relative Encoder")); } @Override @@ -138,6 +146,16 @@ public void testInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + if (SubsystemConstants.DRIVEBASE_ENABLED && SubsystemConstants.LAUNCHER_ENABLED) { + Translation2d speakerPosition = + DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Blue + ? new Translation2d(0.0, 5.55) + : new Translation2d(16.5, 5.55); + Translation2d robotPosition = subsystems.drivebaseSubsystem.getPose().getTranslation(); + Translation2d robotToSpeaker = speakerPosition.minus(robotPosition); + double distance = robotToSpeaker.getNorm(); + subsystems.launcherSubsystem.updateDistanceEntry(distance); + } } @Override diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 077cc3fb..adaf6840 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -12,8 +12,6 @@ public class Subsystems { public static class SubsystemConstants { - - private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = true; public static final boolean LIMELIGHT_ENABLED = false; public static final boolean CLIMB_ENABLED = false; diff --git a/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java b/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java index 0373645d..79ac0317 100644 --- a/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java +++ b/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java @@ -14,10 +14,8 @@ import frc.team2412.robot.subsystems.IntakeSubsystem; public class IntakeDiagnosticCommand extends SequentialCommandGroup { - private final IntakeSubsystem intakeSubsystem; public IntakeDiagnosticCommand(IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; addCommands( new FeederInCommand(intakeSubsystem).withTimeout(0.5), new WaitCommand(0.5), diff --git a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java index fe254e5f..99e896a1 100644 --- a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java +++ b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java @@ -7,18 +7,14 @@ import frc.team2412.robot.subsystems.LauncherSubsystem; public class LauncherDiagnosticCommand extends SequentialCommandGroup { - private final LauncherSubsystem launcherSubsystem; - private final double Angle; - public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) { - this.launcherSubsystem = launcherSubsystem; - this.Angle = launcherSubsystem.getAngle(); + double angle = launcherSubsystem.getAngle(); addCommands( - new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 10), + new SetAngleLaunchCommand(launcherSubsystem, 500, angle + 10), new WaitCommand(2), - new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 20), + new SetAngleLaunchCommand(launcherSubsystem, 500, angle + 20), new WaitCommand(1), - new SetAngleLaunchCommand(launcherSubsystem, 500, Angle), + new SetAngleLaunchCommand(launcherSubsystem, 500, angle), new WaitCommand(5), new StopLauncherCommand(launcherSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java index 933117c2..9f7b71f7 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -31,8 +31,8 @@ public void execute() { * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), 0.0); Rotation2d turn = - new Rotation2d() - .fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); + Rotation2d.fromDegrees( + 2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); drivebaseSubsystem.drive(move, turn, false); } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 32e841b3..37520fd0 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -9,9 +9,6 @@ public class AllInCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final Controls controls; - private boolean rumbledIntakeFront = false; - private boolean rumbledIntakeLeft = false; - private boolean rumbledIntakeRight = false; private boolean rumbledIndex = false; public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { @@ -24,7 +21,7 @@ public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { public void initialize() { intakeSubsystem.intakeIn(); intakeSubsystem.indexIn(); - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override @@ -39,7 +36,7 @@ public void execute() { } if (controls != null && !rumbledIndex) { - Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + Commands.race(new RumbleCoDriveControllerCommand(controls), new WaitCommand(3)).schedule(); rumbledIndex = true; } } @@ -56,8 +53,13 @@ public void end(boolean interrupted) { intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); + if (interrupted) { + return; + } + if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + Commands.race(new RumbleCoDriveControllerCommand(controls), new WaitCommand(1)).schedule(); + Commands.race(new RumbleDriveControllerCommand(controls), new WaitCommand(1)).schedule(); } // rumbledIntakeFront = false; diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java index 17e459e9..9486c837 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java @@ -15,7 +15,7 @@ public AllInNoSensorCommand(IntakeSubsystem intakeSubsystem) { public void initialize() { intakeSubsystem.intakeIn(); intakeSubsystem.indexIn(); - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java index d1de3762..3b172efb 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java @@ -15,7 +15,7 @@ public AllInSensorOverrideCommand(IntakeSubsystem intakeSubsystem) { public void initialize() { intakeSubsystem.intakeIn(); intakeSubsystem.indexIn(); - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java index 38ac9166..e789a5dd 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java @@ -13,7 +13,7 @@ public FeederInCommand(IntakeSubsystem intakeSubsystem) { @Override public void initialize() { - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/RumbleCoDriveControllerCommand.java similarity index 81% rename from src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java rename to src/main/java/frc/team2412/robot/commands/intake/RumbleCoDriveControllerCommand.java index b0b0d842..151d222d 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/RumbleCoDriveControllerCommand.java @@ -4,10 +4,10 @@ import frc.team2412.robot.Controls; import frc.team2412.robot.Controls.ControlConstants; -public class RumbleCommand extends Command { +public class RumbleCoDriveControllerCommand extends Command { private final Controls controls; - public RumbleCommand(Controls controls) { + public RumbleCoDriveControllerCommand(Controls controls) { this.controls = controls; } diff --git a/src/main/java/frc/team2412/robot/commands/intake/RumbleDriveControllerCommand.java b/src/main/java/frc/team2412/robot/commands/intake/RumbleDriveControllerCommand.java new file mode 100644 index 00000000..291da267 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/RumbleDriveControllerCommand.java @@ -0,0 +1,28 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.Controls; +import frc.team2412.robot.Controls.ControlConstants; + +public class RumbleDriveControllerCommand extends Command { + private final Controls controls; + + public RumbleDriveControllerCommand(Controls controls) { + this.controls = controls; + } + + @Override + public void initialize() { + controls.vibrateDriveController(ControlConstants.RUMBLE_VIBRATION); + } + // exist + @Override + public void end(boolean interrupted) { + controls.vibrateDriveController(0.0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java index c0f6c347..11bcd66e 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java @@ -2,8 +2,8 @@ import static frc.team2412.robot.Subsystems.SubsystemConstants.*; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -23,9 +23,13 @@ public class FullTargetCommand extends Command { private static final InterpolatingTreeMap LAUNCHER_DATA = LauncherDataLoader.fromCSV( FileSystems.getDefault() - .getPath(Filesystem.getDeployDirectory().getPath(), "launcher_data.csv")); - private static final double YAW_TARGET_VIBRATION_TOLERANCE = 10; // degrees - private Pose2d SPEAKER_POSE; + .getPath( + Filesystem.getDeployDirectory().getPath(), + LauncherSubsystem.USE_THROUGHBORE + ? "launcher_data_throughbore.csv" + : "launcher_data_lamprey.csv")); + + private Translation2d SPEAKER_POSITION; private DrivebaseSubsystem drivebaseSubsystem; private LauncherSubsystem launcherSubsystem; @@ -51,20 +55,24 @@ public FullTargetCommand( public void initialize() { CommandScheduler.getInstance().schedule(yawAlignmentCommand); - SPEAKER_POSE = - DriverStation.getAlliance().get().equals(Alliance.Blue) - ? new Pose2d(0.0, 5.55, Rotation2d.fromRotations(0)) - : new Pose2d(16.5, 5.55, Rotation2d.fromRotations(0)); + SPEAKER_POSITION = + DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Blue + ? new Translation2d(0.0, 5.55) + : new Translation2d(16.5, 5.55); } @Override public void execute() { - Pose2d robotPose = drivebaseSubsystem.getPose(); - Pose2d relativeSpeaker = robotPose.relativeTo(SPEAKER_POSE); - yawTarget = - Rotation2d.fromRadians( - Math.atan2(relativeSpeaker.getY(), relativeSpeaker.getX()) + Math.PI); - double distance = relativeSpeaker.getTranslation().getNorm(); + // look ahead half a second into the future + var fieldSpeed = drivebaseSubsystem.getFieldSpeeds().times(0.5); + Translation2d robotPosition = + drivebaseSubsystem + .getPose() + .getTranslation() + .plus(new Translation2d(fieldSpeed.vxMetersPerSecond, fieldSpeed.vyMetersPerSecond)); + Translation2d robotToSpeaker = SPEAKER_POSITION.minus(robotPosition); + yawTarget = robotToSpeaker.getAngle(); + double distance = robotToSpeaker.getNorm(); LauncherDataPoint dataPoint = LAUNCHER_DATA.get(distance); launcherSubsystem.setAngleWithOffset(dataPoint.angle); launcherSubsystem.launch(dataPoint.rpm); diff --git a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java index d7b04974..daf62b08 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java @@ -2,22 +2,42 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.subsystems.LauncherSubsystem; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; // this command adjusts the angle using a speed that is input by the user public class ManualAngleCommand extends Command { private final LauncherSubsystem launcherSubsystem; private final DoubleSupplier launcherAngle; + private final BooleanSupplier powerControl; + private final BooleanSupplier ignoreLimits; - public ManualAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) { + public ManualAngleCommand( + LauncherSubsystem launcherSubsystem, + DoubleSupplier angleSpeed, + BooleanSupplier powerControl, + BooleanSupplier ignoreLimits) { this.launcherSubsystem = launcherSubsystem; this.launcherAngle = angleSpeed; + this.powerControl = powerControl; + this.ignoreLimits = ignoreLimits; addRequirements(launcherSubsystem); } + @Override + public void initialize() { + launcherSubsystem.resetManualAngleSetpoint(); + } + @Override public void execute() { - launcherSubsystem.setAngleManual(launcherAngle.getAsDouble()); + launcherSubsystem.setAngleManual( + launcherAngle.getAsDouble(), powerControl.getAsBoolean(), ignoreLimits.getAsBoolean()); + } + + @Override + public void end(boolean interrupted) { + launcherSubsystem.restoreLimits(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java index feca4ddd..e355294d 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.team2412.robot.Controls; import frc.team2412.robot.Robot; -import frc.team2412.robot.commands.intake.RumbleCommand; +import frc.team2412.robot.commands.intake.RumbleCoDriveControllerCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; // this command can be used as a preset in controls, allowing the user to input a speed and angle // value when they keybind it multiple times. @@ -39,6 +39,6 @@ public boolean isFinished() { public void end(boolean interrupted) { Robot robot = Robot.getInstance(); Controls controls = robot.controls; - Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); + Commands.race(new RumbleCoDriveControllerCommand(controls), new WaitCommand(1)).schedule(); } } diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java new file mode 100644 index 00000000..8b965412 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java @@ -0,0 +1,51 @@ +package frc.team2412.robot.commands.launcher; + +import edu.wpi.first.math.filter.MedianFilter; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.LauncherSubsystem; + +public class SyncPivotRelativeEncoderCommand extends Command { + + private int MAX_SAMPLES = 50; // 50 samples/second + private final double ANGLE_SPEED_TOLERANCE = 0.1; + private final MedianFilter filter; + + private final LauncherSubsystem launcherSubsystem; + + private int samples; + + public SyncPivotRelativeEncoderCommand(LauncherSubsystem launcherSubsystem) { + this.launcherSubsystem = launcherSubsystem; + filter = new MedianFilter(MAX_SAMPLES); + samples = 0; + } + + @Override + public void execute() { + + // if the pivot is moving, reset the syncing because now inaccurate. + if (Math.abs(launcherSubsystem.getAngleSpeed()) >= ANGLE_SPEED_TOLERANCE) { + filter.reset(); + samples = 0; + return; + } + + // add current value to median filter + filter.calculate(launcherSubsystem.getAngle()); + samples++; + } + + @Override + public void end(boolean interrupted) { + + if (samples > MAX_SAMPLES) { + launcherSubsystem.zeroRelativeEncoder(filter.lastValue()); + } + } + + @Override + public boolean isFinished() { + // stop when finished sampling for avg + return samples > MAX_SAMPLES; + } +} diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index b2b4f1c4..3aa74044 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -40,8 +40,6 @@ *

2D field poses are just the projection of the 3D pose onto the XY plane. */ public class AprilTagsProcessor { - private static class FilterInfo {} - public static final Transform3d ROBOT_TO_CAM = new Transform3d( Units.inchesToMeters(27.0 / 2.0 - 0.94996), @@ -50,7 +48,8 @@ private static class FilterInfo {} new Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(-30), 0)); // TODO Measure these - private static final Vector STANDARD_DEVS = VecBuilder.fill(1, 1, Units.degreesToRadians(30)); + private static final Vector STANDARD_DEVS = + VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(30)); private static final double MAX_POSE_AMBIGUITY = 0.1; @@ -81,7 +80,6 @@ private static boolean resultIsValid(PhotonPipelineResult result) { private final PhotonPoseEstimator photonPoseEstimator; private final DrivebaseWrapper aprilTagsHelper; private final FieldObject2d rawVisionFieldObject; - private final FieldObject2d adjustedFieldObject; // These are always set with every pipeline result private double lastRawTimestampSeconds = 0; @@ -101,7 +99,6 @@ private static boolean resultIsValid(PhotonPipelineResult result) { public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { this.aprilTagsHelper = aprilTagsHelper; rawVisionFieldObject = aprilTagsHelper.getField().getObject("RawVision"); - adjustedFieldObject = aprilTagsHelper.getField().getObject("AdjustedVision"); var networkTables = NetworkTableInstance.getDefault(); // if (Robot.isSimulation()) { // networkTables.stopServer(); @@ -159,11 +156,8 @@ public void update() { if (latestPose.isPresent()) { lastValidTimestampSeconds = latestPose.get().timestampSeconds; lastFieldPose = latestPose.get().estimatedPose.toPose2d(); - var oldPose = aprilTagsHelper.getEstimatedPosition(); - var adjustedPose = new Pose2d(lastFieldPose.getTranslation(), oldPose.getRotation()); rawVisionFieldObject.setPose(lastFieldPose); - adjustedFieldObject.setPose(adjustedPose); - aprilTagsHelper.addVisionMeasurement(adjustedPose, lastValidTimestampSeconds, STANDARD_DEVS); + aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastValidTimestampSeconds, STANDARD_DEVS); var estimatedPose = aprilTagsHelper.getEstimatedPosition(); aprilTagsHelper.getField().setRobotPose(estimatedPose); photonPoseEstimator.setLastPose(estimatedPose); diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 3e6fc02a..9c54c813 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -32,6 +32,7 @@ import java.io.File; import java.util.EnumSet; import java.util.Map; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import swervelib.SwerveDrive; @@ -49,7 +50,7 @@ public class DrivebaseSubsystem extends SubsystemBase { Robot.getInstance().getRobotType() == RobotType.BONK ? 3.0 : Robot.getInstance().getRobotType() == RobotType.COMPETITION - ? 4.7 + ? 6.0 : Robot.getInstance().getRobotType() == RobotType.CRANE ? 3.0 : 1.0; // Auto align stuff, dw abt it @@ -71,13 +72,13 @@ public class DrivebaseSubsystem extends SubsystemBase { private static final PIDConstants AUTO_TRANSLATION_PID = Robot.getInstance().getRobotType() == RobotType.COMPETITION - ? new PIDConstants(5, 0, 0.5) // practice + ? new PIDConstants(5.3, 0, 0.6) // practice : Robot.getInstance().getRobotType() == RobotType.BONK ? new PIDConstants(6, 0, 0.1) // bonk : Robot.getInstance().getRobotType() == RobotType.CRANE ? new PIDConstants(3.9, 0, 0.2) // crane : new PIDConstants(0.1, 0, 0.1); // bobot TODO: tune - private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(4.0, 0, 0.2); + private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(5.5, 0, 0); private static final double MAX_AUTO_SPEED = 500.0; // this seems to only affect rotation for some reason @@ -91,9 +92,11 @@ public class DrivebaseSubsystem extends SubsystemBase { private GenericEntry headingCorrectionEntry; private GenericEntry translationSpeedEntry; private GenericEntry rotationSpeedEntry; + private GenericEntry turboRotationMultiplierEntry; private GenericEntry xWheelsEntry; private GenericEntry flipTranslationEntry; + @SuppressWarnings("StaticAssignmentInConstructor") public DrivebaseSubsystem() { initShuffleboard(); @@ -188,13 +191,19 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO * @param rotation Rotation2d value of robot rotation. CW is positive TODO: is this true? */ public Command driveJoystick( - DoubleSupplier forward, DoubleSupplier strafe, Supplier rotation) { + DoubleSupplier forward, + DoubleSupplier strafe, + Supplier rotation, + BooleanSupplier turboRotation) { return this.run( () -> { Rotation2d constrainedRotation = Rotation2d.fromRotations( SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND) * MAX_SPEED + * (turboRotation.getAsBoolean() + ? turboRotationMultiplierEntry.getDouble(1.0) + : 1) * rotationSpeedEntry.getDouble(1.0) * -1); Translation2d constrainedTranslation = @@ -248,6 +257,10 @@ public ChassisSpeeds getRobotSpeeds() { return swerveDrive.getRobotVelocity(); } + public ChassisSpeeds getFieldSpeeds() { + return swerveDrive.getFieldVelocity(); + } + /** Set the robot's pose. TODO: does this change yaw too? does this affect field oriented? */ public void setPose(Pose2d pose) { swerveDrive.resetOdometry(pose); @@ -322,6 +335,13 @@ private void initShuffleboard() { .withSize(2, 1) .withProperties(Map.of("Min", 0.0)) .getEntry(); + turboRotationMultiplierEntry = + drivebaseTab + .addPersistent("Turbo rotation multiplier", 1.0) + .withWidget(BuiltInWidgets.kNumberSlider) + .withSize(2, 1) + .withProperties(Map.of("Min", 0.5, "Max", 5.0)) + .getEntry(); xWheelsEntry = drivebaseTab .addPersistent("X Wheels", xWheelsEnabled) @@ -437,6 +457,23 @@ private SysIdRoutine getAngleSysIdRoutine() { this)); } + public Command debugDriveFullPower() { + return this.runEnd( + () -> { + for (SwerveModule module : swerveDrive.getModules()) { + module.getDriveMotor().set(1.0); + module.setAngle(0); + } + }, + () -> { + for (SwerveModule module : swerveDrive.getModules()) { + module.getDriveMotor().set(0.0); + module.setAngle(0); + } + }) + .withName("DriveFullPower"); + } + public Command driveSysIdQuasistatic(SysIdRoutine.Direction direction) { return getDriveSysIdRoutine().quasistatic(direction); } diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 266e67fc..ec0da9c4 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -25,17 +25,26 @@ public class IntakeSubsystem extends SubsystemBase { public static final double INTAKE_REJECT_SPEED = -0.4; public static final double INDEX_UPPER_IN_SPEED = 1.0; - public static final double INDEX_UPPER_REVERSE_SPEED = -0.3; + public static final double INDEX_UPPER_REVERSE_SPEED = -1.0; public static final double INGEST_LOWER_IN_SPEED = 0.8; - public static final double INGEST_LOWER_REVERSE_SPEED = -0.3; + public static final double INGEST_LOWER_REVERSE_SPEED = -1.0; public static final double FEEDER_SHOOT_SPEED = 1.0; - public static final double FEEDER_IN_SPEED = 0.65; - public static final double FEEDER_REVERSE_SPEED = -0.3; + public static final double FEEDER_IN_SPEED = 1.0; + public static final double FEEDER_REVERSE_SPEED = -1.0; - private static final boolean enableFrontAndSideIntakes = false; + private static final boolean enableFrontAndSideIntakes = true; + + @SuppressWarnings("ComplexBooleanConstant") + private static final boolean FRONT_MOTOR_ENABLED = enableFrontAndSideIntakes && false; + + @SuppressWarnings("ComplexBooleanConstant") + private static final boolean LEFT_MOTOR_ENABLED = enableFrontAndSideIntakes && true; + + @SuppressWarnings("ComplexBooleanConstant") + private static final boolean RIGHT_MOTOR_ENABLED = enableFrontAndSideIntakes && true; // Motors private final CANSparkMax intakeMotorFront; @@ -51,12 +60,18 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkLimitSwitch indexSensor; private final SparkLimitSwitch feederSensor; private final DigitalInput feederSensorIR; - private final SparkLimitSwitch intakeFrontSensor; + // private final SparkLimitSwitch intakeFrontSensor; // debounce ! ! + @SuppressWarnings("UnusedVariable") private final Debouncer intakeFrontSensorDebouncer; + + @SuppressWarnings("UnusedVariable") private final Debouncer intakeRightSensorDebouncer; + + @SuppressWarnings("UnusedVariable") private final Debouncer intakeLeftSensorDebouncer; + private final Debouncer feederSensorDebouncer; private final Debouncer feederSensorIRDebouncer; @@ -80,13 +95,14 @@ public class IntakeSubsystem extends SubsystemBase { // reject override private GenericEntry rejectOverride; - private boolean feederSensorSignal; - public IntakeSubsystem() { - intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless); - intakeMotorLeft = new CANSparkMax(INTAKE_MOTOR_LEFT, MotorType.kBrushless); - intakeMotorRight = new CANSparkMax(INTAKE_MOTOR_RIGHT, MotorType.kBrushless); + intakeMotorFront = + FRONT_MOTOR_ENABLED ? new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless) : null; + intakeMotorLeft = + LEFT_MOTOR_ENABLED ? new CANSparkMax(INTAKE_MOTOR_LEFT, MotorType.kBrushless) : null; + intakeMotorRight = + RIGHT_MOTOR_ENABLED ? new CANSparkMax(INTAKE_MOTOR_RIGHT, MotorType.kBrushless) : null; ingestMotor = new CANSparkFlex(INGEST_MOTOR, MotorType.kBrushless); indexMotorUpper = new CANSparkFlex(INDEX_MOTOR_UPPER, MotorType.kBrushless); @@ -97,19 +113,26 @@ public IntakeSubsystem() { feederSensor = feederMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); feederSensorIR = new DigitalInput(FEEDER_SENSOR); - intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakeFrontSensor = + // intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // intakeBackSensor = // intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeLeftSensor = + LEFT_MOTOR_ENABLED + ? intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen) + : null; + intakeRightSensor = + RIGHT_MOTOR_ENABLED + ? intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen) + : null; // sensor must be true for 0.1 seconds before being actually true intakeFrontSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); intakeRightSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); intakeLeftSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); - feederSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); - feederSensorIRDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); + feederSensorDebouncer = new Debouncer(0.01, Debouncer.DebounceType.kBoth); + feederSensorIRDebouncer = new Debouncer(0.01, Debouncer.DebounceType.kBoth); resetMotors(); @@ -137,9 +160,15 @@ private void configureMotor(CANSparkBase motor, boolean invert) { } private void resetMotors() { - configureMotor(intakeMotorFront, true); - configureMotor(intakeMotorLeft, true); - configureMotor(intakeMotorRight, true); + if (FRONT_MOTOR_ENABLED) { + configureMotor(intakeMotorFront, 25, true); + } + if (LEFT_MOTOR_ENABLED) { + configureMotor(intakeMotorLeft, 25, true); + } + if (RIGHT_MOTOR_ENABLED) { + configureMotor(intakeMotorRight, 25, true); + } configureMotor(ingestMotor, true); configureMotor(indexMotorUpper, 40, false); @@ -151,9 +180,15 @@ private void resetMotors() { public void intakeSet(double speed) { if (enableFrontAndSideIntakes) { - intakeMotorFront.set(speed); - intakeMotorLeft.set(speed); - intakeMotorRight.set(speed); + if (FRONT_MOTOR_ENABLED) { + intakeMotorFront.set(speed); + } + if (LEFT_MOTOR_ENABLED) { + intakeMotorLeft.set(speed); + } + if (RIGHT_MOTOR_ENABLED) { + intakeMotorRight.set(speed); + } } ingestMotor.set(speed); } @@ -170,9 +205,15 @@ public void intakeReverse() { public void intakeSteal() { if (enableFrontAndSideIntakes) { - intakeMotorLeft.set(INTAKE_IN_SPEED); - intakeMotorRight.set(INTAKE_IN_SPEED); - intakeMotorFront.set(INTAKE_REJECT_SPEED); + if (LEFT_MOTOR_ENABLED) { + intakeMotorLeft.set(INTAKE_IN_SPEED); + } + if (RIGHT_MOTOR_ENABLED) { + intakeMotorRight.set(INTAKE_IN_SPEED); + } + if (FRONT_MOTOR_ENABLED) { + intakeMotorFront.set(INTAKE_REJECT_SPEED); + } } ingestMotor.set(INTAKE_REJECT_SPEED); indexMotorUpper.set(INTAKE_IN_SPEED); @@ -184,19 +225,19 @@ public void intakeStop() { } public void intakeFrontStop() { - if (enableFrontAndSideIntakes) { + if (FRONT_MOTOR_ENABLED) { intakeMotorFront.set(0); } } public void intakeLeftStop() { - if (enableFrontAndSideIntakes) { + if (LEFT_MOTOR_ENABLED) { intakeMotorLeft.set(0); } } public void intakeRightStop() { - if (enableFrontAndSideIntakes) { + if (RIGHT_MOTOR_ENABLED) { intakeMotorRight.set(0); } } @@ -207,19 +248,19 @@ public void intakeReject() { } public void intakeFrontReject() { - if (enableFrontAndSideIntakes) { + if (FRONT_MOTOR_ENABLED) { intakeMotorFront.set(INTAKE_REJECT_SPEED); } } public void intakeLeftReject() { - if (enableFrontAndSideIntakes) { + if (LEFT_MOTOR_ENABLED) { intakeMotorLeft.set(INTAKE_REJECT_SPEED); } } public void intakeRightReject() { - if (enableFrontAndSideIntakes) { + if (RIGHT_MOTOR_ENABLED) { intakeMotorRight.set(INTAKE_REJECT_SPEED); } } @@ -239,7 +280,7 @@ public void indexStop() { } // feeder methods - public void feederIn() { + public void feedUntilNoteLaunched() { feederMotor.set(setFeederInSpeedEntry.getDouble(FEEDER_IN_SPEED)); } @@ -274,7 +315,8 @@ public boolean feederSensorHasNote() { } public boolean intakeFrontSeesNote() { - return intakeFrontSensor.isPressed(); + return false; + // return intakeFrontSensor.isPressed(); } public boolean intakeLeftSeesNote() { @@ -290,21 +332,24 @@ public boolean debouncedIntakeFrontSensor() { // if (intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed())) { // return true; // } - return intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + return false; + // return intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } public boolean debouncedIntakeLeftSensor() { // if (intakeLeftSensorDebouncer.calculate(intakeLeftSensor.isPressed())) { // return true; // } - return intakeLeftSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + return false; + // return intakeLeftSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } public boolean debouncedIntakeRightSensor() { // if (intakeRightSensorDebouncer.calculate(intakeRightSensor.isPressed())) { // return true; // } - return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + return false; + // return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } // override methods on shuffleboard @@ -317,25 +362,43 @@ public boolean getRejectOverride() { } public boolean isIntakeOn() { - return (intakeMotorFront.get() > 0 + return ((LEFT_MOTOR_ENABLED && intakeMotorLeft.get() > 0) || indexMotorUpper.get() > 0 || ingestMotor.get() > 0 || feederMotor.get() > 0); } + public boolean isLeftIntakeRunning() { + return Math.abs(intakeMotorLeft.getEncoder().getVelocity()) > 1.0; + } + + public boolean isRightIntakeRunning() { + return Math.abs(intakeMotorRight.getEncoder().getVelocity()) > 1.0; + } + + public boolean isIndexRunning() { + return Math.abs(indexMotorUpper.getEncoder().getVelocity()) > 1.0; + } + // logging public void initShuffleboard() { if (Robot.isDebugMode()) { shuffleboardTab - .addDouble("Front Intake Motor Temp", () -> intakeMotorFront.getMotorTemperature()) + .addDouble( + "Front Intake Motor Temp", + () -> FRONT_MOTOR_ENABLED ? intakeMotorFront.getMotorTemperature() : -1) .withSize(1, 1) .withWidget(BuiltInWidgets.kTextView); shuffleboardTab - .addDouble("Left Intake Motor Temp", () -> intakeMotorLeft.getMotorTemperature()) + .addDouble( + "Left Intake Motor Temp", + () -> LEFT_MOTOR_ENABLED ? intakeMotorLeft.getMotorTemperature() : -1) .withSize(1, 1) .withWidget(BuiltInWidgets.kTextView); shuffleboardTab - .addDouble("Right Intake Motor Temp", () -> intakeMotorRight.getMotorTemperature()) + .addDouble( + "Right Intake Motor Temp", + () -> RIGHT_MOTOR_ENABLED ? intakeMotorRight.getMotorTemperature() : -1) .withSize(1, 1) .withWidget(BuiltInWidgets.kTextView); shuffleboardTab @@ -399,8 +462,7 @@ public void initShuffleboard() { * returns true if all the motors are set to be not moving */ public boolean isIntakeRunning() { - return (intakeMotorFront.get() != 0 - && intakeMotorLeft.get() != 0 - && intakeMotorRight.get() != 0); + return (!LEFT_MOTOR_ENABLED || intakeMotorLeft.get() != 0) + && (!RIGHT_MOTOR_ENABLED || intakeMotorRight.get() != 0); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index f28bb5ce..31d5871b 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -7,7 +7,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; @@ -29,18 +28,29 @@ import frc.team2412.robot.Robot; import frc.team2412.robot.util.SparkPIDWidget; import java.util.Map; +import java.util.Optional; public class LauncherSubsystem extends SubsystemBase { // CONSTANTS + public static final boolean USE_THROUGHBORE = true; + // HARDWARE private static final double PIVOT_GEARING_RATIO = 1.0 / 180.0; + private static final double PIVOT_TO_ENCODER_GEARING_RATIO = 1.0 / 2.0; private static final float PIVOT_SOFTSTOP_FORWARD = 0.93f; private static final float PIVOT_SOFTSTOP_BACKWARD = 0.635f; + private static final float PIVOT_SOFTSTOP_FORWARD_THROUGHBORE = 0.93f; + private static final float PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE = 0.38f; private static final float PIVOT_DISABLE_OFFSET = 0.04f; - private static final int PIVOT_OFFSET = 36; + private static final int PIVOT_OFFSET = USE_THROUGHBORE ? 40 : 36; + + // offset stuff + private static final double ENCODER_DIFFERENCE_TOLERANCE = 15; + private static final double OFFSET_SYNCING_TOLERANCE = 0.06; + // ANGLE VALUES - public static final int AMP_AIM_ANGLE = 288 + PIVOT_OFFSET; + public static final int AMP_AIM_ANGLE = 290 + PIVOT_OFFSET; public static final int SUBWOOFER_AIM_ANGLE = 252 + PIVOT_OFFSET; public static final int PODIUM_AIM_ANGLE = 238 + PIVOT_OFFSET; public static final int TRAP_AIM_ANGLE = 317 + PIVOT_OFFSET; @@ -57,8 +67,9 @@ public class LauncherSubsystem extends SubsystemBase { // RPM public static final int SPEAKER_SHOOT_SPEED_RPM = 4500; public static final int TRAP_SHOOT_SPEED_RPM = 4500; - public static final double ANGLE_MAX_SPEED = 1; - public static final double MAX_SET_ANGLE_OFFSET = 20; + public static final int LOBBING_RPM = 4700; + public static final double ANGLE_MAX_SPEED = 0.3; // percent output + public static final double MAX_SET_ANGLE_OFFSET = 15; // 3392 RPM = 50% Speed // 1356 RPM = 20% Speed // 1017 RPM = 15% Speed @@ -71,7 +82,9 @@ public class LauncherSubsystem extends SubsystemBase { private final RelativeEncoder launcherTopEncoder; private final RelativeEncoder launcherBottomEncoder; private final SparkAbsoluteEncoder launcherAngleEncoder; + private final SparkAbsoluteEncoder launcherAngleThroughboreEncoder; private final SparkPIDController launcherAngleOnePIDController; + private final SparkPIDController launcherAngleTwoPIDController; // private final SparkPIDController launcherAngleTwoPIDController; private final ArmFeedforward launcherPivotFF = new ArmFeedforward(0.40434, 0.096771, 0.0056403); // arm FF values: @@ -88,6 +101,9 @@ public class LauncherSubsystem extends SubsystemBase { private double rpmSetpoint; private double angleSetpoint; private double manualAngleSetpoint; + private boolean ignoreLimits; + + private Optional relativeEncoderStartPosition; private GenericEntry setLauncherSpeedEntry; @@ -99,8 +115,6 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherAngleSpeedEntry; - private GenericEntry launcherIsAtSpeed; - private GenericEntry launcherAngleManual; private GenericEntry speakerDistanceEntry; @@ -111,6 +125,8 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherFlywheelSetpointEntry; + private GenericEntry launcherDisabledEntry; + // Constructors public LauncherSubsystem() { @@ -125,7 +141,10 @@ public LauncherSubsystem() { // encoders launcherTopEncoder = launcherTopMotor.getEncoder(); launcherBottomEncoder = launcherBottomMotor.getEncoder(); - launcherAngleEncoder = launcherAngleOneMotor.getAbsoluteEncoder(Type.kDutyCycle); + launcherAngleEncoder = + launcherAngleOneMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + launcherAngleThroughboreEncoder = + launcherAngleTwoMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); manualAngleSetpoint = launcherAngleEncoder.getPosition(); // PID controllers @@ -136,8 +155,10 @@ public LauncherSubsystem() { launcherBottomPIDController.setFeedbackDevice(launcherBottomEncoder); launcherAngleOnePIDController = launcherAngleOneMotor.getPIDController(); launcherAngleOnePIDController.setFeedbackDevice(launcherAngleEncoder); - // launcherAngleTwoPIDController = launcherAngleTwoMotor.getPIDController(); - // launcherAngleTwoPIDController.setFeedbackDevice(launcherAngleEncoder); + launcherAngleTwoPIDController = launcherAngleTwoMotor.getPIDController(); + launcherAngleTwoPIDController.setFeedbackDevice(launcherAngleThroughboreEncoder); + + relativeEncoderStartPosition = Optional.empty(); configMotors(); initShuffleboard(); @@ -163,20 +184,38 @@ public void configMotors() { launcherAngleOneMotor.setSmartCurrentLimit(100); launcherAngleTwoMotor.setSmartCurrentLimit(100); - launcherAngleOneMotor.setSoftLimit( - CANSparkBase.SoftLimitDirection.kForward, PIVOT_SOFTSTOP_FORWARD); - launcherAngleOneMotor.setSoftLimit( - CANSparkBase.SoftLimitDirection.kReverse, PIVOT_SOFTSTOP_BACKWARD); - launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); - launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + if (!USE_THROUGHBORE) { + launcherAngleOneMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kForward, PIVOT_SOFTSTOP_FORWARD); + launcherAngleOneMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kReverse, PIVOT_SOFTSTOP_BACKWARD); + launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); - launcherAngleTwoMotor.follow(launcherAngleOneMotor, true); + launcherAngleTwoMotor.follow(launcherAngleOneMotor, true); + + launcherAngleOnePIDController.setP(5.2); + launcherAngleOnePIDController.setI(0); + launcherAngleOnePIDController.setD(0.066248); + launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); + } else { + launcherAngleTwoMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kForward, PIVOT_SOFTSTOP_FORWARD_THROUGHBORE); + launcherAngleTwoMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kReverse, PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE); + launcherAngleTwoMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + launcherAngleTwoMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + + launcherAngleTwoMotor.setInverted(true); + launcherAngleOneMotor.follow(launcherAngleTwoMotor, true); + + launcherAngleTwoPIDController.setP(5.2); + launcherAngleTwoPIDController.setI(0); + launcherAngleTwoPIDController.setD(0.066248); + launcherAngleTwoPIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); + } // PID - launcherAngleOnePIDController.setP(5.2); - launcherAngleOnePIDController.setI(0); - launcherAngleOnePIDController.setD(0.066248); - launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); launcherTopPIDController.setP(0.002); // 7.7633E-05); launcherTopPIDController.setI(0); launcherTopPIDController.setD(0.001); @@ -192,6 +231,9 @@ public void configMotors() { launcherAngleTwoMotor.getEncoder().setPosition(launcherAngleEncoder.getPosition()); launcherAngleTwoMotor.getEncoder().setPositionConversionFactor(PIVOT_GEARING_RATIO); + // launcherAngleOneMotor.setIdleMode(IdleMode.kCoast); + // launcherAngleTwoMotor.setIdleMode(IdleMode.kCoast); + launcherTopMotor.burnFlash(); launcherBottomMotor.burnFlash(); launcherAngleOneMotor.burnFlash(); @@ -227,14 +269,29 @@ public void ampLaunch(double speed) { public double getLauncherSpeed() { return launcherTopEncoder.getVelocity(); } + + public double convertEncoderRotationsToPivotRotations(double encoderRotations) { + return 1 - PIVOT_TO_ENCODER_GEARING_RATIO * (1 - encoderRotations); + } + + public double convertPivotRotationsToEncoderRotations(double pivotRotations) { + return 1 - (1 - pivotRotations) / PIVOT_TO_ENCODER_GEARING_RATIO; + } + // returns the degrees of the angle of the launcher public double getAngle() { - // get position returns a double in the form of rotations - return Units.rotationsToDegrees(launcherAngleEncoder.getPosition()); + return Units.rotationsToDegrees(getPosition()); + } + + public double getPosition() { + if (!USE_THROUGHBORE) { + return launcherAngleEncoder.getPosition(); + } + return convertEncoderRotationsToPivotRotations(launcherAngleThroughboreEncoder.getPosition()); } /** - * Sets the launcher angle, taking the offset into account. + * s Sets the launcher angle, taking the offset into account. * * @param launcherAngle Launcher angle. PIVOT_OFFSET will be added to this. */ @@ -248,18 +305,26 @@ public void setAngle(double launcherAngle) { } else { angleSetpoint = launcherAngle; } - launcherAngleOnePIDController.setReference( - Units.degreesToRotations(angleSetpoint), - ControlType.kPosition, - 0, - launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + if (!USE_THROUGHBORE) { + launcherAngleOnePIDController.setReference( + Units.degreesToRotations(angleSetpoint), + ControlType.kPosition, + 0, + launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + } else { + launcherAngleTwoPIDController.setReference( + convertPivotRotationsToEncoderRotations(Units.degreesToRotations(angleSetpoint)), + ControlType.kPosition, + 0, + launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + } manualAngleSetpoint = Units.degreesToRotations(launcherAngle); // launcherAngleTwoPIDController.setReference( // Units.degreesToRotations(angleSetpoint), ControlType.kPosition); } public boolean isAtAngle(double tolerance) { - return MathUtil.isNear(angleSetpoint, launcherAngleEncoder.getPosition(), tolerance); + return MathUtil.isNear(angleSetpoint, getAngle(), tolerance); } public boolean isAtAngle() { @@ -278,7 +343,26 @@ public double getAngleSpeed() { return launcherAngleEncoder.getVelocity(); } - public void setAngleManual(double joystickInput) { + public void restoreLimits() { + this.ignoreLimits = false; + } + + public void setAngleManual(double joystickInput, boolean powerControl, boolean ignoreLimits) { + this.ignoreLimits = ignoreLimits; + if (powerControl || ignoreLimits) { + if (!USE_THROUGHBORE) { + launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.5 : joystickInput); + } else { + launcherAngleTwoMotor.set(ignoreLimits ? joystickInput * 0.5 : joystickInput); + } + manualAngleSetpoint = + MathUtil.clamp( + Units.degreesToRotations(getAngle()), + PIVOT_SOFTSTOP_BACKWARD, + PIVOT_SOFTSTOP_FORWARD); + return; + } + manualAngleSetpoint = MathUtil.clamp( manualAngleSetpoint + joystickInput * MANUAL_MODIFIER, @@ -287,22 +371,39 @@ public void setAngleManual(double joystickInput) { if (Units.degreesToRotations(getAngle()) > PIVOT_SOFTSTOP_BACKWARD && Units.degreesToRotations(getAngle()) < PIVOT_SOFTSTOP_FORWARD) { - launcherAngleOnePIDController.setReference( - manualAngleSetpoint, - ControlType.kPosition, - 0, - launcherPivotFF.calculate( - Units.degreesToRadians( - Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), - 0)); + if (!USE_THROUGHBORE) { + launcherAngleOnePIDController.setReference( + manualAngleSetpoint, + ControlType.kPosition, + 0, + launcherPivotFF.calculate( + Units.degreesToRadians( + Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), + 0)); + } else { + launcherAngleTwoPIDController.setReference( + convertPivotRotationsToEncoderRotations(manualAngleSetpoint), + ControlType.kPosition, + 0, + launcherPivotFF.calculate( + Units.degreesToRadians( + Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), + 0)); + } } } private void initShuffleboard() { if (Robot.isDebugMode()) { - Shuffleboard.getTab("Launcher") - .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) - .withPosition(2, 0); + if (!USE_THROUGHBORE) { + Shuffleboard.getTab("Launcher") + .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) + .withPosition(2, 0); + } else { + Shuffleboard.getTab("Launcher") + .add(new SparkPIDWidget(launcherAngleTwoPIDController, "launcherAngleThroughborePID")) + .withPosition(2, 0); + } Shuffleboard.getTab("Launcher") .add(new SparkPIDWidget(launcherTopPIDController, "launcherTopPID")) .withPosition(0, 0); @@ -316,13 +417,18 @@ private void initShuffleboard() { .addDouble("Top FlyWheel Temp", () -> launcherTopMotor.getMotorTemperature()); } - launcherIsAtSpeed = - Shuffleboard.getTab("Match") - .add("flywheels at target speed", false) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kBooleanBox) - .withPosition(0, 2) - .getEntry(); + Shuffleboard.getTab("Launcher") + .addDouble( + "relative encoder", () -> launcherAngleOneMotor.getEncoder().getPosition() * 360); + Shuffleboard.getTab("Launcher") + .addDouble("relative encoder offset", () -> relativeEncoderStartPosition.orElse(0.0)); + + Shuffleboard.getTab("Launcher") + .addDouble( + "relative encoder (with offset)", + () -> + (launcherAngleOneMotor.getEncoder().getPosition() * 360 + + relativeEncoderStartPosition.orElse(0.0))); launcherAngleSpeedEntry = Shuffleboard.getTab("Launcher") .add("Launcher angle Speed", 0) @@ -353,8 +459,7 @@ private void initShuffleboard() { .withPosition(5, 0) .getEntry(); - setLauncherAngleEntry = - Shuffleboard.getTab("Launcher").add("Launcher Angle Setpoint", getAngle()).getEntry(); + Shuffleboard.getTab("Launcher").addDouble("Launcher Angle Setpoint", () -> manualAngleSetpoint); launcherAngleManual = Shuffleboard.getTab("Launcher") @@ -394,29 +499,83 @@ private void initShuffleboard() { launch(setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED_RPM)); }) .withName("Full Manual")); + + Shuffleboard.getTab("Launcher").addBoolean("Ignoring Limits", () -> ignoreLimits); + + launcherDisabledEntry = Shuffleboard.getTab("Launcher").add("Angle Insane", false).getEntry(); } public void updateDistanceEntry(double distance) { speakerDistanceEntry.setDouble(distance); } + public double getAngleOneMotorAngle() { + return Units.rotationsToDegrees(launcherAngleOneMotor.getEncoder().getPosition()) + + relativeEncoderStartPosition.orElse(0.0); + } + + public void zeroRelativeEncoder(double pivotAngle) { + + double currentRelativePosition = + Units.rotationsToDegrees(launcherAngleOneMotor.getEncoder().getPosition()); + double offset = pivotAngle - currentRelativePosition; + + if (relativeEncoderStartPosition.isEmpty() + || Math.abs(relativeEncoderStartPosition.orElse(0.0) - offset) > OFFSET_SYNCING_TOLERANCE) { + relativeEncoderStartPosition = Optional.of(offset); + } + } + + public void resetManualAngleSetpoint() { + manualAngleSetpoint = Units.degreesToRotations(getAngle()); + } + @Override public void periodic() { launcherAngleEntry.setDouble(getAngle()); launcherSpeedEntry.setDouble(getLauncherSpeed()); launcherAngleSpeedEntry.setDouble(getAngleSpeed()); - launcherIsAtSpeed.setBoolean(isAtSpeed()); launcherAngleManual.setDouble(manualAngleSetpoint); angleSetpointEntry.setDouble(angleSetpoint); launcherFlywheelSetpointEntry.setDouble(rpmSetpoint); + launcherDisabledEntry.setBoolean(false); + + // PIVOT ENCODER SANITY CHECKS + // compares the relative encoder angle vs the absolute encoder angle + if (relativeEncoderStartPosition.isPresent() + && Math.abs(getAngle() - getAngleOneMotorAngle()) >= ENCODER_DIFFERENCE_TOLERANCE) { + if (!ignoreLimits) { + launcherAngleOneMotor.disable(); + launcherAngleTwoMotor.disable(); + launcherDisabledEntry.setBoolean(true); + } + DriverStation.reportError( + "Pivot encoder deviated too far from motor encoder angle ... .. Reported pivot angle of " + + getAngle() + + " and motor angle of " + + getAngleOneMotorAngle() + + ". Is overidden: " + + ignoreLimits, + false); + } - // sanity check the pivot encoder - if (launcherAngleEncoder.getPosition() >= PIVOT_SOFTSTOP_FORWARD + PIVOT_DISABLE_OFFSET - || launcherAngleEncoder.getPosition() <= PIVOT_SOFTSTOP_BACKWARD - PIVOT_DISABLE_OFFSET) { - launcherAngleOneMotor.disable(); + if (getPosition() + >= (USE_THROUGHBORE ? PIVOT_SOFTSTOP_FORWARD_THROUGHBORE : PIVOT_SOFTSTOP_FORWARD) + + PIVOT_DISABLE_OFFSET + || launcherAngleEncoder.getPosition() + <= (USE_THROUGHBORE ? PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE : PIVOT_SOFTSTOP_BACKWARD) + - PIVOT_DISABLE_OFFSET) { + if (!ignoreLimits) { + launcherAngleOneMotor.disable(); + launcherAngleTwoMotor.disable(); + launcherDisabledEntry.setBoolean(true); + } DriverStation.reportError( - "Launcher encoder angle is insane!!!! Reports angle of " + getAngle() + " degrees.", - true); + "Launcher encoder angle is insane!!!! Reports angle of " + + getAngle() + + " degrees. Is overridden: " + + ignoreLimits, + false); } } diff --git a/src/main/java/frc/team2412/robot/util/AmpAlign.java b/src/main/java/frc/team2412/robot/util/AmpAlign.java index 654cb808..a07a0c2a 100644 --- a/src/main/java/frc/team2412/robot/util/AmpAlign.java +++ b/src/main/java/frc/team2412/robot/util/AmpAlign.java @@ -10,19 +10,27 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; import frc.team2412.robot.subsystems.DrivebaseSubsystem; +import frc.team2412.robot.subsystems.LauncherSubsystem; import java.util.List; public class AmpAlign { private static final Pose2d BLUE_AMP_POSES = // amp on the blue side - new Pose2d(new Translation2d(1.8, 7.3), Rotation2d.fromDegrees(270)); + + new Pose2d(new Translation2d(1.71, 7.65), Rotation2d.fromDegrees(270)); private static final Pose2d RED_AMP_POSES = // amp on the red side - new Pose2d(new Translation2d(14.5, 7.3), Rotation2d.fromDegrees(270)); + new Pose2d(new Translation2d(14.61, 7.65), Rotation2d.fromDegrees(270)); + + private static final double LAUNCHER_AMP_PRESET_DISTANCE = 2; + + private static Command ampAlign( + DrivebaseSubsystem drivebaseSubsystem, LauncherSubsystem launcherSubsystem) { - private static Command ampAlign(DrivebaseSubsystem drivebaseSubsystem) { Pose2d robotPose = drivebaseSubsystem.getPose(); boolean isBlue; if (!DriverStation.getAlliance().isEmpty()) { @@ -31,7 +39,7 @@ private static Command ampAlign(DrivebaseSubsystem drivebaseSubsystem) { isBlue = false; } // figures out which amp to go to - Pose2d ampPose = (isBlue) ? BLUE_AMP_POSES : RED_AMP_POSES; + Pose2d ampPose = isBlue ? BLUE_AMP_POSES : RED_AMP_POSES; // sets the point for the path to go to List bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, ampPose); // this is flipped @@ -50,25 +58,44 @@ private static Command ampAlign(DrivebaseSubsystem drivebaseSubsystem) { path = path.flipPath(); } - return AutoBuilder.followPath(path); + return AutoBuilder.followPath(path) + .deadlineWith( + Commands.waitUntil( + () -> + drivebaseSubsystem + .getPose() + .getTranslation() + .minus(ampPose.getTranslation()) + .getNorm() + < LAUNCHER_AMP_PRESET_DISTANCE) + .andThen( + new SetAngleAmpLaunchCommand( + launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.AMP_AIM_ANGLE))); } - public static Command ampPreset(DrivebaseSubsystem drivebaseSubsystem) { - return new AlignCommand(drivebaseSubsystem); + public static Command ampPreset( + DrivebaseSubsystem drivebaseSubsystem, LauncherSubsystem launcherSubsystem) { + return new AlignCommand(drivebaseSubsystem, launcherSubsystem); } private static class AlignCommand extends Command { private final DrivebaseSubsystem drivebaseSubsystem; + private final LauncherSubsystem launcherSubsystem; private Command ampCommand = null; - public AlignCommand(DrivebaseSubsystem drivebaseSubsystem) { + public AlignCommand( + DrivebaseSubsystem drivebaseSubsystem, LauncherSubsystem launcherSubsystem) { this.drivebaseSubsystem = drivebaseSubsystem; - addRequirements(drivebaseSubsystem); + this.launcherSubsystem = launcherSubsystem; + addRequirements(drivebaseSubsystem, launcherSubsystem); } @Override public void initialize() { - ampCommand = ampAlign(drivebaseSubsystem); + ampCommand = ampAlign(drivebaseSubsystem, launcherSubsystem); + ampCommand.initialize(); // launcherSubsystem.setAngle(LauncherSubsystem.TRAP_AIM_ANGLE); // launcherSubsystem.launch(LauncherSubsystem.TRAP_SHOOT_SPEED_RPM); diff --git a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java index e0c2ef47..1c63382c 100644 --- a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java +++ b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java @@ -64,7 +64,7 @@ public void addOption(String name, T object) { /** * Removes the given object from the list of options. * - * @param object the option + * @param name the name of the option */ public void removeOption(String name) { m_map.remove(name); @@ -115,6 +115,7 @@ public T getSelected() { * * @param listener The function to call that accepts the new value */ + @SuppressWarnings("LockNotBeforeTry") // Assigning to m_listener shouldn't error public void onChange(Consumer listener) { requireNonNullParam(listener, "listener", "onChange"); m_mutex.lock(); diff --git a/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java b/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java index d35cc748..fc3a09a0 100644 --- a/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java +++ b/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.math.interpolation.InverseInterpolator; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import frc.team2412.robot.Robot; import java.io.BufferedReader; @@ -40,7 +41,8 @@ public static InterpolatingTreeMap fromCSV(Path path) debugWriter.append(debugPrefix + msg); debugWriter.newLine(); } catch (IOException e) { - e.printStackTrace(); + DriverStation.reportWarning( + "Error writing to CSV interpreter log: " + e.getMessage(), e.getStackTrace()); } }; diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index d8290074..56e39318 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -20,6 +20,23 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - AutonomousField.configureShuffleboardTab(tab, 7, 0, "Available Auto Variants", r::addPeriodic); + AutonomousField.configureShuffleboardTab(tab, 6, 0, "Available Auto Variants", r::addPeriodic); + tab.addBoolean("Left Intake Running", s.intakeSubsystem::isLeftIntakeRunning) + .withPosition(0, 4) + .withSize(2, 1); + tab.addBoolean("Right Intake Running", s.intakeSubsystem::isRightIntakeRunning) + .withPosition(2, 4) + .withSize(2, 1); + tab.addBoolean("Index Running", s.intakeSubsystem::isIndexRunning) + .withPosition(4, 4) + .withSize(2, 1); + tab.addBoolean( + "Has Note", + () -> s.intakeSubsystem.indexSensorHasNote() || s.intakeSubsystem.feederSensorHasNote()) + .withPosition(6, 4) + .withSize(2, 1); + tab.addBoolean("Flywheels At Speed", () -> s.launcherSubsystem.isAtSpeed(400)) + .withPosition(8, 4) + .withSize(2, 1); } } diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index a8109423..eeaa138d 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -46,7 +46,7 @@ private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { isBlue = false; } // figures out which trap to go to - Pose2d trapPose = robotPose.nearest(List.of((isBlue) ? BLUE_TRAP_POSES : RED_TRAP_POSES)); + Pose2d trapPose = robotPose.nearest(List.of(isBlue ? BLUE_TRAP_POSES : RED_TRAP_POSES)); // sets the point for the path to go to List bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, trapPose); // this is flipped @@ -74,7 +74,10 @@ public static Command trapPreset( private static class AlignCommand extends Command { private final DrivebaseSubsystem drivebaseSubsystem; + + @SuppressWarnings("UnusedVariable") private final LauncherSubsystem launcherSubsystem; + private Command trapCommand = null; public AlignCommand( diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 02e247d9..9df1141e 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -49,7 +49,7 @@ public class AutoLogic { // rpm to rev up launcher before launching public static final double REV_RPM = 2500; - public static final double STAGE_ANGLE = 247; + public static final double STAGE_ANGLE = 262; public static enum StartPosition { AMP_SIDE_SUBWOOFER( @@ -86,6 +86,7 @@ public static enum StartPosition { // presets new AutoPath("Test Path Rotate", "5mForwardRotate180"), new AutoPath("Test Path", "DiameterTest"), + new AutoPath("Master PID Test", "MasterPIDTest"), new AutoPath("Tune Translational PID", "TuneTranslationalPID"), new AutoPath("Tune Rotational PID", "TuneRotationalPID"), new AutoPath("Stand Still", "PresetSourceSide1Score"), @@ -113,6 +114,7 @@ public static enum StartPosition { // presets new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), new AutoPath("Autoline N2 N1", "PresetMidAutoline3Score"), + new AutoPath("Autoline N2 N3", "PresetMidAutoline3Score2"), new AutoPath("Centerline N5 N4", "PresetSourceSideCenterline3Score2"), new AutoPath("Centerline N5 N3", "PresetSourceSideCenterline3Score2"), // vision @@ -259,11 +261,11 @@ public static void initShuffleBoard() { gameObjects.addOption(String.valueOf(i), i); } - tab.add("Starting Position", startPositionChooser).withPosition(5, 0).withSize(2, 1); - tab.add("Launch Type", isVision).withPosition(5, 1); - tab.add("Game Objects", gameObjects).withPosition(6, 1); - tab.add("Available Auto Variants", availableAutos).withPosition(5, 2).withSize(2, 1); - autoDelayEntry = tab.add("Auto Delay", 0).withPosition(5, 3).withSize(1, 1).getEntry(); + tab.add("Starting Position", startPositionChooser).withPosition(4, 0).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(4, 1); + tab.add("Game Objects", gameObjects).withPosition(5, 1); + tab.add("Available Auto Variants", availableAutos).withPosition(4, 2).withSize(2, 1); + autoDelayEntry = tab.add("Auto Delay", 0).withPosition(4, 3).withSize(1, 1).getEntry(); isVision.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); startPositionChooser.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); @@ -370,6 +372,10 @@ public static BooleanSupplier hasNoNote() { return (INTAKE_ENABLED ? () -> !s.intakeSubsystem.isIntakeRunning() : () -> true); } + public static BooleanSupplier hasNote() { + return (INTAKE_ENABLED ? () -> s.intakeSubsystem.feederSensorHasNote() : () -> true); + } + // registered commands public static Command subwooferLaunch() { @@ -404,37 +410,26 @@ public static Command subwooferLaunch() { .until(untilFeederHasNoNote())) .andThen(new WaitCommand(0.4)), Commands.none(), - hasNoNote())) + hasNote())) : Commands.none()) .withName("Auto - SubwooferLaunchCommand"); } public static Command visionLaunch() { - - // return (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED - // ? stopFeeder() - // .andThen( - // Commands.either(Commands.none(), index(), s.intakeSubsystem::feederSensorHasNote)) - // .andThen( - // new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls) - // .until(isReadyToLaunch()) - // .andThen(new WaitCommand(FEEDER_DELAY)) - // .andThen(new FeederInCommand(s.intakeSubsystem).until(untilNoNote()))) - // : Commands.none()); - return (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED ? stopFeeder() .andThen( Commands.either( new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls) .until(isReadyToLaunch()) + .andThen(Commands.waitUntil(hasNote())) .andThen(new WaitCommand(FEEDER_DELAY)) .andThen( new FeederInCommand(s.intakeSubsystem) .until(untilFeederHasNoNote())) .andThen(new WaitCommand(0.4)), Commands.none(), - hasNoNote())) + hasNote())) : Commands.none()) .withName("Auto - VisionLaunchCommand"); } @@ -453,7 +448,7 @@ public static Command stopLaunching() { public static Command setAngleRetracted() { return (LAUNCHER_ENABLED && INTAKE_ENABLED - ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, LauncherSubsystem.RETRACTED_ANGLE) + ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, STAGE_ANGLE) : Commands.none()) .withName("Auto - SetPivotRetractedCommand"); } @@ -470,14 +465,15 @@ public static Command setAngleSubwoofer() { .withName("Auto - SetPivotSubwooferCommand"); } - public static Command feederIn() { + public static Command feedUntilNoteLaunched() { return (INTAKE_ENABLED && LAUNCHER_ENABLED ? Commands.waitUntil(isReadyToLaunch()) - .andThen(Commands.waitSeconds(FEEDER_DELAY)) - .andThen(new FeederInCommand(s.intakeSubsystem)) - .andThen(Commands.waitSeconds(0.1)) + .andThen(Commands.waitUntil(hasNote())) + .andThen(new WaitCommand(FEEDER_DELAY)) + .andThen(new FeederInCommand(s.intakeSubsystem).until(untilFeederHasNoNote())) + .andThen(new WaitCommand(0.4)) : Commands.none()) - .withName("Auto - FeedCommand"); + .withName("Auto - FeedUntilNoteLaunchedCommand"); } public static Command noteSteal() { diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index a739ed25..eabff37d 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -71,7 +71,7 @@ public boolean isVision() { * Checks the x, y, and rotation of the auto's starting position and compares it with the expected * starting position, returning true if it is considered close enough to be the same. * - * @param expectedStartPosition + * @param expectedStartPosition The expected start position * @return if it is matching */ public boolean matchesStartPosition(StartPosition expectedStartPosition) { diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 7d5004a5..0475ff03 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -78,8 +78,12 @@ public SwerveModule( // Configure voltage comp, current limit, and ramp rate. angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); - angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit); - driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit); + angleMotor.setCurrentLimit( + configuration.physicalCharacteristics.angleMotorStatorLimit, + configuration.physicalCharacteristics.angleMotorSupplyLimit); + driveMotor.setCurrentLimit( + configuration.physicalCharacteristics.driveMotorStatorLimit, + configuration.physicalCharacteristics.driveMotorSupplyLimit); angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 741c7aae..7bb212f1 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -110,11 +110,12 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configureSparkFlex(() -> motor.setSmartCurrentLimit(supplyLimit)); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 2582134d..0a8a0bcf 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -157,11 +157,12 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configureSparkMax(() -> motor.setSmartCurrentLimit(supplyLimit)); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index d52b34bd..853a6b08 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -91,11 +91,12 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configureSparkMax(() -> motor.setSmartCurrentLimit(supplyLimit)); } /** diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java index 3e19d4da..4d2505e5 100644 --- a/src/main/java/swervelib/motors/SwerveMotor.java +++ b/src/main/java/swervelib/motors/SwerveMotor.java @@ -147,9 +147,10 @@ public abstract class SwerveMotor { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit; */ - public abstract void setCurrentLimit(int currentLimit); + public abstract void setCurrentLimit(int statorLimit, int supplyLimit); /** * Set the maximum rate the open/closed loop output can change by. diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 113eaaa7..dd052981 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -400,15 +400,18 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { + public void setCurrentLimit(int statorLimit, int supplyLimit) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.CurrentLimits); cfg.apply( - configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) - .withStatorCurrentLimitEnable(true)); + configuration.CurrentLimits.withStatorCurrentLimit(statorLimit) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(supplyLimit) + .withSupplyCurrentLimitEnable(true)); } /** diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 060680b9..36c6320e 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -360,12 +360,13 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configuration.continuousCurrentLimit = currentLimit; - configuration.peakCurrentLimit = currentLimit; + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configuration.continuousCurrentLimit = statorLimit; + configuration.peakCurrentLimit = statorLimit; configChanged = true; } diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 11439164..35d35bbe 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -6,7 +6,9 @@ public class SwerveModulePhysicalCharacteristics { /** Current limits for the Swerve Module. */ - public final int driveMotorCurrentLimit, angleMotorCurrentLimit; + public final int driveMotorStatorLimit, angleMotorStatorLimit; + + public final int driveMotorSupplyLimit, angleMotorSupplyLimit; /** The time it takes for the motor to go from 0 to full throttle in seconds. */ public final double driveMotorRampRate, angleMotorRampRate; /** Wheel grip tape coefficient of friction on carpet, as described by the vendor. */ @@ -40,8 +42,10 @@ public SwerveModulePhysicalCharacteristics( MotorConfigDouble conversionFactors, double wheelGripCoefficientOfFriction, double optimalVoltage, - int driveMotorCurrentLimit, - int angleMotorCurrentLimit, + int driveMotorStatorLimit, + int angleMotorStatorLimit, + int driveMotorSupplyLimit, + int angleMotorSupplyLimit, double driveMotorRampRate, double angleMotorRampRate) { this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; @@ -55,8 +59,10 @@ public SwerveModulePhysicalCharacteristics( } } - this.driveMotorCurrentLimit = driveMotorCurrentLimit; - this.angleMotorCurrentLimit = angleMotorCurrentLimit; + this.driveMotorStatorLimit = driveMotorStatorLimit; + this.angleMotorStatorLimit = angleMotorStatorLimit; + this.driveMotorSupplyLimit = driveMotorSupplyLimit; + this.angleMotorSupplyLimit = angleMotorSupplyLimit; this.driveMotorRampRate = driveMotorRampRate; this.angleMotorRampRate = angleMotorRampRate; } @@ -76,6 +82,6 @@ public SwerveModulePhysicalCharacteristics( */ public SwerveModulePhysicalCharacteristics( MotorConfigDouble conversionFactors, double driveMotorRampRate, double angleMotorRampRate) { - this(conversionFactors, 1.19, 12, 40, 20, driveMotorRampRate, angleMotorRampRate); + this(conversionFactors, 1.19, 12, 40, 20, 40, 20, driveMotorRampRate, angleMotorRampRate); } } diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index 0d6877b6..af8b5e62 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -16,7 +16,9 @@ public class PhysicalPropertiesJson { */ public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); /** The current limit in AMPs to apply to the motors. */ - public MotorConfigInt currentLimit = new MotorConfigInt(40, 20); + public MotorConfigInt statorLimit = new MotorConfigInt(40, 20); + + public MotorConfigInt supplyLimit = new MotorConfigInt(40, 20); /** The minimum number of seconds to take for the motor to go from 0 to full throttle. */ public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25); /** @@ -37,8 +39,10 @@ public SwerveModulePhysicalCharacteristics createPhysicalProperties() { conversionFactor, wheelGripCoefficientOfFriction, optimalVoltage, - currentLimit.drive, - currentLimit.angle, + statorLimit.drive, + statorLimit.angle, + supplyLimit.drive, + supplyLimit.angle, rampRate.drive, rampRate.angle); }