diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 7d46ee87..b1993e77 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,18 +4,30 @@ "holonomicMode": true, "pathFolders": [ "3NoteAutoThroughAmp", + "3NoteAutoThroughAmpLeft", "5NoteAutoAroundStage", "7NoteAuto", "FrontRow4NoteAuto", "MiddleRush3NoteAuto", + "FrontRow4NoteAutoFarShoot", "SourceSide3NoteAuto", + "SourceSide3NoteAutoFront", + "SourceSide3NoteAutoRight", "SpeakerSide3NoteAuto", - "Zayde3NoteAuto" + "Zayde3NoteAuto", + "test" + ], + "autoFolders": [ + "3NoteAutoThroughAmpVarients", + "frontRow", + "FarNotesAroundStageVarients", + "OtherAutos", + "SourceSide3NoteAutoVarients", + "misc" ], - "autoFolders": [], "defaultMaxVel": 4.0, "defaultMaxAccel": 4.0, "defaultMaxAngVel": 400.0, - "defaultMaxAngAccel": 600.0, + "defaultMaxAngAccel": 400.0, "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index 7752faf1..774465e5 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.10' + implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.13' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index a1e8eeb5..76667308 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7531929553170285, - "y": 6.932078092184688 + "x": 0.6849446477990359, + "y": 6.688334136763285 }, "rotation": 60.1093232393867 }, @@ -18,41 +18,23 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" } }, { "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp2" - } - }, { "type": "wait", "data": { - "waitTime": 0.1 + "waitTime": 0.7 } }, { @@ -60,53 +42,294 @@ "data": { "name": "FeedNote" } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp3" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp4" - } - }, + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp5" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp6" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -124,6 +347,6 @@ ] } }, - "folder": null, + "folder": "3NoteAutoThroughAmpVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto new file mode 100644 index 00000000..e9269e1a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -0,0 +1,397 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6849446477990359, + "y": 6.688334136763285 + }, + "rotation": 60.1093232393867 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "3NoteAutoThroughAmpVarients", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto index d75d1da0..19ccb576 100644 --- a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.2699301408104018, - "y": 6.932078092184688 + "x": 0.7402504945854966, + "y": 6.605014900170646 }, "rotation": 52.489999999999995 }, @@ -18,7 +18,13 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" } }, { @@ -28,7 +34,7 @@ { "type": "wait", "data": { - "waitTime": 0.5 + "waitTime": 0.7 } }, { @@ -36,77 +42,235 @@ "data": { "name": "FeedNote" } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage1" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage2" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage3" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage4" - } - }, + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage2" + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage5" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage6" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage4" + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage6" + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -114,10 +278,16 @@ } ] } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } } ] } }, - "folder": null, + "folder": "FarNotesAroundStageVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto index 485a8327..e3b7463e 100644 --- a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto @@ -18,7 +18,13 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" } }, { @@ -28,55 +34,7 @@ { "type": "wait", "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage1" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage2" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage3" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage4" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 + "waitTime": 0.7 } }, { @@ -84,53 +42,305 @@ "data": { "name": "FeedNote" } - }, + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage5" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage6" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage7" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage8" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage8" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -148,6 +358,6 @@ ] } }, - "folder": null, + "folder": "FarNotesAroundStageVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/7NoteAuto.auto b/src/main/deploy/pathplanner/autos/7NoteAuto.auto index 9011e633..bebc1a87 100644 --- a/src/main/deploy/pathplanner/autos/7NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/7NoteAuto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.1882930808115142, - "y": 4.148007169253776 + "x": 0.6751948895821795, + "y": 4.387391197585246 }, - "rotation": -45.77083940144205 + "rotation": -59.76613049448032 }, "command": { "type": "sequential", @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitch" } }, { @@ -26,147 +26,521 @@ "data": { "commands": [ { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.5 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto1" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto2" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto3" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto4" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto8" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto9" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto5" - } - }, - { - "type": "path", - "data": { - "pathName": "7NoteAuto6" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "7NoteAuto7" - } - }, - { - "type": "path", - "data": { - "pathName": "7NoteAuto8" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "7NoteAuto9" - } - }, - { - "type": "path", - "data": { - "pathName": "7NoteAuto10" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto10" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -184,6 +558,6 @@ ] } }, - "folder": null, + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index faac7163..ff4fe261 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForCloseShot" + "name": "PreparePitchForCloseShot" } }, { @@ -26,87 +26,341 @@ "data": { "commands": [ { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.5 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto1" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto2" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto3" - } - }, - { - "type": "path", - "data": { - "pathName": "FrontRow4NoteAuto4" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "FrontRow4NoteAuto5" - } - }, - { - "type": "path", - "data": { - "pathName": "FrontRow4NoteAuto6" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } } ] @@ -124,6 +378,6 @@ ] } }, - "folder": null, + "folder": "frontRow", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto new file mode 100644 index 00000000..3b26902b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto @@ -0,0 +1,365 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3973572133190786, + "y": 5.565873600441298 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAutoFarShoot2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAutoFarShoot3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAutoFarShoot4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAutoFarShoot5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "frontRow", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootSide.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootSide.auto new file mode 100644 index 00000000..ac715aa8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootSide.auto @@ -0,0 +1,359 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6751948895821795, + "y": 4.387391197585246 + }, + "rotation": -59.53445508054011 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "frontRow", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index 096bb93b..4feb902e 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -18,29 +18,42 @@ { "type": "named", "data": { - "name": "PrepareForCloseEjectFromShooter" + "name": "PreparePitchForCloseEjectFromShooter" } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "MiddleRush3NoteAuto1" - } - }, { "type": "named", "data": { - "name": "FeedNote" + "name": "PrepareShootingForCloseEjectFromShooter" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.05 + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] @@ -56,7 +69,7 @@ { "type": "named", "data": { - "name": "PrepareForEjectFromShooter" + "name": "PreparePitchForEjectFromShooter" } }, { @@ -64,27 +77,73 @@ "data": { "commands": [ { - "type": "path", + "type": "race", "data": { - "pathName": "MiddleRush3NoteAuto2" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "MiddleRush3NoteAuto3" - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.05 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShootingForCloseEjectFromShooter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -100,7 +159,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitch" } }, { @@ -108,75 +167,213 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "MiddleRush3NoteAuto4" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "MiddleRush3NoteAuto5" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "MiddleRush3NoteAuto6" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "MiddleRush3NoteAuto7" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto8" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "MiddleRush3NoteAuto8" - } - }, - { - "type": "path", - "data": { - "pathName": "MiddleRush3NoteAuto9" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto9" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -194,6 +391,6 @@ ] } }, - "folder": null, + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index 73a447bc..e4913ae4 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7547034812379487, - "y": 3.84449444955228 + "x": 0.6946944060158917, + "y": 4.426390230452671 }, - "rotation": 0 + "rotation": -58.62699485989161 }, "command": { "type": "sequential", @@ -18,47 +18,23 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" } }, { "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto2" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto3" - } - }, { "type": "wait", "data": { - "waitTime": 0.1 + "waitTime": 0.8 } }, { @@ -66,29 +42,191 @@ "data": { "name": "FeedNote" } - }, + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ { - "type": "path", + "type": "race", "data": { - "pathName": "SourceSide3NoteAuto4" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "SourceSide3NoteAuto5" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -106,6 +244,6 @@ ] } }, - "folder": null, + "folder": "SourceSide3NoteAutoVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoFront.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoFront.auto new file mode 100644 index 00000000..9b1977f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoFront.auto @@ -0,0 +1,282 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6946944060158917, + "y": 4.426390230452671 + }, + "rotation": -60.10109816138545 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoFront4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoFront5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "SourceSide3NoteAutoVarients", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto new file mode 100644 index 00000000..28036156 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -0,0 +1,256 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6946944060158917, + "y": 4.426390230452671 + }, + "rotation": -60.10109816138545 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "SourceSide3NoteAutoVarients", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto index 286efbda..8a4a7a79 100644 --- a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto @@ -18,7 +18,13 @@ { "type": "named", "data": { - "name": "PrepareForCloseEjectFromShooter" + "name": "PreparePitchForCloseEjectFromShooter" + } + }, + { + "type": "named", + "data": { + "name": "PrepareShootingForCloseEjectFromShooter" } }, { @@ -56,7 +62,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitch" } }, { @@ -64,75 +70,297 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "SpeakerSide3NoteAuto2" - } - }, - { - "type": "path", - "data": { - "pathName": "SpeakerSide3NoteAuto3" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SpeakerSide3NoteAuto4" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "SpeakerSide3NoteAuto5" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "SpeakerSide3NoteAuto6" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "SpeakerSide3NoteAuto7" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -150,6 +378,6 @@ ] } }, - "folder": null, + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto index d36b7d2c..497eccc0 100644 --- a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 2, - "y": 2 + "x": 1.2894639873786793, + "y": 2.0234181313433055 }, "rotation": 0 }, @@ -18,7 +18,13 @@ { "type": "named", "data": { - "name": "PrepareForCloseEjectFromShooter" + "name": "PreparePitchForCloseEjectFromShooter" + } + }, + { + "type": "named", + "data": { + "name": "PrepareShootingForCloseEjectFromShooter" } }, { @@ -56,7 +62,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PreparePitch" } }, { @@ -64,75 +70,213 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "Zayde3NoteAuto2" - } - }, - { - "type": "path", - "data": { - "pathName": "Zayde3NoteAuto3" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "Zayde3NoteAuto4" - } - }, - { - "type": "path", - "data": { - "pathName": "Zayde3NoteAuto5" - } - }, - { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "Zayde3NoteAuto6" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "Zayde3NoteAuto7" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -150,6 +294,6 @@ ] } }, - "folder": null, + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto new file mode 100644 index 00000000..8c0a7b35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -0,0 +1,109 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3239288017037572, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "test1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "test2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": "misc", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index e9427238..ac8af41f 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.8159166009543433, - "y": 6.932078092184688 + "x": 0.6924444618120005, + "y": 6.677362186633205 }, "prevControl": null, "nextControl": { - "x": 2.446567628177991, - "y": 6.443479097784407 + "x": 1.4993411183403562, + "y": 6.595503105536126 }, "isLocked": false, "linkedName": "3NoteAutoThroughAmpFirstFeed" @@ -20,7 +20,7 @@ "y": 6.995245539786855 }, "prevControl": { - "x": 2.6570624081419103, + "x": 2.329626083753592, "y": 6.794303731057604 }, "nextControl": null, @@ -30,40 +30,22 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, - "rotation": 28.47, + "rotation": 24.32091962058401, "rotateFast": false }, "reversed": false, "folder": "3NoteAutoThroughAmp", "previewStartingState": { - "rotation": 38.0, + "rotation": 59.30027744918559, "velocity": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index 13331f8c..b9f74d07 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -28,38 +28,20 @@ "linkedName": "3NoteAutoThroughAmpSecondCollection" } ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ + "rotationTargets": [ { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } + "waypointRelativePos": 0.45, + "rotationDegrees": -20.0, + "rotateFast": false } ], + "constraintZones": [], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path index dfc4f69c..3f34b4d5 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index 5ff5405f..77b1f652 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -30,36 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - }, - { - "type": "named", - "data": { - "name": "AlignToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path index 772b1882..b4b688f9 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path new file mode 100644 index 00000000..ac1c6503 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.879292519148419, + "y": 6.995245539786855 + }, + "prevControl": null, + "nextControl": { + "x": 5.793817953431632, + "y": 7.107573740088099 + }, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpFirstCollection" + }, + { + "anchor": { + "x": 8.255803553860629, + "y": 7.443288126012872 + }, + "prevControl": { + "x": 7.85101693718827, + "y": 7.380566970160069 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpLeftSecondCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 7.643378071479572, + "rotateFast": false + }, + "reversed": false, + "folder": "3NoteAutoThroughAmpLeft", + "previewStartingState": { + "rotation": 28.370000000000005, + "velocity": 1.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path new file mode 100644 index 00000000..2aa81026 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.255803553860629, + "y": 7.443288126012872 + }, + "prevControl": null, + "nextControl": { + "x": 7.489795261280664, + "y": 7.197587352921185 + }, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpLeftSecondCollection" + }, + { + "anchor": { + "x": 3.7171194532412826, + "y": 6.269094533438474 + }, + "prevControl": { + "x": 4.61365091744268, + "y": 6.576108926865741 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpThirdFeed" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 10.719999999999999, + "rotateFast": false + }, + "reversed": false, + "folder": "3NoteAutoThroughAmpLeft", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path new file mode 100644 index 00000000..2cebea92 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.7171194532412826, + "y": 6.269094533438474 + }, + "prevControl": null, + "nextControl": { + "x": 4.931616623796628, + "y": 6.590561913518194 + }, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpThirdFeed" + }, + { + "anchor": { + "x": 8.284709527165534, + "y": 5.76674167432842 + }, + "prevControl": { + "x": 7.229641501536524, + "y": 6.373767113731411 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpLeftThirdCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -20.136303428248176, + "rotateFast": false + }, + "reversed": false, + "folder": "3NoteAutoThroughAmpLeft", + "previewStartingState": { + "rotation": 10.759999999999991, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index c5d8c5bd..b618eda3 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2699301408104018, - "y": 6.932078092184688 + "x": 0.7113445212805922, + "y": 6.648373860128002 }, "prevControl": null, "nextControl": { - "x": 1.6306711948340775, - "y": 7.780307057051169 + "x": 1.0720855753042686, + "y": 7.496602824994483 }, "isLocked": false, "linkedName": null @@ -57,36 +57,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 1.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, @@ -96,7 +72,7 @@ "reversed": false, "folder": "5NoteAutoAroundStage", "previewStartingState": { - "rotation": 55.06, + "rotation": 58.240519915187164, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path index f422cfe7..004aba22 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index cb1650c4..114ad791 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path index c57fb19e..3ea93ec7 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index 3db82942..a9cf3d4a 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path index e3f4857e..80bb5fda 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index 4409d6f9..121ffadd 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -30,36 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.8, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path index e8c20446..fbeb59a4 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligningToNote", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto1.path b/src/main/deploy/pathplanner/paths/7NoteAuto1.path index fd1ba5c3..6bc28e07 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.172432558641841, - "y": 4.20214579146498 + "x": 0.6751948895821795, + "y": 4.387391197585246 }, "prevControl": null, "nextControl": { - "x": 2.09865958924317, - "y": 4.1046482092964185 + "x": 1.6014219201835096, + "y": 4.289893615416685 }, "isLocked": false, "linkedName": null @@ -36,30 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, @@ -69,7 +51,7 @@ "reversed": false, "folder": "7NoteAuto", "previewStartingState": { - "rotation": -49.101008357498635, + "rotation": -59.03624346792648, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto10.path b/src/main/deploy/pathplanner/paths/7NoteAuto10.path index 0d9ba444..549e1d98 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto10.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto10.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.10000000000000009, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto2.path b/src/main/deploy/pathplanner/paths/7NoteAuto2.path index 3e9c03e8..1b8f0264 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto2.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto3.path b/src/main/deploy/pathplanner/paths/7NoteAuto3.path index 5f0bf0c7..008b7e87 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto3.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.8, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto4.path b/src/main/deploy/pathplanner/paths/7NoteAuto4.path index 62d67501..635bf9c8 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto4.path @@ -36,30 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index feeda5c4..4a2b2fe6 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -30,36 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.75, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto6.path b/src/main/deploy/pathplanner/paths/7NoteAuto6.path index c44fd3ef..500dfd0f 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto6.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index 8a219a75..fdd92c63 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto8.path b/src/main/deploy/pathplanner/paths/7NoteAuto8.path index 4b2e2f5d..82b9c1b9 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto8.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Stopaligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index 8a5a11c8..03d94dc1 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index 2ff87b9f..5f353d9b 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.0480159688234565, - "y": 6.323768972626285 + "x": 1.9719127324240409, + "y": 6.415340906691315 }, "isLocked": false, "linkedName": "CloseShotPoint" }, { "anchor": { - "x": 2.895296810271618, - "y": 6.990384785778517 + "x": 2.8441688792209487, + "y": 6.969716047694203 }, "prevControl": { - "x": 2.077024083376788, - "y": 6.657362289989598 + "x": 2.1157799657104936, + "y": 6.531168783642987 }, "nextControl": null, "isLocked": false, @@ -30,34 +30,16 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 27.77000000000001, "rotateFast": false }, "reversed": false, @@ -66,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index 4984fa91..ea0e2cdd 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.895296810271618, - "y": 6.990384785778517 + "x": 2.8441688792209487, + "y": 6.969716047694203 }, "prevControl": null, "nextControl": { - "x": 2.164048427036783, - "y": 6.78789880547959 + "x": 2.112920495986114, + "y": 6.7672300673952765 }, "isLocked": false, "linkedName": "FrontRow4NoteAutoFirstCollection" @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index 144fb5fd..46bf26d1 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.895296810271618, + "x": 2.820780570336069, "y": 5.565873600441298 }, "prevControl": { - "x": 2.795296810271618, + "x": 2.720780570336069, "y": 5.565873600441298 }, "nextControl": null, @@ -30,34 +30,16 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -9.652155522293434, "rotateFast": false }, "reversed": false, @@ -66,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index 6659bb1c..f9fd2991 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.895296810271618, + "x": 2.820780570336069, "y": 5.565873600441298 }, "prevControl": null, "nextControl": { - "x": 2.396113343463435, + "x": 2.3215971035278864, "y": 5.555053936962998 }, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index 91ecdd87..122b6ac8 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -30,34 +30,16 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 1, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -8.487656913310161, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index b14fe81b..439da821 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot2.path new file mode 100644 index 00000000..df221fce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8441688792209487, + "y": 6.969716047694203 + }, + "prevControl": null, + "nextControl": { + "x": 2.2126845393291923, + "y": 6.747527113287845 + }, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoFirstCollection" + }, + { + "anchor": { + "x": 2.820780570336069, + "y": 5.565873600441298 + }, + "prevControl": { + "x": 1.1134340217398382, + "y": 6.186207700050727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoSecondCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -18.823282103833993, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAutoFarShoot", + "previewStartingState": { + "rotation": 27.819999999999993, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot3.path new file mode 100644 index 00000000..4073b31f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.820780570336069, + "y": 5.565873600441298 + }, + "prevControl": null, + "nextControl": { + "x": 2.547148467218551, + "y": 5.586611458258547 + }, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoSecondCollection" + }, + { + "anchor": { + "x": 2.0791600728094584, + "y": 5.615860732909115 + }, + "prevControl": { + "x": 2.371652819315141, + "y": 5.606110974692258 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoFarShootSecondFeed" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.3322198538695922, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAutoFarShoot", + "previewStartingState": { + "rotation": -19.82440273715712, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot4.path new file mode 100644 index 00000000..11ab0490 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0791600728094584, + "y": 5.615860732909115 + }, + "prevControl": null, + "nextControl": { + "x": 1.6111716784003651, + "y": 4.777381526259489 + }, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoFarShootSecondFeed" + }, + { + "anchor": { + "x": 2.7337543047179125, + "y": 4.097305368134887 + }, + "prevControl": { + "x": 1.757418051653207, + "y": 4.260644340766117 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoThirdCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -0.4057415406159514, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAutoFarShoot", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot5.path new file mode 100644 index 00000000..ebc6628e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.7337543047179125, + "y": 4.097305368134887 + }, + "prevControl": null, + "nextControl": { + "x": 2.488649917917414, + "y": 4.065649176428995 + }, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoThirdCollection" + }, + { + "anchor": { + "x": 2.030411281725178, + "y": 4.097305368134887 + }, + "prevControl": { + "x": 2.2839049953634354, + "y": 4.1046482092964185 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -38.64999999999998, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAutoFarShoot", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path index 57ee08a9..82b00e9d 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index 06caaf6b..33b5742d 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path index 4cfc2e8f..1086a5fd 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index 111d74b7..d10eaa4b 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path index 45b515ec..85825320 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 95a38025..464bdc93 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "ReCollectNote", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path index f4c6b380..7aa818ba 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index b8525708..7dff55e2 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -30,36 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "RecollectNote", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path index 12de1af1..4c96d09a 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 2c8e2fe2..6834105c 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.7547034812379487, - "y": 3.84449444955228 + "x": 0.6946944060158917, + "y": 4.426390230452671 }, "prevControl": null, "nextControl": { - "x": 1.7086006002997924, - "y": 3.3530929033689056 + "x": 1.494174579798092, + "y": 3.7341573970558875 }, "isLocked": false, - "linkedName": null + "linkedName": "SourceSide3NoteAutoStartPoint" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 2.778681091803989 }, "prevControl": { - "x": 2.059866537183144, - "y": 3.1400057581152936 + "x": 2.1084093474600265, + "y": 3.3051680355142192 }, "nextControl": null, "isLocked": false, @@ -35,17 +35,17 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, - "rotation": -44.60000000000002, + "rotation": -45.25, "rotateFast": false }, "reversed": false, "folder": "SourceSide3NoteAuto", "previewStartingState": { - "rotation": -38.659808254090066, + "rotation": -59.03624346792651, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path index 907bbf0c..251aa288 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path index bf6a72f5..b9902f4a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path index 63fbc590..2415d65e 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path index 2f2f8330..0c583b16 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront4.path new file mode 100644 index 00000000..335192c9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.663059280534515, + "y": 4.584108541436456 + }, + "prevControl": null, + "nextControl": { + "x": 1.8852482149408742, + "y": 4.326837143702777 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondFeed" + }, + { + "anchor": { + "x": 2.7155331803541096, + "y": 4.1046482092964185 + }, + "prevControl": { + "x": 2.130825458232113, + "y": 4.385307915914978 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoFrontSecondCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.591140271194542, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoFront", + "previewStartingState": { + "rotation": -60.25511870305777, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront5.path new file mode 100644 index 00000000..9f918b1b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.7155331803541096, + "y": 4.1046482092964185 + }, + "prevControl": null, + "nextControl": { + "x": 2.4348734737355513, + "y": 4.151424827066179 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoFrontSecondCollection" + }, + { + "anchor": { + "x": 1.733224207189155, + "y": 5.262369499097973 + }, + "prevControl": { + "x": 2.282849465983831, + "y": 4.8881565569398955 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -10.370000000000005, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoFront", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path new file mode 100644 index 00000000..b63bab9b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6946944060158917, + "y": 4.426390230452671 + }, + "prevControl": null, + "nextControl": { + "x": 4.036972632349823, + "y": 1.239580370898635 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoStartPoint" + }, + { + "anchor": { + "x": 8.281950694955519, + "y": 2.525937359567027 + }, + "prevControl": { + "x": 5.896343188697769, + "y": 1.204497907571314 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 33.340707346477025, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": -57.42594286542754, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path new file mode 100644 index 00000000..873f305e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.281950694955519, + "y": 2.525937359567027 + }, + "prevControl": null, + "nextControl": { + "x": 5.299941312133336, + "y": 0.46776617769759743 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondCollection" + }, + { + "anchor": { + "x": 1.663059280534515, + "y": 4.584108541436456 + }, + "prevControl": { + "x": 2.271155311541392, + "y": 2.315442579603108 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondFeed" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.019999999999982, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": 35.706691400603006, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path new file mode 100644 index 00000000..74276a9e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.663059280534515, + "y": 4.584108541436456 + }, + "prevControl": null, + "nextControl": { + "x": 2.835933559191062, + "y": 1.474204638550123 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondFeed" + }, + { + "anchor": { + "x": 8.281950694955519, + "y": 0.8185908109707958 + }, + "prevControl": { + "x": 6.304650355779585, + "y": 1.170691918848627 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightFirstCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -8.458072159819006, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": -44.00999999999999, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path new file mode 100644 index 00000000..ca033ed3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.281950694955519, + "y": 0.8185908109707958 + }, + "prevControl": null, + "nextControl": { + "x": 5.919731497582653, + "y": 1.5670166952869513 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightFirstCollection" + }, + { + "anchor": { + "x": 3.038275372325393, + "y": 2.746067463965915 + }, + "prevControl": { + "x": 4.832175134435738, + "y": 1.8125939385781908 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -42.51999999999998, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": -20.376435213836338, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path index 1d925fff..fc3d2ce1 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index 0dc4c0f2..073c7dff 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path index 9dc9c24f..5a791c52 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index 6ee78d46..4fcab2a8 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path index 5be32108..3423b676 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index 7d81e619..ed0dac52 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -30,36 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "RecollectNote", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path index a48e02a0..0fd50f8c 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path index f42b13ad..5150ef96 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index 205bc137..afc99303 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path index 8ae59228..74a17c7e 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index 36985c78..755b6489 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -36,36 +36,12 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path index dc588074..357406d4 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index ceb512de..dea0fc5b 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -30,36 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path index dc99d94b..e44da0e0 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path @@ -30,30 +30,12 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.25, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path b/src/main/deploy/pathplanner/paths/test1.path similarity index 58% rename from src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path rename to src/main/deploy/pathplanner/paths/test1.path index 8a0c8c1a..cc487614 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path +++ b/src/main/deploy/pathplanner/paths/test1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 0.7531929553170285, - "y": 6.932078092184688 + "x": 1.3239288017037572, + "y": 5.531335051274091 }, "prevControl": null, "nextControl": { - "x": 1.7531929553170258, - "y": 6.932078092184688 + "x": 2.3062377748687117, + "y": 6.303149244475128 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.8159166009543433, - "y": 6.932078092184688 + "x": 5.8261782620431335, + "y": 5.531335051274091 }, "prevControl": { - "x": 1.6424807611249173, - "y": 6.9176251055322355 + "x": 4.60998620002938, + "y": 6.478561561111727 }, "nextControl": null, "isLocked": false, - "linkedName": "3NoteAutoThroughAmpFirstFeed" + "linkedName": "testAuto1" } ], "rotationTargets": [], @@ -35,17 +35,17 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { - "velocity": 0.0, - "rotation": 38.0, + "velocity": 0, + "rotation": -27.897271030947643, "rotateFast": false }, "reversed": false, - "folder": "3NoteAutoThroughAmp", + "folder": "test", "previewStartingState": { - "rotation": -1.3639275316029946, + "rotation": 0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/test2.path b/src/main/deploy/pathplanner/paths/test2.path new file mode 100644 index 00000000..ac373394 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/test2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.8261782620431335, + "y": 5.531335051274091 + }, + "prevControl": null, + "nextControl": { + "x": 5.416882856557735, + "y": 6.0692661556263285 + }, + "isLocked": false, + "linkedName": "testAuto1" + }, + { + "anchor": { + "x": 4.235773257871302, + "y": 6.537032333323926 + }, + "prevControl": { + "x": 5.0660582232845375, + "y": 6.221290163378048 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 13.139999999999986, + "rotateFast": false + }, + "reversed": false, + "folder": "test", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index b6e35daf..176cc495 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -9,14 +9,18 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.LEDAutoSetupCommand; import frc.trigon.robot.commands.factories.AmpCommands; import frc.trigon.robot.commands.factories.AutonomousCommands; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.commands.factories.ShootingCommands; +import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ampaligner.AmpAligner; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerCommands; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerConstants; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; @@ -27,7 +31,10 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { - public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator(); + public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( + CameraConstants.FRONT_TAG_CAMERA, + CameraConstants.REAR_TAG_CAMERA + ); public static final Swerve SWERVE = new Swerve(); public static final Intake INTAKE = new Intake(); public static final Climber CLIMBER = new Climber(); @@ -69,20 +76,25 @@ private void bindControllerCommands() { OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); - OperatorConstants.ALIGN_TO_SPEAKER_TRIGGER.whileTrue(CommandConstants.FACE_SPEAKER_COMMAND); OperatorConstants.ALIGN_TO_RIGHT_STAGE.whileTrue(CommandConstants.ALIGN_TO_RIGHT_STAGE_COMMAND); OperatorConstants.ALIGN_TO_LEFT_STAGE.whileTrue(CommandConstants.ALIGN_TO_LEFT_STAGE_COMMAND); OperatorConstants.ALIGN_TO_MIDDLE_STAGE.whileTrue(CommandConstants.ALIGN_TO_MIDDLE_STAGE_COMMAND); OperatorConstants.EJECT_NOTE_TRIGGER.whileTrue(CommandConstants.EJECT_COMMAND); + OperatorConstants.HIGH_EJECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getHighEjectNoteCommand()); OperatorConstants.COLLECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getNoteCollectionCommand()); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); + OperatorConstants.LED_AUTO_SETUP_TRIGGER.whileTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); + OperatorConstants.CLIMB_TRIGGER.whileTrue(GeneralCommands.getClimbCommand()); OperatorConstants.MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_DOWN_MANUALLY_COMMAND); OperatorConstants.MOVE_CLIMBER_UP_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_UP_MANUALLY_COMMAND); + OperatorConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY.whileTrue(CommandConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY_COMMAND); + OperatorConstants.MOVE_RIGHT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_RIGHT_CLIMBER_UP_MANUALLY_COMMAND); + OperatorConstants.MOVE_LEFT_CLIMBER_DOWN_MANUALLY.whileTrue(CommandConstants.MOVE_LEFT_CLIMBER_DOWN_MANUALLY_COMMAND); + OperatorConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY_COMMAND); OperatorConstants.OVERRIDE_IS_CLIMBING_TRIGGER.onTrue(CommandConstants.OVERRIDE_IS_CLIMBING_COMMAND); OperatorConstants.SPEAKER_SHOT_TRIGGER.whileTrue(CommandConstants.SHOOT_AT_SPEAKER_COMMAND); @@ -91,8 +103,11 @@ private void bindControllerCommands() { OperatorConstants.DELIVERY_TRIGGER.whileTrue(CommandConstants.DELIVERY_COMMAND); OperatorConstants.MANUAL_LOW_DELIVERY_TRIGGER.whileTrue(ShootingCommands.getManualLowDeliveryCommand()); - OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand()); + OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(true)); + OperatorConstants.AMP_WITHOUT_ALIGN_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(false)); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); + OperatorConstants.REALIGN_AMP_ALIGNER_TRIGGER.whileTrue(AmpAlignerCommands.getSetPositionCommand(AmpAlignerConstants.REALIGN_AMP_ALIGNER_ANGLE)); + OperatorConstants.BLOCK_TRIGGER.whileTrue(GeneralCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); } diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index b99eccd5..1d00b378 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -11,6 +10,7 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -21,38 +21,60 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.NOTE_DETECTION_CAMERA; private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : - new PIDController(0, 0, 0); + new PIDController(0.0002, 0, 0); public AlignToNoteCommand() { addCommands( - getSetCurrentLEDColorCommand().asProxy(), - GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.SELF_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), + getSetCurrentLEDColorCommand(), + GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::shouldAlignToNote).asProxy(), new RunCommand(CAMERA::trackObject) ); } private Command getSetCurrentLEDColorCommand() { return GeneralCommands.getContinuousConditionalCommand( - LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS), - LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), + LEDStripCommands.getBreatheCommand( + IntakeConstants.NOTE_DETECTION_CAMERA_HAS_TARGETS_BREATHING_LEDS_COLOR, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_LEDS_AMOUNT, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_SHOULD_LOOP, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ), + LEDStripCommands.getBreatheCommand( + IntakeConstants.NOTE_DETECTION_CAMERA_HAS_NO_TARGETS_BREATHING_LEDS_COLOR, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_LEDS_AMOUNT, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_SHOULD_LOOP, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ), CAMERA::hasTargets - ); + ).asProxy(); } private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); } + private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); + final double xValue = CommandConstants.calculateDriveStickAxisValue(xPower); + final double yValue = CommandConstants.calculateDriveStickAxisValue(yPower); + + return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); + } + private MirrorableRotation2d getTargetAngle() { - final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); } - private boolean hasTarget() { + private boolean shouldAlignToNote() { return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 0cafd238..8cf855ea 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -17,6 +17,7 @@ import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; +import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.Logger; @@ -27,7 +28,7 @@ import org.trigon.utilities.mirrorable.MirrorableRotation2d; public class CommandConstants { - public static boolean SHOULD_ALIGN_TO_NOTE = true; + public static boolean SHOULD_ALIGN_TO_NOTE = false; private static final XboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; private static final double MINIMUM_TRANSLATION_SHIFT_POWER = 0.18, @@ -44,7 +45,7 @@ public class CommandConstants { () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), - RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), false), new Rotation2d()).get())), + RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), false), new Rotation2d()).get())), SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), @@ -85,11 +86,22 @@ public class CommandConstants { SHOULD_ALIGN_TO_NOTE = false; Logger.recordOutput("ShouldAlignToNote", false); }).ignoringDisable(true), - DEFAULT_LEDS_COMMAND = LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), + DEFAULT_LEDS_COMMAND = LEDStripCommands.getBreatheCommand( + LEDStripConstants.DEFAULT_COMMAND_COLOR, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_LEDS_AMOUNT, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_CYCLE_TIME_SECONDS, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_SHOULD_LOOP, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ), DEFAULT_INTAKE_COMMAND = IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOP), DEFAULT_CLIMBER_COMMAND = ClimberCommands.getStopCommand(), MOVE_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), MOVE_CLIMBER_UP_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_UP_VOLTAGE).alongWith(new InstantCommand(() -> RobotContainer.CLIMBER.setIsClimbing(true))), + MOVE_RIGHT_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE, 0), + MOVE_RIGHT_CLIMBER_UP_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_UP_VOLTAGE, 0), + MOVE_LEFT_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(0, ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), + MOVE_LEFT_CLIMBER_UP_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(0, ClimberConstants.MOVE_CLIMBER_UP_VOLTAGE), OVERRIDE_IS_CLIMBING_COMMAND = new InstantCommand(() -> { RobotContainer.CLIMBER.setIsClimbing(false); Logger.recordOutput("IsClimbing", false); diff --git a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java index cf270e1b..a47124b2 100644 --- a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java +++ b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java @@ -22,6 +22,12 @@ public class LEDAutoSetupCommand extends SequentialCommandGroup { private static final double TOLERANCE_METERS = 0.1, TOLERANCE_DEGREES = 2; + private static final int AMOUNT_OF_SECTIONS = 3; + private final Supplier[] LEDColors = new Supplier[]{ + () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES), + () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS), + () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS) + }; private final Supplier autoName; private Pose2d autoStartPose; @@ -35,16 +41,14 @@ public LEDAutoSetupCommand(Supplier autoName) { addCommands( getUpdateAutoStartPoseCommand(), - LEDStripCommands.getThreeSectionColorCommand( - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS), + LEDStripCommands.getSectionColorCommand( + AMOUNT_OF_SECTIONS, + LEDColors, LEDStripConstants.RIGHT_CLIMBER_LEDS ).alongWith( - LEDStripCommands.getThreeSectionColorCommand( - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS), + LEDStripCommands.getSectionColorCommand( + AMOUNT_OF_SECTIONS, + LEDColors, LEDStripConstants.LEFT_CLIMBER_LEDS ) ) @@ -71,6 +75,6 @@ else if (difference > tolerance) } private Pose2d getCurrentRobotPose() { - return RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + return RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java index 627282d5..8b1e47b9 100644 --- a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java +++ b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java @@ -71,7 +71,7 @@ private double calculateNoteZDifference(double t) { private void configureStartingStats() { startingTimeSeconds = Timer.getFPGATimestamp(); - final Pose2d currentRobotPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d currentRobotPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); final Rotation2d currentRobotAngle = currentRobotPose.getRotation(); final double startingTangentialVelocity = getStartingTangentialVelocity(); @@ -97,7 +97,7 @@ private void configureNoteInAmpStats() { FieldConstants.AMP_TRANSLATION.get().getY(), notePose.getZ()); initialXYVelocity = new Translation2d(0, 0); - initialZVelocity = 2; + initialZVelocity = 0; } private double getStartingTangentialVelocity() { diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 1fcecc9c..561723d8 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -1,6 +1,7 @@ package frc.trigon.robot.commands.factories; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.CommandConstants; @@ -16,6 +17,8 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; public class AmpCommands { + private static boolean IS_FEEDING_NOTE = false; + public static Command getAutonomousScoreInAmpCommand() { return new ParallelCommandGroup( getAutonomousPrepareForAmpCommand(), @@ -24,12 +27,12 @@ public static Command getAutonomousScoreInAmpCommand() { ); } - public static Command getScoreInAmpCommand() { - return new ParallelCommandGroup( + public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { + return new InstantCommand(() -> IS_FEEDING_NOTE = false).andThen(new ParallelCommandGroup( getPrepareForAmpCommand(), GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), - GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) - ); + shouldAlignToAmp ? GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) : GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND) + )); } /** @@ -44,9 +47,9 @@ private static Command getAutonomousPrepareForAmpCommand() { return new ParallelCommandGroup( GeneralCommands.runWhen( AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN) - .alongWith(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.AMP_PITCH)), - () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation().getDistance( - FieldConstants.IN_FRONT_OF_AMP_POSE.get().getTranslation()) < FieldConstants.MINIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS + .alongWith(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.PREPARE_AMP_PITCH)), + () -> RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation().getDistance( + FieldConstants.IN_FRONT_OF_AMP_POSE.get().getTranslation()) < FieldConstants.MAXIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS ), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND) ); @@ -55,13 +58,13 @@ private static Command getAutonomousPrepareForAmpCommand() { private static Command getPrepareForAmpCommand() { return new ParallelCommandGroup( AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN), - PitcherCommands.getSetTargetPitchCommand(ShootingConstants.AMP_PITCH), + GeneralCommands.getContinuousConditionalCommand(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.SHOOT_AMP_PITCH), PitcherCommands.getSetTargetPitchCommand(ShootingConstants.PREPARE_AMP_PITCH), () -> IS_FEEDING_NOTE), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND) ); } private static Command getFeedToAmpCommand() { - return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch() && RobotContainer.AMP_ALIGNER.atTargetState()); + return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()).alongWith(new InstantCommand(() -> IS_FEEDING_NOTE = true)), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch() && RobotContainer.AMP_ALIGNER.atTargetState()); } private static Command getPathfindToAmpCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 18e7b78c..195262a7 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -5,11 +5,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; @@ -19,6 +19,7 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.ShooterCommands; +import frc.trigon.robot.subsystems.shooter.ShooterConstants; import org.trigon.utilities.mirrorable.MirrorablePose2d; import java.util.Optional; @@ -37,20 +38,24 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { if (DriverStation.isEnabled()) return; final Pose2d autoStartPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathName.get()); - RobotContainer.POSE_ESTIMATOR.resetPose(new MirrorablePose2d(autoStartPose, true).get()); + final MirrorablePose2d correctedAutoStartPose = new MirrorablePose2d(autoStartPose, true); + RobotContainer.POSE_ESTIMATOR.resetPose(correctedAutoStartPose.get()); } ).ignoringDisable(true); } public static Command getNoteCollectionCommand() { - return IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT).onlyIf(() -> !RobotContainer.INTAKE.hasNote()); + return new ParallelCommandGroup( + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), + ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND) + ); } public static Command getFeedNoteCommand() { return new ParallelCommandGroup( IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_SHOOTING), GeneralCommands.getVisualizeNoteShootingCommand() - ).until(() -> !RobotContainer.INTAKE.hasNote()); + ).withTimeout(AutonomousConstants.AUTONOMOUS_FEEDING_TIME_SECONDS); } public static Command getAlignToNoteCommand() { @@ -72,30 +77,34 @@ public static Command getStopAligningToNoteCommand() { ); } - public static Command getPrepareForShooterEjectionCommand(boolean isClose) { - return isClose ? getPrepareForCloseShooterEjectionCommand() : getPrepareForShooterEjectionCommand(); - } - - private static Command getPrepareForShooterEjectionCommand() { + public static Command getPreparePitchForSpeakerShotCommand() { return new ParallelCommandGroup( - PitcherCommands.getSetTargetPitchCommand(ShootingConstants.EJECT_FROM_SHOOTER_PITCH), - ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND) + ShootingCommands.getUpdateShootingCalculationsCommand(false), + PitcherCommands.getReachTargetPitchFromShootingCalculationsCommand() ); } - private static Command getPrepareForCloseShooterEjectionCommand() { + public static Command getPreparePitchCommand(Rotation2d pitch) { return new ParallelCommandGroup( - PitcherCommands.getSetTargetPitchCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_PITCH), - ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND) + ShootingCommands.getUpdateShootingCalculationsCommand(false), + PitcherCommands.getSetTargetPitchCommand(pitch) ); } + public static Command getPrepareShooterForSpeakerShotCommand() { + return ShooterCommands.getReachTargetShootingVelocityFromShootingCalculationsCommand(); + } + + public static Command getPrepareShooterCommand(double targetVelocityRotationsPerSecond) { + return ShooterCommands.getSetTargetVelocityCommand(targetVelocityRotationsPerSecond, targetVelocityRotationsPerSecond * ShooterConstants.RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO); + } + private static Optional calculateRotationOverride() { NOTE_DETECTION_CAMERA.trackObject(); if (RobotContainer.INTAKE.hasNote() || !NOTE_DETECTION_CAMERA.hasTargets()) return Optional.empty(); - final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); final Rotation2d targetRotation = NOTE_DETECTION_CAMERA.getTrackedObjectYaw().minus(currentRotation); return Optional.of(targetRotation); } @@ -106,8 +115,8 @@ private static void overrideRotation(Supplier> rotationOver private static Command getSetCurrentLEDColorCommand() { return GeneralCommands.getContinuousConditionalCommand( - LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS), - LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), + LEDStripCommands.getStaticColorCommand(IntakeConstants.NOTE_DETECTION_CAMERA_HAS_TARGETS_BREATHING_LEDS_COLOR, LEDStrip.LED_STRIPS), + LEDStripCommands.getStaticColorCommand(IntakeConstants.NOTE_DETECTION_CAMERA_HAS_NO_TARGETS_BREATHING_LEDS_COLOR, LEDStrip.LED_STRIPS), NOTE_DETECTION_CAMERA::hasTargets ).asProxy().until(() -> !IS_ALIGNING_TO_NOTE); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 5acb142b..b77f326d 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -1,13 +1,15 @@ package frc.trigon.robot.commands.factories; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.AlignToNoteCommand; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.VisualizeNoteShootingCommand; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerCommands; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; @@ -16,6 +18,7 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.pitcher.PitcherConstants; +import frc.trigon.robot.subsystems.shooter.ShooterCommands; import org.littletonrobotics.junction.Logger; import java.util.function.BooleanSupplier; @@ -31,7 +34,7 @@ public static Command getClimbCommand() { Logger.recordOutput("IsClimbing", true); } ), - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB).until(() -> OperatorConstants.CONTINUE_TRIGGER.getAsBoolean() && RobotContainer.CLIMBER.atTargetState()), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB).until(OperatorConstants.CONTINUE_TRIGGER), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB) ); } @@ -39,11 +42,33 @@ public static Command getClimbCommand() { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), - LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), - IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT) + LEDStripCommands.getBreatheCommand( + IntakeConstants.COLLECTION_BREATHING_LEDS_COLOR, + IntakeConstants.COLLECTION_BREATHING_LEDS_AMOUNT, + IntakeConstants.COLLECTION_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.COLLECTION_BREATHING_SHOULD_LOOP, + IntakeConstants.COLLECTION_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), + ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).unless(RobotContainer.INTAKE::hasNote) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); } + public static Command getHighEjectNoteCommand() { + return new ParallelCommandGroup( + PitcherCommands.getSetTargetPitchCommand(ShootingConstants.HIGH_EJECT_PITCH), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.EJECT) + ); + } + + public static Command getBlockCommand() { + return new ParallelCommandGroup( + AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN), + PitcherCommands.getSetTargetPitchCommand(PitcherConstants.BLOCK_PITCH) + ); + } + public static Command getVisualizeNoteShootingCommand() { return new InstantCommand( () -> new VisualizeNoteShootingCommand() @@ -111,6 +136,10 @@ public static Command runWhen(Command command, BooleanSupplier condition) { return new WaitUntilCommand(condition).andThen(command); } + public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) { + return new WaitUntilCommand(condition).andThen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition))); + } + public static Command duplicate(Command command) { return new FunctionalCommand( command::initialize, diff --git a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java index 8bf7c3a4..31411379 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java @@ -43,7 +43,7 @@ public static Command getWarmSpeakerShotCommand() { public static Command getCloseSpeakerShotCommand() { return new ParallelCommandGroup( getPrepareCloseSpeakerShotCommand(), - getFeedNoteWhenPitcherAndShooterReadyCommand() + GeneralCommands.runWhenContinueTriggerPressed(getFeedNoteWhenPitcherAndShooterReadyCommand()) ); } diff --git a/src/main/java/frc/trigon/robot/constants/2024-crescendo-home-tag-layout.json b/src/main/java/frc/trigon/robot/constants/2024-crescendo-home-tag-layout.json new file mode 100644 index 00000000..122b7fba --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/2024-crescendo-home-tag-layout.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.98, + "z": 1.508 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.508 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index f409be51..1eee0566 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -3,11 +3,11 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathConstraints; import frc.trigon.robot.commands.factories.AutonomousCommands; -import frc.trigon.robot.commands.factories.ShootingCommands; import frc.trigon.robot.subsystems.shooter.ShooterCommands; public class AutonomousConstants { public static final PathConstraints REAL_TIME_CONSTRAINTS = new PathConstraints(2.5, 2.5, 4, 4); + public static final double AUTONOMOUS_FEEDING_TIME_SECONDS = 0.7; static { registerCommands(); @@ -17,11 +17,15 @@ private static void registerCommands() { NamedCommands.registerCommand("Collect", AutonomousCommands.getNoteCollectionCommand()); NamedCommands.registerCommand("AlignToNote", AutonomousCommands.getAlignToNoteCommand()); NamedCommands.registerCommand("StopAligningToNote", AutonomousCommands.getStopAligningToNoteCommand()); - NamedCommands.registerCommand("PrepareForShooting", ShootingCommands.getWarmSpeakerShotCommand()); + NamedCommands.registerCommand("PrepareForShooting", AutonomousCommands.getPrepareShooterForSpeakerShotCommand()); + NamedCommands.registerCommand("PreparePitch", AutonomousCommands.getPreparePitchForSpeakerShotCommand()); + NamedCommands.registerCommand("PrepareForShootingCloseShot", AutonomousCommands.getPrepareShooterCommand(ShootingConstants.CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PreparePitchForCloseShot", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_SHOT_PITCH)); NamedCommands.registerCommand("StopShooting", ShooterCommands.getStopCommand()); NamedCommands.registerCommand("FeedNote", AutonomousCommands.getFeedNoteCommand()); - NamedCommands.registerCommand("PrepareForCloseShot", ShootingCommands.getPrepareCloseSpeakerShotCommand()); - NamedCommands.registerCommand("PrepareForEjectFromShooter", AutonomousCommands.getPrepareForShooterEjectionCommand(false)); - NamedCommands.registerCommand("PrepareForCloseEjectFromShooter", AutonomousCommands.getPrepareForShooterEjectionCommand(true)); + NamedCommands.registerCommand("PreparePitchForEjectFromShooter", AutonomousCommands.getPreparePitchCommand(ShootingConstants.EJECT_FROM_SHOOTER_PITCH)); + NamedCommands.registerCommand("PrepareShootingForEjectFromShooter", AutonomousCommands.getPrepareShooterCommand(ShootingConstants.EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PreparePitchForCloseEjectFromShooter", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_PITCH)); + NamedCommands.registerCommand("PrepareShootingForCloseEjectFromShooter", AutonomousCommands.getPrepareShooterCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index f226cd99..5532f946 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -1,11 +1,44 @@ package frc.trigon.robot.constants; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import org.trigon.hardware.RobotHardwareStats; public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.005, - THETA_STD_EXPONENT = 0.01; + TRANSLATIONS_STD_EXPONENT = RobotHardwareStats.isSimulation() ? 0.2 : 0.002, + THETA_STD_EXPONENT = 0.02; - public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); + private static final Transform3d + FRONT_CENTER_TO_CAMERA = new Transform3d( + new Translation3d(0.325, 0.0465, 0.192), + new Rotation3d(0, Units.degreesToRadians(-31), 0) + ), + REAR_CENTER_TO_CAMERA = new Transform3d( + new Translation3d(-0.325 + 0.00975, 0, 0.095), + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33), Math.PI + Units.degreesToRadians(0)) + ); + + public static final AprilTagCamera + FRONT_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "FrontTagCamera", + FRONT_CENTER_TO_CAMERA, + THETA_STD_EXPONENT, + TRANSLATIONS_STD_EXPONENT + ), + REAR_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "RearTagCamera", + REAR_CENTER_TO_CAMERA, + THETA_STD_EXPONENT, + TRANSLATIONS_STD_EXPONENT + ); + + public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 4a171cb1..8852cd24 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -3,16 +3,18 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.*; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; +import java.io.IOException; import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); + private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double FIELD_LENGTH_METERS = 16.54175, @@ -29,12 +31,22 @@ public class FieldConstants { TARGET_DELIVERY_POSITION = new MirrorableTranslation3d(2.5, 7, 0, true); public static final MirrorablePose2d IN_FRONT_OF_AMP_POSE = new MirrorablePose2d(1.842, 8.204 - 0.405, Rotation2d.fromDegrees(-90), true); - public static final double MINIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS = 2.5; + public static final double MAXIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS = 2.5; + + private static AprilTagFieldLayout createAprilTagFieldLayout() { + try { + return SHOULD_USE_HOME_TAG_LAYOUT ? + AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json") : + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + } catch (IOException e) { + throw new RuntimeException(e); + } + } private static HashMap fieldLayoutToTagIdToPoseMap() { final HashMap tagIdToPose = new HashMap<>(); for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - tagIdToPose.put(aprilTag.ID, aprilTag.pose); + tagIdToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); return tagIdToPose; } } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index aa2f51d8..2e7bc27c 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -6,8 +6,7 @@ import org.trigon.hardware.misc.XboxController; public class OperatorConstants { - private static final int - DRIVER_CONTROLLER_PORT = 0; + private static final int DRIVER_CONTROLLER_PORT = 0; private static final int DRIVER_CONTROLLER_EXPONENT = 1; private static final double DRIVER_CONTROLLER_DEADBAND = 0; public static final XboxController DRIVER_CONTROLLER = new XboxController( @@ -21,20 +20,24 @@ public class OperatorConstants { public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), - SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r().or(DRIVER_CONTROLLER.a()), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER = DRIVER_CONTROLLER.b(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), - ALIGN_TO_SPEAKER_TRIGGER = DRIVER_CONTROLLER.a(), ALIGN_TO_RIGHT_STAGE = OPERATOR_CONTROLLER.j(), ALIGN_TO_LEFT_STAGE = OPERATOR_CONTROLLER.h(), ALIGN_TO_MIDDLE_STAGE = OPERATOR_CONTROLLER.u(), TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER = OPERATOR_CONTROLLER.o(), TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER = OPERATOR_CONTROLLER.p(), + LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), CLIMB_TRIGGER = OPERATOR_CONTROLLER.c(), OVERRIDE_IS_CLIMBING_TRIGGER = OPERATOR_CONTROLLER.i(), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), MOVE_CLIMBER_UP_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f2(), + MOVE_RIGHT_CLIMBER_DOWN_MANUALLY = OPERATOR_CONTROLLER.f3(), + MOVE_RIGHT_CLIMBER_UP_MANUALLY = OPERATOR_CONTROLLER.f4(), + MOVE_LEFT_CLIMBER_DOWN_MANUALLY = OPERATOR_CONTROLLER.f5(), + MOVE_LEFT_CLIMBER_UP_MANUALLY = OPERATOR_CONTROLLER.f6(), CONTINUE_TRIGGER = DRIVER_CONTROLLER.leftBumper().or(OPERATOR_CONTROLLER.k()), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), @@ -42,13 +45,17 @@ public class OperatorConstants { BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), COLLECT_NOTE_TRIGGER = DRIVER_CONTROLLER.leftTrigger(), EJECT_NOTE_TRIGGER = OPERATOR_CONTROLLER.e(), + HIGH_EJECT_NOTE_TRIGGER = OPERATOR_CONTROLLER.l(), SPEAKER_SHOT_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.s()), CLOSE_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.x(), WARM_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.w().and(SPEAKER_SHOT_TRIGGER.negate()), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), - MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m(), + MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m().or(DRIVER_CONTROLLER.x()), AMP_TRIGGER = OPERATOR_CONTROLLER.a(), + AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), - ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), + BLOCK_TRIGGER = OPERATOR_CONTROLLER.b(), + REALIGN_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.equals(), + RESET_AMP_ALIGNER_POSITION_TRIGGER = OPERATOR_CONTROLLER.minus(), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index d7b6a1d1..4eac1cc6 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -5,15 +5,13 @@ import org.trigon.utilities.FilesHandler; public class RobotConstants { - private static final boolean - IS_SIMULATION = true, - IS_REPLAY = false; + private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; - public static final String LOGGING_PATH = IS_SIMULATION && Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; + public static final String LOGGING_PATH = Robot.IS_REAL ? "/media/sda1/akitlogs/" : FilesHandler.DEPLOY_PATH + "logs/"; public static void init() { - RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY); + RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, REPLAY_TYPE); RobotHardwareStats.setPeriodicTimeSeconds(PERIODIC_TIME_SECONDS); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 7f814afc..1b5620fd 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -9,21 +9,24 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, - DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; + DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, + FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 3; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, - AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 20, + AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 37, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 45, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), - AMP_PITCH = Rotation2d.fromDegrees(45), - MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(12), - EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12), - CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12); - + PREPARE_AMP_PITCH = Rotation2d.fromDegrees(50), + SHOOT_AMP_PITCH = Rotation2d.fromDegrees(44), + HIGH_EJECT_PITCH = Rotation2d.fromDegrees(45), + MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(20), + EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(20), + CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(20); public static final Pose3d ROBOT_RELATIVE_PITCHER_PIVOT_POINT = new Pose3d(0.2521, 0, 0.15545, new Rotation3d(0, 0, Math.PI)); public static final Transform3d PITCHER_PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.4209, 0, -0.0165, new Rotation3d()); diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index 50a4e46d..3906b0b9 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -86,7 +86,7 @@ public Translation3d getRobotFieldRelativeVelocity() { */ private Translation2d predictFutureTranslation(double predictionTime) { final Translation2d fieldRelativeVelocity = getRobotFieldRelativeVelocity().toTranslation2d(); - final Translation2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); + final Translation2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation(); return currentPose.plus(fieldRelativeVelocity.times(predictionTime)); } @@ -101,7 +101,7 @@ private Translation2d predictFutureTranslation(double predictionTime) { * @return the target state of the robot so the note will reach the shooting target, as a {@linkplain ShootingCalculations.TargetShootingState} */ private TargetShootingState calculateTargetShootingState(MirrorableTranslation3d shootingTarget, double standingShootingVelocityRotationsPerSecond, boolean reachFromAbove) { - final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); + final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation(); final MirrorableRotation2d standingTargetRobotAngle = getAngleToTarget(currentTranslation, shootingTarget); final double standingTangentialVelocity = angularVelocityToTangentialVelocity(standingShootingVelocityRotationsPerSecond); final Rotation2d standingTargetPitch = calculateTargetPitch(standingTangentialVelocity, reachFromAbove, currentTranslation, standingTargetRobotAngle, shootingTarget); @@ -137,7 +137,7 @@ private TargetShootingState calculateTargetShootingState(TargetShootingState sta private TargetShootingState calculateTargetShootingState(Translation3d shootingVector) { final MirrorableRotation2d targetRobotAngle = new MirrorableRotation2d(getYaw(shootingVector), false); final Rotation2d targetPitch = getPitch(shootingVector); - final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()); + final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()) + ShootingConstants.SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND; return new TargetShootingState(targetRobotAngle, targetPitch, targetVelocity); } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java index 8e943d1f..0be42fa9 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java @@ -43,7 +43,7 @@ protected SimulationObjectDetectionCameraIO(String hostname) { @Override protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - final Rotation2d closestObjectYaw = getClosestVisibleObjectYaw(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); + final Rotation2d closestObjectYaw = getClosestVisibleObjectYaw(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose()); if (closestObjectYaw == null) { inputs.hasTargets = false; } else { @@ -63,7 +63,7 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { private void updateHeldObjectPose() { if (heldObject.length == 0) return; - heldObject[0] = getHeldObjectPose(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); + heldObject[0] = getHeldObjectPose(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose()); } private void updateObjectEjection() { @@ -79,7 +79,7 @@ private void updateObjectEjection() { private void updateObjectCollection() { if (heldObject.length == 1 || !isCollecting()) return; - final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); final Translation2d robotTranslation = robotPose.getTranslation(); for (Translation2d objectPlacement : objectsOnField) { if (objectPlacement.getDistance(robotTranslation) <= PICKING_UP_TOLERANCE_METERS) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 45c69b9c..ad9ea89d 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -5,10 +5,11 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.trigon.robot.Robot; +import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; /** * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. @@ -44,10 +45,12 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; this.translationStandardDeviationExponent = translationStandardDeviationExponent; - if (Robot.IS_REAL) - aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); - else - aprilTagCameraIO = new AprilTagCameraIO(); + if (RobotHardwareStats.isSimulation()) { + aprilTagCameraIO = AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA.createIOFunction.apply(name); + aprilTagCameraIO.addSimulatedCameraToVisionSimulation(robotCenterToCamera); + return; + } + aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); } public void update() { @@ -58,7 +61,7 @@ public void update() { } public boolean hasNewResult() { - return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp(); + return (inputs.hasResult && inputs.distanceFromBestTag != Double.POSITIVE_INFINITY) && isNewTimestamp(); } public Pose2d getEstimatedRobotPose() { @@ -78,7 +81,7 @@ public double getLatestResultTimestampSeconds() { } /** - * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calibrated gain. * The theta deviation is infinity when we assume the robot's heading because we already assume that the heading is correct. * * @return the standard deviations for the pose estimation strategy used @@ -103,7 +106,7 @@ public double getDistanceToBestTagMeters() { * @return the robot's pose */ private Pose2d calculateBestRobotPose() { - final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + final Rotation2d gyroHeadingAtTimestamp = RobotHardwareStats.isSimulation() ? RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose().getRotation() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp); } @@ -114,30 +117,42 @@ private Pose2d calculateBestRobotPose() { * @return the robot's pose */ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { - if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult) + if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult || inputs.poseAmbiguity > AprilTagCameraConstants.MAXIMUM_AMBIGUITY) return null; - final Translation2d fieldRelativeRobotTranslation = getFieldRelativeRobotTranslation(gyroHeading); - if (!isWithinBestTagRangeForSolvePNP()) - return new Pose2d(fieldRelativeRobotTranslation, gyroHeading); - - final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); - return new Pose2d(fieldRelativeRobotTranslation, solvePNPHeading); + return new Pose2d(getFieldRelativeRobotTranslation(gyroHeading), gyroHeading); + final Rotation2d solvePNPHeading = getSolvePNPHeading(); + return new Pose2d(getFieldRelativeRobotTranslation(solvePNPHeading), solvePNPHeading); } - private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { - final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); + private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading) { + final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]); + if (bestTagPose == null) + return null; + + correctTargetRelativeRotation(); - final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(gyroHeading, bestTagPose); + final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); - final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(gyroHeading); + final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(currentHeading); return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); } + /** + * When the roll of the camera changes, the target pitch and yaw are also affected. + * This method corrects the yaw and pitch based on the camera's mount position roll. + */ + private void correctTargetRelativeRotation() { + final Translation2d targetRotation = new Translation2d(inputs.bestTargetRelativePitchRadians, inputs.bestTargetRelativeYawRadians); + targetRotation.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); + inputs.bestTargetRelativePitchRadians = targetRotation.getX(); + inputs.bestTargetRelativeYawRadians = targetRotation.getY(); + } + private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); - final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneDistanceToTag(tagPose, robotPlaneTargetYawRadians); + final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneXYDistanceToTag(tagPose, robotPlaneTargetYawRadians); final double headingOffsetToUsedTagRadians = gyroHeading.getRadians() - robotPlaneTargetYawRadians + robotCenterToCamera.getRotation().getZ(); return new Translation2d(robotPlaneCameraDistanceToUsedTagMeters, Rotation2d.fromRadians(headingOffsetToUsedTagRadians)); } @@ -157,9 +172,9 @@ private double projectToPlane(double targetAngleRadians, double cameraAngleRadia return Math.atan(Math.tan(targetAngleRadians) * Math.cos(cameraAngleRadians)); } - private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { - double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); - double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); + private double calculateRobotPlaneXYDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { + final double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + final double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); } @@ -189,14 +204,14 @@ private double calculateStandardDeviations(double exponent, double distance, int } private boolean isWithinBestTagRangeForSolvePNP() { - return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; + return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS; } private void logCameraInfo() { Logger.processInputs("Cameras/" + name, inputs); if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) logUsedTags(); - if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) { + if (!inputs.hasResult || inputs.distanceFromBestTag == Double.POSITIVE_INFINITY || robotPose == null) { logEstimatedRobotPose(); logSolvePNPPose(); } else { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index a9cddad5..960a6ee0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -1,21 +1,57 @@ package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.trigon.hardware.RobotHardwareStats; import java.util.function.Function; public class AprilTagCameraConstants { - public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; + public static final double MAXIMUM_AMBIGUITY = 0.4; + + public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSimulation"); + private static final int + SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600, + SIMULATION_CAMERA_RESOLUTION_HEIGHT = 1200, + SIMULATION_CAMERA_FPS = 60, + SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, + SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5, + SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 10; + private static final Rotation2d SIMULATION_CAMERA_FOV = Rotation2d.fromDegrees(70); + private static final double + SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0.25, + SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0.08; + public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); + + static { + if (RobotHardwareStats.isSimulation()) { + configureSimulationCameraProperties(); + VISION_SIMULATION.addAprilTags(FieldConstants.APRIL_TAG_FIELD_LAYOUT); + } + } + + private static void configureSimulationCameraProperties() { + SIMULATION_CAMERA_PROPERTIES.setCalibration(SIMULATION_CAMERA_RESOLUTION_WIDTH, SIMULATION_CAMERA_RESOLUTION_HEIGHT, SIMULATION_CAMERA_FOV); + SIMULATION_CAMERA_PROPERTIES.setCalibError(SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR, SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS); + SIMULATION_CAMERA_PROPERTIES.setFPS(SIMULATION_CAMERA_FPS); + SIMULATION_CAMERA_PROPERTIES.setAvgLatencyMs(SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setLatencyStdDevMs(SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setExposureTimeMs(SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS); + } + public enum AprilTagCameraType { PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + SIMULATION_CAMERA(AprilTagSimulationCameraIO::new), LIMELIGHT(AprilTagLimelightIO::new); final Function createIOFunction; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index c9233bb2..5ad54f5b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -1,12 +1,21 @@ package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; import org.littletonrobotics.junction.AutoLog; public class AprilTagCameraIO { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { } + /** + * Adds the simulated camera to the pose estimation simulation. + * + * @param robotToCamera the transform of the robot's origin point to the camera + */ + protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) { + } + @AutoLog public static class AprilTagCameraInputs { public boolean hasResult = false; @@ -16,5 +25,6 @@ public static class AprilTagCameraInputs { public double bestTargetRelativeYawRadians = 0; public double bestTargetRelativePitchRadians = 0; public double distanceFromBestTag = 0; + public double poseAmbiguity = 0; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 065e8275..1f10f356 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -21,10 +21,11 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { inputs.hasResult = results.targets_Fiducials.length > 0; - if (inputs.hasResult) + if (inputs.hasResult) { updateHasResultInputs(inputs, results); - else - updateNoResultInputs(inputs); + return; + } + updateNoResultInputs(inputs); } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { @@ -39,8 +40,9 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Limeli } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { - inputs.visibleTagIDs = new int[0]; inputs.cameraSolvePNPPose = new Pose3d(); + inputs.visibleTagIDs = new int[0]; + inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; } private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 5e183bc4..ea9a39f6 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,24 +1,20 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; -import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.TargetCorner; +import org.photonvision.targeting.PhotonTrackedTarget; import java.util.List; public class AprilTagPhotonCameraIO extends AprilTagCameraIO { - private final PhotonCamera photonCamera; + final PhotonCamera photonCamera; public AprilTagPhotonCameraIO(String cameraName) { photonCamera = new PhotonCamera(cameraName); @@ -26,53 +22,64 @@ public AprilTagPhotonCameraIO(String cameraName) { @Override protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { - final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); + final PhotonPipelineResult latestResult = getLatestPipelineResult(); - inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); - if (inputs.hasResult) + inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); + if (inputs.hasResult) { updateHasResultInputs(inputs, latestResult); - else - updateNoResultInputs(inputs); + return; + } + updateNoResultInputs(inputs); + } + + private PhotonPipelineResult getLatestPipelineResult() { + final List unreadResults = photonCamera.getAllUnreadResults(); + return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { - final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + final PhotonTrackedTarget bestTarget = getBestTarget(latestResult); + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(bestTarget); - inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult, bestTarget); inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); - inputs.visibleTagIDs = getVisibleTagIDs(latestResult); - inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult, bestTarget); + inputs.distanceFromBestTag = getDistanceFromBestTag(bestTarget); + inputs.poseAmbiguity = bestTarget.getPoseAmbiguity(); } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { - inputs.visibleTagIDs = new int[]{}; inputs.cameraSolvePNPPose = new Pose3d(); + inputs.visibleTagIDs = new int[0]; + inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; } - private Point getTagCenter(List tagCorners) { - double tagCornerSumX = 0; - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) { - tagCornerSumX += tagCorner.x; - tagCornerSumY += tagCorner.y; + /** + * Calculates the best tag from the pipeline result based on the area that the tag takes up. + * + * @param result the camera's pipeline result + * @return the best target + */ + private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { + PhotonTrackedTarget bestTarget = result.getBestTarget(); + for (PhotonTrackedTarget target : result.getTargets()) { + if (target.getArea() > bestTarget.area) + bestTarget = target; } - return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + return bestTarget; } /** * Estimates the camera's rotation relative to the apriltag. * - * @param result the camera's pipeline result + * @param bestTag the best apriltag visible to the camera * @return the estimated rotation */ - private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - final List tagCorners = result.getBestTarget().getDetectedCorners(); - final Point tagCenter = getTagCenter(tagCorners); - if (photonCamera.getCameraMatrix().isPresent()) - return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); - return null; + private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) { + final Rotation3d cameraRotation = bestTag.getBestCameraToTarget().getRotation(); + return new Rotation3d(0, Units.degreesToRadians(cameraRotation.getY()), Units.degreesToRadians(cameraRotation.getZ())); } /** @@ -81,44 +88,35 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { * @param result the camera's pipeline result * @return the estimated pose */ - private Pose3d getSolvePNPPose(PhotonPipelineResult result) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; + private Pose3d getSolvePNPPose(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) { + if (result.getMultiTagResult().isPresent()) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()); - final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); - final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(bestTarget.getFiducialId()); + final Transform3d targetToCamera = bestTarget.getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } - private int[] getVisibleTagIDs(PhotonPipelineResult result) { - final int[] visibleTagIDs = new int[result.getTargets().size()]; - - for (int i = 1; i < visibleTagIDs.length; i++) - visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); + private int[] getVisibleTagIDs(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) { + final List targets = result.getTargets(); + final int[] visibleTagIDs = new int[targets.size()]; + visibleTagIDs[0] = bestTarget.getFiducialId(); + + boolean hasSeenBestTarget = false; + for (int i = 0; i < visibleTagIDs.length; i++) { + final int targetID = targets.get(i).getFiducialId(); + if (targetID == visibleTagIDs[0]) { + hasSeenBestTarget = true; + continue; + } + visibleTagIDs[hasSeenBestTarget ? i : i + 1] = targetID; + } return visibleTagIDs; } - private double getDistanceFromBestTag(PhotonPipelineResult result) { - return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); - } - - private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { - double fx = camIntrinsics.get(0, 0); - double cx = camIntrinsics.get(0, 2); - double xOffset = cx - pixel.x; - - double fy = camIntrinsics.get(1, 1); - double cy = camIntrinsics.get(1, 2); - double yOffset = cy - pixel.y; - - // calculate yaw normally - var yaw = new Rotation2d(fx, xOffset); - // correct pitch based on yaw - var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); - - return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + private double getDistanceFromBestTag(PhotonTrackedTarget bestTag) { + return bestTag.getBestCameraToTarget().getTranslation().getNorm(); } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java new file mode 100644 index 00000000..8c7d31ac --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -0,0 +1,21 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import org.photonvision.simulation.PhotonCameraSim; + +public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { + private final PhotonCameraSim cameraSim; + + public AprilTagSimulationCameraIO(String cameraName) { + super(cameraName); + + cameraSim = new PhotonCameraSim(photonCamera, AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES); + cameraSim.enableDrawWireframe(true); + } + + @Override + protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) { + AprilTagCameraConstants.VISION_SIMULATION.addCamera(cameraSim, robotToCamera); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 4b3447a5..23481254 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -10,7 +10,9 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; import java.util.ArrayList; import java.util.Comparator; @@ -34,10 +36,8 @@ public PoseEstimator(AprilTagCamera... aprilTagCameras) { this.aprilTagCameras = aprilTagCameras; putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); - PathPlannerLogging.setLogActivePathCallback((pose) -> { - field.getObject("path").setPoses(pose); - Logger.recordOutput("Path", pose.toArray(new Pose2d[0])); - }); + + logTargetPath(); } @Override @@ -47,7 +47,7 @@ public void close() { public void periodic() { updateFromVision(); - field.setRobotPose(getCurrentPose()); + field.setRobotPose(getCurrentEstimatedPose()); } /** @@ -63,10 +63,17 @@ public void resetPose(Pose2d currentPose) { /** * @return the estimated pose of the robot, relative to the blue alliance's driver station right corner */ - public Pose2d getCurrentPose() { + public Pose2d getCurrentEstimatedPose() { return poseEstimator6328.getEstimatedPose(); } + /** + * @return the odometry's estimated pose of the robot, relative to the blue alliance's driver station right corner + */ + public Pose2d getCurrentOdometryPose() { + return poseEstimator6328.getOdometryPose(); + } + /** * Updates the pose estimator with the given SWERVE wheel positions and gyro rotations. * This function accepts an array of SWERVE wheel positions and an array of gyro rotations because the odometry can be updated at a faster rate than the main loop (which is 50 hertz). @@ -88,13 +95,23 @@ public void setGyroHeadingToBestSolvePNPHeading() { } final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); - resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + resetPose(new Pose2d(getCurrentEstimatedPose().getTranslation(), bestRobotHeading)); + } + + private void logTargetPath() { + PathPlannerLogging.setLogActivePathCallback((pose) -> { + field.getObject("path").setPoses(pose); + Logger.recordOutput("Path", pose.toArray(new Pose2d[0])); + }); + PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("TargetPPPose", pose)); } private void updateFromVision() { getViableVisionObservations().stream() .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp)) .forEach(poseEstimator6328::addVisionObservation); + if (RobotHardwareStats.isSimulation()) + AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose()); } private List getViableVisionObservations() { @@ -112,7 +129,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera if (!aprilTagCamera.hasNewResult()) return null; final Pose2d robotPose = aprilTagCamera.getEstimatedRobotPose(); - if (robotPose == null) + if (robotPose == null || robotPose.getTranslation() == null || robotPose.getRotation() == null) return null; return new PoseEstimator6328.VisionObservation( diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index 72d74799..d8967b54 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.numbers.N3; public class PoseEstimatorConstants { - public static final double ODOMETRY_FREQUENCY_HERTZ = 50; + public static final double ODOMETRY_FREQUENCY_HERTZ = 250; /** * The vector represents how ambiguous each value of the odometry is. diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java index 8ed7a9c3..a8df8d6d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import org.trigon.hardware.RobotHardwareStats; @@ -68,6 +69,10 @@ public void updateMechanism() { PitcherConstants.PITCHER_AND_AMP_ALIGNER_MECHANISM.setSecondJointCurrentAngle(getCurrentAngle()); } + public void setPosition(Rotation2d position) { + motor.setPosition(position.getRotations()); + } + public AmpAlignerConstants.AmpAlignerState getTargetState() { return targetState; } @@ -108,8 +113,8 @@ void setTargetAngle(Rotation2d targetAngle) { } private void configurePositionResettingTrigger() { - final Trigger hitLimitSwitchTrigger = new Trigger(this::hasHitForwardLimit).debounce(AmpAlignerConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); - hitLimitSwitchTrigger.onTrue(new InstantCommand(() -> motor.setPosition(AmpAlignerConstants.LIMIT_SWITCH_PRESSED_ANGLE.getRotations())).ignoringDisable(true)); + final Trigger hitLimitSwitchTrigger = new Trigger(this::hasHitForwardLimit).debounce(AmpAlignerConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS).or(OperatorConstants.RESET_AMP_ALIGNER_POSITION_TRIGGER); + hitLimitSwitchTrigger.onTrue(new InstantCommand(() -> setPosition(AmpAlignerConstants.LIMIT_SWITCH_PRESSED_ANGLE)).ignoringDisable(true)); } private boolean hasHitForwardLimit() { diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java index 48c40bd6..7205e9a5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java @@ -1,7 +1,11 @@ package frc.trigon.robot.subsystems.ampaligner; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.NetworkTablesCommand; @@ -30,4 +34,17 @@ public static Command getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerSta RobotContainer.AMP_ALIGNER ); } + + public static Command getSetTargetVoltageCommand(double voltage) { + return new StartEndCommand( + () -> RobotContainer.AMP_ALIGNER.drive(Units.Volts.of(voltage)), + RobotContainer.AMP_ALIGNER::stop + ); + } + + public static Command getSetPositionCommand(Rotation2d position) { + return new InstantCommand( + () -> RobotContainer.AMP_ALIGNER.setPosition(position) + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java index e07e79ea..6709371f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java @@ -18,21 +18,21 @@ public class AmpAlignerConstants { private static final String MOTOR_NAME = "AmpAlignerMotor"; static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); - private static final InvertedValue INVERTED_VALUE = InvertedValue.Clockwise_Positive; + private static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final ForwardLimitTypeValue FORWARD_LIMIT_TYPE_VALUE = ForwardLimitTypeValue.NormallyOpen; private static final ForwardLimitSourceValue FORWARD_LIMIT_SOURCE_VALUE = ForwardLimitSourceValue.LimitSwitchPin; private static final double - P = RobotHardwareStats.isSimulation() ? 50 : 0, + P = RobotHardwareStats.isSimulation() ? 50 : 20, I = RobotHardwareStats.isSimulation() ? 0 : 0, D = RobotHardwareStats.isSimulation() ? 0 : 0, - KS = RobotHardwareStats.isSimulation() ? 0.055582 : 0, - KV = RobotHardwareStats.isSimulation() ? 6 : 0, - KA = RobotHardwareStats.isSimulation() ? 0.12886 : 0; - static final double KG = RobotHardwareStats.isSimulation() ? 0.11971 : 0; + KS = RobotHardwareStats.isSimulation() ? 0.055582 : 0.37527, + KV = RobotHardwareStats.isSimulation() ? 6 : 4.5959, + KA = RobotHardwareStats.isSimulation() ? 0.12886 : 0.16118; + static final double KG = RobotHardwareStats.isSimulation() ? 0.11971 : 0.13609; private static final double - MOTION_MAGIC_ACCELERATION = RobotHardwareStats.isSimulation() ? 6 : 0, - MOTION_MAGIC_CRUISE_VELOCITY = RobotHardwareStats.isSimulation() ? 6 : 0; + MOTION_MAGIC_ACCELERATION = RobotHardwareStats.isSimulation() ? 6 : 5, + MOTION_MAGIC_CRUISE_VELOCITY = RobotHardwareStats.isSimulation() ? 6 : 5; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; private static final double GEAR_RATIO = 48; @@ -43,9 +43,9 @@ public class AmpAlignerConstants { private static final double AMP_ALIGNER_MASS_KILOGRAMS = 1.1; public static final double AMP_ALIGNER_LENGTH_METERS = 0.52; private static final Rotation2d - AMP_ALIGNER_MINIMUM_ANGLE = Rotation2d.fromDegrees(173.7 - 156), + AMP_ALIGNER_MINIMUM_ANGLE = Rotation2d.fromDegrees(17.7), AMP_ALIGNER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(173.7); - private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(173.7 - 156); + private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(17.7); private static final boolean SIMULATE_GRAVITY = true; private static final SingleJointedArmSimulation SIMULATION = new SingleJointedArmSimulation( GEARBOX, @@ -58,16 +58,17 @@ public class AmpAlignerConstants { ); static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(1.5).per(Units.Second.of(1)), - Units.Volts.of(3), + Units.Volts.of(1).per(Units.Second.of(1)), + Units.Volts.of(2), Units.Second.of(1000) ); public static final Transform3d PITCHER_TO_AMP_ALIGNER = new Transform3d(-0.4838, 0, 0.1472, new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(6.3), 0)); + public static final Rotation2d REALIGN_AMP_ALIGNER_ANGLE = Rotation2d.fromDegrees(5); static final Rotation2d READY_FOR_DEFAULT_PITCHER_MOVEMENT_ANGLE = Rotation2d.fromDegrees(80); static final Rotation2d LIMIT_SWITCH_PRESSED_ANGLE = Rotation2d.fromDegrees(173.7); - static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.1; + static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.01; static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.3); static { @@ -117,8 +118,8 @@ private static void configureMotor() { } public enum AmpAlignerState { - OPEN(Rotation2d.fromDegrees(173.7 - 156)), - CLOSE(Rotation2d.fromDegrees(173.7)); + OPEN(Rotation2d.fromDegrees(17.7)), + CLOSE(Rotation2d.fromDegrees(173)); public final Rotation2d targetAngle; diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index f30fedd5..0edf7ece 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Voltage; @@ -9,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; @@ -38,8 +40,6 @@ public class Climber extends MotorSubsystem { public Climber() { setName("Climber"); GeneralCommands.getDelayedCommand(3, this::configureChangingDefaultCommand).schedule(); - configurePositionResettingTrigger(rightMotor); - configurePositionResettingTrigger(leftMotor); } @Override @@ -127,18 +127,11 @@ void setTargetState(ClimberConstants.ClimberState targetState) { void setTargetPosition(double targetRightPositionRotations, double targetLeftPositionRotations, boolean affectedByRobotWeight) { rightMotor.setControl(determineRequest(affectedByRobotWeight) - .withPosition(targetRightPositionRotations)); - leftMotor.setControl(determineRequest(affectedByRobotWeight) - .withPosition(targetLeftPositionRotations)); - } - - private void configurePositionResettingTrigger(TalonFXMotor motor) { - final Trigger positionResettingTrigger = new Trigger(() -> hasHitReverseLimit(motor)).debounce(ClimberConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); - positionResettingTrigger.onFalse(new InstantCommand(() -> motor.setPosition(ClimberConstants.LIMIT_SWITCH_PRESSED_POSITION))); - } - - private boolean hasHitReverseLimit(TalonFXMotor motor) { - return motor.getSignal(TalonFXSignal.REVERSE_LIMIT) == 0; + .withPosition(targetRightPositionRotations) + .withFeedForward(affectedByRobotWeight ? ClimberConstants.ON_CHAIN_KG : 0)); + leftMotor.setControl(determineRequest(affectedByRobotWeight).withFeedForward(ClimberConstants.ON_CHAIN_KG) + .withPosition(targetLeftPositionRotations) + .withFeedForward(affectedByRobotWeight ? ClimberConstants.ON_CHAIN_KG : 0)); } private DynamicMotionMagicVoltage determineRequest(boolean affectedByRobotWeight) { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index dcffff8c..c422529a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -1,6 +1,7 @@ package frc.trigon.robot.subsystems.climber; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; @@ -10,6 +11,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -31,31 +33,38 @@ public class ClimberConstants { LEFT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; static final boolean ENABLE_FOC = true; - private static final double //TODO: calibrate - GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 0, - GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, - GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, - GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0, - GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 0, - GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0; - private static final double //TODO: calibrate - ON_CHAIN_P = RobotHardwareStats.isSimulation() ? GROUNDED_P : 0, - ON_CHAIN_I = RobotHardwareStats.isSimulation() ? GROUNDED_I : 0, - ON_CHAIN_D = RobotHardwareStats.isSimulation() ? GROUNDED_D : 0, - ON_CHAIN_KS = RobotHardwareStats.isSimulation() ? GROUNDED_KS : 0, - ON_CHAIN_KV = RobotHardwareStats.isSimulation() ? GROUNDED_KV : 0, - ON_CHAIN_KA = RobotHardwareStats.isSimulation() ? GROUNDED_KA : 0; + private static final double + LEFT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 50, + LEFT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.078964, + LEFT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9056, + LEFT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.18439; + private static final double + RIGHT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 50, + RIGHT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.079947, + RIGHT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9986, + RIGHT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.21705; + private static final double + ON_CHAIN_P = RobotHardwareStats.isSimulation() ? RIGHT_GROUNDED_P : 1, + ON_CHAIN_I = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_I : 0, + ON_CHAIN_D = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_D : 0, + ON_CHAIN_KS = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KS : 0.079947, + ON_CHAIN_KV = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KV : 7.9986, + ON_CHAIN_KA = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KA : 0.21705; + static final double ON_CHAIN_KG = 0.4; static final double - MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / GROUNDED_KV : 0, - MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / GROUNDED_KA : 0, - MAX_ON_CHAIN_VELOCITY = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KV) - 0.75 : 0, - MAX_ON_CHAIN_ACCELERATION = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KA) - 50 : 0; + MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KV : 1.5, + MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KA : 1.5, + MAX_ON_CHAIN_VELOCITY = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KV) - 0.75 : 1, + MAX_ON_CHAIN_ACCELERATION = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KA) - 50 : 1; static final int GROUNDED_SLOT = 0, ON_CHAIN_SLOT = 1; - private static final double - REVERSE_SOFT_LIMIT_POSITION_ROTATIONS = 0, - FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3.183; + private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = -2.9; + private static final double LIMIT_SWITCH_RESET_POSITION = 0; static final double GEAR_RATIO = 68.57; private static final int @@ -71,7 +80,7 @@ public class ClimberConstants { static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Second.of(1)), - Units.Volts.of(8), + Units.Volts.of(5), null, null ); @@ -115,15 +124,13 @@ public class ClimberConstants { MOVE_CLIMBER_DOWN_VOLTAGE = -4, MOVE_CLIMBER_UP_VOLTAGE = 4; static final double CLIMBER_TOLERANCE_ROTATIONS = 0.01; - static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.1; - static final double LIMIT_SWITCH_PRESSED_POSITION = 0; static { - configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_MOTOR_SIMULATION); - configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_MOTOR_SIMULATION); + configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_MOTOR_SIMULATION, RIGHT_GROUNDED_P, RIGHT_GROUNDED_I, RIGHT_GROUNDED_D, RIGHT_GROUNDED_KS, RIGHT_GROUNDED_KV, RIGHT_GROUNDED_KA, IntakeConstants.MASTER_MOTOR_ID); + configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_MOTOR_SIMULATION, LEFT_GROUNDED_P, LEFT_GROUNDED_I, LEFT_GROUNDED_D, LEFT_GROUNDED_KS, LEFT_GROUNDED_KV, LEFT_GROUNDED_KA, IntakeConstants.FOLLOWER_MOTOR_ID); } - private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, SimpleMotorSimulation simulation) { + private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, SimpleMotorSimulation simulation, double groundedP, double groundedI, double groundedD, double groundedKS, double groundedKV, double groundedKA, int limitSwitchID) { final TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = invertedValue; @@ -131,12 +138,12 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; - config.Slot0.kP = GROUNDED_P; - config.Slot0.kI = GROUNDED_I; - config.Slot0.kD = GROUNDED_D; - config.Slot0.kS = GROUNDED_KS; - config.Slot0.kV = GROUNDED_KV; - config.Slot0.kA = GROUNDED_KA; + config.Slot0.kP = groundedP; + config.Slot0.kI = groundedI; + config.Slot0.kD = groundedD; + config.Slot0.kS = groundedKS; + config.Slot0.kV = groundedKV; + config.Slot0.kA = groundedKA; config.Slot1.kP = ON_CHAIN_P; config.Slot1.kI = ON_CHAIN_I; @@ -145,10 +152,15 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal config.Slot1.kV = ON_CHAIN_KV; config.Slot1.kA = ON_CHAIN_KA; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_SOFT_LIMIT_POSITION_ROTATIONS; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS; + config.HardwareLimitSwitch.ForwardLimitRemoteSensorID = limitSwitchID; + config.HardwareLimitSwitch.ForwardLimitSource = ForwardLimitSourceValue.RemoteTalonFX; + config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = true; + config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = LIMIT_SWITCH_RESET_POSITION; + + config.MotionMagic.MotionMagicAcceleration = MAX_GROUNDED_ACCELERATION; + config.MotionMagic.MotionMagicCruiseVelocity = MAX_GROUNDED_VELOCITY; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -161,12 +173,13 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal motor.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); motor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); motor.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); + motor.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); } public enum ClimberState { REST(0, false), - PREPARE_FOR_CLIMB(3.183, false), //TODO: calibrate - CLIMB(0.1, true); //TODO: calibrate + PREPARE_FOR_CLIMB(-2.9, false), + CLIMB(0, true); public final double positionRotations; public final boolean affectedByRobotWeight; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 20674f31..72abf97f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -3,9 +3,8 @@ import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; @@ -66,8 +65,12 @@ void sendStaticBrakeRequest() { void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; - if (targetState == IntakeConstants.IntakeState.FEED_SHOOTING || targetState == IntakeConstants.IntakeState.FEED_AMP || targetState == IntakeConstants.IntakeState.EJECT) + if (targetState == IntakeConstants.IntakeState.FEED_SHOOTING || targetState == IntakeConstants.IntakeState.FEED_AMP) getFeedingIndicationLEDsCommand().schedule(); + if (targetState == IntakeConstants.IntakeState.EJECT) + getEjectingIndicationLEDsCommand().schedule(); + if (targetState == IntakeConstants.IntakeState.STOP) + CommandConstants.DEFAULT_LEDS_COMMAND.schedule(); setTargetVoltage(targetState.voltage); } @@ -80,22 +83,39 @@ void setTargetVoltage(double targetVoltage) { * Indicates to the driver that a note has been collected by rumbling the controller and flashing the robot's LEDs. */ void indicateCollection() { - if (DriverStation.isAutonomous()) + if (!DriverStation.isAutonomous()) OperatorConstants.DRIVER_CONTROLLER.rumble(IntakeConstants.RUMBLE_DURATION_SECONDS, IntakeConstants.RUMBLE_POWER); getCollectionIndicationLEDsCommand().schedule(); } private Command getCollectionIndicationLEDsCommand() { - return new SequentialCommandGroup( - LEDStripCommands.getBlinkingCommand(Color.kOrange, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS), - LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS) - ); + return LEDStripCommands.getBlinkingCommand( + IntakeConstants.COLLECTION_INDICATION_BLINKING_FIRST_COLOR, + IntakeConstants.COLLECTION_INDICATION_BLINKING_SECOND_COLOR, + IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, + LEDStrip.LED_STRIPS + ).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); } private Command getFeedingIndicationLEDsCommand() { - return new SequentialCommandGroup( - LEDStripCommands.getBlinkingCommand(Color.kYellow, IntakeConstants.FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BLINKING_TIME_SECONDS), - LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS) + return LEDStripCommands.getBreatheCommand( + IntakeConstants.FEEDING_INDICATION_COLOR, + IntakeConstants.FEEDING_INDICATION_BREATHING_LEDS_AMOUNT, + IntakeConstants.FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.FEEDING_INDICATION_BREATHING_SHOULD_LOOP, + IntakeConstants.FEEDING_INDICATION_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ); + } + + private Command getEjectingIndicationLEDsCommand() { + return LEDStripCommands.getBreatheCommand( + IntakeConstants.EJECTING_INDICATION_COLOR, + IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, + IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, + IntakeConstants.EJECTING_INDICATION_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS ); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index dd445a3d..a9e3fbf4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -15,7 +15,7 @@ public static Command getDebuggingCommand() { public static Command getSetTargetStateCommand(IntakeConstants.IntakeState targetState) { if (targetState == IntakeConstants.IntakeState.COLLECT) - return getCollectionCommand(); + return getCollectionCommand().andThen(getCorrectNotePositionCommand()); return new StartEndCommand( () -> RobotContainer.INTAKE.setTargetState(targetState), RobotContainer.INTAKE::stop, @@ -40,13 +40,11 @@ private static Command getCollectionCommand() { (interrupted) -> { if (!interrupted) { RobotContainer.INTAKE.indicateCollection(); - RobotContainer.INTAKE.sendStaticBrakeRequest(); } }, RobotContainer.INTAKE::hasNote, RobotContainer.INTAKE - ), - getWaitForNoteToStopCommand().andThen(getStopIntakeCommand()) + ) ); } @@ -58,7 +56,7 @@ private static Command getStopIntakeCommand() { } - private static Command getWaitForNoteToStopCommand() { - return new WaitCommand(IntakeConstants.NOTE_STOPPING_SECONDS); + private static Command getCorrectNotePositionCommand() { + return getSetTargetStateCommand(IntakeConstants.IntakeState.CORRECT_NOTE_POSITION).onlyWhile(RobotContainer.INTAKE::hasNote).andThen(getStopIntakeCommand()); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 33eebd0a..b1c05699 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -6,8 +6,10 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.trigon.robot.misc.objectdetectioncamera.SimulationObjectDetectionCameraIO; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -17,10 +19,10 @@ import java.util.function.DoubleSupplier; public class IntakeConstants { - private static final int + public static final int MASTER_MOTOR_ID = 16, - FOLLOWER_MOTOR_ID = 17, - DISTANCE_SENSOR_CHANNEL = 0; + FOLLOWER_MOTOR_ID = 17; + private static final int DISTANCE_SENSOR_CHANNEL = 2; private static final String MASTER_MOTOR_NAME = "MasterIntakeMotor", FOLLOWER_MOTOR_NAME = "FollowerIntakeMotor", @@ -37,8 +39,8 @@ public class IntakeConstants { private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double GEAR_RATIO = 1.5; private static final double - DISTANCE_SENSOR_SCALING_SLOPE = 0.0004, - DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = 400; + DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; static final boolean FOC_ENABLED = true; private static final int MOTOR_AMOUNT = 2; @@ -49,7 +51,7 @@ public class IntakeConstants { GEAR_RATIO, MOMENT_OF_INERTIA ); - private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 5; + private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 14; private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER = () -> SimulationObjectDetectionCameraIO.HAS_OBJECTS ? NOTE_DISTANCE_THRESHOLD_CENTIMETERS - 1 : NOTE_DISTANCE_THRESHOLD_CENTIMETERS + 1; private static final double MAX_DISPLAYABLE_VELOCITY = 12; @@ -59,24 +61,50 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.6; + static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS + () -> !RobotHardwareStats.isSimulation() && (DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS) ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); - static final double NOTE_STOPPING_SECONDS = 1; - private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate - private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.25; //TODO: calibrate + private static final double NOTE_COLLECTION_CURRENT = 50; + private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.1; static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> Math.abs(MASTER_MOTOR.getSignal(TalonFXSignal.TORQUE_CURRENT)) > IntakeConstants.NOTE_COLLECTION_CURRENT ).debounce(NOTE_COLLECTION_TIME_THRESHOLD_SECONDS); + static final Color + COLLECTION_INDICATION_BLINKING_FIRST_COLOR = Color.kOrangeRed, + COLLECTION_INDICATION_BLINKING_SECOND_COLOR = Color.kBlack, + FEEDING_INDICATION_COLOR = Color.kPurple, + EJECTING_INDICATION_COLOR = Color.kDarkBlue; + static final double COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; + static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; + static final int + FEEDING_INDICATION_BREATHING_LEDS_AMOUNT = 5, + EJECTING_INDICATION_BREATHING_LEDS_AMOUNT = 9; static final double - COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2, - FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; - static final double - COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2, - FEEDING_INDICATION_BLINKING_TIME_SECONDS = 1; + FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.05, + EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 1; + static final boolean + FEEDING_INDICATION_BREATHING_SHOULD_LOOP = false, + FEEDING_INDICATION_BREATHING_IS_INVERTED = false, + EJECTING_INDICATION_BREATHING_SHOULD_LOOP = true, + EJECTING_INDICATION_BREATHING_IS_INVERTED = true; + public static final Color + COLLECTION_BREATHING_LEDS_COLOR = Color.kOrangeRed, + NOTE_DETECTION_CAMERA_HAS_TARGETS_BREATHING_LEDS_COLOR = Color.kGreen, + NOTE_DETECTION_CAMERA_HAS_NO_TARGETS_BREATHING_LEDS_COLOR = Color.kRed; + public static final int + COLLECTION_BREATHING_LEDS_AMOUNT = 5, + ALIGN_TO_NOTE_BREATHING_LEDS_AMOUNT = 5; + public static final double + COLLECTION_BREATHING_CYCLE_TIME_SECONDS = 0.2, + ALIGN_TO_NOTE_BREATHING_CYCLE_TIME_SECONDS = 0.2; + public static final boolean + COLLECTION_BREATHING_SHOULD_LOOP = true, + COLLECTION_BREATHING_IS_INVERTED = false, + ALIGN_TO_NOTE_BREATHING_SHOULD_LOOP = true, + ALIGN_TO_NOTE_BREATHING_IS_INVERTED = false; static { configureMasterMotor(); @@ -92,6 +120,8 @@ private static void configureMasterMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; + config.HardwareLimitSwitch.ReverseLimitEnable = false; + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; MASTER_MOTOR.applyConfiguration(config); @@ -100,6 +130,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); } private static void configureFollowerMotor() { @@ -110,8 +141,12 @@ private static void configureFollowerMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; + config.HardwareLimitSwitch.ReverseLimitEnable = false; + FOLLOWER_MOTOR.applyConfiguration(config); + FOLLOWER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); + final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); FOLLOWER_MOTOR.setControl(followerRequest); } @@ -122,11 +157,12 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(5), //TODO: calibrate - EJECT(-2), //TODO: calibrate + COLLECT(4.5), + EJECT(-2), STOP(0), - FEED_SHOOTING(8), //TODO: calibrate - FEED_AMP(4); //TODO: calibrate + FEED_SHOOTING(7), + FEED_AMP(5), + CORRECT_NOTE_POSITION(-1.1); public final double voltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index e5930d16..d0556aa1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -7,29 +7,45 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.commands.factories.GeneralCommands; +import java.util.function.Supplier; + public class LEDStrip extends SubsystemBase { public static LEDStrip[] LED_STRIPS = new LEDStrip[0]; private static final AddressableLED LED = LEDStripConstants.LED; private final int indexOffset; private final boolean inverted; private final int numberOfLEDs; + private int lastBreatheLED; + private double lastLEDMovementTime = 0; private double rainbowFirstPixelHue = 0; private boolean areLEDsOnForBlinking = false; - private double lastBlinkTime = 0; + private boolean alternateColor = true; + private int amountOfColorFlowLEDs = 0; static { GeneralCommands.getDelayedCommand( 1, - () -> LEDStripConstants.LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getBlinkingCommand(Color.kRed, LEDStripConstants.LOW_BATTERY_BLINKING_INTERVAL_SECONDS, LED_STRIPS).withTimeout(LEDStripConstants.LOW_BATTERY_BLINKING_TIME_SECONDS)) + () -> LEDStripConstants.LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getAlternateColorCommand( + LEDStripConstants.LOW_BATTERY_FIRST_COLOR, + LEDStripConstants.LOW_BATTERY_SECOND_COLOR, + LEDStripConstants.LOW_BATTERY_ALTERNATE_COLOR_INTERVAL_SECONDS, + LED_STRIPS + ).withTimeout(LEDStripConstants.LOW_BATTERY_ALTERNATING_TIME_SECONDS)) ); } + @Override + public void periodic() { + LED.setData(LEDStripConstants.LED_BUFFER); + } + public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { this.indexOffset = indexOffset; this.inverted = inverted; this.numberOfLEDs = numberOfLEDs; - addLEDStripToLEDStripsArrayArray(this); + resetLEDSettings(); + addLEDStripToLEDStripsArray(this); } public static void setDefaultCommandForAllLEDS(Command command) { @@ -41,57 +57,144 @@ public int getNumberOfLEDS() { return numberOfLEDs; } - void clearLedColors() { + void resetLEDSettings() { + lastBreatheLED = indexOffset; + lastLEDMovementTime = Timer.getFPGATimestamp(); + rainbowFirstPixelHue = 0; + areLEDsOnForBlinking = false; + alternateColor = true; + amountOfColorFlowLEDs = 0; + } + + void clearLEDColors() { staticColor(Color.kBlack); } - void blink(Color color, double blinkingIntervalSeconds) { + void blink(Color firstColor, Color secondColor, double blinkingIntervalSeconds) { double currentTime = Timer.getFPGATimestamp(); - if (currentTime - lastBlinkTime > blinkingIntervalSeconds) { - lastBlinkTime = currentTime; + if (currentTime - lastLEDMovementTime > blinkingIntervalSeconds) { + lastLEDMovementTime = currentTime; areLEDsOnForBlinking = !areLEDsOnForBlinking; } if (areLEDsOnForBlinking) - staticColor(color); + staticColor(firstColor); else - clearLedColors(); + staticColor(secondColor); } void staticColor(Color color) { - for (int index = 0; index <= numberOfLEDs; index++) - setLEDColor(color, index); + setLEDColors(color, 0, numberOfLEDs - 1); } void rainbow() { - for (int led = 0; led <= numberOfLEDs; led++) { + for (int led = 0; led < numberOfLEDs; led++) { final int hue = (int) (rainbowFirstPixelHue + (led * 180 / numberOfLEDs) % 180); LEDStripConstants.LED_BUFFER.setHSV(led + indexOffset, hue, 255, 128); - LED.setData(LEDStripConstants.LED_BUFFER); + } + if (inverted) { + rainbowFirstPixelHue -= 3; + if (rainbowFirstPixelHue < 0) + rainbowFirstPixelHue += 180; + return; } rainbowFirstPixelHue += 3; rainbowFirstPixelHue %= 180; } - void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { - final int ledsPerSection = (int) Math.floor(numberOfLEDs / 3.0); - setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); + void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, boolean inverted) { + clearLEDColors(); + inverted = this.inverted != inverted; + double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastLEDMovementTime > moveLEDTimeSeconds) { + lastLEDMovementTime = currentTime; + if (inverted) + lastBreatheLED--; + else + lastBreatheLED++; + } + if (inverted ? (lastBreatheLED < indexOffset) : (lastBreatheLED >= numberOfLEDs + indexOffset)) { + if (!shouldLoop) { + getDefaultCommand().schedule(); + return; + } + lastBreatheLED = inverted ? indexOffset + numberOfLEDs : indexOffset; + } + for (int i = 0; i < breathingLEDs; i++) { + if (lastBreatheLED - i >= indexOffset && lastBreatheLED - i < indexOffset + numberOfLEDs) + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i, color); + else if (lastBreatheLED - i < indexOffset + numberOfLEDs) + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i + numberOfLEDs, color); + } } - private void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { - for (int i = 0; i < ledsPerSection; i++) - setLEDColor(inverted ? thirdSectionColor : firstSectionColor, i); - for (int i = ledsPerSection; i < 2 * ledsPerSection; i++) - setLEDColor(secondSectionColor, i); - for (int i = 2 * ledsPerSection; i <= numberOfLEDs; i++) - setLEDColor(inverted ? firstSectionColor : thirdSectionColor, i); + void colorFlow(Color color, double cycleTimeSeconds, boolean shouldLoop, boolean inverted) { + clearLEDColors(); + inverted = this.inverted != inverted; + double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastLEDMovementTime > moveLEDTimeSeconds) { + lastLEDMovementTime = currentTime; + if (inverted) + amountOfColorFlowLEDs--; + else + amountOfColorFlowLEDs++; + } + if (inverted ? amountOfColorFlowLEDs < 0 : amountOfColorFlowLEDs >= numberOfLEDs) { + if (!shouldLoop) { + getDefaultCommand().schedule(); + return; + } + amountOfColorFlowLEDs = inverted ? numberOfLEDs : 0; + } + if (inverted) { + setLEDColors(color, numberOfLEDs - amountOfColorFlowLEDs, numberOfLEDs - 1); + return; + } + setLEDColors(color, 0, amountOfColorFlowLEDs); } - private void setLEDColor(Color color, int index) { - LEDStripConstants.LED_BUFFER.setLED(index + indexOffset, color); - LED.setData(LEDStripConstants.LED_BUFFER); + void alternateColor(Color firstColor, Color secondColor, double intervalSeconds) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastLEDMovementTime > intervalSeconds) { + alternateColor = !alternateColor; + lastLEDMovementTime = currentTime; + } + if (alternateColor) { + for (int i = 0; i < numberOfLEDs; i++) + LEDStripConstants.LED_BUFFER.setLED(i + indexOffset, i % 2 == 0 ? firstColor : secondColor); + return; + } + for (int i = 0; i < numberOfLEDs; i++) + LEDStripConstants.LED_BUFFER.setLED(i + indexOffset, i % 2 == 0 ? secondColor : firstColor); + } + + void sectionColor(int amountOfSections, Supplier[] colors) { + if (amountOfSections != colors.length) + throw new IllegalArgumentException("Amount of sections must be equal to the amount of colors"); + final int LEDSPerSection = (int) Math.floor(numberOfLEDs / amountOfSections); + setSectionColor(amountOfSections, LEDSPerSection, colors); + + for (int i = 0; i < amountOfSections; i++) + setLEDColors(colors[i].get(), LEDSPerSection * i, i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1); + } + + private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier[] colors) { + if (inverted) { + for (int i = 0; i < amountOfSections; i++) + setLEDColors(colors[amountOfSections - i - 1].get(), LEDSPerSection * i, i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1); + return; + } + for (int i = 0; i < amountOfSections; i++) + setLEDColors(colors[i].get(), LEDSPerSection * i, i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1); + } + + private void setLEDColors(Color color, int startIndex, int endIndex) { + for (int i = 0; i <= endIndex - startIndex; i++) + LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); } - private void addLEDStripToLEDStripsArrayArray(LEDStrip ledStrip) { + private void addLEDStripToLEDStripsArray(LEDStrip ledStrip) { final LEDStrip[] newLEDStrips = new LEDStrip[LED_STRIPS.length + 1]; System.arraycopy(LED_STRIPS, 0, newLEDStrips, 0, LED_STRIPS.length); newLEDStrips[LED_STRIPS.length] = ledStrip; diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 0e9f895d..162a2e44 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -2,8 +2,8 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.InitExecuteCommand; import java.util.function.Consumer; @@ -11,38 +11,73 @@ public class LEDStripCommands { public static Command getStaticColorCommand(Color color, LEDStrip... ledStrips) { - return new StartEndCommand( - () -> runForLEDs((ledStrip -> ledStrip.staticColor(color)), ledStrips), - () -> { - }, + return new ExecuteEndCommand( + () -> runForLEDs((LEDStrip -> LEDStrip.staticColor(color)), ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), ledStrips ).ignoringDisable(true); } - public static Command getBlinkingCommand(Color color, double blinkingIntervalSeconds, LEDStrip... ledStrips) { - return new RunCommand( - () -> runForLEDs((ledStrip -> ledStrip.blink(color, blinkingIntervalSeconds)), ledStrips), + public static Command getBlinkingCommand(Color firstColor, Color secondColor, double blinkingIntervalSeconds, LEDStrip... ledStrips) { + return new ExecuteEndCommand( + () -> runForLEDs((LEDStrip -> LEDStrip.blink(firstColor, secondColor, blinkingIntervalSeconds)), ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), ledStrips ).ignoringDisable(true); } public static Command getRainbowCommand(LEDStrip... ledStrips) { - return new RunCommand( + return new ExecuteEndCommand( () -> runForLEDs((LEDStrip::rainbow), ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), + ledStrips + ).ignoringDisable(true); + } + + public static Command getBreatheCommand(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, boolean inverted, LEDStrip... ledStrips) { + return new FunctionalCommand( + () -> { + if (!shouldLoop) + runForLEDs(LEDStrip::resetLEDSettings, ledStrips); + }, + () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop, inverted), ledStrips), + (interrupted) -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), + () -> false, + ledStrips + ).ignoringDisable(true); + } + + public static Command getColorFlowCommand(Color color, double cycleTimeSeconds, boolean shouldLoop, boolean inverted, LEDStrip... ledStrips) { + return new FunctionalCommand( + () -> { + if (!shouldLoop) + runForLEDs(LEDStrip::resetLEDSettings, ledStrips); + }, + () -> runForLEDs((LEDStrip) -> LEDStrip.colorFlow(color, cycleTimeSeconds, shouldLoop, inverted), ledStrips), + (interrupted) -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), + () -> false, + ledStrips + ).ignoringDisable(true); + } + + public static Command getAlternateColorCommand(Color firstColor, Color secondColor, double intervalSeconds, LEDStrip... ledStrips) { + return new ExecuteEndCommand( + () -> runForLEDs((LEDStrip -> LEDStrip.alternateColor(firstColor, secondColor, intervalSeconds)), ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), ledStrips ).ignoringDisable(true); } - public static Command getThreeSectionColorCommand(Supplier firstSectionColor, Supplier secondSectionColor, Supplier thirdSectionColor, LEDStrip... ledStrips) { + public static Command getSectionColorCommand(int amountOfSections, Supplier[] colors, LEDStrip... ledStrips) { return new InitExecuteCommand( - () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), - () -> runForLEDs((ledStrip) -> ledStrip.threeSectionColor(firstSectionColor.get(), secondSectionColor.get(), thirdSectionColor.get()), ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), + () -> runForLEDs((LEDStrip) -> LEDStrip.sectionColor(amountOfSections, colors), ledStrips), ledStrips ).ignoringDisable(true); } public static void runForLEDs(Consumer action, LEDStrip... ledStrips) { - for (LEDStrip ledStrip : ledStrips) - action.accept(ledStrip); + for (LEDStrip LEDStrip : ledStrips) + action.accept(LEDStrip); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index 58094fb6..09ba3679 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -4,14 +4,15 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.Robot; public class LEDStripConstants { - private static final int PORT = 0; + private static final int PORT = 9; private static final int - RIGHT_CLIMBER_NUMBER_OF_LEDS = 24, - LEFT_CLIMBER_NUMBER_OF_LEDS = 24; + RIGHT_CLIMBER_NUMBER_OF_LEDS = 22, + LEFT_CLIMBER_NUMBER_OF_LEDS = 22; private static final boolean RIGHT_CLIMBER_INVERTED = false, LEFT_CLIMBER_INVERTED = false; @@ -20,12 +21,21 @@ public class LEDStripConstants { static final double MINIMUM_BATTERY_VOLTAGE = 10.5; static final Trigger LOW_BATTERY_TRIGGER = new Trigger(() -> !DriverStation.isEnabled() && Robot.IS_REAL && RobotController.getBatteryVoltage() < LEDStripConstants.MINIMUM_BATTERY_VOLTAGE); - static final double LOW_BATTERY_BLINKING_INTERVAL_SECONDS = 0.2; - static final double LOW_BATTERY_BLINKING_TIME_SECONDS = 5; + static final Color + LOW_BATTERY_FIRST_COLOR = Color.kOrange, + LOW_BATTERY_SECOND_COLOR = Color.kYellow; + static final double LOW_BATTERY_ALTERNATE_COLOR_INTERVAL_SECONDS = 0.2; + static final double LOW_BATTERY_ALTERNATING_TIME_SECONDS = 10; + public static final Color DEFAULT_COMMAND_COLOR = Color.kLightSeaGreen; + public static final int DEFAULT_COMMAND_BREATHING_LEDS_AMOUNT = 7; + public static final double DEFAULT_COMMAND_BREATHING_CYCLE_TIME_SECONDS = 1.5; + public static final boolean + DEFAULT_COMMAND_BREATHING_SHOULD_LOOP = true, + DEFAULT_COMMAND_BREATHING_IS_INVERTED = false; public static final LEDStrip RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, 0), - LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, RIGHT_CLIMBER_NUMBER_OF_LEDS - 1); + LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, RIGHT_CLIMBER_NUMBER_OF_LEDS); static { LED.setLength(RIGHT_CLIMBER_NUMBER_OF_LEDS + LEFT_CLIMBER_NUMBER_OF_LEDS); diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index 5401c481..77d70a65 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -1,6 +1,6 @@ package frc.trigon.robot.subsystems.pitcher; -import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,14 +19,15 @@ import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; +import org.trigon.utilities.Conversions; public class Pitcher extends MotorSubsystem { private final ShootingCalculations shootingCalculations = ShootingCalculations.getInstance(); private final TalonFXMotor masterMotor = PitcherConstants.MASTER_MOTOR, followerMotor = PitcherConstants.FOLLOWER_MOTOR; + private final MotionMagicVoltage positionRequest = new MotionMagicVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED); private final CANcoderEncoder encoder = PitcherConstants.ENCODER; - private final MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED); private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(PitcherConstants.FOC_ENABLED); private Rotation2d targetPitch = PitcherConstants.DEFAULT_PITCH; @@ -37,8 +38,8 @@ public Pitcher() { @Override public void updateLog(SysIdRoutineLog log) { log.motor("Pitcher") - .angularPosition(Units.Rotations.of(masterMotor.getSignal(TalonFXSignal.POSITION))) - .angularVelocity(Units.RotationsPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + .angularPosition(Units.Rotations.of(masterMotor.getSignal(TalonFXSignal.ROTOR_POSITION) / PitcherConstants.GEAR_RATIO)) + .angularVelocity(Units.RotationsPerSecond.of(masterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / PitcherConstants.GEAR_RATIO)) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @@ -56,7 +57,9 @@ public void stop() { @Override public void updatePeriodically() { masterMotor.update(); + followerMotor.update(); encoder.update(); + Logger.recordOutput("PitcherAngleDegrees", Conversions.rotationsToDegrees(encoder.getSignal(CANcoderSignal.POSITION))); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index d575b662..4714aaf4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -20,6 +20,7 @@ import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.SingleJointedArmSimulation; +import org.trigon.utilities.Conversions; import org.trigon.utilities.mechanisms.DoubleJointedArmMechanism2d; public class PitcherConstants { @@ -42,25 +43,26 @@ public class PitcherConstants { private static final boolean FOLLOWER_OPPOSES_MASTER = true; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - P = RobotHardwareStats.isSimulation() ? 100 : 0, + P = RobotHardwareStats.isSimulation() ? 100 : 0.5, I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 20 : 0, - KS = RobotHardwareStats.isSimulation() ? 0.2 : 0, - KV = RobotHardwareStats.isSimulation() ? 32 : 0, + D = RobotHardwareStats.isSimulation() ? 20 : 0.5, + KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.3, + KV = RobotHardwareStats.isSimulation() ? 32 : 23, KA = RobotHardwareStats.isSimulation() ? 0 : 0, - KG = RobotHardwareStats.isSimulation() ? 0.2 : 0; + KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.37; private static final double - EXPO_KV = KV + 5, - EXPO_KA = 0.2; + MOTION_MAGIC_CRUISE_VELOCITY = 0.35, + MOTION_MAGIC_ACCELERATION = 3, + MOTION_MAGIC_JERK = 32; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; - private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; + private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseClosedLoopSign; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; - private static final double GEAR_RATIO = 227.77777; + static final double GEAR_RATIO = 200; private static final Rotation2d - REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(12), + REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(73); private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.Clockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = 0; + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.34932 + Conversions.degreesToRotations(360); private static final AbsoluteSensorRangeValue ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final boolean FOC_ENABLED = true; @@ -70,7 +72,7 @@ public class PitcherConstants { PITCHER_LENGTH_METERS = 0.5, PITCHER_MASS_KILOGRAMS = 11; private static final Rotation2d - PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(12), + PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(13), PITCHER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(73); private static final boolean SIMULATE_GRAVITY = true; private static final SingleJointedArmSimulation SIMULATION = new SingleJointedArmSimulation( @@ -84,9 +86,9 @@ public class PitcherConstants { ); static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(5).per(Units.Second.of(1)), - Units.Volts.of(9), - Units.Second.of(1000) + Units.Volts.of(1).per(Units.Second.of(1)), + Units.Volts.of(2.5), + Units.Second.of(100) ); static final Pose3d PITCHER_VISUALIZATION_ORIGIN_POINT = new Pose3d(0.2521, 0, 0.15545, new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(-12), 0)); @@ -97,9 +99,10 @@ public class PitcherConstants { new Color8Bit(Color.kGreen) ); - public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(12); - static final Rotation2d PITCH_TOLERANCE = Rotation2d.fromDegrees(2); + public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(13); + static final Rotation2d PITCH_TOLERANCE = Rotation2d.fromDegrees(1); public static final Transform3d VISUALIZATION_PITCHER_PIVOT_POINT_TO_HELD_NOTE = new Transform3d(0.24, 0, 0.02, new Rotation3d()); + public static final Rotation2d BLOCK_PITCH = Rotation2d.fromDegrees(70); static { configureMasterMotor(); @@ -124,8 +127,9 @@ private static void configureMasterMotor() { config.Slot0.kA = KA; config.Slot0.kG = KG; - config.MotionMagic.MotionMagicExpo_kV = EXPO_KV; - config.MotionMagic.MotionMagicExpo_kA = EXPO_KA; + config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_CRUISE_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; config.Slot0.GravityType = GRAVITY_TYPE_VALUE; config.Slot0.StaticFeedforwardSign = STATIC_FEEDFORWARD_SIGN_VALUE; @@ -143,10 +147,11 @@ private static void configureMasterMotor() { MASTER_MOTOR.setPhysicsSimulation(SIMULATION); MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 1000); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); } @@ -163,6 +168,8 @@ private static void configureFollowerMotor() { final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); FOLLOWER_MOTOR.setControl(followerRequest); + + FOLLOWER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); } public static void configureEncoder() { diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index bb7c00a4..5891b54d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -24,16 +24,23 @@ public class ShooterConstants { LEFT_MOTOR = new TalonFXMotor(LEFT_MOTOR_ID, LEFT_MOTOR_NAME); private static final InvertedValue - RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive, + RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive, LEFT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double - P = RobotHardwareStats.isSimulation() ? 15 : 0, - I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 0 : 0, - KS = RobotHardwareStats.isSimulation() ? 0.35586 : 0, - KV = RobotHardwareStats.isSimulation() ? 0 : 0, - KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0; + RIGHT_P = RobotHardwareStats.isSimulation() ? 15 : 10, + RIGHT_I = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_D = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_KS = RobotHardwareStats.isSimulation() ? 0.35586 : 6.3793, + RIGHT_KV = RobotHardwareStats.isSimulation() ? 0 : 0.071122, + RIGHT_KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0.90291; + private static final double + LEFT_P = RobotHardwareStats.isSimulation() ? 15 : 10, + LEFT_I = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_D = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_KS = RobotHardwareStats.isSimulation() ? 0.35586 : 5.0484, + LEFT_KV = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0.79623; private static final double GEAR_RATIO = 1; private static final int @@ -48,8 +55,8 @@ public class ShooterConstants { LEFT_SIMULATION = new FlywheelSimulation(LEFT_GEARBOX, GEAR_RATIO, MOMENT_OF_INERTIA); static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(0.25).per(Units.Second), - Units.Volts.of(7), + Units.Volts.of(5).per(Units.Second), + Units.Volts.of(9), Units.Second.of(1000) ); @@ -59,15 +66,15 @@ public class ShooterConstants { LEFT_MECHANISM = new SpeedMechanism2d("LeftShooterMechanism", MAX_DISPLAYABLE_VELOCITY); public static final double WHEEL_DIAMETER_METERS = edu.wpi.first.math.util.Units.inchesToMeters(4); - public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1; - static final double VELOCITY_TOLERANCE = 3; + public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1.5; + static final double VELOCITY_TOLERANCE = 0.4; static { - configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_SIMULATION); - configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_SIMULATION); + configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_SIMULATION, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_KS, RIGHT_KV, RIGHT_KA); + configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_SIMULATION, LEFT_P, LEFT_I, LEFT_D, LEFT_KS, LEFT_KV, LEFT_KA); } - private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, FlywheelSimulation simulation) { + private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, FlywheelSimulation simulation, double p, double i, double d, double kS, double kV, double kA) { final TalonFXConfiguration config = new TalonFXConfiguration(); config.Audio.BeepOnBoot = false; @@ -76,12 +83,12 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal config.MotorOutput.Inverted = invertedValue; config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; - config.Slot0.kP = P; - config.Slot0.kI = I; - config.Slot0.kD = D; - config.Slot0.kS = KS; - config.Slot0.kV = KV; - config.Slot0.kA = KA; + config.Slot0.kP = p; + config.Slot0.kI = i; + config.Slot0.kD = d; + config.Slot0.kS = kS; + config.Slot0.kV = kV; + config.Slot0.kA = kA; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index e1fad94b..6e42773a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -95,7 +95,7 @@ public ChassisSpeeds getSelfRelativeVelocity() { } public ChassisSpeeds getFieldRelativeVelocity() { - return ChassisSpeeds.fromRobotRelativeSpeeds(getSelfRelativeVelocity(), RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation()); + return ChassisSpeeds.fromRobotRelativeSpeeds(getSelfRelativeVelocity(), RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation()); } /** @@ -111,16 +111,16 @@ public boolean atPose(MirrorablePose2d pose2d) { public boolean atXAxisPosition(double xAxisPosition) { final double currentXAxisVelocity = getFieldRelativeVelocity().vxMetersPerSecond; - return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getX(), xAxisPosition, currentXAxisVelocity); + return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getX(), xAxisPosition, currentXAxisVelocity); } public boolean atYAxisPosition(double yAxisPosition) { final double currentYAxisVelocity = getFieldRelativeVelocity().vyMetersPerSecond; - return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getY(), yAxisPosition, currentYAxisVelocity); + return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getY(), yAxisPosition, currentYAxisVelocity); } public boolean atAngle(MirrorableRotation2d angle) { - final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES; + final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES; final boolean isAngleStill = Math.abs(getSelfRelativeVelocity().omegaRadiansPerSecond) < SwerveConstants.ROTATION_VELOCITY_TOLERANCE; Logger.recordOutput("Swerve/AtTargetAngle/isStill", isAngleStill); Logger.recordOutput("Swerve/AtTargetAngle/atTargetAngle", atTargetAngle); @@ -134,6 +134,11 @@ public double[] getDriveWheelPositionsRadians() { return swerveModulesPositions; } + public Rotation2d getDriveRelativeAngle() { + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); + return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; + } + public void runWheelRadiusCharacterization(double omegaRadiansPerSecond) { selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)); } @@ -158,7 +163,7 @@ void lockSwerve() { * @param targetPose the target pose, relative to the blue alliance driver station's right corner */ void pidToPose(MirrorablePose2d targetPose) { - final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); final Pose2d mirroredTargetPose = targetPose.get(); final double xSpeed = SwerveConstants.TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), mirroredTargetPose.getX()); final double ySpeed = SwerveConstants.TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), mirroredTargetPose.getY()); @@ -177,7 +182,7 @@ void initializeDrive(boolean closedLoop) { } void resetRotationController() { - SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.reset(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getDegrees()); + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.reset(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation().getDegrees()); } void setClosedLoop(boolean closedLoop) { @@ -298,7 +303,7 @@ private void updateHardware() { private void configurePathPlanner() { AutoBuilder.configureHolonomic( - RobotContainer.POSE_ESTIMATOR::getCurrentPose, + RobotContainer.POSE_ESTIMATOR::getCurrentEstimatedPose, (pose) -> { }, this::getSelfRelativeVelocity, @@ -316,7 +321,7 @@ private boolean atTranslationPosition(double currentTranslationPosition, double } private double calculateProfiledAngleSpeedToTargetAngle(MirrorableRotation2d targetAngle) { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); return Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees(), targetAngle.get().getDegrees())); } @@ -329,11 +334,6 @@ private ChassisSpeeds fieldRelativeSpeedsToSelfRelativeSpeeds(ChassisSpeeds fiel return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getDriveRelativeAngle()); } - private Rotation2d getDriveRelativeAngle() { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; - } - private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaPower) { return new ChassisSpeeds( xPower * SwerveConstants.MAX_SPEED_METERS_PER_SECOND, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 95ecaf8e..30b588c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -149,7 +149,7 @@ private static Command getCurrentDriveToPoseCommand(MirrorablePose2d targetPose, private static Command getPathfindToPoseCommand(MirrorablePose2d targetPose, PathConstraints pathConstraints) { final Pose2d targetMirroredPose = targetPose.get(); - final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); if (currentPose.getTranslation().getDistance(targetMirroredPose.getTranslation()) < 0.35) return createOnTheFlyPathCommand(targetPose, pathConstraints); return AutoBuilder.pathfindToPose(targetMirroredPose, pathConstraints); @@ -163,7 +163,7 @@ private static Command getPIDToPoseCommand(MirrorablePose2d targetPose) { private static Command createOnTheFlyPathCommand(MirrorablePose2d targetPose, PathConstraints constraints) { List bezierPoints = PathPlannerPath.bezierFromPoses( - RobotContainer.POSE_ESTIMATOR.getCurrentPose(), + RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), targetPose.get() ); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index c58a3ad4..7d54d3a5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -15,7 +15,6 @@ import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Gyro; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Signal; -import org.trigon.utilities.Conversions; import java.util.function.DoubleSupplier; @@ -23,14 +22,14 @@ public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_YAW = 0, - GYRO_MOUNT_POSITION_PITCH = 0, - GYRO_MOUNT_POSITION_ROLL = 0; + GYRO_MOUNT_POSITION_YAW = 86.152039 , + GYRO_MOUNT_POSITION_PITCH = 0.087891 , + GYRO_MOUNT_POSITION_ROLL = -0.351562 ; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(225.263672 - 360), - FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-256.904297 + 360), - REAR_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(108.369141), - REAR_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-36.035156); + FRONT_LEFT_STEER_ENCODER_OFFSET = -1.561, + FRONT_RIGHT_STEER_ENCODER_OFFSET = -3.431, + REAR_LEFT_STEER_ENCODER_OFFSET = -0.971 + 0.437, + REAR_RIGHT_STEER_ENCODER_OFFSET = -2.008; private static final int FRONT_LEFT_ID = 1, FRONT_RIGHT_ID = 2, @@ -78,13 +77,13 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0.05) : - new PIDConstants(5, 0, 0), + new PIDConstants(3, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0.1) : - new PIDConstants(6.5, 0, 0), + new PIDConstants(2, 0, 0.1), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(2.5, 0, 0.2) : - new PIDConstants(3, 0, 0); + new PIDConstants(6.5, 0, 0); private static final double MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 720, MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 720; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index 4766cde2..b02b4fe9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -33,7 +33,7 @@ public class SwerveModule { public SwerveModule(int moduleID, double offsetRotations) { driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME); steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME); - steerEncoder = new CANcoderEncoder(moduleID, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); + steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); configureHardware(offsetRotations); } @@ -174,15 +174,17 @@ private void configureHardware(double offsetRotations) { } private void configureSignals() { - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - + + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 85b215f0..cba24647 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -25,19 +25,19 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 100, - STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 50; + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 80, + STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 30; private static final double STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, STEER_MOTOR_I = 0, STEER_MOTOR_D = 0; private static final double - DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 50, + DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 52, DRIVE_MOTOR_I = 0, DRIVE_MOTOR_D = 0, - DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 0, - DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0, - DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 0; + DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 6.2176, + DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0.0017378, + DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 2.4345; static final boolean ENABLE_FOC = true; static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = generateDriveConfiguration(), @@ -56,11 +56,11 @@ public class SwerveModuleConstants { static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(5).per(Units.Second), - Units.Volts.of(20), + Units.Volts.of(8), Units.Second.of(1000) ); - static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049149 * 2; + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049274 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; static SimpleMotorSimulation createDriveSimulation() { diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 0e80a16c..6d1f485d 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,57 +1,57 @@ { - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.3.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2024.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2024.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.3.1" - } - ] + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-2" + } + ] } \ No newline at end of file