diff --git a/src/main/deploy/pathplanner/autos/CLB_Citrus_V2.auto b/src/main/deploy/pathplanner/autos/CLB_Citrus_V2.auto new file mode 100644 index 0000000..0f98a38 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CLB_Citrus_V2.auto @@ -0,0 +1,204 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CLB_Entry" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CLB_Intake_V2" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CLB_StageShoot1_V2" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + }, + { + "type": "named", + "data": { + "name": "flywheelLayup" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "Citrus Auto", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Force Warmup.auto b/src/main/deploy/pathplanner/autos/Force Warmup.auto new file mode 100644 index 0000000..a15c30b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Force Warmup.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Force Warmup" + } + } + ] + } + }, + "resetOdom": false, + "folder": "Force Warmup", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/JustAim.auto b/src/main/deploy/pathplanner/autos/JustAim.auto new file mode 100644 index 0000000..1cc468e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/JustAim.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + } + ] + } + }, + "resetOdom": false, + "folder": "TestingSpartan", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/JustConstraint.auto b/src/main/deploy/pathplanner/autos/JustConstraint.auto new file mode 100644 index 0000000..7473c45 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/JustConstraint.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "JustConstraint" + } + } + ] + } + }, + "resetOdom": false, + "folder": "TestingSpartan", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/JustKick.auto b/src/main/deploy/pathplanner/autos/JustKick.auto new file mode 100644 index 0000000..043497e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/JustKick.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + }, + { + "type": "named", + "data": { + "name": "flywheelLayup" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "TestingSpartan", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto index 9154942..ce260be 100644 --- a/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto +++ b/src/main/deploy/pathplanner/autos/Static Shoot Preload.auto @@ -16,6 +16,12 @@ "pathName": "CenterBackout" } }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, { "type": "race", "data": { @@ -29,13 +35,13 @@ { "type": "named", "data": { - "name": "flywheelLayup" + "name": "flywheelDynamic" } }, { "type": "named", "data": { - "name": "hoodLayup" + "name": "hoodDynamic" } } ] @@ -48,13 +54,13 @@ { "type": "named", "data": { - "name": "flywheelLayup" + "name": "flywheelDynamic" } }, { "type": "named", "data": { - "name": "hoodLayup" + "name": "hoodDynamic" } }, { diff --git a/src/main/deploy/pathplanner/autos/[Left Swiper Visual].auto b/src/main/deploy/pathplanner/autos/[Left Swiper Visual].auto new file mode 100644 index 0000000..52a2b16 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Left Swiper Visual].auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Entry 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Intake 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Left Swipe] StageShoot1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Reentry 1" + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Left] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto b/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto new file mode 100644 index 0000000..cfe2522 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Left] Bump Disrupt.auto @@ -0,0 +1,57 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bump Disrupt Entry" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotUp" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bump Disrupt Block" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Left] Bump Disrupt", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto new file mode 100644 index 0000000..263fa32 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Left] Double Swipe.auto @@ -0,0 +1,402 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Entry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Intake 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] StageShoot1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Reentry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] Intake 2" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Left Swipe] StageShoot2" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Left] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto b/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto new file mode 100644 index 0000000..099198e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Bump Disrupt.auto @@ -0,0 +1,57 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bump Disrupt Entry" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "pivotUp" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bump Disrupt Block" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Bump Disrupt", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Right] Bump.auto b/src/main/deploy/pathplanner/autos/[Right] Bump.auto new file mode 100644 index 0000000..723eebf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Bump.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "crossBumpAllianceZone" + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Bump", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Right] Double Swipe Visualizer.auto b/src/main/deploy/pathplanner/autos/[Right] Double Swipe Visualizer.auto new file mode 100644 index 0000000..43fcb1f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Double Swipe Visualizer.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Entry 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Reentry 1" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 2" + } + }, + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot2" + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto new file mode 100644 index 0000000..524cd71 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/[Right] Double Swipe.auto @@ -0,0 +1,438 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "toggleWarm" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Entry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "flywheelLayup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "toggleHub" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.25 + } + }, + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Reentry 1" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "hoodLayup" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] Intake 2" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "[Right Swipe] StageShoot2" + } + }, + { + "type": "named", + "data": { + "name": "pivotDown" + } + }, + { + "type": "named", + "data": { + "name": "rollerIntake" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIntake" + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "driveHubLock" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.5 + } + }, + { + "type": "named", + "data": { + "name": "flywheelDynamic" + } + }, + { + "type": "named", + "data": { + "name": "hoodDynamic" + } + }, + { + "type": "named", + "data": { + "name": "kickerIndex" + } + }, + { + "type": "named", + "data": { + "name": "agitatorIndex" + } + }, + { + "type": "named", + "data": { + "name": "indexerIndex" + } + }, + { + "type": "named", + "data": { + "name": "pivotShake" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": "[Right] Double Swipe", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CLB_Entry.path b/src/main/deploy/pathplanner/paths/CLB_Entry.path index 952286d..3d59e6f 100644 --- a/src/main/deploy/pathplanner/paths/CLB_Entry.path +++ b/src/main/deploy/pathplanner/paths/CLB_Entry.path @@ -62,7 +62,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 2.0, + "velocity": 0.5, "rotation": -107.64200000000001 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/CLB_Intake.path b/src/main/deploy/pathplanner/paths/CLB_Intake.path index 6867b3b..1d1efb8 100644 --- a/src/main/deploy/pathplanner/paths/CLB_Intake.path +++ b/src/main/deploy/pathplanner/paths/CLB_Intake.path @@ -53,8 +53,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.811910669975181, - "maxWaypointRelativePos": 0.3513647642680001, + "minWaypointRelativePos": 1.1505739365293643, + "maxWaypointRelativePos": 0.2808912896691613, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path b/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path new file mode 100644 index 0000000..a567993 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CLB_Intake_V2.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.13, + "y": 7.017 + }, + "prevControl": null, + "nextControl": { + "x": 8.126000184885955, + "y": 6.575320550590469 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.13, + "y": 5.054753442150735 + }, + "prevControl": { + "x": 8.157996058825054, + "y": 5.354131000583097 + }, + "nextControl": { + "x": 8.09464321482716, + "y": 4.676663508663737 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.846747503566333, + "y": 5.607 + }, + "prevControl": { + "x": 7.198343383255684, + "y": 5.160743691163518 + }, + "nextControl": { + "x": 6.510342368045648, + "y": 6.0339757489301 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.342, + "y": 7.37 + }, + "prevControl": { + "x": 6.445509272467903, + "y": 6.771526390870186 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.854771784232354, + "rotationDegrees": -110.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.3305210918114133, + "maxWaypointRelativePos": 1.3548387096774213, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "CLB Citrus Auto", + "idealStartingState": { + "velocity": 0.5, + "rotation": -107.64200000000001 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path b/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path new file mode 100644 index 0000000..b1c516c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CLB_StageShoot1_V2.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.342, + "y": 7.37 + }, + "prevControl": null, + "nextControl": { + "x": 5.099155940801128, + "y": 7.402306101453735 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1800082843706226, + "y": 7.37 + }, + "prevControl": { + "x": 3.5462722834950475, + "y": 7.420690225765222 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9609958506224051, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 179.51226418354418 + }, + "reversed": false, + "folder": "CLB Citrus Auto", + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path b/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path index ee4c07b..483775b 100644 --- a/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path +++ b/src/main/deploy/pathplanner/paths/CLB_TrenchShoot.path @@ -61,7 +61,7 @@ "rotation": -29.999999999999996 }, "reversed": false, - "folder": "Josh", + "folder": "Misc", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/CLR_Intake.path b/src/main/deploy/pathplanner/paths/CLR_Intake.path index 6dbda4a..7cc8d8f 100644 --- a/src/main/deploy/pathplanner/paths/CLR_Intake.path +++ b/src/main/deploy/pathplanner/paths/CLR_Intake.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 8.01254981548159, - "y": 2.7933775898403677 + "y": 2.793377589840365 }, "nextControl": { "x": 7.820769955384298, diff --git a/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path b/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path index eaaab47..5ac52fb 100644 --- a/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path +++ b/src/main/deploy/pathplanner/paths/Copy of CLB_Intake.path @@ -112,7 +112,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Josh", + "folder": "Misc", "idealStartingState": { "velocity": 2.0, "rotation": -107.64200000000001 diff --git a/src/main/deploy/pathplanner/paths/Force Warmup.path b/src/main/deploy/pathplanner/paths/Force Warmup.path new file mode 100644 index 0000000..600e861 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Force Warmup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3144935805991436, + "y": 7.0 + }, + "prevControl": { + "x": 2.3144935805991436, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Force Warmup", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/JustConstraint.path b/src/main/deploy/pathplanner/paths/JustConstraint.path new file mode 100644 index 0000000..114277e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/JustConstraint.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.959202247191011, + "y": 7.3572696629213485 + }, + "prevControl": null, + "nextControl": { + "x": 4.959202247191014, + "y": 7.3572696629213485 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.239426966292136, + "y": 7.3572696629213485 + }, + "prevControl": { + "x": 7.239426966292136, + "y": 7.3572696629213485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.1498759305210926, + "maxWaypointRelativePos": 0.5687344913151384, + "constraints": { + "maxVelocity": 0.1, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Testing", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path new file mode 100644 index 0000000..34c841d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Block.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.264507845935014, + "y": 7.276134094155207 + }, + "prevControl": null, + "nextControl": { + "x": 7.0373344200118195, + "y": 7.290847159455852 + }, + "isLocked": false, + "linkedName": "[Left Bump Disrupt] Entry" + }, + { + "anchor": { + "x": 8.153552068474243, + "y": 6.616262482172326 + }, + "prevControl": { + "x": 8.048357809895414, + "y": 6.928791486051591 + }, + "nextControl": { + "x": 8.245845591456037, + "y": 6.342061178872171 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.153552068474243, + "y": 5.225609814878692 + }, + "prevControl": { + "x": 8.153552068474243, + "y": 6.0411559832069495 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.164179104477597, + "rotationDegrees": -124.509 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 2.0, + "rotation": -124.509 + }, + "reversed": false, + "folder": "[Left] Bump Disrupt", + "idealStartingState": { + "velocity": 4.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path new file mode 100644 index 0000000..a2f64d5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump Disrupt Entry.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.655090117679368, + "y": 7.434229990952901 + }, + "prevControl": null, + "nextControl": { + "x": 4.65509011767937, + "y": 7.434229990952901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.264507845935014, + "y": 7.276134094155207 + }, + "prevControl": { + "x": 5.289926685119949, + "y": 7.453350886478522 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Bump Disrupt] Entry" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 3.5, + "rotation": 0.0 + }, + "reversed": false, + "folder": "[Left] Bump Disrupt", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path new file mode 100644 index 0000000..1695217 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Block.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.009974856443805, + "y": 0.6719662921348311 + }, + "prevControl": null, + "nextControl": { + "x": 7.02796968517205, + "y": 0.8661313326451534 + }, + "isLocked": false, + "linkedName": "[Right Bump Disrupt] Entry" + }, + { + "anchor": { + "x": 7.840887140551429, + "y": 1.3579805493452808 + }, + "prevControl": { + "x": 7.547923178609416, + "y": 0.8548467886187774 + }, + "nextControl": { + "x": 8.155124140109843, + "y": 1.8976484398912565 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.134630422747337, + "y": 2.9974779383457086 + }, + "prevControl": { + "x": 8.134630422747337, + "y": 1.9974779383457086 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.24647302904564, + "rotationDegrees": 124.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 124.0 + }, + "reversed": false, + "folder": "[Right] Bump Disrupt", + "idealStartingState": { + "velocity": 4.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path new file mode 100644 index 0000000..8020582 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump Disrupt Entry.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.639534881514021, + "y": 0.6719662921348311 + }, + "prevControl": null, + "nextControl": { + "x": 4.639534881514023, + "y": 0.6719662921348311 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.009974856443805, + "y": 0.6719662921348311 + }, + "prevControl": { + "x": 5.009974856443805, + "y": 0.6719662921348312 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Bump Disrupt] Entry" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 4.5, + "rotation": 0.0 + }, + "reversed": false, + "folder": "[Right] Bump Disrupt", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path new file mode 100644 index 0000000..d8ceeea --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Entry 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6127078651673195, + "y": 7.3878426966292965 + }, + "prevControl": null, + "nextControl": { + "x": 4.733719101122375, + "y": 7.530516853932667 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 7.03115730337087 + }, + "prevControl": { + "x": 7.34261797752687, + "y": 7.194213483146151 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Swipe] Entry 1 to Intake 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.30788381742738447, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8157676348547667, + "rotationDegrees": -102.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -102.265 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path new file mode 100644 index 0000000..ea52a5b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 1.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.933696629212264, + "y": 7.03115730337087 + }, + "prevControl": null, + "nextControl": { + "x": 7.933696629212264, + "y": 6.337868868494916 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Entry 1 to Intake 1" + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 5.062089807762963 + }, + "prevControl": { + "x": 7.969051968270556, + "y": 5.309577181178402 + }, + "nextControl": { + "x": 7.892932584269662, + "y": 4.776741493156222 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.985932584269663, + "y": 6.0426292134831465 + }, + "prevControl": { + "x": 6.985932584269663, + "y": 4.554741573033708 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Swipe] Intake 1 to StageShoot1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5560165975103781, + "rotationDegrees": -102.40100556375558 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.04565756823821024, + "maxWaypointRelativePos": 0.4267990074441744, + "constraints": { + "maxVelocity": 0.75, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": -102.265 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 2.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 2.path new file mode 100644 index 0000000..c77db16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Intake 2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.812685393258427, + "y": 7.061730337078652 + }, + "prevControl": null, + "nextControl": { + "x": 6.802494382022473, + "y": 6.144539325842698 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Reentry 1 to Intake 2" + }, + { + "anchor": { + "x": 5.922158156517307, + "y": 5.567559408383813 + }, + "prevControl": { + "x": 6.5082850713280145, + "y": 5.576717641427731 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Swipe] Intake2 to StageShoot2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.2, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 3.5, + "rotation": -102.265 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path new file mode 100644 index 0000000..8575490 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] Reentry 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.184685393258427, + "y": 7.367460674157303 + }, + "prevControl": null, + "nextControl": { + "x": 3.4323011519061213, + "y": 7.333015992865022 + }, + "isLocked": false, + "linkedName": "[Left Swipe] PostShoot1 to Reentry 1" + }, + { + "anchor": { + "x": 6.812685393258427, + "y": 7.061730337078652 + }, + "prevControl": { + "x": 4.871526943777191, + "y": 7.505709086836602 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Left Swipe] Reentry 1 to Intake 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0240663900414924, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6315352697095409, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -102.265 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": -66.52706715066263 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path new file mode 100644 index 0000000..35d4473 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot1.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.985932584269663, + "y": 6.0426292134831465 + }, + "prevControl": null, + "nextControl": { + "x": 7.000023991279187, + "y": 7.047947238549122 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Intake 1 to StageShoot1" + }, + { + "anchor": { + "x": 6.539180929244987, + "y": 7.367460674157303 + }, + "prevControl": { + "x": 6.8276652701283815, + "y": 7.316979209602038 + }, + "nextControl": { + "x": 6.292922834860365, + "y": 7.410553027888035 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.2228876404494375, + "y": 7.367460674157303 + }, + "prevControl": { + "x": 6.3729928437130265, + "y": 7.316116691868749 + }, + "nextControl": { + "x": 4.081494382022472, + "y": 7.418415730337078 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.184685393258427, + "y": 7.367460674157303 + }, + "prevControl": { + "x": 3.87767415730337, + "y": 7.347078651685393 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.953526970954383, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.11, + "maxWaypointRelativePos": 1.76, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 2.0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot2.path b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot2.path new file mode 100644 index 0000000..73206ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Left Swipe] StageShoot2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.922158156517307, + "y": 5.567559408383813 + }, + "prevControl": null, + "nextControl": { + "x": 4.560471910112359, + "y": 5.543269662921348 + }, + "isLocked": false, + "linkedName": "[Left Swipe] Intake2 to StageShoot2" + }, + { + "anchor": { + "x": 2.991056179775281, + "y": 5.567559408383813 + }, + "prevControl": { + "x": 4.580853932584269, + "y": 5.553460674157304 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 4.5, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Left] Double Swipe", + "idealStartingState": { + "velocity": 3.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path new file mode 100644 index 0000000..091927d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Entry 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6127078651673195, + "y": 0.6811573033707043 + }, + "prevControl": null, + "nextControl": { + "x": 4.733719101122375, + "y": 0.5384831460673336 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 1.0378426966291308 + }, + "prevControl": { + "x": 7.34261797752687, + "y": 0.8747865168538498 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Entry 1 to Intake 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.30788381742738447, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 0.8157676348547667, + "rotationDegrees": 102.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 102.265 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0.0, + "rotation": -0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path new file mode 100644 index 0000000..34f2507 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 1.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.933696629212264, + "y": 1.0378426966291308 + }, + "prevControl": null, + "nextControl": { + "x": 7.933696629212264, + "y": 1.7311311315050846 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Entry 1 to Intake 1" + }, + { + "anchor": { + "x": 7.933696629212264, + "y": 2.9975487986925713 + }, + "prevControl": { + "x": 7.969051968270556, + "y": 2.750061425277132 + }, + "nextControl": { + "x": 7.892932584269662, + "y": 3.282897113299312 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.985932584269663, + "y": 2.0263707865168543 + }, + "prevControl": { + "x": 6.840959608233403, + "y": 3.3887350357286063 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Intake 1 to StageShoot1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5560165975103781, + "rotationDegrees": 102.40100556375558 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.04565756823821024, + "maxWaypointRelativePos": 0.3712158808933023, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 3.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": 102.265 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 2.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 2.path new file mode 100644 index 0000000..fcbfbc9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Intake 2.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.281471910117554, + "y": 1.0694157303404221 + }, + "prevControl": null, + "nextControl": { + "x": 7.3516932933484975, + "y": 2.0031704948881686 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Reentry 1 to Intake 2" + }, + { + "anchor": { + "x": 6.924786516853931, + "y": 3.0260898876404485 + }, + "prevControl": { + "x": 7.639184049069801, + "y": 2.8367736612095276 + }, + "nextControl": { + "x": 6.683127875040819, + "y": 3.090129725530952 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.099314606746768, + "y": 1.8643146067449163 + }, + "prevControl": { + "x": 6.048760951336342, + "y": 3.005068379793869 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Intake2 to StageShoot2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.36327543424318565, + "maxWaypointRelativePos": 0.6531017369727078, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 2.0, + "rotation": 102.265 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path new file mode 100644 index 0000000..efd2a95 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] Reentry 1.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.184685393258427, + "y": 0.7015393258426981 + }, + "prevControl": null, + "nextControl": { + "x": 4.184685393258427, + "y": 0.7015393258426981 + }, + "isLocked": false, + "linkedName": "[Right Swipe] PostShoot1 to Reentry 1" + }, + { + "anchor": { + "x": 7.281471910117554, + "y": 1.0694157303404221 + }, + "prevControl": { + "x": 6.496764044950233, + "y": 0.5904382022506188 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "[Right Swipe] Reentry 1 to Intake 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0240663900414924, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 0.4373443983402455, + "rotationDegrees": -0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 102.265 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": 66.52706715066263 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path new file mode 100644 index 0000000..ef87df6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot1.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.985932584269663, + "y": 2.0263707865168543 + }, + "prevControl": null, + "nextControl": { + "x": 7.000023991279187, + "y": 1.0210527614508784 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Intake 1 to StageShoot1" + }, + { + "anchor": { + "x": 6.516881888575901, + "y": 0.5990589998344193 + }, + "prevControl": { + "x": 6.805366229459295, + "y": 0.6495404643896843 + }, + "nextControl": { + "x": 6.270623794191279, + "y": 0.555966646103687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.193517213729849, + "y": 0.5990589998344193 + }, + "prevControl": { + "x": 6.344711612736678, + "y": 0.6104449028086686 + }, + "nextControl": { + "x": 4.943529440613317, + "y": 0.5965864925568396 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.184685393258427, + "y": 0.7015393258426981 + }, + "prevControl": { + "x": 3.87767415730337, + "y": 0.7219213483146074 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.953526970954383, + "rotationDegrees": -180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.2709677419354811, + "maxWaypointRelativePos": 0.7533498759305253, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -180.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 2.0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot2.path b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot2.path new file mode 100644 index 0000000..246a498 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/[Right Swipe] StageShoot2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.099314606746768, + "y": 1.8643146067449163 + }, + "prevControl": null, + "nextControl": { + "x": 6.099864155797287, + "y": 1.2693087830908234 + }, + "isLocked": false, + "linkedName": "[Right Swipe] Intake2 to StageShoot2" + }, + { + "anchor": { + "x": 5.894926982172234, + "y": 0.5588599145239721 + }, + "prevControl": { + "x": 6.243320177334823, + "y": 0.5793536318864778 + }, + "nextControl": { + "x": 5.0219236310432755, + "y": 0.5075067762222673 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.92333796460896, + "y": 0.6749909795781683 + }, + "prevControl": { + "x": 3.7264351950132233, + "y": 0.6813754391366251 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8481327800829823, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "[Right] Double Swipe", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 2c87ad1..52b4bd1 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,15 +4,30 @@ "holonomicMode": true, "pathFolders": [ "CLB Citrus Auto", + "CRB Citrus Auto", "Default NZ Auto", "Depot Auto", - "CRB Citrus Auto", - "Testing" + "Force Warmup", + "Misc", + "[Right] Bump Disrupt", + "Testing", + "[Left] Bump", + "[Left] Bump Disrupt", + "[Left] Double Swipe", + "[Right] Bump", + "[Right] Double Swipe" ], "autoFolders": [ "Citrus Auto", "Default NZ Auto", - "TestingSpartan" + "Force Warmup", + "[Right] Bump Disrupt", + "TestingSpartan", + "[Left] Bump", + "[Left] Bump Disrupt", + "[Left] Double Swipe", + "[Right] Bump", + "[Right] Double Swipe" ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 79ec7a9..4fd9673 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Code-2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 124; - public static final String GIT_SHA = "084ebbc6a93af8a95535c60ea91df476b60ff34f"; - public static final String GIT_DATE = "2026-03-18 01:21:54 EDT"; - public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2026-04-08 14:47:16 EDT"; - public static final long BUILD_UNIX_TIME = 1775674036805L; + public static final int GIT_REVISION = 158; + public static final String GIT_SHA = "b6c923628e233616407657cfb9223cd528923c30"; + public static final String GIT_DATE = "2026-04-13 01:23:41 EDT"; + public static final String GIT_BRANCH = "test/2026/FullRobotV3"; + public static final String BUILD_DATE = "2026-04-13 23:35:41 EDT"; + public static final long BUILD_UNIX_TIME = 1776137741238L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index e6c670e..86f98b8 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,367 +1,377 @@ -// Copyright (c) 2025-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot; - -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.IOException; -import java.nio.file.Path; -import lombok.Getter; -import lombok.RequiredArgsConstructor; - -/** - * Contains information for location of field element and other useful reference points. - * - *

NOTE: All constants are defined relative to the field coordinate system, and from the - * perspective of the blue alliance station - */ -public class FieldConstants { - public static final FieldType fieldType = FieldType.WELDED; - - // AprilTag related constants - public static final int aprilTagCount = AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); - public static final double aprilTagWidth = Units.inchesToMeters(6.5); - public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; - - // Field dimensions - public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); - public static final double fieldWidth = AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); - - /** - * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) - */ - public static class LinesVertical { - public static final double center = fieldLength / 2.0; - public static final double starting = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); - public static final double allianceZone = starting; - public static final double hubCenter = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; - public static final double neutralZoneNear = center - Units.inchesToMeters(120); - public static final double neutralZoneFar = center + Units.inchesToMeters(120); - public static final double oppHubCenter = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; - public static final double oppAllianceZone = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); - } - - /** - * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) - * - *

NOTE: The field element start and end are always left to right from the perspective of the - * alliance station - */ - public static class LinesHorizontal { - - public static final double center = fieldWidth / 2.0; - - // Right of hub - public static final double rightBumpStart = Hub.nearRightCorner.getY(); - public static final double rightBumpEnd = rightBumpStart - RightBump.width; - public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); - public static final double rightTrenchOpenEnd = 0; - - // Left of hub - public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); - public static final double leftBumpStart = leftBumpEnd + LeftBump.width; - public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); - public static final double leftTrenchOpenStart = fieldWidth; - } - - /** Hub related constants */ - public static class Hub { - - // Dimensions - public static final double width = Units.inchesToMeters(47.0); - public static final double height = - Units.inchesToMeters(72.0); // includes the catcher at the top - public static final double innerWidth = Units.inchesToMeters(41.7); - public static final double innerHeight = Units.inchesToMeters(56.5); - - // Relevant reference points on alliance side - public static final Translation3d topCenterPoint = - new Translation3d( - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, - fieldWidth / 2.0, - height); - public static final Translation3d innerCenterPoint = - new Translation3d( - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, - fieldWidth / 2.0, - innerHeight); - - public static final Translation2d nearLeftCorner = - new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d nearRightCorner = - new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); - public static final Translation2d farLeftCorner = - new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d farRightCorner = - new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); - - // Relevant reference points on the opposite side - public static final Translation3d oppTopCenterPoint = - new Translation3d( - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, - fieldWidth / 2.0, - height); - public static final Translation2d oppNearLeftCorner = - new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d oppNearRightCorner = - new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); - public static final Translation2d oppFarLeftCorner = - new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d oppFarRightCorner = - new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); - - // Hub faces - public static final Pose2d nearFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); - public static final Pose2d farFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); - public static final Pose2d rightFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); - public static final Pose2d leftFace = - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); - } - - /** Left Bump related constants */ - public static class LeftBump { - - // Dimensions - public static final double width = Units.inchesToMeters(73.0); - public static final double height = Units.inchesToMeters(6.513); - public static final double depth = Units.inchesToMeters(44.4); - - // Relevant reference points on alliance side - public static final Translation2d nearLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d nearRightCorner = Hub.nearLeftCorner; - public static final Translation2d farLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d farRightCorner = Hub.farLeftCorner; - - // Relevant reference points on opposing side - public static final Translation2d oppNearLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; - public static final Translation2d oppFarLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; - } - - /** Right Bump related constants */ - public static class RightBump { - // Dimensions - public static final double width = Units.inchesToMeters(73.0); - public static final double height = Units.inchesToMeters(6.513); - public static final double depth = Units.inchesToMeters(44.4); - - // Relevant reference points on alliance side - public static final Translation2d nearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d nearRightCorner = Hub.nearLeftCorner; - public static final Translation2d farLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d farRightCorner = Hub.farLeftCorner; - - // Relevant reference points on opposing side - public static final Translation2d oppNearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; - public static final Translation2d oppFarLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); - public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; - } - - /** Left Trench related constants */ - public static class LeftTrench { - // Dimensions - public static final double width = Units.inchesToMeters(65.65); - public static final double depth = Units.inchesToMeters(47.0); - public static final double height = Units.inchesToMeters(40.25); - public static final double openingWidth = Units.inchesToMeters(50.34); - public static final double openingHeight = Units.inchesToMeters(22.25); - - // Relevant reference points on alliance side - public static final Translation3d openingTopLeft = - new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); - public static final Translation3d openingTopRight = - new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); - - // Relevant reference points on opposing side - public static final Translation3d oppOpeningTopLeft = - new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); - public static final Translation3d oppOpeningTopRight = - new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); - } - - public static class RightTrench { - - // Dimensions - public static final double width = Units.inchesToMeters(65.65); - public static final double depth = Units.inchesToMeters(47.0); - public static final double height = Units.inchesToMeters(40.25); - public static final double openingWidth = Units.inchesToMeters(50.34); - public static final double openingHeight = Units.inchesToMeters(22.25); - - // Relevant reference points on alliance side - public static final Translation3d openingTopLeft = - new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); - public static final Translation3d openingTopRight = - new Translation3d(LinesVertical.hubCenter, 0, openingHeight); - - // Relevant reference points on opposing side - public static final Translation3d oppOpeningTopLeft = - new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); - public static final Translation3d oppOpeningTopRight = - new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); - } - - /** Tower related constants */ - public static class Tower { - // Dimensions - public static final double width = Units.inchesToMeters(49.25); - public static final double depth = Units.inchesToMeters(45.0); - public static final double height = Units.inchesToMeters(78.25); - public static final double innerOpeningWidth = Units.inchesToMeters(32.250); - public static final double frontFaceX = Units.inchesToMeters(43.51); - - public static final double uprightHeight = Units.inchesToMeters(72.1); - - // Rung heights from the floor - public static final double lowRungHeight = Units.inchesToMeters(27.0); - public static final double midRungHeight = Units.inchesToMeters(45.0); - public static final double highRungHeight = Units.inchesToMeters(63.0); - - // Relevant reference points on alliance side - public static final Translation2d centerPoint = - new Translation2d( - frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); - public static final Translation2d leftUpright = - new Translation2d( - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) - + innerOpeningWidth / 2 - + Units.inchesToMeters(0.75)); - public static final Translation2d rightUpright = - new Translation2d( - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) - - innerOpeningWidth / 2 - - Units.inchesToMeters(0.75)); - - // Relevant reference points on opposing side - public static final Translation2d oppCenterPoint = - new Translation2d( - fieldLength - frontFaceX, - AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); - public static final Translation2d oppLeftUpright = - new Translation2d( - fieldLength - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) - + innerOpeningWidth / 2 - + Units.inchesToMeters(0.75)); - public static final Translation2d oppRightUpright = - new Translation2d( - fieldLength - frontFaceX, - (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) - - innerOpeningWidth / 2 - - Units.inchesToMeters(0.75)); - } - - public static class Depot { - // Dimensions - public static final double width = Units.inchesToMeters(42.0); - public static final double depth = Units.inchesToMeters(27.0); - public static final double height = Units.inchesToMeters(1.125); - public static final double distanceFromCenterY = Units.inchesToMeters(75.93); - - // Relevant reference points on alliance side - public static final Translation3d depotCenter = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); - public static final Translation3d leftCorner = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); - public static final Translation3d rightCorner = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); - } - - public static class Outpost { - // Dimensions - public static final double width = Units.inchesToMeters(31.8); - public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); - public static final double height = Units.inchesToMeters(7.0); - - // Relevant reference points on alliance side - public static final Translation2d centerPoint = - new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); - } - - @RequiredArgsConstructor - public enum FieldType { - ANDYMARK("andymark"), - WELDED("welded"); - - @Getter private final String jsonFolder; - } - - public enum AprilTagLayoutType { - OFFICIAL("2026-official"), - NONE("2026-none"); - - private final String name; - private volatile AprilTagFieldLayout layout; - private volatile String layoutString; - - AprilTagLayoutType(String name) { - this.name = name; - } - - public AprilTagFieldLayout getLayout() { - if (layout == null) { - synchronized (this) { - if (layout == null) { - try { - Path p = - Constants.disableHAL - ? Path.of( - "src", - "main", - "deploy", - "apriltags", - fieldType.getJsonFolder(), - name + ".json") - : Path.of( - Filesystem.getDeployDirectory().getPath(), - "apriltags", - fieldType.getJsonFolder(), - name + ".json"); - layout = new AprilTagFieldLayout(p); - layoutString = new ObjectMapper().writeValueAsString(layout); - } catch (IOException e) { - throw new RuntimeException(e); - } - } - } - } - return layout; - } - - public String getLayoutString() { - if (layoutString == null) { - getLayout(); - } - return layoutString; - } - } -} +// // Copyright (c) 2025-2026 Littleton Robotics +// // http://github.com/Mechanical-Advantage +// // +// // Use of this source code is governed by an MIT-style +// // license that can be found in the LICENSE file at +// // the root directory of this project. + +// package frc.robot; + +// import com.fasterxml.jackson.databind.ObjectMapper; +// import edu.wpi.first.apriltag.AprilTagFieldLayout; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.geometry.Translation3d; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.Filesystem; +// import java.io.IOException; +// import java.nio.file.Path; +// import lombok.Getter; +// import lombok.RequiredArgsConstructor; + +// /** +// * Contains information for location of field element and other useful reference points. +// * +// *

NOTE: All constants are defined relative to the field coordinate system, and from the +// * perspective of the blue alliance station +// */ +// public class FieldConstants { +// public static final FieldType fieldType = FieldType.WELDED; + +// // AprilTag related constants +// public static final int aprilTagCount = +// AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); +// public static final double aprilTagWidth = Units.inchesToMeters(6.5); +// public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + +// // Field dimensions +// public static final double fieldLength = +// AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); +// public static final double fieldWidth = +// AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); + +// /** +// * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) +// */ +// public static class LinesVertical { +// public static final double center = fieldLength / 2.0; +// public static final double starting = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); +// public static final double allianceZone = starting; +// public static final double hubCenter = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; +// public static final double neutralZoneNear = center - Units.inchesToMeters(120); +// public static final double neutralZoneFar = center + Units.inchesToMeters(120); +// public static final double oppHubCenter = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; +// public static final double oppAllianceZone = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); +// } + +// /** +// * Officially defined and relevant horizontal lines found on the field (defined by Y-axis +// offset) +// * +// *

NOTE: The field element start and end are always left to right from the perspective of +// the +// * alliance station +// */ +// public static class LinesHorizontal { + +// public static final double center = fieldWidth / 2.0; + +// // Right of hub +// public static final double rightBumpStart = Hub.nearRightCorner.getY(); +// public static final double rightBumpEnd = rightBumpStart - RightBump.width; +// public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); +// public static final double rightTrenchOpenEnd = 0; + +// // Left of hub +// public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); +// public static final double leftBumpStart = leftBumpEnd + LeftBump.width; +// public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); +// public static final double leftTrenchOpenStart = fieldWidth; +// } + +// /** Hub related constants */ +// public static class Hub { + +// // Dimensions +// public static final double width = Units.inchesToMeters(47.0); +// public static final double height = +// Units.inchesToMeters(72.0); // includes the catcher at the top +// public static final double innerWidth = Units.inchesToMeters(41.7); +// public static final double innerHeight = Units.inchesToMeters(56.5); + +// // Relevant reference points on alliance side +// public static final Translation3d topCenterPoint = +// new Translation3d( +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, +// fieldWidth / 2.0, +// height); +// public static final Translation3d innerCenterPoint = +// new Translation3d( +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, +// fieldWidth / 2.0, +// innerHeight); + +// public static final Translation2d nearLeftCorner = +// new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); +// public static final Translation2d nearRightCorner = +// new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); +// public static final Translation2d farLeftCorner = +// new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); +// public static final Translation2d farRightCorner = +// new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + +// // Relevant reference points on the opposite side +// public static final Translation3d oppTopCenterPoint = +// new Translation3d( +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, +// fieldWidth / 2.0, +// height); +// public static final Translation2d oppNearLeftCorner = +// new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / +// 2.0); +// public static final Translation2d oppNearRightCorner = +// new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / +// 2.0); +// public static final Translation2d oppFarLeftCorner = +// new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / +// 2.0); +// public static final Translation2d oppFarRightCorner = +// new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / +// 2.0); + +// // Hub faces +// public static final Pose2d nearFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); +// public static final Pose2d farFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); +// public static final Pose2d rightFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); +// public static final Pose2d leftFace = +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); +// } + +// /** Left Bump related constants */ +// public static class LeftBump { + +// // Dimensions +// public static final double width = Units.inchesToMeters(73.0); +// public static final double height = Units.inchesToMeters(6.513); +// public static final double depth = Units.inchesToMeters(44.4); + +// // Relevant reference points on alliance side +// public static final Translation2d nearLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d nearRightCorner = Hub.nearLeftCorner; +// public static final Translation2d farLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d farRightCorner = Hub.farLeftCorner; + +// // Relevant reference points on opposing side +// public static final Translation2d oppNearLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; +// public static final Translation2d oppFarLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; +// } + +// /** Right Bump related constants */ +// public static class RightBump { +// // Dimensions +// public static final double width = Units.inchesToMeters(73.0); +// public static final double height = Units.inchesToMeters(6.513); +// public static final double depth = Units.inchesToMeters(44.4); + +// // Relevant reference points on alliance side +// public static final Translation2d nearLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d nearRightCorner = Hub.nearLeftCorner; +// public static final Translation2d farLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d farRightCorner = Hub.farLeftCorner; + +// // Relevant reference points on opposing side +// public static final Translation2d oppNearLeftCorner = +// new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; +// public static final Translation2d oppFarLeftCorner = +// new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); +// public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; +// } + +// /** Left Trench related constants */ +// public static class LeftTrench { +// // Dimensions +// public static final double width = Units.inchesToMeters(65.65); +// public static final double depth = Units.inchesToMeters(47.0); +// public static final double height = Units.inchesToMeters(40.25); +// public static final double openingWidth = Units.inchesToMeters(50.34); +// public static final double openingHeight = Units.inchesToMeters(22.25); + +// // Relevant reference points on alliance side +// public static final Translation3d openingTopLeft = +// new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); +// public static final Translation3d openingTopRight = +// new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + +// // Relevant reference points on opposing side +// public static final Translation3d oppOpeningTopLeft = +// new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); +// public static final Translation3d oppOpeningTopRight = +// new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); +// } + +// public static class RightTrench { + +// // Dimensions +// public static final double width = Units.inchesToMeters(65.65); +// public static final double depth = Units.inchesToMeters(47.0); +// public static final double height = Units.inchesToMeters(40.25); +// public static final double openingWidth = Units.inchesToMeters(50.34); +// public static final double openingHeight = Units.inchesToMeters(22.25); + +// // Relevant reference points on alliance side +// public static final Translation3d openingTopLeft = +// new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); +// public static final Translation3d openingTopRight = +// new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + +// // Relevant reference points on opposing side +// public static final Translation3d oppOpeningTopLeft = +// new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); +// public static final Translation3d oppOpeningTopRight = +// new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); +// } + +// /** Tower related constants */ +// public static class Tower { +// // Dimensions +// public static final double width = Units.inchesToMeters(49.25); +// public static final double depth = Units.inchesToMeters(45.0); +// public static final double height = Units.inchesToMeters(78.25); +// public static final double innerOpeningWidth = Units.inchesToMeters(32.250); +// public static final double frontFaceX = Units.inchesToMeters(43.51); + +// public static final double uprightHeight = Units.inchesToMeters(72.1); + +// // Rung heights from the floor +// public static final double lowRungHeight = Units.inchesToMeters(27.0); +// public static final double midRungHeight = Units.inchesToMeters(45.0); +// public static final double highRungHeight = Units.inchesToMeters(63.0); + +// // Relevant reference points on alliance side +// public static final Translation2d centerPoint = +// new Translation2d( +// frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); +// public static final Translation2d leftUpright = +// new Translation2d( +// frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) +// + innerOpeningWidth / 2 +// + Units.inchesToMeters(0.75)); +// public static final Translation2d rightUpright = +// new Translation2d( +// frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) +// - innerOpeningWidth / 2 +// - Units.inchesToMeters(0.75)); + +// // Relevant reference points on opposing side +// public static final Translation2d oppCenterPoint = +// new Translation2d( +// fieldLength - frontFaceX, +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); +// public static final Translation2d oppLeftUpright = +// new Translation2d( +// fieldLength - frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) +// + innerOpeningWidth / 2 +// + Units.inchesToMeters(0.75)); +// public static final Translation2d oppRightUpright = +// new Translation2d( +// fieldLength - frontFaceX, +// (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) +// - innerOpeningWidth / 2 +// - Units.inchesToMeters(0.75)); +// } + +// public static class Depot { +// // Dimensions +// public static final double width = Units.inchesToMeters(42.0); +// public static final double depth = Units.inchesToMeters(27.0); +// public static final double height = Units.inchesToMeters(1.125); +// public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + +// // Relevant reference points on alliance side +// public static final Translation3d depotCenter = +// new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); +// public static final Translation3d leftCorner = +// new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); +// public static final Translation3d rightCorner = +// new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); +// } + +// public static class Outpost { +// // Dimensions +// public static final double width = Units.inchesToMeters(31.8); +// public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); +// public static final double height = Units.inchesToMeters(7.0); + +// // Relevant reference points on alliance side +// public static final Translation2d centerPoint = +// new Translation2d(0, +// AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); +// } + +// @RequiredArgsConstructor +// public enum FieldType { +// ANDYMARK("andymark"), +// WELDED("welded"); + +// @Getter private final String jsonFolder; +// } + +// public enum AprilTagLayoutType { +// OFFICIAL("2026-official"), +// NONE("2026-none"); + +// private final String name; +// private volatile AprilTagFieldLayout layout; +// private volatile String layoutString; + +// AprilTagLayoutType(String name) { +// this.name = name; +// } + +// public AprilTagFieldLayout getLayout() { +// if (layout == null) { +// synchronized (this) { +// if (layout == null) { +// try { +// Path p = +// Constants.disableHAL +// ? Path.of( +// "src", +// "main", +// "deploy", +// "apriltags", +// fieldType.getJsonFolder(), +// name + ".json") +// : Path.of( +// Filesystem.getDeployDirectory().getPath(), +// "apriltags", +// fieldType.getJsonFolder(), +// name + ".json"); +// layout = new AprilTagFieldLayout(p); +// layoutString = new ObjectMapper().writeValueAsString(layout); +// } catch (IOException e) { +// throw new RuntimeException(e); +// } +// } +// } +// } +// return layout; +// } + +// public String getLayoutString() { +// if (layoutString == null) { +// getLayout(); +// } +// return layoutString; +// } +// } +// } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cc038e5..e2f05f5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,10 +13,14 @@ package frc.robot; +import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.Mode; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.FullSubsystem; @@ -42,6 +46,9 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; private Timer m_gcTimer = new Timer(); + private Timer m_matchTimer = new Timer(); + private Timer m_shiftTimer = new Timer(); + private boolean shiftBoolean = false; // Flag to ensure we only apply the alliance offset once per DS connection @AutoLogOutput(key = "Robot/HasInitializedAlliancePose") @@ -148,6 +155,12 @@ public void disabledInit() { } else { hasInitializedAlliancePose = false; } + + // Handle PathPlanner States Initialization by Running an Auto Routine + // Should not actually drive, only warmups up the PathPlanner library + // See: https://www.chiefdelphi.com/t/pathplanner-initial-lag-after-deploying-code/455068/7 + Command forcedAutoInitCommand = new PathPlannerAuto("Force Warmup").ignoringDisable(true); + CommandScheduler.getInstance().schedule(forcedAutoInitCommand); } /** This function is called periodically when disabled. */ @@ -170,9 +183,13 @@ public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); + // schedule the autonomous command (example) + // 3847: Time Delay to prevent Auto from firing with a delay + Command delaggerWaitCommand = new WaitCommand(0.01); + // schedule the autonomous command (example) if (autonomousCommand != null) { - CommandScheduler.getInstance().schedule(autonomousCommand); + CommandScheduler.getInstance().schedule(delaggerWaitCommand.andThen(autonomousCommand)); } } @@ -189,17 +206,54 @@ public void teleopInit() { // this line or comment it out. if (autonomousCommand != null) { autonomousCommand.cancel(); + autonomousCommand = null; } // Handle edge case where alliance data isn't recieved before enabled if (!hasInitializedAlliancePose) { robotContainer.applyAlliancePoseOffset(); } + m_matchTimer.reset(); + m_shiftTimer.stop(); + m_shiftTimer.reset(); + m_matchTimer.start(); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + + if (!DriverStation.getGameSpecificMessage().isEmpty()) { + SmartDashboard.putBoolean("Game MSG Recieved", true); + if (!m_shiftTimer.isRunning()) { + shiftBoolean = + !(((DriverStation.getGameSpecificMessage().charAt(0) == 'B') + && (DriverStation.getAlliance().get() == Alliance.Blue)) + || ((DriverStation.getGameSpecificMessage().charAt(0) == 'R') + && (DriverStation.getAlliance().get() == Alliance.Red))); + } + } else { + SmartDashboard.putBoolean("Game MSG Recieved", false); + } + if (m_matchTimer.hasElapsed(10)) { + if (!m_shiftTimer.isRunning() && !m_matchTimer.hasElapsed(110)) { + m_shiftTimer.start(); + } else if (m_shiftTimer.advanceIfElapsed(25)) { + shiftBoolean = !shiftBoolean; + } else if (m_matchTimer.hasElapsed(110)) { + m_shiftTimer.stop(); + SmartDashboard.putBoolean("EndGame", true); + shiftBoolean = true; + } else SmartDashboard.putBoolean("EndGame", false); + + } else { + shiftBoolean = true; + } + + SmartDashboard.putBoolean("SHOOT", shiftBoolean); + SmartDashboard.putNumber("Match Timer", 140 - m_matchTimer.get()); + SmartDashboard.putNumber("Shift Timer", 25 - m_shiftTimer.get()); + } /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5daf686..db1b8ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.CrossBumpCommand; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.agitator.AgitatorIO; import frc.robot.subsystems.agitator.AgitatorIOSim; @@ -125,10 +126,14 @@ public RobotContainer() { swerveSubsystem::addVisionMeasurement, swerveSubsystem::getRotation, swerveSubsystem::getChassisSpeeds, - new VisionIOLimelight(VisionConstants.cameraYellow, swerveSubsystem::getRotation), - new VisionIOLimelight(VisionConstants.cameraPurple, swerveSubsystem::getRotation)); - // new VisionIOLimelight(VisionConstants.cameraPink, swerveSubsystem::getRotation)); - // new VisionIOLimelight(VisionConstants.cameraOrange, swerveSubsystem::getRotation)); + new VisionIOLimelight( + VisionConstants.cameraYellow, + swerveSubsystem::getRotation, + swerveSubsystem::getYawVelocityRate), + new VisionIOLimelight( + VisionConstants.cameraPink, + swerveSubsystem::getRotation, + swerveSubsystem::getYawVelocityRate)); shotCalculator = new ShotCalculator(swerveSubsystem); @@ -233,7 +238,7 @@ public RobotContainer() { // ------- Indexer Auto NamedCommands -------- \\ - NamedCommands.registerCommand("indexerIndex", indexerSubsystem.runCurrentCommand()); + NamedCommands.registerCommand("indexerIndex", indexerSubsystem.indexCommand()); // ------- Kicker Auto NamedCommands -------- \\ @@ -266,6 +271,14 @@ public RobotContainer() { () -> -driver.getLeftX(), () -> shotCalculator.getCorrectTargetRotation())); + NamedCommands.registerCommand( + "crossBumpNeutralZone", + new CrossBumpCommand(swerveSubsystem, CrossBumpCommand.CrossDirection.TO_NEUTRAL)); + + NamedCommands.registerCommand( + "crossBumpAllianceZone", + new CrossBumpCommand(swerveSubsystem, CrossBumpCommand.CrossDirection.TO_ALLIANCE)); + // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -319,10 +332,11 @@ private void configureButtonBindings() { driver .leftBumper() - .whileTrue(indexerSubsystem.runCurrentCommand()) + .whileTrue(indexerSubsystem.indexCommand()) .whileTrue(kickerSubsystem.indexCommand()) .whileTrue(agitatorSubsystem.indexCommand()) - .whileTrue(pivotSubsystem.runSaltAndPepperCommand()); + .whileTrue(pivotSubsystem.runSaltAndPepperCommand()) + .whileTrue(rollerSubsystem.intakeCommand()); driver .rightBumper() @@ -331,7 +345,11 @@ private void configureButtonBindings() { swerveSubsystem, () -> -driver.getLeftY(), () -> -driver.getLeftX(), - () -> shotCalculator.getCorrectTargetRotation())); + () -> shotCalculator.getCorrectTargetRotation())) + .whileTrue( + hoodSubsystem.dynamicUpdatedShootCommandWithLinearCompensation( + () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) + .whileFalse(hoodSubsystem.trenchHoodCommand()); driver .b() @@ -351,8 +369,19 @@ private void configureButtonBindings() { swerveSubsystem) .ignoringDisable(true)); - driver.povUp().onTrue(hoodSubsystem.runDebuggingUpCommand()); - driver.povDown().onTrue(hoodSubsystem.runDebuggingDownCommand()); + driver.povUp().onTrue(Commands.runOnce(() -> hoodSubsystem.shotCompensation--, hoodSubsystem)); + driver + .povDown() + .onTrue(Commands.runOnce(() -> hoodSubsystem.shotCompensation++, hoodSubsystem)); + + driver + .povLeft() + .onTrue(Commands.runOnce(() -> hoodSubsystem.shotCompensation = 0, hoodSubsystem)); + + driver.povRight().whileTrue(flywheelSubsystem.runDebuggingVelocityCommand()); + + driver.leftTrigger().whileTrue(hoodSubsystem.runDebuggingVoltageUpCommand()); + driver.rightTrigger().whileTrue(hoodSubsystem.runDebuggingVoltageDownCommand()); // ------- Operator Controls -------- \\ @@ -367,14 +396,12 @@ private void configureButtonBindings() { operator.b().whileTrue(rollerSubsystem.runUnjamCommand()); operator - .y() - .whileTrue( - hoodSubsystem.dynamicUpdatedShootCommand( - () -> Units.degreesToRadians(shotCalculator.getCorrectedTargetAngle()))) + .rightTrigger() .whileTrue( flywheelSubsystem.dynamicUpdatedShootCommand( () -> shotCalculator.getCorrectTargetVelocity())); + // Disabled toggleWarm on Operator because of fat-keying during intaking operator.a().onTrue(flywheelSubsystem.toggleWarm()); // Shifted from DPad Left operator @@ -426,6 +453,17 @@ public Command getAutonomousCommand() { return autoChooser.get(); } + /** + * See: https://www.chiefdelphi.com/t/fatal-robot-code-crash/427074/26 + * + *

Use this to clear the autonomous command connected to main {@link Robot} class. + * + * @return + */ + // public void killAutonomousBuilder(){ + // autoChooser. + // } + // Only use for Driver Reset Button Binding public Rotation2d returnGlobalSwerveOffset() { if (!DriverStation.getAlliance().isPresent()) { diff --git a/src/main/java/frc/robot/commands/CrossBumpCommand.java b/src/main/java/frc/robot/commands/CrossBumpCommand.java new file mode 100644 index 0000000..6c0c879 --- /dev/null +++ b/src/main/java/frc/robot/commands/CrossBumpCommand.java @@ -0,0 +1,344 @@ +package frc.robot.commands; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import java.util.Optional; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class CrossBumpCommand extends Command { + + // ----------------------------------Direction Enum------------------------------// + public enum CrossDirection { + // from alliance zone toward the neutral zone. + TO_NEUTRAL, + + // from neutral zone toward the alliance zone. + TO_ALLIANCE + } + + // ----------------------------------Internal State Machine------------------------------// + + private enum BumpState { + + // Driving at approach speed before any pitch is detected. + // Exits when pitch exceeds PITCH_ENTRY_THRESHOLD_RAD. + APPROACHING, + + // Exits when pitch velocity crosses zero (peak detected) + CLIMBING, + + // Exits when pitch drops below -PITCH_ENTRY_THRESHOLD_RAD (descent confirmed) + // or if peak pitch was small (robot straddled >:| the upward ramp). + CRESTING, + + // Continue at BUMP_SPEED_MS. + // Exits when pitch rises above -PITCH_FLAT_THRESHOLD_RAD for SETTLE_DEBOUNCE_SECONDS (all + // wheels back on flat ground). + DESCENDING, + + // Command ends after SETTLE_DEBOUNCE_SECONDS of flat pitch. + LEVELING, + + // isFinished() returns true. + COMPLETE, + + /** + * Safety state: bump was not detected within APPROACH_TIMEOUT_SECONDS. Robot may have stopped, + * missed the bump, or negligently our pitch sign is inverted. isFinished() returns true to + * prevent blocking the Command scheduler since we have a addRequirements call. + */ + TIMED_OUT + } + + // ----------------------------------Internal Constants ------------------------------// + + public static final class BumpConstants { + + // Start at 1.0 m/s/low value and tune up if the robot stalls on the ramp edge. + public static double APPROACH_SPEED_MS = 4.0; + + public static double BUMP_SPEED_MS = 5.0; + + // Speed during the LEVELING phase (all wheels back on flat ground) in m/s. + public static double SETTLE_SPEED_MS = 0.4; + + /** + * Pitch threshold in radians to detect ramp entry (APPROACHING to CLIMBING). 3 degrees = 0.052 + * rad. Too low: false positives. Too high: robot is already significantly on the ramp before + * detecting it. + */ + public static double PITCH_ENTRY_THRESHOLD_RAD = Units.degreesToRadians(3.0); + + /** + * Minimum peak pitch to confirm a real bump. 5 degrees = 0.087 rad. Prevents false cresting + * detection on flat ground. The bump's actual peak is 8.35 deg (AI found) This threshold should + * be below 8.35 deg to catch cases if the robot crosses near the ramp edge. + */ + public static double PITCH_PEAK_MIN_RAD = Units.degreesToRadians(5.0); + + /** Use something small like 1.5 degrees */ + public static double PITCH_FLAT_THRESHOLD_RAD = Units.degreesToRadians(1.5); + + // How long in [seconds] our pitch must remain below PITCH_FLAT_THRESHOLD_RAD before the command + // ends. + public static double SETTLE_DEBOUNCE_SECONDS = 0.15; + + public static double APPROACH_TIMEOUT_SECONDS = 4.0; + + // 6.0 seconds is already very generous + public static double TOTAL_TIMEOUT_SECONDS = 8.0; + + // When pitch velocity drops below this value (near zero or negative), + // the robot is at or past the peak. + public static double PITCH_PEAK_VELOCITY_THRESHOLD_RAD_PER_SEC = Units.degreesToRadians(0.5); + } + + // ----------------------------------Begin Here------------------------------// + + private final SwerveSubsystem drive; + private final CrossDirection direction; + + // +1 for TO_NEUTRAL, -1 for TO_ALLIANCE. + private double driveSign; + private double pitchSign; + + private BumpState state = BumpState.APPROACHING; + + // Peak pitch magnitude observed during the CLIMBING phase (radians). + private double peakPitchRad = 0.0; + + // Timer started when the command begins, used for both timeouts. + private final Timer totalTimer = new Timer(); + + /** Timer started when APPROACHING begins, used for approach timeout. */ + private final Timer approachTimer = new Timer(); + + // I'm sure there are better ways to do this but I have familiairty with debouncers + private final Debouncer flatDebouncer = + new Debouncer(BumpConstants.SETTLE_DEBOUNCE_SECONDS, DebounceType.kRising); + + // Signing of this command is decided in construction time (not part of state machine) + public CrossBumpCommand(SwerveSubsystem drive, CrossDirection direction) { + this.drive = drive; + this.direction = direction; + // Don't add in the drive sign early into construction, FMS not ready + addRequirements(drive); + } + + @Override + public void initialize() { + state = BumpState.APPROACHING; + peakPitchRad = 0.0; + + totalTimer.reset(); + totalTimer.start(); + approachTimer.reset(); + approachTimer.start(); + + // Reset debouncer to ensure it starts in the not-debounced state + flatDebouncer.calculate(false); + + Optional alliance = DriverStation.getAlliance(); + boolean isRed = alliance.isPresent() && alliance.get() == Alliance.Red; + + double allianceMultiplier = isRed ? -1.0 : 1.0; + double directionMultiplier = (direction == CrossDirection.TO_NEUTRAL) ? 1.0 : -1.0; + + // Pre-Calculate driveSign to prevent double signing logic in run-time + this.driveSign = directionMultiplier * allianceMultiplier; + + Logger.recordOutput("CrossBump/CalculatedDriveSign", driveSign); + Logger.recordOutput("CrossBump/CalculatedPitchSign", pitchSign); + Logger.recordOutput("CrossBump/Direction", direction.name()); + Logger.recordOutput("CrossBump/State", state.name()); + } + + // The use of transitionTo() makes the logic much easier to read and ensures consistent logging on + // state changes. + // NOTE: Use this is as a gold standard template + @Override + public void execute() { + // Raw data from SwerveSubsystem + double rawPitch = drive.getPitchPositionRad(); + double rawPitchRate = drive.getPitchVelocityRadPerSec(); + + double pitch = rawPitch * driveSign; + double pitchRate = rawPitchRate * driveSign; + double absPitch = Math.abs(pitch); + + // total timeout for safety + if (totalTimer.hasElapsed(BumpConstants.TOTAL_TIMEOUT_SECONDS)) { + transitionTo(BumpState.TIMED_OUT); + drive.stop(); + logState(pitch, pitchRate, 0.0); + return; + } + + double speedMs; + + switch (state) { + case APPROACHING: + speedMs = BumpConstants.APPROACH_SPEED_MS; + + // Approach timeout for bump not reached + if (approachTimer.hasElapsed(BumpConstants.APPROACH_TIMEOUT_SECONDS)) { + transitionTo(BumpState.TIMED_OUT); + drive.stop(); + break; + } + + // Bump detected: pitch is rising and exceeds entry threshold. + // Important: Also check that the pitch sign matches the direction of travel: + // Might be better: just check |pitch| > threshold regardless of sign + if (pitch > BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { + peakPitchRad = absPitch; // peak tracker + transitionTo(BumpState.CLIMBING); + } + break; + + case CLIMBING: + speedMs = BumpConstants.BUMP_SPEED_MS; + + // Track the peak pitch so we can confirm our crossing was a real bump + if (absPitch > peakPitchRad) { + peakPitchRad = absPitch; + } + + // Apex detection: pitch velocity has crossed zero (going from positive + // to zero/negative means the tilt is decreasing). + // Require peakPitchRad > minimum to filter out noise on flat ground. + boolean peakReached = + pitchRate < BumpConstants.PITCH_PEAK_VELOCITY_THRESHOLD_RAD_PER_SEC + && peakPitchRad > BumpConstants.PITCH_PEAK_MIN_RAD; + + if (peakReached) { + transitionTo(BumpState.CRESTING); + } + break; + + case CRESTING: + speedMs = BumpConstants.BUMP_SPEED_MS; + + // Descent confirmation: pitch has gone negative enough on the other + // side of the bump. + if (absPitch < -BumpConstants.PITCH_ENTRY_THRESHOLD_RAD) { + transitionTo(BumpState.DESCENDING); + } + + // Edge case: if the bump detection was very small (robot barely pitched) and + // pitch has already returned near flat, skip straight to LEVELING. + // if (absPitch < BumpConstants.PITCH_FLAT_THRESHOLD_RAD + // && peakPitchRad > BumpConstants.PITCH_PEAK_MIN_RAD) { + // transitionTo(BumpState.LEVELING); + // } + break; + + case DESCENDING: + speedMs = BumpConstants.BUMP_SPEED_MS; + + // Rear wheels have cleared the ramp when pitch returns near flat. + // Debouncer prevents early transition due to mid-ramp bounce. + boolean nowFlat = absPitch < BumpConstants.PITCH_FLAT_THRESHOLD_RAD; + if (flatDebouncer.calculate(nowFlat)) { + transitionTo(BumpState.LEVELING); + } + break; + + case LEVELING: + // Slow down slightly to confirm swerve stability before stopping. + speedMs = BumpConstants.SETTLE_SPEED_MS; + + // The debouncer already fired to get us here, so just run for one + // more SETTLE_DEBOUNCE_SECONDS worth of time for a smooth finish. + // Use the flat debouncer again and reset it explicitly to restart the settle timer). + // Simpler way would be to just stop after a tiny delay timer. + // We transition to COMPLETE here and let WPI isFinished() end the command. + transitionTo(BumpState.COMPLETE); + break; + + // Falling cases are intended here, do not make mods without checking with Kaden first + case COMPLETE: + case TIMED_OUT: + default: + drive.stop(); + logState(pitch, pitchRate, 0.0); + return; + } + + // ----------------------------------Swerve Drive Commands------------------------------// + // NOTE: This is assuming the robot should be aligned before calling this command. + // THE USER MUST GET THIS RIGHT + // If the robot is not aligned with the bump, add a heading controller here in a future + // improvement. + double vxFieldRelative = driveSign * speedMs; + + // Was originally going to use robotRelative speeds + ChassisSpeeds fieldRelativeSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + new ChassisSpeeds(vxFieldRelative, 0.0, 0.0), drive.getRotation()); + + drive.runVelocity(fieldRelativeSpeeds); + logState(pitch, pitchRate, vxFieldRelative); + } + + @Override + public boolean isFinished() { + return state == BumpState.COMPLETE || state == BumpState.TIMED_OUT; + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + + if (interrupted) { + Logger.recordOutput("CrossBump/State", "INTERRUPTED"); + } else { + Logger.recordOutput("CrossBump/State", state.name()); + } + + Logger.recordOutput("CrossBump/ElapsedSeconds", totalTimer.get()); + totalTimer.stop(); + approachTimer.stop(); + } + + // ----------------------------------Internal Helper Methods------------------------------// + + private void transitionTo(BumpState next) { + Logger.recordOutput("CrossBump/PreviousState", state.name()); + state = next; + Logger.recordOutput("CrossBump/State", state.name()); + + // Reset the flat debouncer whenever we leave DESCENDING so it starts + // fresh if somehow re-entered. + // If this triggers, honestly, ggs. + if (next == BumpState.CRESTING || next == BumpState.APPROACHING) { + flatDebouncer.calculate(false); + } + } + + /** NEW: Template logging format we should be using from now on */ + private void logState(double pitch, double pitchRate, double vx) { + Logger.recordOutput("CrossBump/State", state.name()); + Logger.recordOutput("CrossBump/PitchRad", drive.getPitchPositionRad()); + Logger.recordOutput("CrossBump/PitchDeg", Math.toDegrees(pitch)); + Logger.recordOutput("CrossBump/PitchRateRadPerSec", pitchRate); + Logger.recordOutput("CrossBump/PeakPitchDeg", Math.toDegrees(peakPitchRad)); + Logger.recordOutput("CrossBump/VxFieldRelative", vx); + Logger.recordOutput("CrossBump/ElapsedSeconds", totalTimer.get()); + } + + // ----------------------------------Public AutoLogOutput Methods------------------------------// + + @AutoLogOutput(key = "CrossBump/ActiveState") + public String getStateName() { + return state.name(); + } +} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index d00f2a7..2d2e28b 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -133,7 +133,8 @@ public static Command joystickDriveAtAngle( Supplier rotationSupplier) { // Create PID controller - PIDController angleController = new PIDController(10, 0.0, 0.2); // Broken atm + PIDController angleController = + new PIDController(10, 0.0, 0.80); // Previous Values P = 10, D = 0.75 angleController.enableContinuousInput(-Math.PI, Math.PI); // Construct command diff --git a/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java b/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java index 2c5d68a..fa25344 100644 --- a/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java +++ b/src/main/java/frc/robot/subsystems/agitator/AgitatorConstants.java @@ -2,7 +2,7 @@ public class AgitatorConstants { public static final int sparkMasterAgitatorCanId = 11; - public static final int masterCurrentLimit = 50; // amps + public static final int masterCurrentLimit = 15; // amps // Temporary Values (Confirm Functionality) public static final double indexingVolts = -6.0; diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 0c0efb5..598add1 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -6,11 +6,11 @@ public class IndexerConstants { // Temporary Values (Confirm Functionality) public static final double stowVolts = 0.0; - public static final double indexingVolts = -4; + public static final double indexingVolts = -12.0; public static final double jugglingVolts = 0.0; public static final double debuggingVolts = 0.0; - public static final double debuggingCurrent = 40; + public static final double debuggingCurrent = 60; // Used as an early warning if the Indexer is jammed public static final double highCurrentThreshold = 50; diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java index 8181c28..ca3a6c2 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSpark.java @@ -36,6 +36,7 @@ public IndexerIOSpark() { masterSparkMaxConfig .idleMode(IdleMode.kCoast) .smartCurrentLimit(IndexerConstants.masterCurrentLimit) + .voltageCompensation(12.0) .signals .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java index 16e96c4..5e76d8e 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotConstants.java @@ -24,8 +24,8 @@ public class PivotConstants { // Temporary Values (Confirm Functionality) public static final double stowAngle = 0.0; - public static final double deployedAngle = -2.198803; // //-2.23 - public static final double halfDeployedAngle = -2.198803 / 2; // //-2.23 + public static final double deployedAngle = -2.19; // Not full stow prototype + public static final double halfDeployedAngle = deployedAngle * 0.2; // was 0.25 public static final double jugglingAngle = Units.degreesToRadians(0.0); public static final double debuggingAngle = Units.degreesToRadians(0.0); // Change as needed diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java index 1e15420..51d23e1 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java @@ -127,7 +127,7 @@ public Command stowCommand() { } public Command runSaltAndPepperCommand() { - return Commands.sequence(deployCommand().withTimeout(0.3), halfDeployCommand().withTimeout(0.3)) + return Commands.sequence(deployCommand().withTimeout(0.5), halfDeployCommand().withTimeout(0.5)) .repeatedly(); } diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java index a5480d9..74177c4 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerConstants.java @@ -2,7 +2,7 @@ public class RollerConstants { public static final int sparkMasterRollerCanId = 10; - public static final int masterCurrentLimit = 45; // amps + public static final int masterCurrentLimit = 50; // amps // Temporary Values (Confirm Functionality) public static final double stowVolts = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index fdfa20e..668adad 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -194,13 +194,17 @@ public void periodic() { fieldToTargetAngle = correctedTargetXYCoords.minus(shooterXYCoords).getAngle(); // Functionally equivalent to atan2(dy, dx) but avoids the manual subtraction + easier to read - distanceToTarget2D = shooterXYCoords.getDistance(correctedTargetXYCoords); + distanceToTarget2D = + shooterXYCoords.getDistance(correctedTargetXYCoords) + + ShootOnTheFlyConstants.shortMissFlywheelCalibration; distanceToTarget3D = shooterPose3d.getTranslation().getDistance(correctedTargetPose3d.getTranslation()); targetSpeedRadPerSec = ShootOnTheFlyConstants.FLYWHEEL_VELOCITY_INTERPOLATOR.get(distanceToTarget2D); - targetAngleDeg = ShootOnTheFlyConstants.HOOD_DEGREES_INTERPOLATOR.get(distanceToTarget2D); + targetAngleDeg = + ShootOnTheFlyConstants.HOOD_DEGREES_INTERPOLATOR.get(distanceToTarget2D) + + ShootOnTheFlyConstants.shortMissHoodCalibration; angularError = fieldToTargetAngle.minus(robotPose.getRotation()); diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java index 64c7032..9a7c6ef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelConstants.java @@ -102,9 +102,9 @@ public class FlywheelConstants { // ------- GOAL CONSTANTS -------- \\ - public static final double debuggingVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3300); + public static final double debuggingVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3500); - public static final double layupVelocity = Units.rotationsPerMinuteToRadiansPerSecond(3000); + public static final double layupVelocity = Units.rotationsPerMinuteToRadiansPerSecond(2500); // Testing Voltage Values public static final double kDebuggingVoltage = 10; // Volts diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java index ae0eb28..3655d95 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodConstants.java @@ -49,7 +49,7 @@ public class HoodConstants { // ------- GOAL CONSTANTS -------- \\ - public static final double layupAngle = Units.degreesToRadians(41.55); // 41.55 @ 1.5m + public static final double layupAngle = Units.degreesToRadians(41.9); // 41.55 @ 1.5m public static final double debuggingAngleDown = Units.degreesToRadians(5.0); // Change as needed public static final double debuggingAngleUp = Units.degreesToRadians(42); // Change as needed @@ -58,4 +58,6 @@ public class HoodConstants { public static final double debuggingVoltageUP = -1.0; public static final double debuggingVoltageDOWN = 1.0; + + public static final double trenchAngle = Units.degreesToRadians(38.84); } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java index 2839501..a96a525 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodSubsystem.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.shooter.hood.HoodConstants.highCurrentThreshold; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -20,6 +21,8 @@ public class HoodSubsystem extends FullSubsystem { private Alert masterDisconnected; + @AutoLogOutput public int shotCompensation = 0; + /* * Defines all possible states for the hood. Each goal state has a DoubleSupplier arguement that updates from Robot State. Note: Only input static choices. */ @@ -147,6 +150,20 @@ public boolean isDrawingHighCurrent() { // vision public Command dynamicUpdatedShootCommand(DoubleSupplier positionRad) { return run(() -> runAngular(positionRad.getAsDouble())).withName("Hood Shoot"); + // To test but this will run the Trench angle once interupptted + // startEnd(() -> runAngular(positionRad.getAsDouble()), () -> + // runAngular(HoodConstants.trenchAngle)).withName("Hood Shoot"); + } + + public Command dynamicUpdatedShootCommandWithLinearCompensation(DoubleSupplier positionRad) { + return run(() -> + runAngular(positionRad.getAsDouble() + Units.degreesToRadians(shotCompensation))) + .withName("Hood Shoot With Angular Compensation"); + } + + // This would be used if we want a while false + public Command trenchHoodCommand() { + return run(() -> runAngular(HoodConstants.trenchAngle)).withName("Hood Trench"); } // ----------------------------------LAYUP COMMANDS------------------------------// diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java index 4798f5c..8876874 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java @@ -22,6 +22,8 @@ public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = Rotation2d.kZero; public double yawVelocityRadPerSec = 0.0; + public double pitchPositionRad = 0.0; + public double pitchVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java index e8797d6..a5d200d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java @@ -31,9 +31,11 @@ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2(pigeonCanId); private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal pitch = pigeon.getRoll(); private final PrimitiveDoubleQueue yawPositionQueue; private final PrimitiveDoubleQueue yawTimestampQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final StatusSignal pitchVelocity = pigeon.getAngularVelocityXWorld(); private final GyroTrimConfigs gyroTrimConfigs = new GyroTrimConfigs().withGyroScalarZ(-2.04); public GyroIOPigeon2() { @@ -41,9 +43,15 @@ public GyroIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().apply(gyroTrimConfigs); pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(odometryFrequency); - yawVelocity.setUpdateFrequency(50.0); + pitch.setUpdateFrequency(odometryFrequency); + + yawVelocity.setUpdateFrequency(odometryFrequency); + pitchVelocity.setUpdateFrequency(odometryFrequency); + pigeon.optimizeBusUtilization(); + yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); var yawClone = yaw.clone(); // Status signals are not thread-safe @@ -54,10 +62,17 @@ public GyroIOPigeon2() { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.connected = + BaseStatusSignal.refreshAll(yaw, yawVelocity, pitch, pitchVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + // Values from [Pigeon 2.0] are in degrees and degrees per second, so convert to radians and + // radians per second + inputs.pitchPositionRad = Units.degreesToRadians(pitch.getValueAsDouble()); // invert if needed + inputs.pitchVelocityRadPerSec = Units.degreesToRadians(pitchVelocity.getValueAsDouble()); + inputs.odometryYawTimestamps = yawTimestampQueue.toArray(); double[] rawYaw = yawPositionQueue.toArray(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index c3ffae1..ca3bc63 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -69,7 +69,7 @@ public class SwerveConstants { public static final int backRightTurnCanId = 6; // Drive motor configuration (MAXSwerve with 14 pinion teeth and 22 spur teeth) - public static final int driveMotorCurrentLimit = 50; + public static final int driveMotorCurrentLimit = 50; // Vortex 80A, NEO 60A // Remeasure the radius in the Makerspace, Run the Wheel Characterization Auto public static final double wheelRadiusMeters = Units.inchesToMeters(1.498); @@ -97,7 +97,7 @@ public class SwerveConstants { // Turn motor configuration public static final boolean turnInverted = false; // <-- Check if Neccessary - public static final int turnMotorCurrentLimit = 20; + public static final int turnMotorCurrentLimit = 20; // 550 35A, SAFE = 20A public static final double turnMotorReduction = 9424.0 / 203.0; public static final DCMotor turnGearbox = DCMotor.getNeo550(1); @@ -115,8 +115,8 @@ public class SwerveConstants { public static final double turnPIDMaxInput = 2 * Math.PI; // Radians // PathPlanner configuration (Get Values) - public static final double robotMassKg = 74.088; - public static final double robotMOI = 6.883; + public static final double robotMassKg = 61.088; + public static final double robotMOI = 6.583; public static final double wheelCOF = 1.2; public static final RobotConfig ppConfig = new RobotConfig( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 98b9003..86648ab 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -71,6 +71,8 @@ public class SwerveSubsystem extends FullSubsystem { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); private Rotation2d rawGyroRotation = new Rotation2d(); + private double rawGyroYawRate = 0.0; + private double rawGyroPitch = 0.0; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { new SwerveModulePosition(), @@ -208,10 +210,13 @@ public void periodic() { if (gyroInputs.connected) { // Use the real gyro angle rawGyroRotation = gyroInputs.odometryYawPositions[i]; + rawGyroYawRate = gyroInputs.yawVelocityRadPerSec; + rawGyroPitch = gyroInputs.pitchPositionRad; } else { // Use the angle delta from the kinematics and module deltas Twist2d twist = kinematics.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + rawGyroYawRate = 0.0; } // Apply update @@ -347,6 +352,22 @@ public Rotation2d getRotation() { return getPose().getRotation(); } + @AutoLogOutput(key = "Odometry/PitchRad") + /** Returns the current pitch position in radians. */ + public double getPitchPositionRad() { + return rawGyroPitch; + } + + /** Returns the current yaw velocity rate */ + public double getYawVelocityRate() { + return rawGyroYawRate; + } + + /** Returns the current pitch velocity in radians per second. */ + public double getPitchVelocityRadPerSec() { + return gyroInputs.pitchVelocityRadPerSec; + } + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index f6e3e71..cd43ec8 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,18 +1,9 @@ -// Copyright (c) 2021-2025 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. +/* + * Unifying the best parts from FRC 1678 (2024), FRC 254 (FRC 2025), FRC 2910 (FRC 2025), FRC 6328 (FRC 2026), etc. + */ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.angularStdDevBaseline; -import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; -import static frc.robot.subsystems.vision.VisionConstants.cameraStdDevFactors; -import static frc.robot.subsystems.vision.VisionConstants.linearStdDevBaseline; -import static frc.robot.subsystems.vision.VisionConstants.maxZError; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -22,29 +13,96 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import frc.robot.util.FullSubsystem; -import java.util.LinkedList; -import java.util.List; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.Map; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class Vision extends FullSubsystem { + /* + * Pipeline: + * 1. Read all cameras. Send single NT flush after all cameras. + * 2. For each observation from each camera, run the rejection checks. + * 3. For single-tag observations that pass intial, run quality gate check. + * 4. For multi-tag observations, run the std. dev. euqation + * + * Then, accumulate accepted observations into a list. + * Sort by timestamps. + * Submit all sorted observations to the pose estimator consumer. + * + * Memory Estimations (I am not a memory expert): + * 8 - Per-LL Oberservations: I am expecting maybe 1-2 frames per 50 hz, maybe 3-4. Double for safety. + * Java's backend defaults ArrayList internal array length to an Object sized at [10]. I don't think that + * matter too much but yeah. + * 16 - Per-LL Oberservation x Cameras (4 poseObs x 4 LL) + * Don't overcommit memory. + */ + + // Capacity Constants - See memory estimation notes in the class-level comment above + // Check these by logging .size() at peak usage and adjusting if resizes occur + private static final int PER_CAMERA_CAPACITY = 8; + private static final int PENDING_OBSERVATION_CAPACITY = 16; + private final VisionConsumer consumer; private final Supplier gyroRotationSupplier; private final Supplier robotSpeedsSupplier; private final VisionIO[] io; private final VisionIOInputsAutoLogged[] inputs; + + // NT check: latencySubscriber not updated (handled in VisionIOLimelight). + // Frame-based check: no frames received for frameDisconnectedTimeoutSec. + private final Timer[] frameDisconnectTimers; private final Alert[] disconnectedAlerts; + private final double[] lastFrameTimestamps; // Tracks previous timestamps to detect new timestamps + + // Logging key strings + private final String[] cameraInputKeys; + private final String[] cameraPosesAcceptedKeys; + private final String[] cameraLogPrefixes; + + // There ought to be a better way to do this, bruh + // Tag Detection time persistence for AdvantageKit/Scope visualisation + private final Map lastTagDetectionTimes = new HashMap<>(); + + // Note: When exclusiveTagId != NO_EXCLUSIVE_TAG, only observations containing that + // tag ID are accepted. Volatile bc I might need in command thread. + private volatile int exclusiveTagId = VisionConstants.NO_EXCLUSIVE_TAG; + + // Per-LL accepted and all pose lists. Cleared and reused each loop. + // JVM has type-erasure for generic casting on ArrayLists + // Bad programming habits, fix later. + // Can be fixed by List> + @SuppressWarnings("unchecked") + private final ArrayList[] perCameraRobotPoses; + + @SuppressWarnings("unchecked") + private final ArrayList[] perCameraRobotPosesAccepted; + + // Global summary lists. Cleared and reused each loop. + private final ArrayList allRobotPoses; + private final ArrayList allRobotPosesAccepted; + + private Rotation3d cachedGyroRotation3d = new Rotation3d(); + private double lastGyroRadians = Double.NaN; // DO NOT USE 0.0 + + private final ArrayList pendingObservations = + new ArrayList<>(PENDING_OBSERVATION_CAPACITY); - // Initialize logging values - // List allTagPoses = new LinkedList<>(); - List allRobotPoses = new LinkedList<>(); - List allRobotPosesAccepted = new LinkedList<>(); - // List allRobotPosesRejected = new LinkedList<>(); + /* + * One fully-processed observation, send this boy + */ + private record PendingObservation(double timestamp, Pose2d pose, Matrix stdDevs) {} + // GG + @SuppressWarnings("unchecked") public Vision( VisionConsumer consumer, Supplier gyroRotationSupplier, @@ -57,139 +115,276 @@ public Vision( // Initialize inputs this.inputs = new VisionIOInputsAutoLogged[io.length]; + frameDisconnectTimers = new Timer[io.length]; + disconnectedAlerts = new Alert[io.length]; + lastFrameTimestamps = new double[io.length]; + cameraInputKeys = new String[io.length]; + cameraPosesAcceptedKeys = new String[io.length]; + cameraLogPrefixes = new String[io.length]; + perCameraRobotPoses = new ArrayList[io.length]; + perCameraRobotPosesAccepted = new ArrayList[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new VisionIOInputsAutoLogged(); - } + cameraInputKeys[i] = "Vision/Camera" + i; + cameraPosesAcceptedKeys[i] = "Vision/Camera" + i + "/RobotPosesAccepted"; + cameraLogPrefixes[i] = "Vision/Camera" + i; - // Initialize disconnected alerts - this.disconnectedAlerts = new Alert[io.length]; - for (int i = 0; i < inputs.length; i++) { + // Initialize disconnected alerts + // I don't think this should be too memory intensive + frameDisconnectTimers[i] = new Timer(); + frameDisconnectTimers[i].start(); + lastFrameTimestamps[i] = 0.0; disconnectedAlerts[i] = - new Alert( - "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + new Alert("Vision camera " + i + " is disconnected.", AlertType.kWarning); + + // Pre-allocate per-camera lists with initial capacity. + // Size up bc we want to avoid re-allocation + perCameraRobotPoses[i] = new ArrayList<>(PER_CAMERA_CAPACITY); + perCameraRobotPosesAccepted[i] = new ArrayList<>(PER_CAMERA_CAPACITY); + } + + // Initial capacity for the camera summary lists + allRobotPoses = new ArrayList<>(io.length * PER_CAMERA_CAPACITY); + allRobotPosesAccepted = new ArrayList<>(io.length * PER_CAMERA_CAPACITY); + } + + // ------- TAG EXCLUSION UTIL -------- \\ + // Based on 254's sorting + + public void setExclusiveTag(int tagId) { + this.exclusiveTagId = tagId; + } + + public void clearExclusiveTag() { + this.exclusiveTagId = VisionConstants.NO_EXCLUSIVE_TAG; + } + + public int getExclusiveTag() { + return exclusiveTagId; + } + + // ------- THROTTLING UTIL -------- \\ + // Based on 2910's sorting + + public void setThrottleValue(int throttleValue) { + for (VisionIO camera : io) { + camera.setThrottleValue(throttleValue); } } @Override public void periodic() { - // Instead of creating new lists each loop, clear allocated lists to reuse memory and reduce GC - // overhead - // allTagPoses.clear(); - allRobotPoses.clear(); - allRobotPosesAccepted.clear(); - // allRobotPosesRejected.clear(); - for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); - Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + Logger.processInputs(cameraInputKeys[i], inputs[i]); } - // Update Robot State - Rotation3d currentGyroRotation = new Rotation3d(gyroRotationSupplier.get()); - ChassisSpeeds currentSpeeds = robotSpeedsSupplier.get(); + // Updated all cameras then flush + NetworkTableInstance.getDefault().flush(); + + // Cache Gyro Rotation + double currentGyroRad = gyroRotationSupplier.get().getRadians(); + // The value of the lastGyroRadians intializes at NaN. + if (Double.isNaN(lastGyroRadians) || currentGyroRad != lastGyroRadians) { + cachedGyroRotation3d = new Rotation3d(gyroRotationSupplier.get()); + lastGyroRadians = currentGyroRad; + } + // Check if Robot is lightning McQueen + // I swear, do this calc once, not per camera. + ChassisSpeeds currentSpeeds = robotSpeedsSupplier.get(); // Calculate Speed Magnitudes double linearSpeed = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); double angularSpeed = Math.abs(currentSpeeds.omegaRadiansPerSecond); + // Clear buffers + allRobotPoses.clear(); + allRobotPosesAccepted.clear(); + pendingObservations.clear(); + // Loop over cameras for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { - // Update disconnected alert - disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); - - // Initialize logging values - // List tagPoses = new LinkedList<>(); - List robotPoses = new LinkedList<>(); - List robotPosesAccepted = new LinkedList<>(); - // List robotPosesRejected = new LinkedList<>(); - - // Add tag poses - // for (int tagId : inputs[cameraIndex].tagIds) { - // var tagPose = aprilTagLayout.getTagPose(tagId); - // if (tagPose.isPresent()) { - // tagPoses.add(tagPose.get()); - // } - // } - - // Loop over pose observations - for (var observation : inputs[cameraIndex].poseObservations) { - // Check whether to reject pose - boolean rejectPose = - observation.tagCount() == 0 // Must have at least one tag - || Math.abs(observation.pose().getZ()) - > maxZError // Must have realistic Z coordinate - - // Must be within the field boundaries - || observation.pose().getX() < 0.0 - || observation.pose().getX() > aprilTagLayout.getFieldLength() - || observation.pose().getY() < 0.0 - || observation.pose().getY() > aprilTagLayout.getFieldWidth(); - - if (!rejectPose) { - if (linearSpeed > VisionConstants.maxLinearSpeed - || angularSpeed > VisionConstants.maxAngularSpeed) { - rejectPose = true; + // Update dual disconnected alert + // Reset timer when new frame arrives + // NT check handled by [cameraIndex].connected + + // Stolen from 6328 + if (inputs[cameraIndex].lastFrameTimestampSec != lastFrameTimestamps[cameraIndex]) { + frameDisconnectTimers[cameraIndex].reset(); + lastFrameTimestamps[cameraIndex] = inputs[cameraIndex].lastFrameTimestampSec; + // Only reset the disconnect timer if the current timestamp is different from the last + // recorded timestamp + } + + boolean frameDisconnected = + frameDisconnectTimers[cameraIndex].hasElapsed( + VisionConstants.frameDisconnectedTimeoutSec); + boolean disconnected = !inputs[cameraIndex].connected || frameDisconnected; + + if (disconnected) { + String reason = + !inputs[cameraIndex].connected + ? "NT disconnected" + : "No Frames Received for " + + VisionConstants.frameDisconnectedTimeoutSec + + " seconds"; + disconnectedAlerts[cameraIndex].setText( + "Vision camera " + cameraIndex + " disconnected (" + reason + ")."); + } + disconnectedAlerts[cameraIndex].set(disconnected); + // --- + + // TO-DO: Add util to update when DS is disabled + + // If exclyusive tags are included and not seen, skip all observations. + int exclusion = exclusiveTagId; + if (exclusion != VisionConstants.NO_EXCLUSIVE_TAG) { + boolean cameraSeesExclusive = false; + for (int id : inputs[cameraIndex].tagIds) { + if (id == exclusion) { + cameraSeesExclusive = true; + break; } } + if (!cameraSeesExclusive) { + // Log that we're filtering this camera, then skip. + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/ExclusiveTagFiltered", true); + continue; + } + } + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/ExclusiveTagFiltered", false); + + // Clear Per-LL Pose lists + ArrayList robotPoses = perCameraRobotPoses[cameraIndex]; + ArrayList robotPosesAccepted = perCameraRobotPosesAccepted[cameraIndex]; + robotPoses.clear(); + robotPosesAccepted.clear(); + + // Update Per-Observation filtering + for (var observation : inputs[cameraIndex].poseObservations) { + + // See pipeline above + boolean reject = (observation.tagCount() == 0); + + if (!reject) reject = (Math.abs(observation.pose().getZ()) > VisionConstants.maxZError); + + // Margin added, recommended by 6328 + if (!reject) { + double x = observation.pose().getX(); + double y = observation.pose().getY(); + reject = + x < -VisionConstants.fieldBorderMargin + || x + > VisionConstants.aprilTagLayout.getFieldLength() + + VisionConstants.fieldBorderMargin + || y < -VisionConstants.fieldBorderMargin + || y + > VisionConstants.aprilTagLayout.getFieldWidth() + + VisionConstants.fieldBorderMargin; + } + + // Technically this shouldnt really be needed + if (!reject) reject = (linearSpeed > VisionConstants.maxLinearSpeed); - if (!rejectPose) { - if (Math.abs( + // Reject if yawRate is too high = timestamp might not be as reliable + if (!reject) reject = (angularSpeed > VisionConstants.maxAngularSpeed); + + // MT2 measurements should already be pre-seeded, disable if redundant + if (!reject) { + double gyroVisionDifferenceDeg = + Math.abs( Math.toDegrees( - (observation.pose().getRotation().minus(currentGyroRotation).getAngle()))) - > VisionConstants.maxGyroError) { - rejectPose = true; - } + observation.pose().getRotation().minus(cachedGyroRotation3d).getAngle())); + reject = (gyroVisionDifferenceDeg > VisionConstants.maxGyroError); } - // Add pose to log + // Log all pose observation robotPoses.add(observation.pose()); - if (rejectPose) { - // robotPosesRejected.add(observation.pose()); - } else { - robotPosesAccepted.add(observation.pose()); - } - // Skip if rejected - if (rejectPose) { + if (reject) { + // Observation logged but not accepted. continue; } - // Calculate dynamically standard deviation scalar factor - double stdDevFactor = - Math.pow(observation.averageTagDistance(), 2.0) - / observation.tagCount(); // Square tag count if vision is more trustworthy + // Single Tag and Multi-Tag need to be weighted differently + // Taken from 254 + final boolean isSingleTag = (observation.tagCount() == 1); + Pose2d acceptedPose2d; + double xyStdDev; + double thetaStdDev; + + if (isSingleTag) { + if (observation.averageTagArea() < VisionConstants.singleTagMinAreaPercent) { + // Rm if too laggy + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagArea", true); + continue; + } + + if (angularSpeed > VisionConstants.singleTagMaxAngularVelocityRadPerSec) { + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagAngularVel", true); + continue; + } + + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagArea", false); + // If singleTag is accepted, use MT2 translation + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/RejectedSingleTagAngularVel", false); + + // If singleTag is accepted, use MT2 translation + acceptedPose2d = observation.pose().toPose2d(); + xyStdDev = calculateXYStdDev(observation, cameraIndex, inputs[cameraIndex]); + thetaStdDev = VisionConstants.singleTagThetaStdDev; + + Logger.recordOutput( + cameraLogPrefixes[cameraIndex] + "/AcceptedSingleTag", acceptedPose2d); - double linearStdDev = linearStdDevBaseline * stdDevFactor; - double angularStdDev = angularStdDevBaseline * stdDevFactor; + } else { + + acceptedPose2d = observation.pose().toPose2d(); + xyStdDev = calculateXYStdDev(observation, cameraIndex, inputs[cameraIndex]); + thetaStdDev = Double.POSITIVE_INFINITY; - if (observation.type() == PoseObservationType.MEGATAG_2) { - linearStdDev *= VisionConstants.linearStdDevMegatag2Factor; - angularStdDev *= VisionConstants.angularStdDevMegatag2Factor; + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/AcceptedMultiTag", acceptedPose2d); } - if (cameraIndex < cameraStdDevFactors.length) { - linearStdDev *= cameraStdDevFactors[cameraIndex]; - angularStdDev *= cameraStdDevFactors[cameraIndex]; + // Utility for forcing AdvantageScope to keep Tag visible + for (int tagId : inputs[cameraIndex].tagIds) { + lastTagDetectionTimes.put(tagId, Timer.getTimestamp()); } - // Send vision observation - consumer.accept( - observation.pose().toPose2d(), - observation.timestamp(), - VecBuilder.fill(linearStdDev, linearStdDev, 9999999999999999999.0)); + // Aggregate accepted poses into a list + robotPosesAccepted.add(observation.pose()); + + // Sorting by timestamp is done at the end of the loop + pendingObservations.add( + new PendingObservation( + observation.timestamp(), + acceptedPose2d, + VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); } - // Log camera data (Disable in Match to save Bandwidth) - // Use Limelight Replay Feature Instead of Logging Poses to Dashboard + // Per-Camera Summary logging + Logger.recordOutput(cameraPosesAcceptedKeys[cameraIndex], toArray(robotPosesAccepted)); Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", - robotPosesAccepted.toArray(new Pose3d[0])); - // allTagPoses.addAll(tagPoses); + cameraLogPrefixes[cameraIndex] + "/TagCount", inputs[cameraIndex].tagIds.length); + Logger.recordOutput(cameraLogPrefixes[cameraIndex] + "/Connected", !disconnected); + allRobotPoses.addAll(robotPoses); allRobotPosesAccepted.addAll(robotPosesAccepted); - // allRobotPosesRejected.addAll(robotPosesRejected); + } // End of Camera Loop + + // Sort accumalated tag poses by timestamp + // Taken from 6328 + pendingObservations.sort(Comparator.comparingDouble(PendingObservation::timestamp)); + + // Submit ordered accpeted pose estimations to vision consumer + for (var observation : pendingObservations) { + consumer.accept(observation.pose(), observation.timestamp(), observation.stdDevs()); } } @@ -201,14 +396,71 @@ public void accept( Matrix visionMeasurementStdDevs); } + private double calculateXYStdDev( + VisionIO.PoseObservation observation, + int cameraIndex, + VisionIOInputsAutoLogged cameraInputs) { + + // Original method + double modelStdDev = + VisionConstants.linearStdDevBaseline + * Math.pow(observation.averageTagDistance(), 2) // Modify to Tune + / Math.pow(observation.tagCount(), 1); // Modify to Tune + + // Per-camera trust multiplier + if (cameraIndex < VisionConstants.cameraStdDevFactors.length) { + modelStdDev *= VisionConstants.cameraStdDevFactors[cameraIndex]; + } + + // Quality scaling from Limelight's native stddevs (254) + // If Limelight provides MT2 std devs and they're larger, + // take a weighted blend in the next calculation. + + double[] llStdDevs = cameraInputs.limelightStdDevs; + if (llStdDevs.length >= VisionConstants.kExpectedStdDevLen + && observation.type() == PoseObservationType.MEGATAG_2) { + double llXStd = llStdDevs[VisionConstants.kMT2XStdDevIndex]; + double llYStd = llStdDevs[VisionConstants.kMT2YStdDevIndex]; + // Take the higher uncertainty + double llXYMax = Math.max(llXStd, llYStd); + + // Blend-- if Limelight is more pessimistic, weight its view more. + // Inverse Variance Weighting between two estimates + double blended = + modelStdDev * (1.0 - VisionConstants.limelightStdDevWeight) + + llXYMax * VisionConstants.limelightStdDevWeight; + modelStdDev = Math.max(modelStdDev, blended); // never trust more than our model alone + } + + return modelStdDev; + } + @Override public void periodicAfterScheduler() { // Log summary data - // Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[0])); + Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[0])); Logger.recordOutput( "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(new Pose3d[0])); - // Logger.recordOutput( - // "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(new Pose3d[0])); + + // Taken from 6328 + ArrayList recentTagPoses = new ArrayList<>(); + double timestampNow = Timer.getTimestamp(); + for (var entry : lastTagDetectionTimes.entrySet()) { + if (timestampNow - entry.getValue() < VisionConstants.tagLogPersistenceSeconds) { + VisionConstants.aprilTagLayout.getTagPose(entry.getKey()).ifPresent(recentTagPoses::add); + } + } + // Log persistent tags + Logger.recordOutput("Vision/Summary/RecentTagPoses", recentTagPoses.toArray(new Pose3d[0])); + } + + // ------- UTIL -------- \\ + + // Refactored toArray Helper method after removing output buffer logic + // See + // https://stackoverflow.com/questions/70154349/why-java-inserts-null-to-array-when-using-list-toarray + private Pose3d[] toArray(ArrayList list) { + return list.toArray(new Pose3d[0]); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 6820c3b..ffa81c6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -7,31 +7,37 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -public class VisionConstants { - // AprilTag layout - Test FieldConstants from 6328 - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); // Fix to 2026 Layout - // FieldConstants(); +public final class VisionConstants { + + private VisionConstants() {} + + // ------- APRILTAG FIELD CONSTANTS -------- \\ + + public static final AprilTagFieldLayout aprilTagLayout; - // Camera names, must match names configured on coprocessor - public static String cameraPurple = "limelight-purple"; // BL - public static String cameraOrange = "limelight-orange"; // BR - public static String cameraYellow = "limelight-yellow"; // FL - public static String cameraPink = "limelight-pink"; // FR + static { + aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + // All California District Events and DCMP should be WELDED fields. + } + + // ------- LIMELIGHT NAME -------- \\ + + // The User MUST get this right + public static final String cameraPurple = "limelight-purple"; + public static final String cameraOrange = "limelight-orange"; + public static final String cameraYellow = "limelight-yellow"; + public static final String cameraPink = "limelight-pink"; // Unused public static String cameraPlaceholder = "limelight-placeholder"; - // Robot to camera transforms - // (Not used by Limelight, configure in LL Finder UI instead) + // ------- LIMELIGHT TRANSFORM (SIM) -------- \\ + public static Transform3d cameraTransformToPurple = new Transform3d(0.1397, -0.3302, 0.1778, new Rotation3d(0.0, 0.38, 90.0)); // updated 1/22 public static Transform3d cameraTransformToOrange = @@ -41,33 +47,71 @@ public class VisionConstants { public static Transform3d cameraTransformToBlue = new Transform3d(0.127, -0.3302, 0.18415, new Rotation3d(0.0, 0.38, -90.0)); // updated 1/22 - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; + // ------- PER-LIMELIGHT TRUST MULTIPLIERS -------- \\ + // Note: This is indexed in the same order that the cameras are passed into the Vision constructor + // Increase the factor equals less trust in that limelight - // Standard deviation baselines at 1 meter - public static double linearStdDevBaseline = 0.04; // Meters - public static double angularStdDevBaseline = Double.POSITIVE_INFINITY; // Radians - - // Standard deviation multipliers for each camera - // Manual Variance Weighting: (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = + public static final double[] cameraStdDevFactors = new double[] { 1.0, // Camera 0 - Yellow - L 1.0, // Camera 1 - Purple - L - 2.0, // Camera 2 - Pink - R - 2.0, // Camera 3 - Orange - L + 1.0, // Camera 2 - Pink - R + 1.0, // Camera 3 - Orange - L }; + // ------- BUMPER CLIPPING MARGIN -------- \\ + + public static final double fieldBorderMargin = 0.5; // meters + + // ------- DISCONNECTION UTIL -------- \\ + + public static final double ntDisconnectedTimeoutMs = 250.0; // please work + public static final double frameDisconnectedTimeoutSec = 0.5; + + // ------- POSE FILTERING CONSTANTS -------- \\ + + public static final double maxZError = 0.5; // meters + + public static final double maxLinearSpeed = 4.0; // m/s + + public static final double maxAngularSpeed = Math.toRadians(720); // rad/s (~2 full rotations) + + public static final double maxGyroError = 5.0; // Degrees + + // Only for when only one tag is present + public static final double singleTagMinAreaPercent = 0.10; // Percent of Image frame + public static final double singleTagMaxAngularVelocityRadPerSec = Math.toRadians(360); // rad/s + public static final double singleTagThetaStdDev = Double.POSITIVE_INFINITY; // Redundant but wtv + + // Standard deviation baselines at 1 meter + public static final double linearStdDevBaseline = 0.04; // Meters + public static final double angularStdDevBaseline = Double.POSITIVE_INFINITY; // Radians + // Multipliers to apply for MegaTag 2 observations public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // Never Trust - // Clamping ranges for vision estimates (see units) - static final double maxLinearSpeed = 2.0; // Meters per second - static final double maxAngularSpeed = - DegreesPerSecond.of(270).in(RadiansPerSecond); // Radians per second - static final double maxGyroError = 1.0; // Degrees - static final double maxTranslationError = 1.0; // Meters - static final int LOCK_MODE = 10; + // Scale factor applied to Std. Devs. for Limelight's native Std. Devs. + // Set to 0.0 to disable the native std dev influence. + public static final double limelightStdDevWeight = 0.5; + + // ------- MISC / UTIL -------- \\ + + // Index constants for limelight NT entry for std. dev. + // Layout: [MT1x, MT1y, MT1z, MT1roll, MT1pitch, MT1yaw, MT2x, MT2y, MT2z, MT2roll, MT2pitch, + // MT2yaw] - IIRC + public static final int kMT2XStdDevIndex = 6; + public static final int kMT2YStdDevIndex = 7; + public static final int kMT2YawStdDevIndex = 11; + public static final int kExpectedStdDevLen = 12; + + // Used for avoiding goofy floating point overflow + public static final double LARGE_VARIANCE = 1e9; + + // Default (-1): No Tag ID filtering + public static final int NO_EXCLUSIVE_TAG = -1; + + // How long AdvnatageScope holds onto the pose visualizer + // So I don't need super-ision to see the poses :skull: + public static final double tagLogPersistenceSeconds = 0.1; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 6428210..62128eb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -13,9 +13,12 @@ public interface VisionIO { @AutoLog public static class VisionIOInputs { + public boolean connected = false; + public double lastFrameTimestampSec = 0.0; public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; + public double[] limelightStdDevs = new double[0]; } /** Represents a robot pose sample used for pose estimation. */ @@ -25,6 +28,7 @@ public static record PoseObservation( double ambiguity, int tagCount, double averageTagDistance, + double averageTagArea, PoseObservationType type) {} public static enum PoseObservationType { @@ -32,5 +36,7 @@ public static enum PoseObservationType { PHOTONVISION } - public default void updateInputs(VisionIOInputs inputs) {} + default void updateInputs(VisionIOInputs inputs) {} + + default void setThrottleValue(int throttleValue) {} } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 36b69ed..a784486 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -16,19 +16,33 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; -import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; -import java.util.Set; +import java.util.ArrayList; import java.util.function.Supplier; /** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { + // Storing NT once avoids repeated static lookup + private static final NetworkTableInstance NT = NetworkTableInstance.getDefault(); + private final Supplier rotationSupplier; + private final Supplier yawRateSupplier; // rad/s + private final DoubleArrayPublisher orientationPublisher; + private final DoubleSubscriber latencySubscriber; // pipeline latency (ms) + private final DoubleArraySubscriber megatag2Subscriber; // botpose_orb_wpiblue + private final DoubleArraySubscriber limelightStdDevsSubscriber; // native units by LL + + // Pre-allocate buffers + private final ArrayList poseObservationCache = new ArrayList<>(8); + + // Rm Hashset method for preventing tagId duplication + // 32 tag is totally unrealistic but let's just not get a overflow error + private static final int MAX_TAGS = 32; + private final int[] tagIdScratchLottery = new int[MAX_TAGS]; + private int tagIdCount = 0; - private final DoubleSubscriber latencySubscriber; - private final DoubleArraySubscriber megatag2Subscriber; + // Default a blank array to store the std dev in case it doesn't work + private static final double[] stdDevCache = new double[0]; /** * Creates a new VisionIOLimelight. @@ -36,79 +50,113 @@ public class VisionIOLimelight implements VisionIO { * @param name The configured name of the Limelight. * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. */ - public VisionIOLimelight(String name, Supplier rotationSupplier) { - var table = NetworkTableInstance.getDefault().getTable(name); + public VisionIOLimelight( + String name, Supplier rotationSupplier, Supplier yawRateSupplier) { + + var table = NT.getTable(name); + this.rotationSupplier = rotationSupplier; - orientationPublisher = - table - .getDoubleArrayTopic("robot_orientation_set") - .publish(); // [yaw,yawrate,pitch,pitchrate,roll,rollrate] - Degrees / Degrees per - // second - latencySubscriber = - table - .getDoubleTopic("tl") - .subscribe(0.0); // Select pipeline's latency in ms. Total Latency = tl + cl. - // Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees, total latency (cl+tl), - // tag count, tag span, average tag distance from camera, average tag area () - // Should be "botpose_wpiblue", but don't fix what isn't broken - // double[] megatag2Cache = new double[] {}; + this.yawRateSupplier = yawRateSupplier; + + orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + // [yaw,yawrate,pitch,pitchrate,roll,rollrate] - Degrees / Degrees per second + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + // Select pipeline's latency in ms. Total Latency = tl + cl. megatag2Subscriber = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + // Translation (X,Y,Z) in meters Rotation(Roll,Pitch,Yaw) in degrees + + limelightStdDevsSubscriber = table.getDoubleArrayTopic("stddevs").subscribe(stdDevCache); } @Override public void updateInputs(VisionIOInputs inputs) { - // Update connection status based on whether an update has been seen in the last 250ms + + // ------- NT CHECK -------- \\ + + // Latency topic not updated in 250ms = NT disconnected inputs.connected = - ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) + < VisionConstants.ntDisconnectedTimeoutMs; + + // ------- ROBOT ORIENTATION PUBLISHING -------- \\ + + // Publish BEFORE reading poses, the User must get this right + // Added yawRate bc it helps with latency compensation + // Note: flush() is NOT called here // Update orientation for MegaTag 2 - orientationPublisher.accept( - new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); - NetworkTableInstance.getDefault() - .flush(); // Increases network traffic but recommended by Limelight - - // Read new pose observations from NetworkTables - Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); + double yawDeg = rotationSupplier.get().getDegrees(); + double yawRateDegPerSec = Units.radiansToDegrees(yawRateSupplier.get()); + orientationPublisher.accept(new double[] {yawDeg, yawRateDegPerSec, 0.0, 0.0, 0.0, 0.0}); + + // ------- LL NATIVE STD DEV -------- \\ + inputs.limelightStdDevs = limelightStdDevsSubscriber.get(stdDevCache); + + // ------- PROCESS THE POSE OBSERVATION -------- \\ + // **insert pain + + // Clear Pre-allocated buffers + poseObservationCache.clear(); + tagIdCount = 0; + + // Process Buffer Frames + // readQueue() returns all frames captured since the last call (0 or multiple) for (var rawSample : megatag2Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; + + // Use the NT server timestamp of this sample (multiply conversion for seconds) + inputs.lastFrameTimestampSec = rawSample.timestamp * 1.0e-6; + + // I might go insane but here we go + // Collect frame tagIds without duplicates (without HashSets) + // botpose_orb_wpiblue data starts at index 11, 7 values per tag for (int i = 11; i < rawSample.value.length; i += 7) { - tagIds.add((int) rawSample.value[i]); + int id = (int) rawSample.value[i]; + boolean found = false; + for (int j = 0; j < tagIdCount; j++) { + if (tagIdScratchLottery[j] == id) { + found = true; + break; + } + } + if (!found && tagIdCount < MAX_TAGS) { + tagIdScratchLottery[tagIdCount++] = id; + } } - poseObservations.add( + + // Build Pose Observation + // botpose_orb_wpiblue array layout: + // [0-2] : X, Y, Z (meters) + // [3-5] : roll, pitch, yaw (degrees) + // [6] : total latency in ms (tl + cl. Use this, not just tl) + // [7] : tag count + // [8] : tag span (i don't care about this) + // [9] : average tag distance (meters) + // [10] : average tag area (percent of image, 0–100) -> convert to fraction + // [11...]: tag data (7 values per tag) + poseObservationCache.add( new PoseObservation( - // Timestamp, based on server timestamp of publish and latency + // Timestamp: NT server publish time minus total pipeline latency rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, - - // 3D pose estimate parsePose(rawSample.value), - - // Ambiguity, zeroed because the pose is already disambiguated 0.0, - - // Tag count - (int) rawSample.value[7], - - // Average tag distance - rawSample.value[9], - - // Observation type + (int) rawSample.value[7], // tagCount + rawSample.value[9], // averageTagDistance (meters) + rawSample.value[10], // averageTagArea: convert % to fraction PoseObservationType.MEGATAG_2)); } - // Save pose observations to inputs object - inputs.poseObservations = new PoseObservation[poseObservations.size()]; - for (int i = 0; i < poseObservations.size(); i++) { - inputs.poseObservations[i] = poseObservations.get(i); - } + // Write Pose Observations + // Note: toArray writes into the provided array and returns it + inputs.poseObservations = poseObservationCache.toArray(new PoseObservation[0]); - // Save tag IDs to inputs objects - inputs.tagIds = new int[tagIds.size()]; - int i = 0; - for (int id : tagIds) { - inputs.tagIds[i++] = id; + // Write Tag ID's + // Only reallocate inputs.tagIds if the count changed + if (inputs.tagIds.length != tagIdCount) { + inputs.tagIds = new int[tagIdCount]; } + System.arraycopy(tagIdScratchLottery, 0, inputs.tagIds, 0, tagIdCount); } /** @@ -125,4 +173,11 @@ private static Pose3d parsePose(double[] rawLLArray) { Units.degreesToRadians(rawLLArray[4]), Units.degreesToRadians(rawLLArray[5]))); } + + @Override + public void setThrottleValue(int throttleValue) { + NT.getTable(orientationPublisher.getTopic().getName().split("/")[0]) + .getEntry("throttle_set") + .setNumber(throttleValue); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index fd24106..ddb2a2a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -67,7 +67,8 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate multitagResult.estimatedPose.ambiguity, // Ambiguity multitagResult.fiducialIDsUsed.size(), // Tag count - totalTagDistance / result.targets.size(), // Average tag distance + totalTagDistance / result.targets.size(), + 0.5, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } else if (!result.targets.isEmpty()) { // Single tag result @@ -93,7 +94,8 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate target.poseAmbiguity, // Ambiguity 1, // Tag count - cameraToTarget.getTranslation().getNorm(), // Average tag distance + cameraToTarget.getTranslation().getNorm(), + 0.5, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } } diff --git a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java index 9ca87db..3281a97 100644 --- a/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java +++ b/src/main/java/frc/robot/util/shooterUtil/ShootOnTheFlyConstants.java @@ -13,6 +13,9 @@ public class ShootOnTheFlyConstants { public static final double shooterHeightOffset = 0.5; // Meters, height of shooter from ground + public static final double shortMissFlywheelCalibration = 0.3; // meters + public static final double shortMissHoodCalibration = -5; // degrees + // ------- Transform Constants -------- \\ public static final Transform3d SHOOTER_TRANSFORM_CENTER = new Transform3d(-0.24, 0, 0.5, Rotation3d.kZero); @@ -52,33 +55,64 @@ public class ShootOnTheFlyConstants { // Key: Distance (meters), Value: Shooter Speed (RPM) // TO-DO: Combine Interpolators using Units.rotationsPerMinuteToRadiansPerSecond() // DO NOT USE THIS - FLYWHEEL_RPM_INTERPOLATOR.put(1.0, 3400.0); // Touching Hub //3400 - FLYWHEEL_RPM_INTERPOLATOR.put(1.5, 3400.0); // 3400 - FLYWHEEL_RPM_INTERPOLATOR.put(2.0, 3400.0); // 3400 - FLYWHEEL_RPM_INTERPOLATOR.put(2.5, 3650.0); // 3650 - FLYWHEEL_RPM_INTERPOLATOR.put(3.0, 3900.0); // 3900 - FLYWHEEL_RPM_INTERPOLATOR.put(3.5, 3970.0); // 3970 - FLYWHEEL_RPM_INTERPOLATOR.put(4.0, 4050.0); // 4050 - FLYWHEEL_RPM_INTERPOLATOR.put(4.5, 4225.0); // 4225 + // FLYWHEEL_RPM_INTERPOLATOR.put(1.0, 3400.0); // Touching Hub //3400 + // FLYWHEEL_RPM_INTERPOLATOR.put(1.5, 3400.0); // 3400 + // FLYWHEEL_RPM_INTERPOLATOR.put(2.0, 3400.0); // 3400 + // FLYWHEEL_RPM_INTERPOLATOR.put(2.5, 3650.0); // 3650 + // FLYWHEEL_RPM_INTERPOLATOR.put(3.0, 3900.0); // 3900 + // FLYWHEEL_RPM_INTERPOLATOR.put(3.5, 3970.0); // 3970 + // FLYWHEEL_RPM_INTERPOLATOR.put(4.0, 4050.0); // 4050 + // FLYWHEEL_RPM_INTERPOLATOR.put(4.5, 4225.0); // 4225 // Key: Distance (meters), Value: Shooter Velocity (rad/sec) - // USE THIS, BUT FIX FIRST + // USE THIS, BUT FIX FIRST\] // Touching Hub - FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.0, Units.rotationsPerMinuteToRadiansPerSecond(3000)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.5, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.0, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.5, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.0, Units.rotationsPerMinuteToRadiansPerSecond(3300)); - // FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.5, Units.rotationsPerMinuteToRadiansPerSecond(3400)); // + FLYWHEEL_VELOCITY_INTERPOLATOR.put(1.5, Units.rotationsPerMinuteToRadiansPerSecond(2500)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.0, Units.rotationsPerMinuteToRadiansPerSecond(2800)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(2.5, Units.rotationsPerMinuteToRadiansPerSecond(2900)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.0, Units.rotationsPerMinuteToRadiansPerSecond(3000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(3.5, Units.rotationsPerMinuteToRadiansPerSecond(3000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.0, Units.rotationsPerMinuteToRadiansPerSecond(3000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(4.5, Units.rotationsPerMinuteToRadiansPerSecond(3100)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(5.0, Units.rotationsPerMinuteToRadiansPerSecond(3200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(5.5, Units.rotationsPerMinuteToRadiansPerSecond(3200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(6.2, Units.rotationsPerMinuteToRadiansPerSecond(3400)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(7.0, Units.rotationsPerMinuteToRadiansPerSecond(3500)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(8.0, Units.rotationsPerMinuteToRadiansPerSecond(3600)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(9.0, Units.rotationsPerMinuteToRadiansPerSecond(3800)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(10.0, Units.rotationsPerMinuteToRadiansPerSecond(4000)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(11.0, Units.rotationsPerMinuteToRadiansPerSecond(4200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(12.0, Units.rotationsPerMinuteToRadiansPerSecond(4500)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(13.0, Units.rotationsPerMinuteToRadiansPerSecond(4700)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(14.0, Units.rotationsPerMinuteToRadiansPerSecond(4950)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(15.0, Units.rotationsPerMinuteToRadiansPerSecond(5200)); + FLYWHEEL_VELOCITY_INTERPOLATOR.put(16.0, Units.rotationsPerMinuteToRadiansPerSecond(5450)); + // Check // Key: Distance (meters), Value: Hood Angle (DEGREES) // Touching Hub, Using Distance 2d - HOOD_DEGREES_INTERPOLATOR.put(2.0, 41.55); - HOOD_DEGREES_INTERPOLATOR.put(2.5, 44.7); - HOOD_DEGREES_INTERPOLATOR.put(3.0, 41.17); - HOOD_DEGREES_INTERPOLATOR.put(3.5, 30.4); - HOOD_DEGREES_INTERPOLATOR.put(4.0, 25.85); + HOOD_DEGREES_INTERPOLATOR.put(1.5, 41.98); + HOOD_DEGREES_INTERPOLATOR.put(2.0, 39.14); + HOOD_DEGREES_INTERPOLATOR.put(2.5, 41.90); + HOOD_DEGREES_INTERPOLATOR.put(3.0, 41.90); + HOOD_DEGREES_INTERPOLATOR.put(3.5, 38.87); + HOOD_DEGREES_INTERPOLATOR.put(4.0, 32.27); + HOOD_DEGREES_INTERPOLATOR.put(4.5, 27.50); + HOOD_DEGREES_INTERPOLATOR.put(5.0, 24.0); + HOOD_DEGREES_INTERPOLATOR.put(5.5, 16.0); + HOOD_DEGREES_INTERPOLATOR.put(6.2, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(7.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(8.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(9.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(10.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(11.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(12.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(13.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(14.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(15.0, 5.0); + HOOD_DEGREES_INTERPOLATOR.put(16.0, 5.0); + // HOOD_DEGREES_INTERPOLATOR.put(4.0, 31.31); // Check } }