diff --git a/build.gradle b/build.gradle index 1bee9653..6206ae0e 100644 --- a/build.gradle +++ b/build.gradle @@ -42,6 +42,13 @@ deploy { jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") jvmArgs.add("-XX:+AlwaysPreTouch") + // Enable VisualVM connection + // jvmArgs.add("-Dcom.sun.management.jmxremote=true") + // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + // jvmArgs.add("-Djava.rmi.server.hostname=10.16.40.2") // Replace TE.AM with team number } // Static files artifact diff --git a/src/main/deploy/pathplanner/autos/3.5 piece.auto b/src/main/deploy/pathplanner/autos/3.5 piece.auto index 57b3d3e2..3cd2d460 100644 --- a/src/main/deploy/pathplanner/autos/3.5 piece.auto +++ b/src/main/deploy/pathplanner/autos/3.5 piece.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "4 piece 3" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece 3" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -121,9 +152,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "4 piece 5" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece 5" + } + } + ] } }, { @@ -135,6 +179,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/Explode the Processor.auto b/src/main/deploy/pathplanner/autos/Explode the Processor.auto index 76e2ab8e..30697443 100644 --- a/src/main/deploy/pathplanner/autos/Explode the Processor.auto +++ b/src/main/deploy/pathplanner/autos/Explode the Processor.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -105,9 +117,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -117,6 +135,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "sequential", "data": { diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto index e31fed76..0fad98d7 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 3 - Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 3 - Double" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -121,9 +152,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 5- Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 5- Double" + } + } + ] } }, { @@ -135,6 +179,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index 7b69f7e7..dec081be 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 3 - Triple" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 3 - Triple" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -121,9 +152,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 5 - Triple" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 5 - Triple" + } + } + ] } }, { @@ -135,6 +179,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto b/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto index 658b3822..ba2aea08 100644 --- a/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto +++ b/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 3 - Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 3 - Double" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -127,14 +158,33 @@ } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 5 - Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 5 - Double" + } + } + ] } } ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto index b10c7323..af339687 100644 --- a/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 3" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 3" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -127,14 +158,33 @@ } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 5" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 5" + } + } + ] } } ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/Process.auto b/src/main/deploy/pathplanner/autos/Process.auto index 2139c178..9c870526 100644 --- a/src/main/deploy/pathplanner/autos/Process.auto +++ b/src/main/deploy/pathplanner/autos/Process.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/SpitSpit.auto b/src/main/deploy/pathplanner/autos/SpitSpit.auto index 1b7d1ae6..a39d66f8 100644 --- a/src/main/deploy/pathplanner/autos/SpitSpit.auto +++ b/src/main/deploy/pathplanner/autos/SpitSpit.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -99,9 +111,28 @@ } }, { - "type": "path", + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "SpitProcess 3" + } + } + ] + } + }, + { + "type": "named", "data": { - "pathName": "SpitProcess 3" + "name": "LocalAlign" } }, { diff --git a/src/main/deploy/pathplanner/autos/local align or else.auto b/src/main/deploy/pathplanner/autos/local align or else.auto new file mode 100644 index 00000000..83ea6729 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/local align or else.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableAprilTags" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "LOCAL TO THE MOON" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 1.path b/src/main/deploy/pathplanner/paths/Explode the processor 1.path index 70b607ab..211d8d79 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 1.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 1.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 2.path b/src/main/deploy/pathplanner/paths/Explode the processor 2.path index 38f8a9b2..b92b2d69 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 2.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 2.path @@ -93,8 +93,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 3.path b/src/main/deploy/pathplanner/paths/Explode the processor 3.path index 65c2d6c3..615f1eee 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 3.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 3.path @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 4.path b/src/main/deploy/pathplanner/paths/Explode the processor 4.path index f417f787..b21d91eb 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 4.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 4.path @@ -88,8 +88,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path index 61094b1e..9e705d5e 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path index 4a76f7bf..382bf178 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path index d1340835..93ce5dda 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path index fa7a2ca6..51a2aa01 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path index 85d95113..10c8f170 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path index 01395827..b8ef5d69 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, + "maxVelocity": 1.7, + "maxAcceleration": 1.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path index 0e07ab9c..836f6160 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 3.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path index cfb242ce..5229593a 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 2.5, + "maxVelocity": 2.7, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path index 1de717a5..2bd6c823 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path index 4a07952b..d460dac7 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.2, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path index 66da82b7..a5f66c73 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path index 547cf7f4..5c7ebed3 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.5, + "maxVelocity": 3.2, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path index bd6fd185..137b9552 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path index baa6f7a1..191a4336 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path index e99cee3c..ffa7b209 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.2, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 1.path b/src/main/deploy/pathplanner/paths/L4 Processor 1.path index cddfa0da..f9e0d5d2 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 1.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 1.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 3.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 2.path b/src/main/deploy/pathplanner/paths/L4 Processor 2.path index 5937d206..16316292 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 2.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 2.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 3.path b/src/main/deploy/pathplanner/paths/L4 Processor 3.path index 6277fd6f..48b84aa2 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 3.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4963990066225168, - "y": 0.8723923841059593 + "x": 1.525653409090909, + "y": 0.9138636363636362 }, "prevControl": null, "nextControl": { - "x": 2.76589620503102, - "y": 1.9878123911389562 + "x": 2.795150607499412, + "y": 2.0292836433966333 }, "isLocked": false, "linkedName": "BottomSource" @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path b/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path index 5b1283fd..9787ee65 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 4.path b/src/main/deploy/pathplanner/paths/L4 Processor 4.path index 755a362e..bd8c6a7b 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 4.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 4.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, + "maxVelocity": 3.2, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path b/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path index 3c2da219..d80985d6 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4963990066225168, - "y": 0.8723923841059593 + "x": 1.525653409090909, + "y": 0.9138636363636362 }, "prevControl": null, "nextControl": { - "x": 2.162950764991328, - "y": 1.5786424526363922 + "x": 2.1922051674597203, + "y": 1.6201137048940693 }, "isLocked": false, "linkedName": "BottomSource" @@ -70,8 +70,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 5.path b/src/main/deploy/pathplanner/paths/L4 Processor 5.path index 94c25125..c0a2d182 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 5.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 5.path @@ -84,8 +84,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 3.25, + "maxVelocity": 2.8, + "maxAcceleration": 3.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path b/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path new file mode 100644 index 00000000..79be5cc7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.219, + "y": 3.905 + }, + "prevControl": null, + "nextControl": { + "x": 6.400941780827734, + "y": 3.9123073630131553 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.654, + "y": 4.005 + }, + "prevControl": { + "x": 6.275475461382252, + "y": 3.9176573199102847 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.3, + "maxAcceleration": 1.2, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Process 2.path b/src/main/deploy/pathplanner/paths/Process 2.path index 2d4c6e07..60581a88 100644 --- a/src/main/deploy/pathplanner/paths/Process 2.path +++ b/src/main/deploy/pathplanner/paths/Process 2.path @@ -93,8 +93,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Process 3.path b/src/main/deploy/pathplanner/paths/Process 3.path index 36f10800..312cfe4e 100644 --- a/src/main/deploy/pathplanner/paths/Process 3.path +++ b/src/main/deploy/pathplanner/paths/Process 3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 1.path b/src/main/deploy/pathplanner/paths/SpitProcess 1.path index c94bfed0..c0b21d5a 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 1.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 1.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.0, + "maxVelocity": 1.7, + "maxAcceleration": 1.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 2.path b/src/main/deploy/pathplanner/paths/SpitProcess 2.path index c1eb5dbe..4267b277 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 2.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 2.path @@ -64,8 +64,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, + "maxVelocity": 3.2, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 3.path b/src/main/deploy/pathplanner/paths/SpitProcess 3.path index 21c12302..6ba5732c 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 3.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 3.path @@ -90,8 +90,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 3.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 4.path b/src/main/deploy/pathplanner/paths/SpitProcess 4.path index 4923baab..aa26e288 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 4.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 4.path @@ -68,8 +68,8 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 1.2, + "maxAcceleration": 1.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index cdbd249b..cb8613b8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -15,8 +15,8 @@ "SpitSpit" ], "autoFolders": [], - "defaultMaxVel": 2.0, - "defaultMaxAccel": 2.0, + "defaultMaxVel": 2.2, + "defaultMaxAccel": 2.2, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aace89c7..19491e53 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -132,9 +132,18 @@ public void robotInit() { @Override public void robotPeriodic() { + // long time0 = System.nanoTime(); CommandScheduler.getInstance().run(); + // long time1 = System.nanoTime(); PeriodicScheduler.getInstance().run(); - LoggerManager.updateLog(); + // long time2 = System.nanoTime(); + // LoggerManager.updateLog(); + // long time3 = System.nanoTime(); + // if (time3 - time0 > 15000000) { + // System.out.println("Command scheduler time: " + (time1 - time0)); + // System.out.println("Periodic scheduler time: " + (time2 - time1)); + // System.out.println("Logger manager time: " + (time3 - time2)); + // } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ce6ba4a..0abeab09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.ProxyCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -118,15 +119,15 @@ public class RobotContainer { // Controller private final CommandXboxController driveController = new CommandXboxController(0); private final CommandXboxController operatorController = new CommandXboxController(1); - private final CommandXboxController pitController = new CommandXboxController(4); + // private final CommandXboxController pitController = new CommandXboxController(4); private final XboxController driveHID = driveController.getHID(); private final XboxController opHID = operatorController.getHID(); private final XboxController operatorControllerHID = operatorController.getHID(); private final PresetBoard presetBoard = new PresetBoard(2); - private final PresetBoard simBoard = new PresetBoard(3); - private final PresetBoard motorBoard = new PresetBoard(5); + // private final PresetBoard simBoard = new PresetBoard(3); + // private final PresetBoard motorBoard = new PresetBoard(5); private final AlertsManager alertsManager; @@ -248,22 +249,22 @@ public RobotContainer() { gantrySubsystem = new GantrySubsystem( RobotConfigConstants.gantrySubsystemEnabled - ? new GantryIOSim(() -> simBoard.getLl2()) + ? new GantryIOSim(() -> /*simBoard.getLl2()*/false) : new GantryIO() {}); liftSubsystem = new LiftSubsystem( RobotConfigConstants.liftSubsystemEnabled - ? new LiftIOSim(() -> simBoard.getLl3()) + ? new LiftIOSim(() -> /*simBoard.getLl3()*/false) : new LiftIO() {}); coralOuttakeSubsystem = new CoralOuttakeSubsystem( RobotConfigConstants.coralOuttakeSubsystemEnabled - ? new CoralOuttakeIOSim(() -> simBoard.getRl2()) + ? new CoralOuttakeIOSim(() -> /*simBoard.getRl2()*/false) : new CoralOuttakeIO() {}); climberSubsystem = new ClimberSubsystem( RobotConfigConstants.climberSubsystemEnabled - ? new ClimberIOSim(() -> simBoard.getRl4()) + ? new ClimberIOSim(() -> /*simBoard.getRl4(*/false) : new ClimberIO() {}); winchSubsystem = new WinchSubsystem( @@ -271,7 +272,7 @@ public RobotContainer() { algaeIntakeSubsystem = new AlgaeSubsystem( RobotConfigConstants.algaeIntakeEnabled - ? new AlgaeIOSim(() -> simBoard.getLl4()) + ? new AlgaeIOSim(() -> /*simBoard.getLl4()*/false) : new AlgaeIO() {}); break; default: @@ -316,8 +317,8 @@ public RobotContainer() { // Otherwise we wouldn't have a drive controller during the match joystickDriveWeight = new JoystickDriveWeight( - () -> -driveController.getLeftY() - 1 * pitController.getLeftY(), - () -> -driveController.getLeftX() - 1 * pitController.getLeftX(), + () -> -driveController.getLeftY(), //- 1 * pitController.getLeftY(), + () -> -driveController.getLeftX(), //- 1 * pitController.getLeftX(), () -> -driveController.getRightX(), driveController.rightBumper(), driveController.leftTrigger(), @@ -513,12 +514,12 @@ private void configureBindings() { followPathCoral.generateTrigger( () -> driveHID.getLeftBumperButton() && !followPathCoral.isAutoalignComplete()); - new Trigger( - () -> - coralOuttakeSubsystem.hasCoral() - && coralOuttakeCommandFactory.ranBack - && !coralOuttakeSubsystem.guillotineCheck()) - .onTrue(setupAutoPlace(() -> CoralPreset.PreMove).onlyIf(() -> premoveLift)); + // new Trigger( + // () -> + // coralOuttakeSubsystem.hasCoral() + // && coralOuttakeCommandFactory.ranBack + // && !coralOuttakeSubsystem.guillotineCheck()) + // .onTrue(setupAutoPlace(() -> CoralPreset.PreMove).onlyIf(() -> premoveLift)); driveController.povDown().onTrue(new InstantCommand(() -> premoveLift = true)); driveController.povUp().onTrue(new InstantCommand(() -> premoveLift = false)); @@ -759,18 +760,18 @@ private void configureBindings() { .setSolenoidState(() -> true) .andThen(algaeCommandFactory.processCommand())); // motor board - new Trigger(() -> motorBoard.getLl2()) - .whileTrue(liftCommandFactory.liftApplyVoltageCommand(() -> 2)); - new Trigger(() -> motorBoard.getLl3()) - .whileTrue(gantryCommandFactory.gantryApplyVoltageCommand(() -> 1)); - new Trigger(() -> motorBoard.getLl4()) - .whileTrue(coralOuttakeCommandFactory.setIntakeVoltage(() -> 4)); - new Trigger(() -> motorBoard.getRl4()) - .whileTrue(algaeCommandFactory.setMotorVoltages(() -> 10, () -> 10)); - new Trigger(() -> motorBoard.getRl3()) - .whileTrue(climberCommandFactory.elevatorApplyVoltageCommand(() -> -1)); - new Trigger(() -> motorBoard.getTrough()) - .onTrue(new InstantCommand(() -> liftSubsystem.resetEncoder())); + // new Trigger(() -> motorBoard.getLl2()) + // .whileTrue(liftCommandFactory.liftApplyVoltageCommand(() -> 2)); + // new Trigger(() -> motorBoard.getLl3()) + // .whileTrue(gantryCommandFactory.gantryApplyVoltageCommand(() -> 1)); + // new Trigger(() -> motorBoard.getLl4()) + // .whileTrue(coralOuttakeCommandFactory.setIntakeVoltage(() -> 4)); + // new Trigger(() -> motorBoard.getRl4()) + // .whileTrue(algaeCommandFactory.setMotorVoltages(() -> 10, () -> 10)); + // new Trigger(() -> motorBoard.getRl3()) + // .whileTrue(climberCommandFactory.elevatorApplyVoltageCommand(() -> -1)); + // new Trigger(() -> motorBoard.getTrough()) + // .onTrue(new InstantCommand(() -> liftSubsystem.resetEncoder())); new Trigger(operatorController.leftTrigger()) .whileTrue(new InstantCommand(() -> algaeIntakeSubsystem.setHasAlgae(false))); @@ -801,74 +802,74 @@ private void configureBindings() { new InstantCommand(() -> operatorControllerHID.setRumble(RumbleType.kBothRumble, 0))); mapPIDtoCommand(); if (TestConfig.testingMode == TestingSetting.pit) { - configurePitBindings(); + // configurePitBindings(); } else { // put in TESTBOARD triggers here } } - private void configurePitBindings() { - new Trigger(() -> (pitController.getHID().getPOV() == 0)) - .whileTrue( - climberCommandFactory.winchApplyVoltageCommand( - (pitController.getHID().getPOV() == 0 ? () -> -2.5 : () -> 2.5))); - new Trigger(() -> (pitController.getHID().getPOV() == 180)) - .whileTrue( - climberCommandFactory.winchApplyVoltageCommand( - (pitController.getHID().getPOV() == 180 ? () -> 2.5 : () -> -2.5))); + // private void configurePitBindings() { + // new Trigger(() -> (pitController.getHID().getPOV() == 0)) + // .whileTrue( + // climberCommandFactory.winchApplyVoltageCommand( + // (pitController.getHID().getPOV() == 0 ? () -> -2.5 : () -> 2.5))); + // new Trigger(() -> (pitController.getHID().getPOV() == 180)) + // .whileTrue( + // climberCommandFactory.winchApplyVoltageCommand( + // (pitController.getHID().getPOV() == 180 ? () -> 2.5 : () -> -2.5))); - new Trigger(() -> pitController.getHID().getPOV() == 270) - .whileTrue(new InstantCommand(() -> autoRampPos = false)); - new Trigger(() -> Math.abs(pitController.getRightY()) > 0.03) - .whileTrue( - climberCommandFactory.elevatorApplyVoltageCommand( - () -> -pitController.getRightY() * 4)); - pitController - .rightBumper() - .whileTrue(gantryCommandFactory.gantrySetVelocityCommand(() -> GantryConstants.alignSpeed)); - pitController - .leftBumper() - .whileTrue( - gantryCommandFactory.gantrySetVelocityCommand(() -> -GantryConstants.alignSpeed)); - pitController - .rightTrigger() - .and(() -> !algaeIntakeSubsystem.hasAlgae()) - .whileTrue( - algaeCommandFactory - .setSolenoidState(() -> true) - .andThen(algaeCommandFactory.setMotorVoltages(() -> 4, () -> 4))); - pitController - .leftTrigger() - .and(() -> algaeIntakeSubsystem.hasAlgae()) - .whileTrue( - algaeCommandFactory - .setSolenoidState(() -> true) - .andThen(algaeCommandFactory.processCommand())); - pitController - .start() - .whileTrue( - new InstantCommand(() -> liftSubsystem.resetEncoder()) - .alongWith(new InstantCommand(() -> climberSubsystem.resetEncoder())) - .alongWith(new InstantCommand(() -> autoRampPos = true))); - pitController - .b() - .whileTrue( - coralOuttakeCommandFactory - .outtake() - .finallyDo(() -> coralOuttakeCommandFactory.outtaking = false)); - pitController.y().and(() -> !coralOuttakeCommandFactory.outtaking).onTrue(runLiftToSafe()); - pitController.back().whileTrue(gantryCommandFactory.gantryHomeCommand()); - pitController - .povRight() - .onTrue(climberCommandFactory.setClampState(() -> !climberSubsystem.getSolenoidState())); - pitController.a().onTrue(setupAutoPlace(() -> coralPreset)); - pitController.x().onTrue(new InstantCommand(() -> AntiTipWeight.setAntiTipEnabled(false))); - } + // new Trigger(() -> pitController.getHID().getPOV() == 270) + // .whileTrue(new InstantCommand(() -> autoRampPos = false)); + // new Trigger(() -> Math.abs(pitController.getRightY()) > 0.03) + // .whileTrue( + // climberCommandFactory.elevatorApplyVoltageCommand( + // () -> -pitController.getRightY() * 4)); + // pitController + // .rightBumper() + // .whileTrue(gantryCommandFactory.gantrySetVelocityCommand(() -> GantryConstants.alignSpeed)); + // pitController + // .leftBumper() + // .whileTrue( + // gantryCommandFactory.gantrySetVelocityCommand(() -> -GantryConstants.alignSpeed)); + // pitController + // .rightTrigger() + // .and(() -> !algaeIntakeSubsystem.hasAlgae()) + // .whileTrue( + // algaeCommandFactory + // .setSolenoidState(() -> true) + // .andThen(algaeCommandFactory.setMotorVoltages(() -> 4, () -> 4))); + // pitController + // .leftTrigger() + // .and(() -> algaeIntakeSubsystem.hasAlgae()) + // .whileTrue( + // algaeCommandFactory + // .setSolenoidState(() -> true) + // .andThen(algaeCommandFactory.processCommand())); + // pitController + // .start() + // .whileTrue( + // new InstantCommand(() -> liftSubsystem.resetEncoder()) + // .alongWith(new InstantCommand(() -> climberSubsystem.resetEncoder())) + // .alongWith(new InstantCommand(() -> autoRampPos = true))); + // pitController + // .b() + // .whileTrue( + // coralOuttakeCommandFactory + // .outtake() + // .finallyDo(() -> coralOuttakeCommandFactory.outtaking = false)); + // pitController.y().and(() -> !coralOuttakeCommandFactory.outtaking).onTrue(runLiftToSafe()); + // pitController.back().whileTrue(gantryCommandFactory.gantryHomeCommand()); + // pitController + // .povRight() + // .onTrue(climberCommandFactory.setClampState(() -> !climberSubsystem.getSolenoidState())); + // pitController.a().onTrue(setupAutoPlace(() -> coralPreset)); + // pitController.x().onTrue(new InstantCommand(() -> AntiTipWeight.setAntiTipEnabled(false))); + // } public Command getAutonomousCommand() { return homing() .andThen(new InstantCommand(() -> autoRampPos = true)) - .andThen(new InstantCommand(() -> premoveLift = true)) + // .andThen(new InstantCommand(() -> premoveLift = true)) .andThen(dashboard.getAutoChooserCommand()); // return new InstantCommand(); } @@ -976,7 +977,11 @@ public void generateNamedCommands() { NamedCommands.registerCommand( "SetupL4", new InstantCommand( - () -> coralPreset = gantryAuto ? CoralPreset.RightL4 : CoralPreset.LeftL4)); + () -> + coralPreset = + gantryAuto + ? CoralPreset.RightL4 + : CoralPreset.LeftL4)); // increment up l2 - l4 for testing!! NamedCommands.registerCommand( "SetupL3", new InstantCommand( @@ -1023,10 +1028,10 @@ public void generateNamedCommands() { NamedCommands.registerCommand( "LocalAlign", - localAlign - .getAutoCommand() - .deadlineFor(autonAutoPlace(() -> coralPreset)) - .until(() -> localAlign.isAutoalignComplete() || !localAlign.isReady()) + (new ProxyCommand(localAlign::getAutoCommand)) // it just explodes as soon as this happens? + .deadlineFor(autonAutoPlace(() -> coralPreset)) // this is not the problem + .until( + () -> localAlign.isAutoalignComplete() || !localAlign.isReady()) // never gets here .alongWith(new InstantCommand(() -> PathplannerWeight.setSpeeds(new ChassisSpeeds())))); NamedCommands.registerCommand( "WaitForLocal", diff --git a/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java b/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java index 172d41be..77941b51 100644 --- a/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java +++ b/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java @@ -57,7 +57,7 @@ public void updateInputs(GyroIOInputs inputs) { @Override public void resetGyro(GyroIOInputs inputs) { rollOffset = inputs.roll.plus(new Rotation2d(rollOffset)).getRadians(); - pitchOffset = inputs.pitch.plus(new Rotation2d(rollOffset)).getRadians(); + pitchOffset = inputs.pitch.plus(new Rotation2d(pitchOffset)).getRadians(); offset = inputs.angleRadiansRaw; } diff --git a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java index 65f25618..865585cd 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java @@ -5,6 +5,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.constants.FieldConstants; import frc.robot.sensors.apriltag.AprilTagVision; import frc.robot.sensors.gyro.Gyro; @@ -101,12 +103,15 @@ public boolean isReady() { && Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()) < 15; } Logger.recordOutput("LocalTagAlign/isAlignReady", ready); + Logger.recordOutput( + "LocalTagAlign/vectorNorm", vector.isPresent() ? vector.get().getNorm() : -1); return ready; } public Command getAutoCommand() { return driveCommandFactory .runVelocityCommand(() -> getSpeeds(), () -> true) + .alongWith(new InstantCommand(() -> System.out.println("getAutoCommand is running"))) .finallyDo( () -> driveCommandFactory.runVelocityCommand(() -> new ChassisSpeeds(), () -> true)); }