diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index da742a2..62dee80 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -5,11 +5,12 @@ "pathFolders": [], "autoFolders": [ "2 Note", - "3 Note" + "3 Note", + "4 Note" ], - "defaultMaxVel": 1.5, - "defaultMaxAccel": 2.5, + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.5, "defaultMaxAngVel": 360.0, - "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 5.0 + "defaultMaxAngAccel": 450.0, + "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/simgui.json b/simgui.json index eb0b3a7..77ba228 100644 --- a/simgui.json +++ b/simgui.json @@ -8,6 +8,7 @@ }, "NTProvider": { "types": { + "/AdvantageKit/RealOutputs/Arm/Mechanism2d/ArmPivot": "Mechanism2d", "/AdvantageKit/RealOutputs/Mechanism2d/ArmShooter": "Mechanism2d", "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous Routine Chooser": "String Chooser" diff --git a/src/main/deploy/pathplanner/autos/CenterSub2Note.auto b/src/main/deploy/pathplanner/autos/CenterSub2Note.auto new file mode 100644 index 0000000..1db7a3c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CenterSub2Note.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.55 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CenSub_CenNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "2 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/CenterSub3NoteLower.auto b/src/main/deploy/pathplanner/autos/CenterSub3NoteLower.auto new file mode 100644 index 0000000..843ab4e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CenterSub3NoteLower.auto @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.55 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CenSub_CenNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CenNote_AngledLowerNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "3 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Lower2_3NoteC.auto b/src/main/deploy/pathplanner/autos/CenterSub4Note.auto similarity index 56% rename from src/main/deploy/pathplanner/autos/Lower2_3NoteC.auto rename to src/main/deploy/pathplanner/autos/CenterSub4Note.auto index 4ba8371..e8ed729 100644 --- a/src/main/deploy/pathplanner/autos/Lower2_3NoteC.auto +++ b/src/main/deploy/pathplanner/autos/CenterSub4Note.auto @@ -5,7 +5,7 @@ "x": 1.35, "y": 5.55 }, - "rotation": 0 + "rotation": 0.0 }, "command": { "type": "sequential", @@ -14,102 +14,78 @@ { "type": "named", "data": { - "name": "armSubwoofer" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "simpleShoot" - } - }, - { - "type": "named", - "data": { - "name": "armStow" + "name": "QueueRevShoot" } }, { "type": "path", "data": { - "pathName": "CenterSub_CenterNote" + "pathName": "CenSub_AngledUpNote" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "CenterNote_CenterSub" + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "armSubwoofer" + "name": "QueueRevShoot" } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.5 + "pathName": "AngledUpNote_CenNote" } }, { "type": "named", "data": { - "name": "simpleShoot" + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "armStow" - } - }, - { - "type": "path", - "data": { - "pathName": "CenterSub_LowerNote" + "name": "QueueRevShoot" } }, { "type": "path", "data": { - "pathName": "LowerNote_CenterSub" + "pathName": "CenNote_AngledLowerNote" } }, { "type": "named", "data": { - "name": "armSubwoofer" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "simpleShoot" + "name": "QueueRevShoot" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmStow" } } ] } }, - "folder": "3 Note", + "folder": "4 Note", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center2Note.auto b/src/main/deploy/pathplanner/autos/CenterSubMid1-3Note.auto similarity index 66% rename from src/main/deploy/pathplanner/autos/Center2Note.auto rename to src/main/deploy/pathplanner/autos/CenterSubMid1-3Note.auto index 22d7cac..1eb3486 100644 --- a/src/main/deploy/pathplanner/autos/Center2Note.auto +++ b/src/main/deploy/pathplanner/autos/CenterSubMid1-3Note.auto @@ -14,66 +14,66 @@ { "type": "named", "data": { - "name": "armSubwoofer" + "name": "ArmInterpolateSpeaker" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "QueueRevShoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "simpleShoot" + "pathName": "CenSub_AngledUpNote" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmInterpolateSpeaker" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "CenterSub_CenterNote" + "name": "QueueRevShoot" } }, { "type": "path", "data": { - "pathName": "CenterNote_CenterSub" + "pathName": "AngledUpNote_Mid1Note" } }, { - "type": "named", + "type": "path", "data": { - "name": "armSubwoofer" + "pathName": "Mid1Note_AngledUpNote" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "simpleShoot" + "name": "QueueRevShoot" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmStow" } } ] } }, - "folder": "2 Note", + "folder": "3 Note", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/CenterSubMid2-3Note.auto b/src/main/deploy/pathplanner/autos/CenterSubMid2-3Note.auto new file mode 100644 index 0000000..c890649 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CenterSubMid2-3Note.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.55 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CenSub_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "AngledUpNote_Mid2Note" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid2Note_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "3 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Lower2_3NoteL.auto b/src/main/deploy/pathplanner/autos/HighCenterSub4Note.auto similarity index 56% rename from src/main/deploy/pathplanner/autos/Lower2_3NoteL.auto rename to src/main/deploy/pathplanner/autos/HighCenterSub4Note.auto index fbb9362..a956ac2 100644 --- a/src/main/deploy/pathplanner/autos/Lower2_3NoteL.auto +++ b/src/main/deploy/pathplanner/autos/HighCenterSub4Note.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 1.35, - "y": 5.55 + "y": 6.0 }, "rotation": 0 }, @@ -14,102 +14,78 @@ { "type": "named", "data": { - "name": "armSubwoofer" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "simpleShoot" - } - }, - { - "type": "named", - "data": { - "name": "armStow" + "name": "QueueRevShoot" } }, { "type": "path", "data": { - "pathName": "CenterSub_CenterNote" + "pathName": "HighCenSub_AngledUpNote" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "CenterNote_CenterSub" + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "armSubwoofer" + "name": "QueueRevShoot" } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.5 + "pathName": "AngledUpNote_CenNote" } }, { "type": "named", "data": { - "name": "simpleShoot" + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "armStow" - } - }, - { - "type": "path", - "data": { - "pathName": "CenterSub_LowerNote" + "name": "QueueRevShoot" } }, { "type": "path", "data": { - "pathName": "LowerNote_LowerSub" + "pathName": "CenNote_AngledLowerNote" } }, { "type": "named", "data": { - "name": "armSubwoofer" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "simpleShoot" + "name": "QueueRevShoot" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmStow" } } ] } }, - "folder": "3 Note", + "folder": "4 Note", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HighCenterSubMid1-3Note.auto b/src/main/deploy/pathplanner/autos/HighCenterSubMid1-3Note.auto new file mode 100644 index 0000000..13fc9ac --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HighCenterSubMid1-3Note.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 6.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "HighCenSub_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "AngledUpNote_Mid1Note" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid1Note_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "3 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HighCenterSubMid2-3Note.auto b/src/main/deploy/pathplanner/autos/HighCenterSubMid2-3Note.auto new file mode 100644 index 0000000..3f3047b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HighCenterSubMid2-3Note.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 6.0 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "HighCenSub_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "AngledUpNote_Mid2Note" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid2Note_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "3 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/UpperDepart.auto b/src/main/deploy/pathplanner/autos/LowerSub2Note.auto similarity index 64% rename from src/main/deploy/pathplanner/autos/UpperDepart.auto rename to src/main/deploy/pathplanner/autos/LowerSub2Note.auto index 06a3f53..caace26 100644 --- a/src/main/deploy/pathplanner/autos/UpperDepart.auto +++ b/src/main/deploy/pathplanner/autos/LowerSub2Note.auto @@ -3,9 +3,9 @@ "startingPose": { "position": { "x": 0.72, - "y": 6.7 + "y": 4.4 }, - "rotation": 60.0 + "rotation": -60.0 }, "command": { "type": "sequential", @@ -14,48 +14,48 @@ { "type": "named", "data": { - "name": "armSubwoofer" + "name": "ArmInterpolateSpeaker" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "QueueRevShoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "simpleShoot" + "pathName": "LowSub_LowerNote" } }, { - "type": "named", + "type": "path", "data": { - "name": "armStow" + "pathName": "LowerNote_LowSub" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 2.0 + "name": "ArmInterpolateSpeaker" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "UpperSub_UpperDepartA" + "name": "QueueRevShoot" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "UpperDepartA_UpperDepartB" + "name": "ArmStow" } } ] } }, - "folder": null, + "folder": "2 Note", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Sweep.auto b/src/main/deploy/pathplanner/autos/Sweep.auto deleted file mode 100644 index 6042dfa..0000000 --- a/src/main/deploy/pathplanner/autos/Sweep.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.38, - "y": 7.45 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.5 - } - }, - { - "type": "path", - "data": { - "pathName": "SweepA_SweepB" - } - }, - { - "type": "path", - "data": { - "pathName": "SweepB_SweepC" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SweepNoShot.auto b/src/main/deploy/pathplanner/autos/SweepNoShot.auto deleted file mode 100644 index dab7ab6..0000000 --- a/src/main/deploy/pathplanner/autos/SweepNoShot.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4, - "y": 7.3 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Center UpperDepartA_UpperDepartB" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/UpperDepartCenter.auto b/src/main/deploy/pathplanner/autos/UpperDepartCenter.auto deleted file mode 100644 index 63b65b7..0000000 --- a/src/main/deploy/pathplanner/autos/UpperDepartCenter.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.72, - "y": 6.7 - }, - "rotation": 60.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 5.5 - } - }, - { - "type": "path", - "data": { - "pathName": "UpperSub_UpperDepartA" - } - }, - { - "type": "path", - "data": { - "pathName": "Center UpperDepartA_UpperDepartB" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/UpperSub2Note.auto b/src/main/deploy/pathplanner/autos/UpperSub2Note.auto new file mode 100644 index 0000000..c024466 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/UpperSub2Note.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 6.7 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "UpSub_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "2 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/UpperSub3NoteHigher.auto b/src/main/deploy/pathplanner/autos/UpperSub3NoteHigher.auto new file mode 100644 index 0000000..fe671d7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/UpperSub3NoteHigher.auto @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 6.7 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "UpSub_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "AngledUpNote_CenNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "3 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/UpperSub4Note.auto b/src/main/deploy/pathplanner/autos/UpperSub4Note.auto new file mode 100644 index 0000000..da72945 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/UpperSub4Note.auto @@ -0,0 +1,91 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.72, + "y": 6.7 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "UpSub_AngledUpNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "AngledUpNote_CenNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "CenNote_AngledLowerNote" + } + }, + { + "type": "named", + "data": { + "name": "ArmInterpolateSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "QueueRevShoot" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + }, + "folder": "4 Note", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Upper2Note.auto b/src/main/deploy/pathplanner/autos/UpperSubMid1-3Note.auto similarity index 66% rename from src/main/deploy/pathplanner/autos/Upper2Note.auto rename to src/main/deploy/pathplanner/autos/UpperSubMid1-3Note.auto index f9754d2..78e6106 100644 --- a/src/main/deploy/pathplanner/autos/Upper2Note.auto +++ b/src/main/deploy/pathplanner/autos/UpperSubMid1-3Note.auto @@ -14,66 +14,66 @@ { "type": "named", "data": { - "name": "armSubwoofer" + "name": "ArmInterpolateSpeaker" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "QueueRevShoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "simpleShoot" + "pathName": "UpSub_AngledUpNote" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmInterpolateSpeaker" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "UpperSub_UpperNote" + "name": "QueueRevShoot" } }, { "type": "path", "data": { - "pathName": "UpperNote_UpperSub" + "pathName": "AngledUpNote_Mid1Note" } }, { - "type": "named", + "type": "path", "data": { - "name": "armSubwoofer" + "pathName": "Mid1Note_AngledUpNote" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "simpleShoot" + "name": "QueueRevShoot" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmStow" } } ] } }, - "folder": "2 Note", + "folder": "3 Note", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Lower2Note.auto b/src/main/deploy/pathplanner/autos/UpperSubMid2-3Note.auto similarity index 63% rename from src/main/deploy/pathplanner/autos/Lower2Note.auto rename to src/main/deploy/pathplanner/autos/UpperSubMid2-3Note.auto index ddc2b68..1d0fb43 100644 --- a/src/main/deploy/pathplanner/autos/Lower2Note.auto +++ b/src/main/deploy/pathplanner/autos/UpperSubMid2-3Note.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.73, - "y": 4.4 + "x": 0.72, + "y": 6.7 }, - "rotation": -60.0 + "rotation": 60.0 }, "command": { "type": "sequential", @@ -14,66 +14,66 @@ { "type": "named", "data": { - "name": "armSubwoofer" + "name": "ArmInterpolateSpeaker" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "QueueRevShoot" } }, { - "type": "named", + "type": "path", "data": { - "name": "simpleShoot" + "pathName": "UpSub_AngledUpNote" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmInterpolateSpeaker" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "LowerSub_LowerNote" + "name": "QueueRevShoot" } }, { "type": "path", "data": { - "pathName": "LowerNote_LowerSub" + "pathName": "AngledUpNote_Mid2Note" } }, { - "type": "named", + "type": "path", "data": { - "name": "armSubwoofer" + "pathName": "Mid2Note_AngledUpNote" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "ArmInterpolateSpeaker" } }, { "type": "named", "data": { - "name": "simpleShoot" + "name": "QueueRevShoot" } }, { "type": "named", "data": { - "name": "armStow" + "name": "ArmStow" } } ] } }, - "folder": "2 Note", + "folder": "3 Note", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Wait Sweep.auto b/src/main/deploy/pathplanner/autos/Wait Sweep.auto deleted file mode 100644 index 27862c5..0000000 --- a/src/main/deploy/pathplanner/autos/Wait Sweep.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.38, - "y": 7.45 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 4.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Wait SweepA_SweepB" - } - }, - { - "type": "path", - "data": { - "pathName": "Wait SweepB_SweepC" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AngledUpNote_CenNote.path b/src/main/deploy/pathplanner/paths/AngledUpNote_CenNote.path new file mode 100644 index 0000000..72d2087 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AngledUpNote_CenNote.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6983391189004604, + "y": 6.866199940733215 + }, + "prevControl": null, + "nextControl": { + "x": 1.8578829648796948, + "y": 6.314543302518826 + }, + "isLocked": false, + "linkedName": "AngledUpperNote" + }, + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": { + "x": 1.4864821164827517, + "y": 5.328199348065364 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CenterNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StowIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeFeed" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 450.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 30.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UpperDepartA_UpperDepartB.path b/src/main/deploy/pathplanner/paths/AngledUpNote_Mid1Note.path similarity index 52% rename from src/main/deploy/pathplanner/paths/UpperDepartA_UpperDepartB.path rename to src/main/deploy/pathplanner/paths/AngledUpNote_Mid1Note.path index b77ee87..6ecf3f0 100644 --- a/src/main/deploy/pathplanner/paths/UpperDepartA_UpperDepartB.path +++ b/src/main/deploy/pathplanner/paths/AngledUpNote_Mid1Note.path @@ -3,37 +3,37 @@ "waypoints": [ { "anchor": { - "x": 1.398182554549633, - "y": 7.287575893046132 + "x": 2.6983391189004604, + "y": 6.866199940733215 }, "prevControl": null, "nextControl": { - "x": 2.516216543299528, - "y": 7.287575893046132 + "x": 3.620875431565678, + "y": 7.062227772603694 }, "isLocked": false, - "linkedName": "DepartA" + "linkedName": "AngledUpperNote" }, { "anchor": { - "x": 7.761332420812989, - "y": 7.431717372694005 + "x": 8.26, + "y": 7.43 }, "prevControl": { - "x": 7.024215657701781, - "y": 7.431717372694005 + "x": 6.713910761154855, + "y": 6.267308488320681 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Mid1Note" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [ { - "name": "Intake", - "waypointRelativePos": 0.75, + "name": "StowIntake", + "waypointRelativePos": 0.0, "command": { "type": "sequential", "data": { @@ -41,7 +41,13 @@ { "type": "named", "data": { - "name": "simpleIntake" + "name": "ArmStow" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFeed" } } ] @@ -53,15 +59,18 @@ "maxVelocity": 3.0, "maxAcceleration": 3.5, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 450.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 25.0, + "rotateFast": true }, "reversed": false, "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": 35.0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AngledUpNote_Mid2Note.path b/src/main/deploy/pathplanner/paths/AngledUpNote_Mid2Note.path new file mode 100644 index 0000000..e6ea68b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AngledUpNote_Mid2Note.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6983391189004604, + "y": 6.866199940733215 + }, + "prevControl": null, + "nextControl": { + "x": 3.620875431565678, + "y": 7.062227772603694 + }, + "isLocked": false, + "linkedName": "AngledUpperNote" + }, + { + "anchor": { + "x": 8.284390022483748, + "y": 5.775783991382798 + }, + "prevControl": { + "x": 6.222445648595001, + "y": 6.771719395290643 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid2Note" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StowIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmStow" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFeed" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 450.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -25.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 35.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenNote_AngledLowerNote.path b/src/main/deploy/pathplanner/paths/CenNote_AngledLowerNote.path new file mode 100644 index 0000000..d840a7f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CenNote_AngledLowerNote.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 1.7735461998701778, + "y": 5.4059974694023465 + }, + "isLocked": false, + "linkedName": "CenterNote" + }, + { + "anchor": { + "x": 2.715725143404199, + "y": 4.0927371341060494 + }, + "prevControl": { + "x": 1.8741891223691944, + "y": 4.347614884602291 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AngledLowNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StowIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeFeed" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 450.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterNote_CenterSub.path b/src/main/deploy/pathplanner/paths/CenNote_CenSub.path similarity index 76% rename from src/main/deploy/pathplanner/paths/CenterNote_CenterSub.path rename to src/main/deploy/pathplanner/paths/CenNote_CenSub.path index 7202be3..0021a9e 100644 --- a/src/main/deploy/pathplanner/paths/CenterNote_CenterSub.path +++ b/src/main/deploy/pathplanner/paths/CenNote_CenSub.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 2.4, + "x": 2.5025391385435403, "y": 5.55 }, "isLocked": false, @@ -20,27 +20,27 @@ "y": 5.55 }, "prevControl": { - "x": 1.85, + "x": 2.346552156622745, "y": 5.55 }, "nextControl": null, "isLocked": false, - "linkedName": "CenterSub" + "linkedName": "CenSub" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, + "maxVelocity": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 450.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0, + "rotateFast": true }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/CenterSub_LowerNote.path b/src/main/deploy/pathplanner/paths/CenSub_AngledUpNote.path similarity index 51% rename from src/main/deploy/pathplanner/paths/CenterSub_LowerNote.path rename to src/main/deploy/pathplanner/paths/CenSub_AngledUpNote.path index 915c5f4..bf03bcc 100644 --- a/src/main/deploy/pathplanner/paths/CenterSub_LowerNote.path +++ b/src/main/deploy/pathplanner/paths/CenSub_AngledUpNote.path @@ -8,56 +8,52 @@ }, "prevControl": null, "nextControl": { - "x": 2.228192998052663, - "y": 5.148506806272872 + "x": 1.8014568575433447, + "y": 6.343261107818512 }, "isLocked": false, - "linkedName": "CenterSub" + "linkedName": "CenSub" }, { "anchor": { - "x": 2.0479614107376367, - "y": 4.1 + "x": 2.6983391189004604, + "y": 6.866199940733215 }, "prevControl": { - "x": 1.7679614107376367, - "y": 4.1 - }, - "nextControl": { - "x": 2.381305428166455, - "y": 4.1 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.72, - "y": 4.1 - }, - "prevControl": { - "x": 1.9746556606074406, - "y": 4.1 + "x": 1.9295962907150175, + "y": 6.232046438036202 }, "nextControl": null, "isLocked": false, - "linkedName": "LowerNote" + "linkedName": "AngledUpperNote" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25, + "rotationDegrees": 30.0, + "rotateFast": true } ], - "rotationTargets": [], "constraintZones": [], "eventMarkers": [ { - "name": "Intake", - "waypointRelativePos": 1.1, + "name": "StowIntake", + "waypointRelativePos": 0.0, "command": { - "type": "sequential", + "type": "parallel", "data": { "commands": [ { "type": "named", "data": { - "name": "simpleIntake" + "name": "IntakeFeed" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" } } ] @@ -66,15 +62,15 @@ } ], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 25.0, + "rotateFast": true }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/CenterSub_CenterNote.path b/src/main/deploy/pathplanner/paths/CenSub_CenNote.path similarity index 66% rename from src/main/deploy/pathplanner/paths/CenterSub_CenterNote.path rename to src/main/deploy/pathplanner/paths/CenSub_CenNote.path index b332119..b8fe9aa 100644 --- a/src/main/deploy/pathplanner/paths/CenterSub_CenterNote.path +++ b/src/main/deploy/pathplanner/paths/CenSub_CenNote.path @@ -8,11 +8,11 @@ }, "prevControl": null, "nextControl": { - "x": 1.85, + "x": 1.7474608614564597, "y": 5.55 }, "isLocked": false, - "linkedName": "CenterSub" + "linkedName": "CenSub" }, { "anchor": { @@ -20,7 +20,7 @@ "y": 5.55 }, "prevControl": { - "x": 2.4, + "x": 1.903447843377255, "y": 5.55 }, "nextControl": null, @@ -32,16 +32,22 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Intake", - "waypointRelativePos": 0.35, + "name": "StowIntake", + "waypointRelativePos": 0.0, "command": { - "type": "sequential", + "type": "parallel", "data": { "commands": [ { "type": "named", "data": { - "name": "simpleIntake" + "name": "IntakeFeed" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" } } ] @@ -50,15 +56,15 @@ } ], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, + "maxVelocity": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 450.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0, + "rotateFast": true }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Center UpperDepartA_UpperDepartB.path b/src/main/deploy/pathplanner/paths/Center UpperDepartA_UpperDepartB.path deleted file mode 100644 index 9b7cd0d..0000000 --- a/src/main/deploy/pathplanner/paths/Center UpperDepartA_UpperDepartB.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.398182554549633, - "y": 7.287575893046132 - }, - "prevControl": null, - "nextControl": { - "x": 2.5162165432995276, - "y": 7.287575893046132 - }, - "isLocked": false, - "linkedName": "DepartA" - }, - { - "anchor": { - "x": 5.824831214394573, - "y": 6.465847987499547 - }, - "prevControl": { - "x": 4.620289596663765, - "y": 7.039378469327556 - }, - "nextControl": { - "x": 6.823182786060513, - "y": 5.990492840360681 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.038155544317041, - "y": 4.089024177862344 - }, - "prevControl": { - "x": 7.346034988758967, - "y": 4.601107817357003 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 1.5, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "simpleIntake" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HighCenSub_AngledUpNote.path b/src/main/deploy/pathplanner/paths/HighCenSub_AngledUpNote.path new file mode 100644 index 0000000..2b7c3e4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HighCenSub_AngledUpNote.path @@ -0,0 +1,82 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.35, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.7184712357903213, + "y": 6.410091577411251 + }, + "isLocked": false, + "linkedName": "HighCenterSub" + }, + { + "anchor": { + "x": 2.6983391189004604, + "y": 6.866199940733215 + }, + "prevControl": { + "x": 1.6168700092646182, + "y": 6.361902612851954 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AngledUpperNote" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 30.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StowIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmStow" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFeed" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 450.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 25.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowSub_LowerNote.path b/src/main/deploy/pathplanner/paths/LowSub_LowerNote.path new file mode 100644 index 0000000..26b7ccb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowSub_LowerNote.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.72, + "y": 4.4 + }, + "prevControl": null, + "nextControl": { + "x": 1.664945298435964, + "y": 4.114134195431138 + }, + "isLocked": false, + "linkedName": "LowSub" + }, + { + "anchor": { + "x": 2.715725143404199, + "y": 4.0927371341060494 + }, + "prevControl": { + "x": 2.2392821357894275, + "y": 4.08479641731247 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AngledLowNote" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StowIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeFeed" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 450.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowerNote_CenterSub.path b/src/main/deploy/pathplanner/paths/LowerNote_CenterSub.path deleted file mode 100644 index 532a3db..0000000 --- a/src/main/deploy/pathplanner/paths/LowerNote_CenterSub.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.72, - "y": 4.1 - }, - "prevControl": null, - "nextControl": { - "x": 2.1370790565241844, - "y": 4.095946825636211 - }, - "isLocked": false, - "linkedName": "LowerNote" - }, - { - "anchor": { - "x": 1.7245565527217366, - "y": 5.55 - }, - "prevControl": { - "x": 2.0820829969984183, - "y": 5.55 - }, - "nextControl": { - "x": 1.6745565527217376, - "y": 5.55 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.35, - "y": 5.55 - }, - "prevControl": { - "x": 1.6, - "y": 5.55 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "CenterSub" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.017520055437452723, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowerNote_LowSub.path b/src/main/deploy/pathplanner/paths/LowerNote_LowSub.path new file mode 100644 index 0000000..ef578b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowerNote_LowSub.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.715725143404199, + "y": 4.0927371341060494 + }, + "prevControl": null, + "nextControl": { + "x": 1.8501870129040305, + "y": 3.9577449486151974 + }, + "isLocked": false, + "linkedName": "AngledLowNote" + }, + { + "anchor": { + "x": 0.72, + "y": 4.394484372262071 + }, + "prevControl": { + "x": 1.259968741963408, + "y": 4.1165592844867875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LowSub" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StowIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeFeed" + } + }, + { + "type": "named", + "data": { + "name": "ArmStow" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 450.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -60.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowerNote_LowerSub.path b/src/main/deploy/pathplanner/paths/LowerNote_LowerSub.path deleted file mode 100644 index 45a0e29..0000000 --- a/src/main/deploy/pathplanner/paths/LowerNote_LowerSub.path +++ /dev/null @@ -1,76 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.72, - "y": 4.1 - }, - "prevControl": null, - "nextControl": { - "x": 1.2335284979624777, - "y": 3.5589686191733687 - }, - "isLocked": false, - "linkedName": "LowerNote" - }, - { - "anchor": { - "x": 0.8973009976744869, - "y": 4.1 - }, - "prevControl": { - "x": 1.1558508234536296, - "y": 3.6521785654624432 - }, - "nextControl": { - "x": 0.7723009976744875, - "y": 4.316506350946108 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.73, - "y": 4.397650730486647 - }, - "prevControl": { - "x": 0.755, - "y": 4.354349460297425 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "LowerSub" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.75, - "rotationDegrees": -60.0, - "rotateFast": false - }, - { - "waypointRelativePos": 0.1, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -60.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowerSub_LowerNote.path b/src/main/deploy/pathplanner/paths/LowerSub_LowerNote.path deleted file mode 100644 index 47920eb..0000000 --- a/src/main/deploy/pathplanner/paths/LowerSub_LowerNote.path +++ /dev/null @@ -1,92 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.73, - "y": 4.397650730486647 - }, - "prevControl": null, - "nextControl": { - "x": 1.5974715190171123, - "y": 4.063791757213145 - }, - "isLocked": false, - "linkedName": "LowerSub" - }, - { - "anchor": { - "x": 1.9398224583486205, - "y": 4.1 - }, - "prevControl": { - "x": 1.7626351455367284, - "y": 4.1 - }, - "nextControl": { - "x": 2.1498224583486203, - "y": 4.1 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.72, - "y": 4.1 - }, - "prevControl": { - "x": 2.2818764757899888, - "y": 4.084932595791118 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "LowerNote" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 0.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 1.0, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "simpleIntake" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -60.0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepA_SweepB.path b/src/main/deploy/pathplanner/paths/Mid1Note_AngledUpNote.path similarity index 53% rename from src/main/deploy/pathplanner/paths/SweepA_SweepB.path rename to src/main/deploy/pathplanner/paths/Mid1Note_AngledUpNote.path index a4284a4..4c1f350 100644 --- a/src/main/deploy/pathplanner/paths/SweepA_SweepB.path +++ b/src/main/deploy/pathplanner/paths/Mid1Note_AngledUpNote.path @@ -3,50 +3,50 @@ "waypoints": [ { "anchor": { - "x": 1.38, - "y": 7.65601216850582 + "x": 8.26, + "y": 7.43 }, "prevControl": null, "nextControl": { - "x": 2.498033988749895, - "y": 7.65601216850582 + "x": 6.576495310398028, + "y": 6.765365266841645 }, "isLocked": false, - "linkedName": "SweepA" + "linkedName": "Mid1Note" }, { "anchor": { - "x": 8.214257072032662, - "y": 7.65601216850582 + "x": 2.6983391189004604, + "y": 6.866199940733215 }, "prevControl": { - "x": 6.693566439458102, - "y": 7.65601216850582 + "x": 3.679131506411161, + "y": 6.869406697146728 }, "nextControl": null, "isLocked": false, - "linkedName": "SweepB" + "linkedName": "AngledUpperNote" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 450.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 25.0, + "rotateFast": true }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 0, + "rotation": 25.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UpperSub_UpperDepartA.path b/src/main/deploy/pathplanner/paths/Mid2Note_AngledUpNote.path similarity index 52% rename from src/main/deploy/pathplanner/paths/UpperSub_UpperDepartA.path rename to src/main/deploy/pathplanner/paths/Mid2Note_AngledUpNote.path index 48632df..3da8b8f 100644 --- a/src/main/deploy/pathplanner/paths/UpperSub_UpperDepartA.path +++ b/src/main/deploy/pathplanner/paths/Mid2Note_AngledUpNote.path @@ -3,56 +3,50 @@ "waypoints": [ { "anchor": { - "x": 0.72283640634488, - "y": 6.703923954138813 + "x": 8.284390022483748, + "y": 5.775783991382798 }, "prevControl": null, "nextControl": { - "x": 1.1668160095579452, - "y": 7.40226272119211 + "x": 6.663374655453015, + "y": 6.929147381442911 }, "isLocked": false, - "linkedName": "UpperSub" + "linkedName": "Mid2Note" }, { "anchor": { - "x": 1.398182554549633, - "y": 7.287575893046132 + "x": 2.6983391189004604, + "y": 6.866199940733215 }, "prevControl": { - "x": 1.198182554549633, - "y": 7.287575893046132 + "x": 3.679131506411161, + "y": 6.869406697146728 }, "nextControl": null, "isLocked": false, - "linkedName": "DepartA" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.15, - "rotationDegrees": 60.0, - "rotateFast": false + "linkedName": "AngledUpperNote" } ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.5, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 450.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 25.0, "rotateFast": true }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 60.0, + "rotation": -25.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepB_SweepC.path b/src/main/deploy/pathplanner/paths/SweepB_SweepC.path deleted file mode 100644 index af9aa93..0000000 --- a/src/main/deploy/pathplanner/paths/SweepB_SweepC.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.214257072032662, - "y": 7.65601216850582 - }, - "prevControl": null, - "nextControl": { - "x": 8.214257072032662, - "y": 6.537978179755925 - }, - "isLocked": false, - "linkedName": "SweepB" - }, - { - "anchor": { - "x": 8.2, - "y": 1.0 - }, - "prevControl": { - "x": 8.2, - "y": 2.520690632574555 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SweepC" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 3.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/UpperSub_UpperNote.path b/src/main/deploy/pathplanner/paths/UpSub_AngledUpNote.path similarity index 59% rename from src/main/deploy/pathplanner/paths/UpperSub_UpperNote.path rename to src/main/deploy/pathplanner/paths/UpSub_AngledUpNote.path index 96229f7..4b1a995 100644 --- a/src/main/deploy/pathplanner/paths/UpperSub_UpperNote.path +++ b/src/main/deploy/pathplanner/paths/UpSub_AngledUpNote.path @@ -8,46 +8,52 @@ }, "prevControl": null, "nextControl": { - "x": 0.9908819866966866, - "y": 6.971969534490619 + "x": 1.7367872698708364, + "y": 6.985028081167274 }, "isLocked": false, "linkedName": "UpperSub" }, { "anchor": { - "x": 2.9041653731455614, - "y": 6.9998147912962505 + "x": 2.6983391189004604, + "y": 6.866199940733215 }, "prevControl": { - "x": 1.9041653731455614, - "y": 6.9998147912962505 + "x": 1.6189686638632539, + "y": 6.097528551538584 }, "nextControl": null, "isLocked": false, - "linkedName": "UpperNote" + "linkedName": "AngledUpperNote" } ], "rotationTargets": [ { - "waypointRelativePos": 0.6, - "rotationDegrees": 0.0, + "waypointRelativePos": 0.35, + "rotationDegrees": 30.0, "rotateFast": false } ], "constraintZones": [], "eventMarkers": [ { - "name": "Intake", - "waypointRelativePos": 0.65, + "name": "StowIntake", + "waypointRelativePos": 0.0, "command": { - "type": "sequential", + "type": "parallel", "data": { "commands": [ { "type": "named", "data": { - "name": "simpleIntake" + "name": "ArmStow" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFeed" } } ] @@ -56,14 +62,14 @@ } ], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, + "maxVelocity": 3.0, + "maxAcceleration": 3.5, "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 450.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 25.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/UpperNote_UpperSub.path b/src/main/deploy/pathplanner/paths/UpperNote_UpperSub.path deleted file mode 100644 index 12fa172..0000000 --- a/src/main/deploy/pathplanner/paths/UpperNote_UpperSub.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.9041653731455614, - "y": 6.9998147912962505 - }, - "prevControl": null, - "nextControl": { - "x": 1.842246296110451, - "y": 7.1657945332213275 - }, - "isLocked": false, - "linkedName": "UpperNote" - }, - { - "anchor": { - "x": 1.0005303159910208, - "y": 7.181675966808486 - }, - "prevControl": { - "x": 1.125530315991021, - "y": 7.398182317754595 - }, - "nextControl": { - "x": 0.8755303159910208, - "y": 6.965169615862377 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.72283640634488, - "y": 6.703923954138813 - }, - "prevControl": { - "x": 0.9254185876647376, - "y": 7.054806584892937 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "UpperSub" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.0, - "rotationDegrees": 60.0, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 60.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Wait SweepA_SweepB.path b/src/main/deploy/pathplanner/paths/Wait SweepA_SweepB.path deleted file mode 100644 index 5afb9a3..0000000 --- a/src/main/deploy/pathplanner/paths/Wait SweepA_SweepB.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.38, - "y": 7.45 - }, - "prevControl": null, - "nextControl": { - "x": 2.498033988749895, - "y": 7.45 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.214257072032662, - "y": 7.45 - }, - "prevControl": { - "x": 6.693566439458102, - "y": 7.45 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 4.0, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Wait SweepB_SweepC.path b/src/main/deploy/pathplanner/paths/Wait SweepB_SweepC.path deleted file mode 100644 index bf3f094..0000000 --- a/src/main/deploy/pathplanner/paths/Wait SweepB_SweepC.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.214257072032662, - "y": 7.45 - }, - "prevControl": null, - "nextControl": { - "x": 8.214257072032662, - "y": 6.331966011250105 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.2, - "y": 1.0 - }, - "prevControl": { - "x": 8.2, - "y": 2.5206906325745555 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SweepC" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 3.5, - "maxAngularVelocity": 360.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/ca/warp7/frc2024/Constants.java b/src/main/java/ca/warp7/frc2024/Constants.java index 9db35b7..e2e39e5 100644 --- a/src/main/java/ca/warp7/frc2024/Constants.java +++ b/src/main/java/ca/warp7/frc2024/Constants.java @@ -14,27 +14,7 @@ public static enum MODE { } public static final MODE CURRENT_MODE = MODE.REAL; - public static final boolean TUNING_MODE = false; - - public static final class ARM { - public static final record Gains(double kP, double kI, double kD, double kS, double kV, double kA, double kG) {} - - public static final ARM.Gains GAINS = - switch (CURRENT_MODE) { - case REAL -> new ARM.Gains(0.33, 0.02, 0.003, 0, 1.27, 0.02, 0.52); - case SIM -> new ARM.Gains(1, 0, 0, 0, 0, 0, 0); - default -> new ARM.Gains(0, 0, 0, 0, 0, 0, 0); - }; - - public static final double MAX_VELOCITY_DEG = 4000; - public static final double MAX_ACCELERATION_DEG = 2000; - - // angle in degrees, - public static final double[] ANGLE = {42.0, 53.0}; - - // distance in meters - public static final double[] DISTANCE = {0.0, 31.0}; - } + public static final boolean TUNING_MODE = true; public static final class CLIMBER { @RequiredArgsConstructor @@ -44,9 +24,9 @@ public static enum STATE { CLIMBER_END_HIGHEST(750), CLIMBER_END(1250); - private final float position; + private final double position; - public float getStatePosition() { + public double getStatePosition() { return position; } } @@ -58,7 +38,7 @@ public static final class DRIVETRAIN { public static final double DRIVE_BASE_RADIUS = Math.hypot(DRIVE_BASE_X / 2.0, DRIVE_BASE_Y / 2.0); - public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); + public static final double WHEEL_DIAMETER = Units.inchesToMeters(3.9); public static final double WHEEL_RADIUS = WHEEL_DIAMETER / 2.0; public static final double MAX_LINEAR_SPEED = Units.feetToMeters(16.5); diff --git a/src/main/java/ca/warp7/frc2024/FieldConstants.java b/src/main/java/ca/warp7/frc2024/FieldConstants.java new file mode 100644 index 0000000..bf40b90 --- /dev/null +++ b/src/main/java/ca/warp7/frc2024/FieldConstants.java @@ -0,0 +1,27 @@ +package ca.warp7.frc2024; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import lombok.RequiredArgsConstructor; + +public final class FieldConstants { + @RequiredArgsConstructor + public enum PointOfInterest { + NONE(new Translation2d()), + SPEAKER(new Translation2d(0.36, 5.55)), + SPEAKER_WALL(new Translation2d(0.0, 5.55)), + AMP(new Translation2d(1.82, 8.2)); + + private final Translation2d blueTranslation; + + public Translation2d getAllianceTranslation() { + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red) { + return new Translation2d(16.54 - blueTranslation.getX(), blueTranslation.getY()); + } else { + return blueTranslation; + } + } + } +} diff --git a/src/main/java/ca/warp7/frc2024/Robot.java b/src/main/java/ca/warp7/frc2024/Robot.java index 4872e48..c0513f5 100644 --- a/src/main/java/ca/warp7/frc2024/Robot.java +++ b/src/main/java/ca/warp7/frc2024/Robot.java @@ -4,6 +4,7 @@ package ca.warp7.frc2024; +import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.littletonrobotics.junction.LogFileUtil; @@ -56,6 +57,8 @@ public void robotInit() { Logger.start(); robotContainer = new RobotContainer(); + + FollowPathCommand.warmupCommand().schedule(); } @Override diff --git a/src/main/java/ca/warp7/frc2024/RobotContainer.java b/src/main/java/ca/warp7/frc2024/RobotContainer.java index 84d3a81..1e7635a 100644 --- a/src/main/java/ca/warp7/frc2024/RobotContainer.java +++ b/src/main/java/ca/warp7/frc2024/RobotContainer.java @@ -4,24 +4,23 @@ package ca.warp7.frc2024; -import ca.warp7.frc2024.Constants.CLIMBER.STATE; +import ca.warp7.frc2024.FieldConstants.PointOfInterest; import ca.warp7.frc2024.subsystems.Intake.IntakeIO; import ca.warp7.frc2024.subsystems.Intake.IntakeIOSim; import ca.warp7.frc2024.subsystems.Intake.IntakeIOSparkMax; import ca.warp7.frc2024.subsystems.Intake.IntakeSubsystem; +import ca.warp7.frc2024.subsystems.arm.ArmConstants; import ca.warp7.frc2024.subsystems.arm.ArmIO; import ca.warp7.frc2024.subsystems.arm.ArmIOSim; import ca.warp7.frc2024.subsystems.arm.ArmIOSparkMax; -import ca.warp7.frc2024.subsystems.arm.ArmSubsystem; -import ca.warp7.frc2024.subsystems.arm.ArmSubsystem.Setpoint; +import ca.warp7.frc2024.subsystems.arm.ArmSubsystemCommands; import ca.warp7.frc2024.subsystems.climber.ClimberIO; import ca.warp7.frc2024.subsystems.climber.ClimberIOSim; -import ca.warp7.frc2024.subsystems.climber.ClimberIOSparkMaxNeo; +import ca.warp7.frc2024.subsystems.climber.ClimberIOSparkMax; import ca.warp7.frc2024.subsystems.climber.ClimberSubsystem; import ca.warp7.frc2024.subsystems.drivetrain.GyroIO; import ca.warp7.frc2024.subsystems.drivetrain.GyroIONavX; import ca.warp7.frc2024.subsystems.drivetrain.SwerveDrivetrainSubsystem; -import ca.warp7.frc2024.subsystems.drivetrain.SwerveDrivetrainSubsystem.PointAtLocation; import ca.warp7.frc2024.subsystems.drivetrain.SwerveModuleIO; import ca.warp7.frc2024.subsystems.drivetrain.SwerveModuleIOFalcon500; import ca.warp7.frc2024.subsystems.drivetrain.SwerveModuleIOSim; @@ -31,15 +30,17 @@ import ca.warp7.frc2024.subsystems.feeder.FeederSubsystem; import ca.warp7.frc2024.subsystems.leds.LEDSubsystem; import ca.warp7.frc2024.subsystems.leds.LEDSubsystem.SparkColor; +import ca.warp7.frc2024.subsystems.shooter.ShooterConstants; import ca.warp7.frc2024.subsystems.shooter.ShooterModuleIO; import ca.warp7.frc2024.subsystems.shooter.ShooterModuleIOSim; import ca.warp7.frc2024.subsystems.shooter.ShooterModuleIOSparkMax550; -import ca.warp7.frc2024.subsystems.shooter.ShooterSubsystem; +import ca.warp7.frc2024.subsystems.shooter.ShooterSubsystemCommands; import ca.warp7.frc2024.subsystems.vision.VisionIO; import ca.warp7.frc2024.subsystems.vision.VisionIOLimelight; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -48,9 +49,9 @@ public class RobotContainer { private final SwerveDrivetrainSubsystem swerveDrivetrainSubsystem; - private final ArmSubsystem armSubsystem; + private final ArmSubsystemCommands armSubsystem; private final IntakeSubsystem intakeSubsystem; - private final ShooterSubsystem shooterSubsystem; + private final ShooterSubsystemCommands shooterSubsystem; private final FeederSubsystem feederSubsystem; private final ClimberSubsystem climberSubsystem; private final LEDSubsystem ledSubsystem; @@ -62,27 +63,36 @@ public class RobotContainer { private final LoggedDashboardChooser autonomousRoutineChooser; /* Commands */ + private final Command vibrateDriver; + private final Command vibrateOperator; + private final Command simpleIntake; + private final Command simpleFeed; + private final Command simpleQueue; + private final Command simpleRev; private final Command simpleShoot; private final Command simpleAmp; - private final Command armStow; - private final Command armSubwoofer; + private final Command noteFlowForward; private final Command noteFlowReverse; private final Command stopNoteFlow; + private final Command intakeFeed; + private final Command queueRev; + private final Command queueRevShoot; + public RobotContainer() { switch (Constants.CURRENT_MODE) { case REAL: - armSubsystem = new ArmSubsystem(new ArmIOSparkMax(11, 10, 0, 1, 2, new Rotation2d(1.543))); + armSubsystem = new ArmSubsystemCommands(new ArmIOSparkMax(11, 10, 0, 1, 2, new Rotation2d(1.543))); intakeSubsystem = new IntakeSubsystem(new IntakeIOSparkMax(31, 4)); - shooterSubsystem = new ShooterSubsystem( + shooterSubsystem = new ShooterSubsystemCommands( new ShooterModuleIOSparkMax550(22, true), new ShooterModuleIOSparkMax550(23, false), new ShooterModuleIOSparkMax550(21, true), new ShooterModuleIOSparkMax550(20, false)); feederSubsystem = new FeederSubsystem(new FeederIOSparkMax(24, 25, 3)); - climberSubsystem = new ClimberSubsystem(new ClimberIOSparkMaxNeo(30)); + climberSubsystem = new ClimberSubsystem(new ClimberIOSparkMax(30)); ledSubsystem = new LEDSubsystem(0); swerveDrivetrainSubsystem = new SwerveDrivetrainSubsystem( new GyroIONavX() {}, @@ -102,9 +112,9 @@ public RobotContainer() { new SwerveModuleIOSim(), new SwerveModuleIOSim(), new SwerveModuleIOSim()); - armSubsystem = new ArmSubsystem(new ArmIOSim() {}); + armSubsystem = new ArmSubsystemCommands(new ArmIOSim() {}); intakeSubsystem = new IntakeSubsystem(new IntakeIOSim() {}); - shooterSubsystem = new ShooterSubsystem( + shooterSubsystem = new ShooterSubsystemCommands( new ShooterModuleIOSim(), new ShooterModuleIOSim(), new ShooterModuleIOSim(), @@ -123,9 +133,9 @@ public RobotContainer() { new SwerveModuleIO() {}, new SwerveModuleIO() {}, new SwerveModuleIO() {}); - armSubsystem = new ArmSubsystem(new ArmIO() {}); + armSubsystem = new ArmSubsystemCommands(new ArmIO() {}); intakeSubsystem = new IntakeSubsystem(new IntakeIO() {}); - shooterSubsystem = new ShooterSubsystem( + shooterSubsystem = new ShooterSubsystemCommands( new ShooterModuleIO() {}, new ShooterModuleIO() {}, new ShooterModuleIO() {}, @@ -135,64 +145,103 @@ public RobotContainer() { ledSubsystem = new LEDSubsystem(0); } + vibrateDriver = Commands.runEnd( + () -> driver.getHID().setRumble(RumbleType.kBothRumble, 0.25), + () -> driver.getHID().setRumble(RumbleType.kBothRumble, 0.0)) + .withTimeout(0.25) + .withName("Vibrate Driver Controller"); + + vibrateOperator = Commands.runEnd( + () -> operator.getHID().setRumble(RumbleType.kBothRumble, 0.25), + () -> operator.getHID().setRumble(RumbleType.kBothRumble, 0.0)) + .withTimeout(0.25) + .withName("Vibrate Operator Controller"); + simpleIntake = Commands.parallel( + intakeSubsystem.runVoltageCommandEnds(10), feederSubsystem.runVoltageCommandEnds(8)) + .until(intakeSubsystem.sensorTrigger()) + .withName("Simple Intake"); + + simpleFeed = Commands.parallel( + shooterSubsystem.runVelocityCommandEnds(-1500, 0, 1, 2, 3), + intakeSubsystem.runVoltageCommandEnds(10), + feederSubsystem.runVoltageCommandEnds(8)) + .until(feederSubsystem.sensorTrigger()) + .withName("Simple Feed"); + + simpleQueue = Commands.parallel( + shooterSubsystem.runVelocityCommandEnds(-8000, 0, 1, 2, 3), + feederSubsystem.runVoltageCommandEnds(-3)) + .until(feederSubsystem.sensorTrigger().negate()) + .withName("Simple Queue"); + + simpleRev = Commands.sequence( + shooterSubsystem.runVelocityCommand(7000, 0, 3), + shooterSubsystem.runVelocityCommand(9500, 1, 2), + Commands.waitSeconds(0.65)) + .withName("Simple Rev"); + + simpleShoot = Commands.sequence( + feederSubsystem.runVoltageCommandEnds(12).withTimeout(0.15), + Commands.waitSeconds(0.25), + shooterSubsystem.stopShooterCommand()) + .withName("Simple Shoot"); + + simpleAmp = Commands.parallel( + shooterSubsystem.runGoalCommandEnds(ShooterConstants.Goal.AMP), + feederSubsystem.runVoltageCommandEnds(-8)) + .withTimeout(0.75) + .withName("Simple Amp"); + + intakeFeed = Commands.sequence( ledSubsystem.solidColorCommand(SparkColor.SKY_BLUE), - intakeSubsystem.runVoltage(10).until(intakeSubsystem.sensorTrigger()), - feederSubsystem.runVoltage(8).until(intakeSubsystem.sensorTrigger())) - .andThen( + simpleIntake.asProxy(), + simpleFeed.asProxy(), Commands.parallel( - shooterSubsystem.runRPMCommand(-1500, 0, 1, 2, 3), ledSubsystem.blinkColorCommand(SparkColor.GREEN, 0.25, 1), - intakeSubsystem.runVoltage(10).until(feederSubsystem.sensorTrigger()), - feederSubsystem.runVoltage(8).until(feederSubsystem.sensorTrigger())), - shooterSubsystem.runRPMCommand(0, 0, 1, 2, 3)); + vibrateDriver, + vibrateOperator)) + .withName("Intake Feed"); - simpleShoot = Commands.sequence( - shooterSubsystem.runRPMCommand(-8000, 0, 1, 2, 3).withTimeout(0.75), - Commands.waitSeconds(0.3), - feederSubsystem.runVoltage(-12).withTimeout(0.1), - shooterSubsystem.runRPMCommand(8500, 0, 1, 2, 3), - Commands.waitSeconds(0.75), - feederSubsystem.runVoltage(12).withTimeout(1), - shooterSubsystem.stopShooterCommand()); - - simpleAmp = Commands.sequence( - Commands.parallel(shooterSubsystem.runRPMCommand(-5000, 0, 1, 2, 3), feederSubsystem.runVoltage(-8)) - .withTimeout(3), - Commands.parallel(shooterSubsystem.stopShooterCommand(), feederSubsystem.runVoltage(0)) - .withTimeout(0.125)); - - armStow = armSubsystem - .setSetpointCommand(Setpoint.HANDOFF_INTAKE) - .until(armSubsystem.atSetpointTrigger(Setpoint.HANDOFF_INTAKE)); - - armSubwoofer = armSubsystem - .setSetpointCommand(Setpoint.SUBWOOFER) - .until(armSubsystem.atSetpointTrigger(Setpoint.SUBWOOFER)); + queueRev = Commands.sequence(simpleQueue.asProxy(), simpleRev.asProxy()).withName("Queue Rev"); + queueRevShoot = Commands.sequence(simpleQueue.asProxy(), simpleRev.asProxy(), simpleShoot.asProxy()) + .withName("Queue Rev Shoot"); noteFlowForward = Commands.parallel( - intakeSubsystem.runVoltage(8), - feederSubsystem.runVoltage(8), - shooterSubsystem.runRPMCommand(6000, 0, 1, 2, 3)); + intakeSubsystem.runVoltageCommandEnds(8), + feederSubsystem.runVoltageCommandEnds(8), + shooterSubsystem.runVelocityCommandEnds(6000, 0, 1, 2, 3)); noteFlowReverse = Commands.parallel( - intakeSubsystem.runVoltage(-11), - feederSubsystem.runVoltage(-8), - shooterSubsystem.runRPMCommand(-6000, 0, 1, 2, 3)); + intakeSubsystem.runVoltageCommandEnds(-11), + feederSubsystem.runVoltageCommandEnds(-8), + shooterSubsystem.runVelocityCommandEnds(-6000, 0, 1, 2, 3)); stopNoteFlow = Commands.parallel( - intakeSubsystem.runVoltage(0), feederSubsystem.runVoltage(0), shooterSubsystem.stopShooterCommand()); + intakeSubsystem.runVoltageCommandEnds(0), + feederSubsystem.runVoltageCommandEnds(0), + shooterSubsystem.stopShooterCommand()); - NamedCommands.registerCommand("simpleIntake", simpleIntake); - NamedCommands.registerCommand("simpleShoot", simpleShoot); - NamedCommands.registerCommand("armSubwoofer", armSubwoofer); - NamedCommands.registerCommand("armStow", armStow); - // NamedCommands.registerCommand("shootAtSubwoofer", Commands.sequence(armSubwoofer, simpleShoot, armStow)); + NamedCommands.registerCommand("ArmStow", armSubsystem.runGoalCommand(ArmConstants.Goal.HANDOFF_INTAKE)); + NamedCommands.registerCommand("ArmSubwoofer", armSubsystem.runGoalCommandUntil(ArmConstants.Goal.SUBWOOFER)); + NamedCommands.registerCommand("ArmPodium", armSubsystem.runGoalCommandUntil(ArmConstants.Goal.PODIUM)); + NamedCommands.registerCommand( + "ArmInterpolateSpeaker", + armSubsystem + .runInterpolation( + () -> swerveDrivetrainSubsystem.getDistanceToPOI(PointOfInterest.SPEAKER_WALL)) + .until(armSubsystem.atGoalTrigger(ArmConstants.Goal.INTERPOLATION))); + NamedCommands.registerCommand("IntakeFeed", Commands.sequence(simpleIntake.asProxy(), simpleFeed.asProxy())); + NamedCommands.registerCommand("QueueRev", Commands.sequence(simpleQueue.asProxy(), simpleRev.asProxy())); + NamedCommands.registerCommand( + "QueueRevShoot", Commands.sequence(simpleQueue.asProxy(), simpleRev.asProxy(), simpleShoot.asProxy())); + NamedCommands.registerCommand("Shoot", simpleShoot.asProxy()); autonomousRoutineChooser = new LoggedDashboardChooser<>("Autonomous Routine Chooser", AutoBuilder.buildAutoChooser()); if (Constants.TUNING_MODE) { + // Shooter SysID routines autonomousRoutineChooser.addOption( "Shooter quasistatic forward", shooterSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); @@ -204,9 +253,24 @@ public RobotContainer() { autonomousRoutineChooser.addOption( "Shooter dynamic reverse", shooterSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - autonomousRoutineChooser.addOption("Zero shooter", Commands.runOnce(() -> { - shooterSubsystem.zeroEncoder(); - })); + + autonomousRoutineChooser.addOption("Zero shooter", shooterSubsystem.zeroEncoderCommand()); + + // Drivetrain SysID routines + autonomousRoutineChooser.addOption( + "Drivetrain quasistatic forward", + swerveDrivetrainSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + + autonomousRoutineChooser.addOption( + "Drivetrain quasistatic reverse", + swerveDrivetrainSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autonomousRoutineChooser.addOption( + "Drivetrain dynamic forward", + swerveDrivetrainSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward)); + + autonomousRoutineChooser.addOption( + "Drivetrain dynamic reverse", + swerveDrivetrainSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } configureDriverBindings(); @@ -214,51 +278,66 @@ public RobotContainer() { } private void configureDriverBindings() { - - swerveDrivetrainSubsystem.setDefaultCommand(SwerveDrivetrainSubsystem.teleopDriveCommand( + swerveDrivetrainSubsystem.setDefaultCommand(swerveDrivetrainSubsystem.teleopDriveCommand( swerveDrivetrainSubsystem, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX(), driver.rightBumper())); + // was as proxy + driver.rightTrigger() + .and(feederSubsystem.sensorTrigger()) + .toggleOnTrue(armSubsystem.runInterpolation( + () -> swerveDrivetrainSubsystem.getDistanceToPOI(PointOfInterest.SPEAKER_WALL))) + .onTrue(queueRev); + + driver.rightTrigger().onFalse(simpleShoot.asProxy().andThen(shooterSubsystem.stopShooterCommand())); + driver.rightTrigger() + .toggleOnFalse(Commands.waitSeconds(0.25) + .andThen(armSubsystem.runGoalCommand(ArmConstants.Goal.HANDOFF_INTAKE))); + driver.start().onTrue(swerveDrivetrainSubsystem.zeroGyroCommand()); driver.rightStick().onTrue(swerveDrivetrainSubsystem.zeroGyroAndPoseCommand()); - driver.leftBumper().whileTrue(armSubsystem.setSetpointCommand(Setpoint.HANDOFF_INTAKE)); + + driver.leftBumper() + .onTrue(armSubsystem.runGoalCommand(ArmConstants.Goal.HANDOFF_INTAKE)) + .onTrue(armSubsystem.setLockoutCommand(true)) + .onFalse(armSubsystem.setLockoutCommand(false)); driver.a() - .onTrue(swerveDrivetrainSubsystem.setPointAtCommand(PointAtLocation.SPEAKER)) - .onFalse(swerveDrivetrainSubsystem.setPointAtCommand(PointAtLocation.NONE)); + .onTrue(swerveDrivetrainSubsystem.setPointAtCommand(PointOfInterest.SPEAKER)) + .onFalse(swerveDrivetrainSubsystem.setPointAtCommand(PointOfInterest.NONE)); + driver.x() + .onTrue(swerveDrivetrainSubsystem.setPointAtCommand(PointOfInterest.AMP)) + .onFalse(swerveDrivetrainSubsystem.setPointAtCommand(PointOfInterest.NONE)); driver.b().onTrue(swerveDrivetrainSubsystem.stopWithXCommand()); - - driver.povDown().onTrue(armSubsystem.setSetpointCommand(Setpoint.INTERPOLATED)); - driver.povUp().onTrue(armSubsystem.setDistance(10.0)); - driver.povLeft().onTrue(armSubsystem.setDistance(100.0)); - driver.povRight().onTrue(armSubsystem.setDistance(2.0)); } private void configureOperatorBindings() { - // spotless:off /* Intaking */ - operator.rightTrigger().and(armSubsystem.atSetpointTrigger(Setpoint.HANDOFF_INTAKE)).onTrue(simpleIntake); - - /* Arm */ - operator.povDown().onTrue(armSubsystem.setSetpointCommand(Setpoint.HANDOFF_INTAKE)); - operator.povUp().onTrue(armSubsystem.setSetpointCommand(Setpoint.PODIUM)); - operator.povRight().onTrue(armSubsystem.setSetpointCommand(Setpoint.SUBWOOFER)); - operator.povLeft().onTrue(armSubsystem.setSetpointCommand(Setpoint.AMP)); + operator.rightTrigger() + .and(armSubsystem.atGoalTrigger(ArmConstants.Goal.HANDOFF_INTAKE)) + .onTrue(intakeFeed); - operator.y().onTrue(armSubsystem.setSetpointCommand(Setpoint.BLOCKER)); + /* Arm Override Setpoints */ + operator.povDown().onTrue(armSubsystem.runGoalCommand(ArmConstants.Goal.HANDOFF_INTAKE)); + operator.povUp().onTrue(armSubsystem.runGoalCommand(ArmConstants.Goal.PODIUM)); + operator.povRight().onTrue(armSubsystem.runGoalCommand(ArmConstants.Goal.SUBWOOFER)); + operator.povLeft().onTrue(armSubsystem.runGoalCommand(ArmConstants.Goal.AMP)); + operator.y().onTrue(armSubsystem.runGoalCommand(ArmConstants.Goal.BLOCKER)); /* Scoring */ - operator.a().and(armSubsystem.atSetpointTrigger(Setpoint.PODIUM)).onTrue(simpleShoot); - operator.a().and(armSubsystem.atSetpointTrigger(Setpoint.SUBWOOFER)).onTrue(simpleShoot); - operator.a().and(armSubsystem.atSetpointTrigger(Setpoint.AMP)).onTrue(simpleAmp); + operator.a().and(armSubsystem.atGoalTrigger(ArmConstants.Goal.PODIUM)).onTrue(queueRevShoot); + operator.a() + .and(armSubsystem.atGoalTrigger(ArmConstants.Goal.SUBWOOFER)) + .onTrue(queueRevShoot); + operator.a().and(armSubsystem.atGoalTrigger(ArmConstants.Goal.AMP)).onTrue(simpleAmp); /* Override Procedures */ - operator.leftBumper().onTrue(noteFlowReverse).onFalse(stopNoteFlow); - operator.rightBumper().onTrue(noteFlowForward).onFalse(stopNoteFlow); + operator.leftBumper().whileTrue(noteFlowReverse); + operator.rightBumper().whileTrue(noteFlowForward); operator.b().onTrue(stopNoteFlow); /* Climbing */ @@ -267,10 +346,6 @@ private void configureOperatorBindings() { operator.leftStick().onTrue(climberSubsystem.toggleClimberLockout()); climberSubsystem.climberLockoutDisabledTrigger().onTrue(ledSubsystem.solidColorCommand(SparkColor.RED)); - climberSubsystem.climberInState(STATE.CLIMBER_START_HIGHEST).onTrue(ledSubsystem.solidColorCommand(SparkColor.ORANGE)); - climberSubsystem.climberInState(STATE.CLIMBER_END).onTrue(ledSubsystem.solidColorCommand(SparkColor.YELLOW)); - - // spotless:on } public Command getAutonomousCommand() { diff --git a/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIO.java b/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIO.java index 29aba9d..bde93e7 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIO.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIO.java @@ -20,4 +20,6 @@ public default void updateInputs(IntakeIOInputs inputs) {} /*Set the intake voltage */ public default void setVoltage(double volts) {} + + public default void stop() {} } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIOSparkMax.java b/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIOSparkMax.java index ca65f57..9cf684a 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIOSparkMax.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeIOSparkMax.java @@ -1,5 +1,8 @@ package ca.warp7.frc2024.subsystems.Intake; +import static ca.warp7.frc2024.util.SparkMaxManager.safeBurnSparkMax; +import static ca.warp7.frc2024.util.SparkMaxManager.safeSparkMax; + import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; @@ -20,13 +23,19 @@ public IntakeIOSparkMax(int kIntakeNeoID, int sensorID) { encoder = motor.getEncoder(); sensor = new DigitalInput(sensorID); - motor.restoreFactoryDefaults(); + /* Factory reset SparkMax */ + safeSparkMax(motor, motor::restoreFactoryDefaults); + + /* Configure motor invert */ motor.setInverted(true); - motor.setSmartCurrentLimit(45); - motor.enableVoltageCompensation(12.0); - motor.setIdleMode(IdleMode.kBrake); - motor.burnFlash(); + /* Configure electrical */ + safeSparkMax(motor, () -> motor.setSmartCurrentLimit(45)); + safeSparkMax(motor, () -> motor.enableVoltageCompensation(12.0)); + safeSparkMax(motor, () -> motor.setIdleMode(IdleMode.kBrake)); + + /* Save configuration */ + safeBurnSparkMax(motor); } @Override @@ -44,4 +53,9 @@ public void updateInputs(IntakeIOInputs inputs) { public void setVoltage(double volts) { motor.setVoltage(volts); } + + @Override + public void stop() { + motor.stopMotor(); + } } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeSubsystem.java b/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeSubsystem.java index e6932f6..08ae558 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeSubsystem.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/Intake/IntakeSubsystem.java @@ -3,37 +3,31 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import lombok.Setter; import org.littletonrobotics.junction.Logger; public class IntakeSubsystem extends SubsystemBase { private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - @Setter - private double volts = 0; - public IntakeSubsystem(IntakeIO intakeIO) { this.io = intakeIO; } - private boolean getSensor() { - return inputs.intakeSensorTriggered; + public Command runVoltageCommand(double volts) { + return this.runOnce(() -> io.setVoltage(volts)); } - public Command runVoltage(double volts) { - return this.startEnd(() -> setVolts(volts), () -> setVolts(0)); + public Command runVoltageCommandEnds(double volts) { + return this.startEnd(() -> io.setVoltage(volts), () -> io.stop()); } public Trigger sensorTrigger() { - return new Trigger(() -> getSensor()).debounce(0); + return new Trigger(() -> inputs.intakeSensorTriggered).debounce(0); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); - - io.setVoltage(volts); } } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmConstants.java b/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmConstants.java new file mode 100644 index 0000000..dfc667c --- /dev/null +++ b/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmConstants.java @@ -0,0 +1,66 @@ +package ca.warp7.frc2024.subsystems.arm; + +import static ca.warp7.frc2024.Constants.CURRENT_MODE; + +import ca.warp7.frc2024.util.LoggedTunableNumber; +import edu.wpi.first.math.util.Units; +import java.util.Arrays; +import java.util.List; +import java.util.function.DoubleSupplier; +import lombok.RequiredArgsConstructor; +import org.apache.commons.math3.fitting.WeightedObservedPoint; + +public final class ArmConstants { + public static final record Gains(double kP, double kI, double kD, double kS, double kV, double kA, double kG) {} + + public static final Gains GAINS = + switch (CURRENT_MODE) { + case REAL -> new Gains(0.33, 0.02, 0.003, 0, 1.27, 0.02, 0.52); + case SIM -> new Gains(1, 0, 0, 0, 0, 0, 0); + default -> new Gains(0, 0, 0, 0, 0, 0, 0); + }; + + public static final double MAX_VELOCITY_DEG = 4000; + public static final double MAX_ACCELERATION_DEG = 2000; + + @RequiredArgsConstructor + public static enum Goal { + HANDOFF_INTAKE(new LoggedTunableNumber("Arm/Goals/HandoffIntakeDegrees", 0)), + STATION_INTAKE(new LoggedTunableNumber("Arm/Goals/StationIntakeDegrees", 0)), + AMP(new LoggedTunableNumber("Arm/Goals/AmpDegrees", 68)), + TRAP(new LoggedTunableNumber("Arm/Goals/TrapDegrees", 3)), + PODIUM(new LoggedTunableNumber("Arm/Goals/PodiumDegrees", 63)), + SUBWOOFER(new LoggedTunableNumber("Arm/Goals/SubwooferDegrees", 50)), + BLOCKER(new LoggedTunableNumber("Arm/Goals/BlockerDegrees", 80)), + INTERPOLATION(() -> 0), + IDLE(() -> 0); + + private final DoubleSupplier armGoalSupplier; + + public double getDegrees() { + return armGoalSupplier.getAsDouble(); + } + + public double getRadians() { + return Units.degreesToRadians(getDegrees()); + } + } + + public static final List POINTS = Arrays.asList( + new WeightedObservedPoint(1, 1.4, 48.0), + new WeightedObservedPoint(1, 2.0, 56.5), + new WeightedObservedPoint(1, 2.5, 61.0), + new WeightedObservedPoint(1, 3.0, 64.0), + new WeightedObservedPoint(1, 3.5, 66.0), + new WeightedObservedPoint(1, 4.0, 67.0), + new WeightedObservedPoint(1, 4.5, 68.0), + new WeightedObservedPoint(1, 5.15, 69.0), + new WeightedObservedPoint(1, 5.47, 69.5), + new WeightedObservedPoint(1, 6, 70)); + + // angle in degrees, + public static final double[] ANGLE = {50.0, 63.0, 66}; + + // distance in meters + public static final double[] DISTANCE = {1.4, 3.17, 3.79}; +} diff --git a/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmSubsystem.java index c3f3bf6..407bfd0 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmSubsystem.java @@ -1,24 +1,22 @@ package ca.warp7.frc2024.subsystems.arm; -import static ca.warp7.frc2024.Constants.ARM.*; +import static ca.warp7.frc2024.subsystems.arm.ArmConstants.*; +import ca.warp7.frc2024.subsystems.arm.ArmConstants.Goal; import ca.warp7.frc2024.util.LoggedTunableNumber; -import ca.warp7.frc2024.util.PolynomialRegression; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.function.DoubleSupplier; -import lombok.RequiredArgsConstructor; +import org.apache.commons.math3.analysis.polynomials.PolynomialFunction; +import org.apache.commons.math3.fitting.PolynomialCurveFitter; import org.littletonrobotics.junction.Logger; // TODO: Use absolute encoder if greater than x degrees @@ -51,46 +49,26 @@ public class ArmSubsystem extends SubsystemBase { new LoggedTunableNumber("Arm/Constraint/MaxAcceleration", MAX_ACCELERATION_DEG); /* Interpolation */ - private static PolynomialRegression polynominal = new PolynomialRegression(DISTANCE, ANGLE, 2, "Distance"); - private static double distance = 0; - private static DoubleSupplier armAngleSupplier = new DoubleSupplier() { - @Override - public double getAsDouble() { - return polynominal.predict(distance); - } - }; + private PolynomialCurveFitter curveFitter = PolynomialCurveFitter.create(3); + private PolynomialFunction polynomialFunction; + protected double distance = 0; /* Setpoints */ - @RequiredArgsConstructor - public enum Setpoint { - HANDOFF_INTAKE(new LoggedTunableNumber("Arm/Setpoint/HandoffIntakeDegrees", 0)), - STATION_INTAKE(new LoggedTunableNumber("Arm/Setpoint/StationIntakeDegrees", 0)), - AMP(new LoggedTunableNumber("Arm/Setpoint/AmpDegrees", 67)), - TRAP(new LoggedTunableNumber("Arm/Setpoint/TrapDegrees", 3)), - PODIUM(new LoggedTunableNumber("Arm/Setpoint/PodiumDegrees", 65)), - SUBWOOFER(new LoggedTunableNumber("Arm/Setpoint/SubwooferDegrees", 50)), - BLOCKER(new LoggedTunableNumber("Arm/Setpoint/BlockerDegrees", 80)), - IDLE(() -> 0), - INTERPOLATED(armAngleSupplier); - - private final DoubleSupplier armSetpointSupplier; - - private double getDegrees() { - return armSetpointSupplier.getAsDouble(); - } - - private double getRadians() { - return Units.degreesToRadians(getDegrees()); - } - } - - private Setpoint setpoint = Setpoint.IDLE; + protected Goal currentGoal = Goal.IDLE; + private double goalDegrees; private Rotation2d armOffset = null; + protected boolean lockout = false; + public ArmSubsystem(ArmIO armIO) { this.io = armIO; + double[] coefficients = curveFitter.fit(ArmConstants.POINTS); + + polynomialFunction = new PolynomialFunction(coefficients); + Logger.recordOutput("Arm/RegressionCoefficients", coefficients); + feedback = new ProfiledPIDController( kP.get(), kI.get(), @@ -105,7 +83,8 @@ public ArmSubsystem(ArmIO armIO) { mechanismLigament = mechanismRoot.append(new MechanismLigament2d( "ArmShooter", 0.5, getIdealIncrementalAngle().getDegrees(), 2, new Color8Bit(Color.kAqua))); - setpoint = Setpoint.HANDOFF_INTAKE; + // Arm starts stowed + currentGoal = Goal.HANDOFF_INTAKE; } private Rotation2d getIdealIncrementalAngle() { @@ -122,6 +101,7 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Arm", inputs); + // Calculate offset from absolute encoder if (armOffset == null) { armOffset = inputs.armExternalAbsolutePosition.minus(inputs.armExternalIncrementalPosition); feedback.reset(getIdealIncrementalAngle().getDegrees()); @@ -152,42 +132,32 @@ public void periodic() { kA); // Log angles in degrees - Logger.recordOutput("Arm/InternalIncrementalAngleDegrees", inputs.armInternalIncrementalPosition.getDegrees()); - Logger.recordOutput("Arm/ExternalIncrementalAngleDegrees", inputs.armExternalIncrementalPosition.getDegrees()); - Logger.recordOutput("Arm/ExternalAbsoluteEncoderAngleDegrees", inputs.armExternalAbsolutePosition.getDegrees()); Logger.recordOutput( "Arm/IdealIncrementalAngleDegrees", getIdealIncrementalAngle().getDegrees()); Logger.recordOutput("Arm/OffsetDegrees", armOffset.getDegrees()); - Logger.recordOutput("Arm/SetpointDegrees", setpoint.getDegrees()); // Log mechanism2d mechanismLigament.setAngle(getIdealIncrementalAngle().getDegrees()); Logger.recordOutput("Arm/Mechanism2d/ArmPivot", mechanism); - // Calculate and voltage if setpoint is not idle - if (setpoint != Setpoint.IDLE) { - double setpointVoltage = - feedback.calculate(getIdealIncrementalAngle().getDegrees(), setpoint.getDegrees()); + // Calculate and set voltage if goal is not idle + // If lockout is engaged, the goal is set to the stow position' - Logger.recordOutput("Arm/FeedbackVoltage", setpointVoltage); - Logger.recordOutput("Arm/SetpointDifference", mechanism); - io.setVoltage(setpointVoltage); + if (lockout || currentGoal == Goal.IDLE) { + goalDegrees = Goal.HANDOFF_INTAKE.getDegrees(); + } else if (currentGoal == Goal.INTERPOLATION) { + goalDegrees = polynomialFunction.value(distance); + } else { + goalDegrees = currentGoal.getDegrees(); } - } - - private boolean atSetpoint(Setpoint setpoint) { - return this.setpoint == setpoint && feedback.atGoal() ? true : false; - } - - public Trigger atSetpointTrigger(Setpoint setpoint) { - return new Trigger(() -> atSetpoint(setpoint)).debounce(0.1); - } - public Command setSetpointCommand(Setpoint setpoint) { - return this.runOnce(() -> this.setpoint = setpoint); + goalDegrees = MathUtil.clamp(goalDegrees, 0, 81); + Logger.recordOutput("Arm/GoalDegrees", goalDegrees); + double goalVoltage = feedback.calculate(getIdealIncrementalAngle().getDegrees(), goalDegrees); + io.setVoltage(goalVoltage); } - public Command setDistance(double distance) { - return this.runOnce(() -> ArmSubsystem.distance = distance); + protected boolean atGoal(Goal goal) { + return this.currentGoal == goal && feedback.atGoal(); } } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmSubsystemCommands.java b/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmSubsystemCommands.java new file mode 100644 index 0000000..c35d0be --- /dev/null +++ b/src/main/java/ca/warp7/frc2024/subsystems/arm/ArmSubsystemCommands.java @@ -0,0 +1,41 @@ +package ca.warp7.frc2024.subsystems.arm; + +import static ca.warp7.frc2024.subsystems.arm.ArmConstants.*; + +import ca.warp7.frc2024.subsystems.arm.ArmConstants.Goal; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.function.DoubleSupplier; + +public class ArmSubsystemCommands extends ArmSubsystem { + + public ArmSubsystemCommands(ArmIO armIO) { + super(armIO); + } + + public Trigger atGoalTrigger(Goal goal) { + return new Trigger(() -> atGoal(goal)).debounce(0.1); + } + + public Command runGoalCommand(Goal goal) { + return super.runOnce(() -> super.currentGoal = goal); + } + + public Command runGoalCommandUntil(Goal goal) { + return super.runOnce(() -> super.currentGoal = goal).until(atGoalTrigger(goal)); + } + + public Command setInterpolationDistanceCommand(DoubleSupplier distance) { + return super.run(() -> super.distance = distance.getAsDouble()); + } + + public Command runInterpolation(DoubleSupplier distance) { + return Commands.run(() -> super.distance = distance.getAsDouble()) + .beforeStarting(runGoalCommand(Goal.INTERPOLATION)); + } + + public Command setLockoutCommand(boolean lockedOut) { + return super.runOnce(() -> super.lockout = lockedOut); + } +} diff --git a/src/main/java/ca/warp7/frc2024/subsystems/climber/ClimberIOSparkMaxNeo.java b/src/main/java/ca/warp7/frc2024/subsystems/climber/ClimberIOSparkMax.java similarity index 59% rename from src/main/java/ca/warp7/frc2024/subsystems/climber/ClimberIOSparkMaxNeo.java rename to src/main/java/ca/warp7/frc2024/subsystems/climber/ClimberIOSparkMax.java index 4a848ae..7ec6c85 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/climber/ClimberIOSparkMaxNeo.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/climber/ClimberIOSparkMax.java @@ -1,6 +1,10 @@ package ca.warp7.frc2024.subsystems.climber; +import static ca.warp7.frc2024.util.SparkMaxManager.safeBurnSparkMax; +import static ca.warp7.frc2024.util.SparkMaxManager.safeSparkMax; + import ca.warp7.frc2024.Constants.CLIMBER.STATE; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; @@ -9,22 +13,31 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -public class ClimberIOSparkMaxNeo implements ClimberIO { +public class ClimberIOSparkMax implements ClimberIO { private final CANSparkMax motor; private final RelativeEncoder intEncoder; - public ClimberIOSparkMaxNeo(int climberSparkMaxId) { + public ClimberIOSparkMax(int climberSparkMaxId) { + /* Create hardware objects */ motor = new CANSparkMax(climberSparkMaxId, MotorType.kBrushless); + intEncoder = motor.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); + + /* Factory reset SparkMax */ + safeSparkMax(motor, motor::restoreFactoryDefaults); - motor.restoreFactoryDefaults(); - motor.setCANTimeout(250); - motor.enableVoltageCompensation(12); - motor.setSmartCurrentLimit(40); + /* Configure motor invert */ motor.setInverted(false); - motor.burnFlash(); - motor.setSoftLimit(SoftLimitDirection.kForward, STATE.CLIMBER_END.getStatePosition()); - intEncoder = motor.getEncoder(SparkRelativeEncoder.Type.kHallSensor, 42); + /* Configure electrical */ + safeSparkMax(motor, () -> motor.setSmartCurrentLimit(40)); + safeSparkMax(motor, () -> motor.enableVoltageCompensation(12.0)); + safeSparkMax(motor, () -> motor.setIdleMode(IdleMode.kBrake)); + + /* Configure soft limits */ + motor.setSoftLimit(SoftLimitDirection.kForward, (float) STATE.CLIMBER_END.getStatePosition()); + + /* Save configurations */ + safeBurnSparkMax(motor); } @Override diff --git a/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/DrivetrainConstants.java b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/DrivetrainConstants.java new file mode 100644 index 0000000..2664f5d --- /dev/null +++ b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/DrivetrainConstants.java @@ -0,0 +1,22 @@ +package ca.warp7.frc2024.subsystems.drivetrain; + +import static ca.warp7.frc2024.Constants.CURRENT_MODE; + +public final class DrivetrainConstants { + public static final record Gains(double kP, double kI, double kD, double kS, double kV) {} + + public static final Gains DRIVE_GAINS = + switch (CURRENT_MODE) { + // case REAL -> new Gains(0.1, 0.0, 0.0, 0.23466, 0.12025); + case REAL -> new Gains(0.15226, 0.0, 0.0, 0.091932, 0.11715); + case SIM -> new Gains(0.1, 0.0, 0.0, 0.0, 0.13); + default -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0); + }; + + public static final Gains STEER_GAINS = + switch (CURRENT_MODE) { + case REAL -> new Gains(6.5, 0.0, 0.0, 0.0, 0.0); + case SIM -> new Gains(10, 0.0, 0.0, 0.0, 0.0); + default -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0); + }; +} diff --git a/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveDrivetrainSubsystem.java b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveDrivetrainSubsystem.java index ae3d648..8fb308d 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveDrivetrainSubsystem.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveDrivetrainSubsystem.java @@ -2,14 +2,17 @@ import static ca.warp7.frc2024.Constants.DRIVETRAIN.*; import static ca.warp7.frc2024.Constants.OI.*; +import static ca.warp7.frc2024.subsystems.drivetrain.DrivetrainConstants.*; import static edu.wpi.first.units.Units.Volts; +import ca.warp7.frc2024.FieldConstants.PointOfInterest; import ca.warp7.frc2024.subsystems.vision.VisionIO; import ca.warp7.frc2024.subsystems.vision.VisionIOInputsAutoLogged; import ca.warp7.frc2024.util.LoggedTunableNumber; import ca.warp7.frc2024.util.SensitivityGainAdjustment; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; @@ -32,7 +35,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import lombok.RequiredArgsConstructor; import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -61,8 +63,8 @@ public class SwerveDrivetrainSubsystem extends SubsystemBase { rawGyroRotation, lastModulePositions, new Pose2d(), - VecBuilder.fill(0.05, 0.05, 0.05), - VecBuilder.fill(0.5, 0.5, 999999999)); + VecBuilder.fill(0.05, 0.05, 0.005), + VecBuilder.fill(2, 2, 999999999)); /* Controllers */ private final PIDController aimAtFeedback; @@ -72,27 +74,23 @@ public class SwerveDrivetrainSubsystem extends SubsystemBase { private final LoggedTunableNumber aimAtkI = new LoggedTunableNumber("Drivetrain/Gains/AimAt/kI", 0); private final LoggedTunableNumber aimAtkD = new LoggedTunableNumber("Drivetrain/Gains/AimAt/kD", 0); - private final SysIdRoutine sysId; + private final LoggedTunableNumber fudge = new LoggedTunableNumber("Drivetrain/Fudge", 0.1); - /* Setpoints */ - @RequiredArgsConstructor - public enum PointAtLocation { - NONE(new Translation2d()), - SPEAKER(new Translation2d(0.25, 5.55)); + private final LoggedTunableNumber drivekS = new LoggedTunableNumber("Drivetrain/Gains/Drive/kS", DRIVE_GAINS.kS()); + private final LoggedTunableNumber drivekV = new LoggedTunableNumber("Drivetrain/Gains/Drive/kV", DRIVE_GAINS.kV()); - private final Translation2d bluePoint; + private final LoggedTunableNumber drivekP = new LoggedTunableNumber("Drivetrain/Gains/Drive/kP", DRIVE_GAINS.kP()); + private final LoggedTunableNumber drivekI = new LoggedTunableNumber("Drivetrain/Gains/Drive/kI", DRIVE_GAINS.kI()); + private final LoggedTunableNumber drivekD = new LoggedTunableNumber("Drivetrain/Gains/Drive/kD", DRIVE_GAINS.kD()); - public Translation2d getTranslatedPoint() { - if (DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red) { - return new Translation2d(16.54 - bluePoint.getX(), bluePoint.getY()); - } else { - return bluePoint; - } - } - } + private final LoggedTunableNumber steerkP = new LoggedTunableNumber("Drivetrain/Gains/Steer/kP", STEER_GAINS.kP()); + private final LoggedTunableNumber steerkI = new LoggedTunableNumber("Drivetrain/Gains/Steer/kI", STEER_GAINS.kI()); + private final LoggedTunableNumber steerkD = new LoggedTunableNumber("Drivetrain/Gains/Steer/kD", STEER_GAINS.kD()); - private PointAtLocation pointAt = PointAtLocation.NONE; + private final SysIdRoutine sysId; + + private PointOfInterest pointAt = PointOfInterest.NONE; + private boolean reversePointing = true; public SwerveDrivetrainSubsystem( GyroIO gyroIO, @@ -115,18 +113,33 @@ public SwerveDrivetrainSubsystem( aimAtFeedback = new PIDController(aimAtkP.get(), aimAtkI.get(), aimAtkD.get()); aimAtFeedback.enableContinuousInput(-180, 180); + // Reset robot pose setPose(new Pose2d(0, 0, new Rotation2d())); + // Set initial gains + for (int i = 0; i < 4; i++) { + swerveModules[i].setDriveFeedforwardGains(drivekS.get(), drivekV.get()); + swerveModules[i].setDriveFeedbackGains(drivekP.get(), drivekI.get(), drivekD.get()); + swerveModules[i].setSteerFeedbackGains(steerkP.get(), steerkI.get(), steerkD.get()); + } + // Configure PathPlanner AutoBuilder.configureHolonomic( this::getPose, this::setPose, () -> swerveDriveKinematics.toChassisSpeeds(getModuleStates()), this::setTargetChassisSpeeds, - new HolonomicPathFollowerConfig(MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), + new HolonomicPathFollowerConfig( + new PIDConstants(2.5), + new PIDConstants(2.5), + MAX_LINEAR_SPEED, + DRIVE_BASE_RADIUS, + new ReplanningConfig(true, true, 1, 0.05)), () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red, this); + + // Log PathPlanner PathPlannerLogging.setLogActivePathCallback((activePath) -> { Logger.recordOutput("Drivetrain/Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); }); @@ -168,7 +181,7 @@ public void periodic() { // Calculate last module position SwerveModulePosition[] modulePositions = getModulePositions(); SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - for (int i = 0; i <= 3; i++) { + for (int i = 0; i < 4; i++) { moduleDeltas[i] = new SwerveModulePosition( modulePositions[i].distanceMeters - lastModulePositions[i].distanceMeters, modulePositions[i].angle); @@ -186,39 +199,16 @@ public void periodic() { // Update pose estimator using odometry poseEstimator.update(rawGyroRotation, modulePositions); - // if (rearVisionInputs.tagCount >= 2) { + // if (rearVisionInputs.tagCount >= 2 && rearVisionInputs.avgTagDist <= 4.5) { // poseEstimator.addVisionMeasurement( - // rearVisionInputs.blueOriginRobotPose, - // rearVisionInputs.timestamp, - // VecBuilder.fill(0.7, 0.7, 999999999)); + // rearVisionInputs.blueOriginRobotPose, rearVisionInputs.timestamp, VecBuilder.fill(1, 1, + // 999999999)); // } - // poseEstimator.addVisionMeasurement(, DEADBAND); + updatePoseEstimateWithVision(); - // Update pose estimator using vision - // if (frontVisionInputs.tagCount >= 2 && frontVisionInputs.avgTagDist < 3.5) { - // poseEstimator.addVisionMeasurement( - // frontVisionInputs.blueOriginRobotPose, - // frontVisionInputs.timestamp, - // VecBuilder.fill(0.5, 0.5, 999999999)); - // } else if (rearVisionInputs.tagCount >= 2 && rearVisionInputs.avgTagDist < 3.5) { - // poseEstimator.addVisionMeasurement( - // rearVisionInputs.blueOriginRobotPose, - // rearVisionInputs.timestamp, - // VecBuilder.fill(0.5, 0.5, 999999999)); - // } else if (frontVisionInputs.tagCount >= 1 && frontVisionInputs.avgTagDist < rearVisionInputs.avgTagDist) { - // poseEstimator.addVisionMeasurement( - // frontVisionInputs.blueOriginRobotPose, - // frontVisionInputs.timestamp, - // VecBuilder.fill(0.7 * frontVisionInputs.avgTagDist, 0.7 * frontVisionInputs.avgTagDist, - // 999999999)); - // } else if (rearVisionInputs.tagCount >= 1) { - // poseEstimator.addVisionMeasurement( - // rearVisionInputs.blueOriginRobotPose, - // rearVisionInputs.timestamp, - // VecBuilder.fill(0.7 * rearVisionInputs.avgTagDist, 0.7 * rearVisionInputs.avgTagDist, - // 999999999)); - // } + Logger.recordOutput("Drivetrain/DistanceToSpeakerWall", getDistanceToPOI(PointOfInterest.SPEAKER_WALL)); + Logger.recordOutput("Drivetrain/DistanceToAmp", getDistanceToPOI(PointOfInterest.AMP)); // Update if PID gains have changed LoggedTunableNumber.ifChanged( @@ -231,8 +221,66 @@ public void periodic() { aimAtkP, aimAtkI, aimAtkD); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + for (int i = 0; i < 4; i++) { + swerveModules[i].setDriveFeedforwardGains(drivekS.get(), drivekV.get()); + } + }, + drivekS, + drivekV); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + for (int i = 0; i < 4; i++) { + swerveModules[i].setDriveFeedbackGains(drivekP.get(), drivekI.get(), drivekD.get()); + } + }, + drivekP, + drivekI, + drivekD); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + for (int i = 0; i < 4; i++) { + swerveModules[i].setSteerFeedbackGains(steerkP.get(), steerkI.get(), steerkD.get()); + } + }, + steerkP, + steerkI, + steerkD); } + public void updatePoseEstimateWithVision() { + if (rearVisionInputs.tagCount >= 1) { + double xyStds; + double rotStds = 999999999; + + double poseDifference = poseEstimator + .getEstimatedPosition() + .getTranslation() + .getDistance(rearVisionInputs.blueOriginRobotPose.getTranslation()); + + if (rearVisionInputs.tagCount >= 2 && rearVisionInputs.avgTagDist <= 4.5) { + xyStds = 1.0; + } else if (rearVisionInputs.avgTagArea > 0.8 && poseDifference < 0.5) { + xyStds = 2.0; + } else if (rearVisionInputs.avgTagArea > 0.1 && poseDifference < 0.3) { + xyStds = 2.5; + } else { + return; + } + + poseEstimator.addVisionMeasurement( + rearVisionInputs.blueOriginRobotPose, + rearVisionInputs.timestamp, + VecBuilder.fill(xyStds, xyStds, rotStds)); + } + } /** * Drive at desired velocities * @@ -245,7 +293,7 @@ public void setTargetChassisSpeeds(ChassisSpeeds speeds) { // Set individual modules target state SwerveModuleState[] optimizedStates = new SwerveModuleState[4]; - for (int i = 0; i <= 3; i++) { + for (int i = 0; i < 4; i++) { optimizedStates[i] = swerveModules[i].setTargetState(swerveModuleStates[i]); } @@ -262,6 +310,11 @@ public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } + @AutoLogOutput(key = "Drivetrain/DistanceToPointOfInterest") + public double getDistanceToPOI(PointOfInterest POI) { + return getPose().getTranslation().getDistance(POI.getAllianceTranslation()); + } + /** * @return Current rotation from pose estimator */ @@ -312,7 +365,7 @@ private void stop() { * @return Command for stop */ public Command stopCommand() { - return this.runOnce(this::stopCommand); + return this.runOnce(this::stop); } /** @@ -329,7 +382,7 @@ private void stopWithX() { * @return Command for stopWithX */ public Command stopWithXCommand() { - return this.runOnce(this::stopWithX); + return this.run(this::stopWithX); } /** @@ -358,7 +411,7 @@ public Command zeroGyroAndPoseCommand() { }); } - public Command setPointAtCommand(PointAtLocation pointAt) { + public Command setPointAtCommand(PointOfInterest pointAt) { return this.runOnce(() -> this.pointAt = pointAt); } @@ -379,7 +432,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { * @param angleSupplier Supplier for the omega (or angle) velocity * @param robotOrientedSupplier Supplier for whether to use robot oriented drive over field oriented */ - public static Command teleopDriveCommand( + public Command teleopDriveCommand( SwerveDrivetrainSubsystem swerveDrivetrainSubsystem, DoubleSupplier xSupplier, DoubleSupplier ySupplier, @@ -420,9 +473,9 @@ public static Command teleopDriveCommand( Logger.recordOutput("OI/y Velocity", yVelocity); Logger.recordOutput("OI/Omega", omega); - Translation2d pointAt = swerveDrivetrainSubsystem.pointAt == PointAtLocation.NONE + Translation2d pointAt = swerveDrivetrainSubsystem.pointAt == PointOfInterest.NONE ? null - : swerveDrivetrainSubsystem.pointAt.getTranslatedPoint(); + : swerveDrivetrainSubsystem.pointAt.getAllianceTranslation(); /* Aim at code courtesy of FRC 418 */ if (pointAt != null) { @@ -448,7 +501,7 @@ public static Command teleopDriveCommand( robotVector.dotProduct(targetVector) / targetVector.getNormSq()); // Perpendicular component of robot's motion to target vector Vector2D perpendicularRobotVector = - robotVector.subtract(parallelRobotVector).scalarMultiply(0.1); + robotVector.subtract(parallelRobotVector).scalarMultiply(fudge.get()); // Adjust aim point using calculated vector Translation2d adjustedPoint = pointAt.minus( new Translation2d(perpendicularRobotVector.getX(), perpendicularRobotVector.getY())); @@ -456,13 +509,18 @@ public static Command teleopDriveCommand( Rotation2d adjustedAngle = new Rotation2d( adjustedPoint.getX() - currentPose.getX(), adjustedPoint.getY() - currentPose.getY()); - double rotateOutput = swerveDrivetrainSubsystem.aimAtFeedback.calculate( - currentPose.getRotation().getDegrees() + 180, adjustedAngle.getDegrees()); + double rotateOutput = reversePointing + ? swerveDrivetrainSubsystem.aimAtFeedback.calculate( + currentPose.getRotation().getDegrees() + 180, adjustedAngle.getDegrees()) + : swerveDrivetrainSubsystem.aimAtFeedback.calculate( + currentPose.getRotation().getDegrees(), adjustedAngle.getDegrees()); + swerveDrivetrainSubsystem.setTargetChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( velocityOutput * Math.cos(moveDirection), velocityOutput * Math.sin(moveDirection), rotateOutput, swerveDrivetrainSubsystem.getRobotRotation())); + } else { if (robotOrientedSupplier.getAsBoolean()) { swerveDrivetrainSubsystem.setTargetChassisSpeeds( @@ -473,6 +531,6 @@ public static Command teleopDriveCommand( } } }, - swerveDrivetrainSubsystem); + this); } } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModule.java b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModule.java index 36e58cc..30cb24a 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModule.java @@ -2,7 +2,6 @@ import static ca.warp7.frc2024.Constants.DRIVETRAIN.WHEEL_RADIUS; -import ca.warp7.frc2024.Constants; import ca.warp7.frc2024.Constants.DRIVETRAIN; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -15,7 +14,7 @@ public class SwerveModule { private final SwerveModuleIO moduleIO; private final String moduleName; - private final SimpleMotorFeedforward driveFeedforward; + private SimpleMotorFeedforward driveFeedforward; private final PIDController driveFeedback; private final PIDController steerFeedback; @@ -29,24 +28,10 @@ public SwerveModule(SwerveModuleIO moduleIO, String moduleName) { this.moduleIO = moduleIO; this.moduleName = moduleName; - switch (Constants.CURRENT_MODE) { - case REAL: - driveFeedforward = new SimpleMotorFeedforward(0.23466, 0.12025); - driveFeedback = new PIDController(0.1, 0.0, 0.0); - steerFeedback = new PIDController(6.5, 0.0, 0); - break; - case SIM: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - driveFeedback = new PIDController(0.1, 0.0, 0.0); - steerFeedback = new PIDController(10.0, 0.0, 0.0); - break; - default: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeedback = new PIDController(0.0, 0.0, 0.0); - steerFeedback = new PIDController(0.0, 0.0, 0.0); - break; - } - // https://www.chiefdelphi.com/t/swerve-modules-flip-180-degrees-periodically-conditionally/393059/11 + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeedback = new PIDController(0.0, 0.0, 0.0); + steerFeedback = new PIDController(0.0, 0.0, 0.0); + steerFeedback.enableContinuousInput(-Math.PI, Math.PI); } @@ -80,6 +65,22 @@ public SwerveModuleState setTargetState(SwerveModuleState state) { return optimizedState; } + public void setDriveFeedforwardGains(double kS, double kV) { + driveFeedforward = new SimpleMotorFeedforward(kS, kV); + } + + public void setDriveFeedbackGains(double kP, double kI, double kD) { + driveFeedback.setP(kP); + driveFeedback.setI(kI); + driveFeedback.setD(kD); + } + + public void setSteerFeedbackGains(double kP, double kI, double kD) { + steerFeedback.setP(kP); + steerFeedback.setI(kI); + steerFeedback.setD(kD); + } + public void runCharacterization(double volts) { angleSetpoint = new Rotation2d(); diff --git a/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModuleIOFalcon500.java b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModuleIOFalcon500.java index 870243f..dcf5d95 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModuleIOFalcon500.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/drivetrain/SwerveModuleIOFalcon500.java @@ -33,7 +33,7 @@ public class SwerveModuleIOFalcon500 implements SwerveModuleIO { private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); private final double STEER_GEAR_RATIO = 150.0 / 7.0; - private final double PHOENIX_TIMEOUT = 2.0; + private final double PHOENIX_TIMEOUT = 8.0; private final VoltageOut driveVoltageOut = new VoltageOut(0, false, false, false, false); private final VoltageOut steerVoltageOut = new VoltageOut(0, false, false, false, false); diff --git a/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIO.java b/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIO.java index a20af93..a9cc4c2 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIO.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIO.java @@ -26,4 +26,6 @@ public default void updateInputs(FeederIOInputs inputs) {} * @param volts */ public default void setVoltage(double volts) {} + + public default void stop() {} } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIOSparkMax.java b/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIOSparkMax.java index 92122ce..fd429dd 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIOSparkMax.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederIOSparkMax.java @@ -1,8 +1,10 @@ package ca.warp7.frc2024.subsystems.feeder; +import static ca.warp7.frc2024.util.SparkMaxManager.safeBurnSparkMax; +import static ca.warp7.frc2024.util.SparkMaxManager.safeSparkMax; + import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,33 +19,33 @@ public class FeederIOSparkMax implements FeederIO { private DigitalInput sensor; public FeederIOSparkMax(int topID, int bottomID, int photoSensorID) { + /* Create hardware object */ topMotor = new CANSparkMax(topID, MotorType.kBrushless); bottomMotor = new CANSparkMax(bottomID, MotorType.kBrushless); encoder = topMotor.getEncoder(); sensor = new DigitalInput(photoSensorID); - topMotor.restoreFactoryDefaults(); - bottomMotor.restoreFactoryDefaults(); + /* Factory reset SparkMaxes */ + safeSparkMax(topMotor, topMotor::restoreFactoryDefaults); + safeSparkMax(bottomMotor, bottomMotor::restoreFactoryDefaults); + /* Configure motor invert and follows */ topMotor.setInverted(true); - bottomMotor.follow(topMotor, false); + safeSparkMax(bottomMotor, () -> bottomMotor.follow(topMotor, false)); - topMotor.setSmartCurrentLimit(20); - bottomMotor.setSmartCurrentLimit(20); - topMotor.enableVoltageCompensation(12); - bottomMotor.enableVoltageCompensation(12); - topMotor.setIdleMode(IdleMode.kBrake); - bottomMotor.setIdleMode(IdleMode.kBrake); + /* Configure electrical */ + safeSparkMax(topMotor, () -> topMotor.setSmartCurrentLimit(20)); + safeSparkMax(bottomMotor, () -> bottomMotor.setSmartCurrentLimit(20)); + safeSparkMax(topMotor, () -> topMotor.enableVoltageCompensation(12.0)); + safeSparkMax(bottomMotor, () -> bottomMotor.enableVoltageCompensation(12.0)); + safeSparkMax(topMotor, () -> topMotor.setIdleMode(IdleMode.kBrake)); + safeSparkMax(bottomMotor, () -> bottomMotor.setIdleMode(IdleMode.kBrake)); - bottomMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 500); + /* Save configurations */ + safeBurnSparkMax(topMotor); + safeBurnSparkMax(bottomMotor); encoder.setPosition(0); - - topMotor.setInverted(true); - bottomMotor.follow(topMotor, false); - - topMotor.burnFlash(); - bottomMotor.burnFlash(); } @Override @@ -63,4 +65,9 @@ public void updateInputs(FeederIOInputs inputs) { public void setVoltage(double volts) { topMotor.setVoltage(volts); } + + @Override + public void stop() { + topMotor.stopMotor(); + } } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederSubsystem.java b/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederSubsystem.java index dd76f54..4611202 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederSubsystem.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/feeder/FeederSubsystem.java @@ -3,37 +3,31 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import lombok.Setter; import org.littletonrobotics.junction.Logger; public class FeederSubsystem extends SubsystemBase { private final FeederIO io; private final FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); - @Setter - private double volts = 0; - public FeederSubsystem(FeederIO feederIO) { this.io = feederIO; } - private boolean getSensor() { - return inputs.feederSensorTriggered; + public Command runVoltageCommand(double volts) { + return this.runOnce(() -> io.setVoltage(volts)); } - public Command runVoltage(double volts) { - return this.startEnd(() -> setVolts(volts), () -> setVolts(0)); + public Command runVoltageCommandEnds(double volts) { + return this.startEnd(() -> io.setVoltage(volts), () -> io.stop()); } public Trigger sensorTrigger() { - return new Trigger(() -> getSensor()).debounce(0.0); + return new Trigger(() -> inputs.feederSensorTriggered).debounce(0.0); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Feeder", inputs); - - io.setVoltage(volts); } } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterConstants.java b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..124a66f --- /dev/null +++ b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,70 @@ +package ca.warp7.frc2024.subsystems.shooter; + +import static ca.warp7.frc2024.Constants.CURRENT_MODE; + +import ca.warp7.frc2024.util.LoggedTunableNumber; +import lombok.RequiredArgsConstructor; + +public final class ShooterConstants { + public static final record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} + + public static final Gains Gains = + switch (CURRENT_MODE) { + case REAL -> new Gains(0.0008, 0.0, 0.01, 0.021949, 0.010201, 0.0010146); + case SIM -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + default -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + }; + + private static final record Speeds( + LoggedTunableNumber topRight, + LoggedTunableNumber topLeft, + LoggedTunableNumber bottomLeft, + LoggedTunableNumber bottomRight) {} + + private static final Speeds IDLE_SPEEDS = new Speeds( + new LoggedTunableNumber("Shooter/Goals/Idle/TopRight", 0), + new LoggedTunableNumber("Shooter/Goals/Idle/TopLeft", 0), + new LoggedTunableNumber("Shooter/Goals/Idle/BottomLeft", 0), + new LoggedTunableNumber("Shooter/Goals/Idle/BottomRight", 0)); + + private static final Speeds TUNING_SPEEDS = new Speeds( + new LoggedTunableNumber("Shooter/Goals/Tuning/TopRight", 0), + new LoggedTunableNumber("Shooter/Goals/Tuning/TopLeft", 0), + new LoggedTunableNumber("Shooter/Goals/Tuning/BottomLeft", 0), + new LoggedTunableNumber("Shooter/Goals/Tuning/BottomRight", 0)); + + private static final Speeds PODIUM_SPEEDS = new Speeds( + new LoggedTunableNumber("Shooter/Goals/Podium/TopRight", 0), + new LoggedTunableNumber("Shooter/Goals/Podium/TopLeft", 0), + new LoggedTunableNumber("Shooter/Goals/Podium/BottomLeft", 0), + new LoggedTunableNumber("Shooter/Goals/Podium/BottomRight", 0)); + + private static final Speeds DEFAULT_SPEEDS = new Speeds( + new LoggedTunableNumber("Shooter/Goals/Podium/TopRight", 7000), + new LoggedTunableNumber("Shooter/Goals/Podium/TopLeft", 9500), + new LoggedTunableNumber("Shooter/Goals/Podium/BottomLeft", 9500), + new LoggedTunableNumber("Shooter/Goals/Podium/BottomRight", 7000)); + + private static final Speeds AMP_SPEEDS = new Speeds( + new LoggedTunableNumber("Shooter/Goals/Podium/TopRight", -8000), + new LoggedTunableNumber("Shooter/Goals/Podium/TopLeft", -8000), + new LoggedTunableNumber("Shooter/Goals/Podium/BottomLeft", -8000), + new LoggedTunableNumber("Shooter/Goals/Podium/BottomRight", -8000)); + + @RequiredArgsConstructor + public enum Goal { + IDLE(IDLE_SPEEDS), + TUNING(TUNING_SPEEDS), + PODIUM(PODIUM_SPEEDS), + AMP(AMP_SPEEDS), + DEFAULT(DEFAULT_SPEEDS); + + private final Speeds speeds; + + public double[] getSpeeds() { + return new double[] { + speeds.topRight.get(), speeds.topLeft.get(), speeds.bottomLeft.get(), speeds.bottomRight.get() + }; + } + } +} diff --git a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModule.java b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModule.java index eda2e2f..72d5aaf 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModule.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModule.java @@ -7,16 +7,14 @@ public class ShooterModule { private final ShooterModuleIO shooterModuleIO; - private final int shooterModuleID; private final String shooterModuleName; private SimpleMotorFeedforward feedforward; private final ShooterModuleIOInputsAutoLogged shooterModuleInputs = new ShooterModuleIOInputsAutoLogged(); - public ShooterModule(ShooterModuleIO shooterModuleIO, int shooterModuleID, String shooterModuleName) { + public ShooterModule(ShooterModuleIO shooterModuleIO, String shooterModuleName) { this.shooterModuleIO = shooterModuleIO; - this.shooterModuleID = shooterModuleID; this.shooterModuleName = shooterModuleName; switch (Constants.CURRENT_MODE) { @@ -49,7 +47,7 @@ public double getVelocityRPM() { * Set closed loop control target velocity * @param velocityRPM */ - public void runShooterTargetVelocity(double velocityRPM) { + public void setVelocity(double velocityRPM) { double velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); shooterModuleIO.setVelocity(velocityRadPerSec, feedforward.calculate(velocityRadPerSec)); @@ -64,7 +62,7 @@ public void configurePID(double kP, double kI, double kD) { * Set open loop control volts * @param volts */ - public void runShooterVolts(double volts) { + public void setVoltage(double volts) { shooterModuleIO.setVoltage(volts); } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModuleIOSparkMax550.java b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModuleIOSparkMax550.java index 0b7f446..7c737b7 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModuleIOSparkMax550.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterModuleIOSparkMax550.java @@ -1,5 +1,8 @@ package ca.warp7.frc2024.subsystems.shooter; +import static ca.warp7.frc2024.util.SparkMaxManager.safeBurnSparkMax; +import static ca.warp7.frc2024.util.SparkMaxManager.safeSparkMax; + import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -16,19 +19,25 @@ public class ShooterModuleIOSparkMax550 implements ShooterModuleIO { private final SparkPIDController feedback; public ShooterModuleIOSparkMax550(int shooterSparkMaxId, boolean invert) { + /* Create hardware objects */ motor = new CANSparkMax(shooterSparkMaxId, MotorType.kBrushless); - - // TODO: FIX config - motor.enableVoltageCompensation(12.0); - motor.setSmartCurrentLimit(15); - motor.setIdleMode(IdleMode.kBrake); - if (invert) { - motor.setInverted(true); - } - - // Specify encoder type: https://www.chiefdelphi.com/t/psa-new-crash-bug-in-revlib-2024-2-2/456242 encoder = motor.getEncoder(); feedback = motor.getPIDController(); + + /* Factory reset SparkMax */ + safeSparkMax(motor, motor::restoreFactoryDefaults); + + /* Configure motor invert */ + motor.setInverted(invert); + + /* Configure electrical */ + safeSparkMax(motor, () -> motor.setSmartCurrentLimit(20)); + safeSparkMax(motor, () -> motor.enableVoltageCompensation(12.0)); + safeSparkMax(motor, () -> motor.setIdleMode(IdleMode.kBrake)); + + /* Save configurations */ + safeBurnSparkMax(motor); + encoder.setAverageDepth(8); encoder.setMeasurementPeriod(8); feedback.setFeedbackDevice(encoder); @@ -45,9 +54,9 @@ public void updateInputs(ShooterModuleIOInputs inputs) { @Override public void configurePID(double kP, double kI, double kD) { - feedback.setP(kP, 0); - feedback.setI(kI, 0); - feedback.setD(kD, 0); + safeSparkMax(motor, () -> feedback.setP(kP, 0)); + safeSparkMax(motor, () -> feedback.setI(kI, 0)); + safeSparkMax(motor, () -> feedback.setD(kD, 0)); } @Override diff --git a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterSubsystem.java index 8c4020b..e8dc6b0 100644 --- a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -1,7 +1,8 @@ package ca.warp7.frc2024.subsystems.shooter; +import static ca.warp7.frc2024.subsystems.shooter.ShooterConstants.*; + import ca.warp7.frc2024.util.LoggedTunableNumber; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import org.littletonrobotics.junction.Logger; @@ -10,11 +11,13 @@ public class ShooterSubsystem extends SubsystemBase { private final ShooterModule[] shooterModules; - private final SysIdRoutine sysId; + protected final SysIdRoutine sysId; + + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Shooter/Gains/kP", Gains.kP()); + private static final LoggedTunableNumber kI = new LoggedTunableNumber("Shooter/Gains/kI", Gains.kI()); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Shooter/Gains/kD", Gains.kD()); - private static final LoggedTunableNumber kP = new LoggedTunableNumber("Shooter/Gains/kP", 0.00065); - private static final LoggedTunableNumber kI = new LoggedTunableNumber("Shooter/Gains/kI", 0); - private static final LoggedTunableNumber kD = new LoggedTunableNumber("Shooter/Gains/kD", 0.01); + protected Goal currentGoal = Goal.IDLE; public ShooterSubsystem( ShooterModuleIO topRightShooterModuleIO, @@ -22,17 +25,20 @@ public ShooterSubsystem( ShooterModuleIO bottomLeftShooterModuleIO, ShooterModuleIO bottomRightShooterModuleIO) { shooterModules = new ShooterModule[] { - new ShooterModule(topRightShooterModuleIO, 0, "TopRight"), - new ShooterModule(topLeftShooterModuleIO, 1, "TopLeft"), - new ShooterModule(bottomLeftShooterModuleIO, 2, "BottomLeft"), - new ShooterModule(bottomRightShooterModuleIO, 3, "BottomRight") + new ShooterModule(topRightShooterModuleIO, "TopRight"), + new ShooterModule(topLeftShooterModuleIO, "TopLeft"), + new ShooterModule(bottomLeftShooterModuleIO, "BottomLeft"), + new ShooterModule(bottomRightShooterModuleIO, "BottomRight") }; sysId = new SysIdRoutine( new SysIdRoutine.Config( - null, null, null, (state) -> Logger.recordOutput("Shooter/SysIdState", state.toString())), + null, + edu.wpi.first.units.Units.Volts.of(10), + edu.wpi.first.units.Units.Seconds.of(13), + (state) -> Logger.recordOutput("Shooter/SysIdState", state.toString())), new SysIdRoutine.Mechanism( - (voltage) -> runShooterVolts(voltage.in(edu.wpi.first.units.Units.Volts), 0), null, this)); + (voltage) -> setVoltage(voltage.in(edu.wpi.first.units.Units.Volts), 0), null, this)); } @Override @@ -40,10 +46,9 @@ public void periodic() { LoggedTunableNumber.ifChanged( hashCode(), () -> { - shooterModules[0].configurePID(kP.get(), kI.get(), kD.get()); - shooterModules[1].configurePID(kP.get(), kI.get(), kD.get()); - shooterModules[2].configurePID(kP.get(), kI.get(), kD.get()); - shooterModules[3].configurePID(kP.get(), kI.get(), kD.get()); + for (int i = 0; i < 4; i++) { + shooterModules[i].configurePID(kP.get(), kI.get(), kD.get()); + } }, kP, kI, @@ -54,61 +59,41 @@ public void periodic() { } } - public void runShooterVolts(double volts, int... shooterModules) { + protected void setVoltage(double volts, int... shooterModules) { + currentGoal = Goal.IDLE; for (var shooterModule : shooterModules) { - this.shooterModules[shooterModule].runShooterVolts(volts); + this.shooterModules[shooterModule].setVoltage(volts); } } - public void setRPM(double RPM, int... shooterModules) { + protected void setVelocity(double RPM, int... shooterModules) { + currentGoal = Goal.IDLE; for (var shooterModule : shooterModules) { - this.shooterModules[shooterModule].runShooterTargetVelocity(RPM); + this.shooterModules[shooterModule].setVelocity(RPM); + } + } + + protected void setGoal(Goal goal) { + currentGoal = goal; + for (int i = 0; i < 4; i++) { + shooterModules[i].setVelocity(goal.getSpeeds()[i]); } } - public double getShooterVelocityRPM(int shooterModule) { + protected double getShooterVelocityRPM(int shooterModule) { return shooterModules[shooterModule].getVelocityRPM(); } - public void stopShooter() { + protected void stopShooter() { + currentGoal = Goal.IDLE; for (var shooterModule : shooterModules) { shooterModule.stopShooter(); } } - public void zeroEncoder() { + protected void zeroEncoder() { for (var shooterModule : shooterModules) { shooterModule.zeroEncoder(); } } - - public Command runRPMCommand(double RPM, int... shooterModules) { - return this.runOnce(() -> setRPM(RPM, shooterModules)); - } - - public Command runVoltageCommand(double volts, int... shooterModules) { - return this.runOnce(() -> runShooterVolts(volts, shooterModules)); - } - - public Command stopShooterCommand() { - return this.runOnce(() -> stopShooter()); - } - - /** - * Shooter quasistatic SysID routine - * @param direction - * @return - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); - } - - /** - * Shooter dynamic SysID routine - * @param direction - * @return - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } } diff --git a/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterSubsystemCommands.java b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterSubsystemCommands.java new file mode 100644 index 0000000..e31bfe3 --- /dev/null +++ b/src/main/java/ca/warp7/frc2024/subsystems/shooter/ShooterSubsystemCommands.java @@ -0,0 +1,67 @@ +package ca.warp7.frc2024.subsystems.shooter; + +import static ca.warp7.frc2024.subsystems.shooter.ShooterConstants.*; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class ShooterSubsystemCommands extends ShooterSubsystem { + + public ShooterSubsystemCommands( + ShooterModuleIO topRightShooterModuleIO, + ShooterModuleIO topLeftShooterModuleIO, + ShooterModuleIO bottomLeftShooterModuleIO, + ShooterModuleIO bottomRightShooterModuleIO) { + super(topRightShooterModuleIO, topLeftShooterModuleIO, bottomLeftShooterModuleIO, bottomRightShooterModuleIO); + } + + public Command runVelocityCommand(double RPM, int... shooterModules) { + return super.runOnce(() -> setVelocity(RPM, shooterModules)); + } + + public Command runVelocityCommandEnds(double RPM, int... shooterModules) { + return super.startEnd(() -> setVelocity(RPM, shooterModules), () -> stopShooter()); + } + + public Command runVoltageCommand(double volts, int... shooterModules) { + return super.runOnce(() -> setVoltage(volts, shooterModules)); + } + + public Command runVoltageCommandEnds(double volts, int... shooterModules) { + return super.startEnd(() -> setVoltage(volts, shooterModules), () -> stopShooter()); + } + + public Command runGoalCommand(Goal goal) { + return super.runOnce(() -> setGoal(goal)); + } + + public Command runGoalCommandEnds(Goal goal) { + return super.startEnd(() -> setGoal(goal), () -> stopShooter()); + } + + public Command stopShooterCommand() { + return super.runOnce(() -> stopShooter()); + } + + public Command zeroEncoderCommand() { + return super.runOnce(() -> zeroEncoder()); + } + + /** + * Shooter quasistatic SysID routine + * @param direction + * @return + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + /** + * Shooter dynamic SysID routine + * @param direction + * @return + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } +} diff --git a/src/main/java/ca/warp7/frc2024/util/LimelightHelpers.java b/src/main/java/ca/warp7/frc2024/util/LimelightHelpers.java index 98b95a1..3b8138e 100644 --- a/src/main/java/ca/warp7/frc2024/util/LimelightHelpers.java +++ b/src/main/java/ca/warp7/frc2024/util/LimelightHelpers.java @@ -1,4 +1,4 @@ -// LimelightHelpers v1.3.1 (March 4, 2024) +// LimelightHelpers v1.4.0 (March 21, 2024) package ca.warp7.frc2024.util; @@ -371,6 +371,33 @@ public LimelightResults() { } } + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + public RawFiducial( + int id, + double txnc, + double tync, + double ta, + double distToCamera, + double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -379,6 +406,7 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; + public RawFiducial[] rawFiducials; public PoseEstimate( Pose2d pose, @@ -387,7 +415,9 @@ public PoseEstimate( int tagCount, double tagSpan, double avgTagDist, - double avgTagArea) { + double avgTagArea, + RawFiducial[] rawFiducials) { + this.pose = pose; this.timestampSeconds = timestampSeconds; this.latency = latency; @@ -395,6 +425,7 @@ public PoseEstimate( this.tagSpan = tagSpan; this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -453,7 +484,63 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr double tagArea = extractBotPoseEntry(poseArray, 10); // getlastchange() in microseconds, ll latency in milliseconds var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); - return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } } public static NetworkTable getLimelightNTTable(String tableName) { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 3ec4c12..a019706 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.6", + "version": "2024.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.6" + "version": "2024.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.6", + "version": "2024.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf8..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 0000000..998c261 --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.1.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.1.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} \ No newline at end of file