diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 000000000..aef6495bc --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // Recommended extensions for working with Java tests in this workspace. + "recommendations": [ + "vscjava.vscode-java-pack", // Java Extension Pack (convenience bundle) + "vscjava.vscode-java-test", // Java Test Runner (Test Explorer integration) + "redhat.java", // Language Support for Java(TM) by Red Hat + "vscjava.vscode-java-dependency", // Java Project Dependency Viewer + "vscjava.vscode-java-debug" // Java Debugger + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json index 80e40ce8a..37fb7f033 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -20,7 +20,9 @@ "workingDirectory": "${workspaceFolder}/build/jni/release", "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + // Ensure native libs are found on Windows as well as Unix-like systems + "PATH": "${workspaceFolder}\\build\\jni\\release;${env:PATH}", + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" } }, diff --git a/src/main/deploy/autos/.bline-web/path-metadata.json b/src/main/deploy/autos/.bline-web/path-metadata.json new file mode 100644 index 000000000..b0c467427 --- /dev/null +++ b/src/main/deploy/autos/.bline-web/path-metadata.json @@ -0,0 +1,107 @@ +{ + "paths": { + "FirstNeutralBump.json": { + "editor_metadata": { + "ranged_constraints": [ + { + "key": "max_velocity_meters_per_sec", + "value": 4.05, + "start_ordinal": 2, + "end_ordinal": 2, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 1.37, + "start_ordinal": 6, + "end_ordinal": 6, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 4.05, + "start_ordinal": 8, + "end_ordinal": 8, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + } + ] + } + }, + "SecondNeutralBump.json": { + "editor_metadata": { + "ranged_constraints": [ + { + "key": "max_velocity_meters_per_sec", + "value": 2.25, + "start_ordinal": 1, + "end_ordinal": 1, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 1.41, + "start_ordinal": 6, + "end_ordinal": 6, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 3.34, + "start_ordinal": 8, + "end_ordinal": 8, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 4.05, + "start_ordinal": 9, + "end_ordinal": 9, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + } + ] + } + } + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json new file mode 100644 index 000000000..f3a856625 --- /dev/null +++ b/src/main/deploy/autos/config.json @@ -0,0 +1,30 @@ +{ + "gui": { + "robot": { + "length_meters": 0.78, + "width_meters": 0.93 + }, + "protrusions": { + "enabled": true, + "distance_meters": 0.21, + "side": "back", + "default_state": "shown", + "show_on_event_keys": [ + "intake" + ], + "hide_on_event_keys": [] + } + }, + "kinematic_constraints": { + "default_max_velocity_meters_per_sec": 4.5, + "default_max_acceleration_meters_per_sec2": 6.0, + "default_intermediate_handoff_radius_meters": 0.2, + "default_max_velocity_deg_per_sec": 720.0, + "default_max_acceleration_deg_per_sec2": 1500.0, + "default_end_translation_tolerance_meters": 0.03, + "default_end_rotation_tolerance_deg": 2.0, + "default_auto_velocity_velocity_safety_factor": 1.0, + "default_auto_velocity_acceleration_safety_factor": 1.0, + "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/pathgroups.json b/src/main/deploy/autos/pathgroups.json new file mode 100644 index 000000000..cfdc77d16 --- /dev/null +++ b/src/main/deploy/autos/pathgroups.json @@ -0,0 +1,22 @@ +{ + "schema_version": 1, + "groups": [ + { + "group_id": "group-be48ead2-6d4e-4a08-9787-7ec4fcb2553e", + "display_name": "Trench Paths", + "path_file_names": [ + "FirstNeutralTrench.json", + "FirstNeutralTrenchFAST.json", + "SecondNeutralTrench.json" + ] + }, + { + "group_id": "group-24f6ec6b-f994-4a13-86c1-eaa4d5454b32", + "display_name": "Bump Paths", + "path_file_names": [ + "FirstNeutralBump.json", + "SecondNeutralBump.json" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/BumpDepot.json b/src/main/deploy/autos/paths/BumpDepot.json new file mode 100644 index 000000000..a7b66dfbf --- /dev/null +++ b/src/main/deploy/autos/paths/BumpDepot.json @@ -0,0 +1,82 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.38, + "y_meters": 2.45, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.09, + "lib_key": "launch" + }, + { + "type": "event_trigger", + "t_ratio": 0.93, + "lib_key": "cancel" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.87864, + "y_meters": 1.92629, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 0.72384, + "y_meters": 2.06724, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.59181, + "y_meters": 2.09673, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 1.8, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 2.0, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 1.8, + "start_ordinal": 2, + "end_ordinal": 3 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/FirstNeutralBump.json b/src/main/deploy/autos/paths/FirstNeutralBump.json new file mode 100644 index 000000000..4ab50087c --- /dev/null +++ b/src/main/deploy/autos/paths/FirstNeutralBump.json @@ -0,0 +1,124 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.45101, + "y_meters": 0.47, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.57376, + "y_meters": 0.82149, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.26254, + "y_meters": 1.38392, + "intermediate_handoff_radius_meters": 0.3 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.80486, + "y_meters": 3.31046, + "intermediate_handoff_radius_meters": 0.48 + }, + "rotation_target": { + "rotation_radians": -2.72134, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 6.94491, + "y_meters": 2.81006, + "intermediate_handoff_radius_meters": 0.6 + }, + { + "type": "translation", + "x_meters": 5.66151, + "y_meters": 2.55139, + "intermediate_handoff_radius_meters": 0.6 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.39036, + "y_meters": 2.47649, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 4.05, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 2.5, + "start_ordinal": 2, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 1.37, + "start_ordinal": 5, + "end_ordinal": 5 + }, + { + "value": 4.05, + "start_ordinal": 7, + "end_ordinal": 7 + } + ], + "max_acceleration_meters_per_sec2": [ + { + "value": 3.0, + "start_ordinal": 7, + "end_ordinal": 7 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/FirstNeutralTrench.json b/src/main/deploy/autos/paths/FirstNeutralTrench.json new file mode 100644 index 000000000..8032f6f63 --- /dev/null +++ b/src/main/deploy/autos/paths/FirstNeutralTrench.json @@ -0,0 +1,136 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.45101, + "y_meters": 0.58991, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.72165, + "y_meters": 0.82345, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.39085, + "y_meters": 1.38548, + "intermediate_handoff_radius_meters": 0.3 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.62127, + "y_meters": 3.30514, + "intermediate_handoff_radius_meters": 0.29 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.10746, + "y_meters": 2.04773, + "intermediate_handoff_radius_meters": 0.3 + }, + { + "type": "rotation", + "rotation_radians": -1.5708, + "t_ratio": 0.75, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 6.71926, + "y_meters": 1.24825, + "intermediate_handoff_radius_meters": 0.35 + }, + { + "type": "translation", + "x_meters": 5.99961, + "y_meters": 0.62864, + "intermediate_handoff_radius_meters": 0.2 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.11942, + "y_meters": 0.47062, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 2.5, + "start_ordinal": 8, + "end_ordinal": 8 + }, + { + "value": 1.8, + "start_ordinal": 2, + "end_ordinal": 4 + }, + { + "value": 2.4, + "start_ordinal": 5, + "end_ordinal": 7 + }, + { + "value": 2.6, + "start_ordinal": 1, + "end_ordinal": 1 + } + ], + "max_acceleration_meters_per_sec2": [ + { + "value": 2.5, + "start_ordinal": 6, + "end_ordinal": 6 + }, + { + "value": 3.0, + "start_ordinal": 2, + "end_ordinal": 4 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json new file mode 100644 index 000000000..c80db5b76 --- /dev/null +++ b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json @@ -0,0 +1,117 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.45101, + "y_meters": 0.47, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.50278, + "y_meters": 0.78978, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.53163, + "y_meters": 1.30249, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.80042, + "y_meters": 3.38659, + "intermediate_handoff_radius_meters": 0.35 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": -1.5708, + "t_ratio": 0.8, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 6.78269, + "y_meters": 1.11611, + "intermediate_handoff_radius_meters": 0.32 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.11942, + "y_meters": 0.47062, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 3.15, + "start_ordinal": 6, + "end_ordinal": 6 + }, + { + "value": 2.15, + "start_ordinal": 3, + "end_ordinal": 4 + }, + { + "value": 3.22, + "start_ordinal": 5, + "end_ordinal": 5 + }, + { + "value": 3.5, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 3.0, + "start_ordinal": 2, + "end_ordinal": 2 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/SecondNeutralBump.json b/src/main/deploy/autos/paths/SecondNeutralBump.json new file mode 100644 index 000000000..8514e4402 --- /dev/null +++ b/src/main/deploy/autos/paths/SecondNeutralBump.json @@ -0,0 +1,134 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.6913, + "y_meters": 0.63044, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 5.75212, + "y_meters": 2.00744, + "intermediate_handoff_radius_meters": 0.39 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.9775, + "y_meters": 3.17868, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.74533, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": 2.35619, + "t_ratio": 1.0, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 7.4589, + "y_meters": 3.70465, + "intermediate_handoff_radius_meters": 0.36 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.43006, + "y_meters": 2.61144, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 6.46606, + "y_meters": 2.52905, + "intermediate_handoff_radius_meters": 0.7 + }, + { + "type": "translation", + "x_meters": 5.87577, + "y_meters": 2.63547, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.37991, + "y_meters": 2.48256, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 2.25, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 2.8, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 4 + }, + { + "value": 1.41, + "start_ordinal": 5, + "end_ordinal": 5 + }, + { + "value": 3.5, + "start_ordinal": 6, + "end_ordinal": 6 + }, + { + "value": 3.34, + "start_ordinal": 7, + "end_ordinal": 7 + }, + { + "value": 4.05, + "start_ordinal": 8, + "end_ordinal": 8 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/SecondNeutralTrench.json b/src/main/deploy/autos/paths/SecondNeutralTrench.json new file mode 100644 index 000000000..5295532e5 --- /dev/null +++ b/src/main/deploy/autos/paths/SecondNeutralTrench.json @@ -0,0 +1,119 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.72169, + "y_meters": 0.51571, + "intermediate_handoff_radius_meters": 0.6 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 5.80138, + "y_meters": 2.24561, + "intermediate_handoff_radius_meters": 0.44 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.1368, + "y_meters": 3.10806, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.74533, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": 2.35619, + "t_ratio": 1.0, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 7.453, + "y_meters": 3.57527, + "intermediate_handoff_radius_meters": 0.38 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.80082, + "y_meters": 2.54808, + "intermediate_handoff_radius_meters": 0.38 + }, + "rotation_target": { + "rotation_radians": 2.35619, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": 1.5708, + "t_ratio": 0.5, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 7.17921, + "y_meters": 1.41252, + "intermediate_handoff_radius_meters": 0.8 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01264, + "y_meters": 0.71, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 2.25, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 2.8, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 5 + }, + { + "value": 4.0, + "start_ordinal": 6, + "end_ordinal": 6 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/bumptotrench.json b/src/main/deploy/autos/paths/bumptotrench.json new file mode 100644 index 000000000..8b25d3db3 --- /dev/null +++ b/src/main/deploy/autos/paths/bumptotrench.json @@ -0,0 +1,71 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.39, + "y_meters": 2.48, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.25, + "lib_key": "launch" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.96984, + "y_meters": 1.43785, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 3.01332, + "y_meters": 0.68216, + "intermediate_handoff_radius_meters": 0.2 + }, + { + "type": "event_trigger", + "t_ratio": 0.48, + "lib_key": "cancel" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01, + "y_meters": 0.47, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.09, + "max_velocity_meters_per_sec": [ + { + "value": 1.3, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 1.3, + "start_ordinal": 1, + "end_ordinal": 1 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/config.json b/src/main/deploy/autos/paths/config.json new file mode 100644 index 000000000..f0b6f1bf5 --- /dev/null +++ b/src/main/deploy/autos/paths/config.json @@ -0,0 +1,3 @@ +{ + "path_elements": [] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/default.json b/src/main/deploy/autos/paths/default.json new file mode 100644 index 000000000..ba18adb09 --- /dev/null +++ b/src/main/deploy/autos/paths/default.json @@ -0,0 +1,28 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.6, + "y_meters": 4.035, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 2.5, + "y_meters": 4.035, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/pathgroups.json b/src/main/deploy/autos/paths/pathgroups.json new file mode 100644 index 000000000..f0b6f1bf5 --- /dev/null +++ b/src/main/deploy/autos/paths/pathgroups.json @@ -0,0 +1,3 @@ +{ + "path_elements": [] +} \ No newline at end of file diff --git a/src/main/deploy/bline-layout.json b/src/main/deploy/bline-layout.json new file mode 100644 index 000000000..6ddeeb295 --- /dev/null +++ b/src/main/deploy/bline-layout.json @@ -0,0 +1,155 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "offset", + "x": 0.0, + "y": 256.0, + "width": 1152.0, + "height": 128.0, + "type": "Number Slider", + "properties": { + "topic": "/SmartDashboard/offset", + "period": 0.06, + "data_type": "double", + "min_value": 1.0, + "max_value": 10.0, + "divisions": 5, + "update_continuously": true + } + } + ] + } + }, + { + "name": "Autos", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Selected auto", + "x": 0.0, + "y": 0.0, + "width": 1024.0, + "height": 512.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Selected auto", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Start pose", + "x": 1024.0, + "y": 0.0, + "width": 640.0, + "height": 384.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Start pose", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Starting Position", + "x": 1152.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Split Button Chooser", + "properties": { + "topic": "/SmartDashboard/Starting Position", + "period": 0.06 + } + }, + { + "title": "Available Auto Variants", + "x": 256.0, + "y": 512.0, + "width": 768.0, + "height": 128.0, + "type": "Split Button Chooser", + "properties": { + "topic": "/SmartDashboard/Available Auto Variants", + "period": 0.06 + } + }, + { + "title": "Auto Delay", + "x": 1024.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Autos/Auto Delay", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "DisplaySpeed", + "x": 0.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Number Slider", + "properties": { + "topic": "/Autos/DisplaySpeed", + "period": 0.06, + "data_type": "double", + "min_value": -4.0, + "max_value": 4.0, + "divisions": 5, + "update_continuously": true + } + }, + { + "title": "Trench Side", + "x": 1408.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Split Button Chooser", + "properties": { + "topic": "/SmartDashboard/Trench Side", + "period": 0.06 + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto b/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto new file mode 100644 index 000000000..268147bb8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path b/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path index 0faa0196f..c314ad181 100644 --- a/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path +++ b/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 7.073284731962036, - "y": 0.45538039519202234 + "y": 0.4553803951920222 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path b/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path index dd366b9ae..180fe2b2c 100644 --- a/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path +++ b/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path @@ -69,7 +69,7 @@ }, "prevControl": { "x": 6.672715268037964, - "y": 0.7546196048079776 + "y": 0.7546196048079777 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 000000000..2d5a76882 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,174 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.013, + "y": 0.473 + }, + "prevControl": null, + "nextControl": { + "x": 4.413177117241754, + "y": 0.48134493453414395 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.652491900973851, + "y": 0.6194161489981196 + }, + "prevControl": { + "x": 5.406164841341612, + "y": 0.5767197827249103 + }, + "nextControl": { + "x": 6.208356120921947, + "y": 0.7157652158383974 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.319931811297383, + "y": 1.0695382317437065 + }, + "prevControl": { + "x": 6.781784998010706, + "y": 0.6813873471860529 + }, + "nextControl": { + "x": 7.72298592377244, + "y": 1.3602503690623626 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.579321000840929, + "y": 2.9450127376980255 + }, + "prevControl": { + "x": 7.9037417902865945, + "y": 2.543643460061272 + }, + "nextControl": { + "x": 7.306481652153864, + "y": 3.282566112377026 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.921627696849682, + "y": 1.70280493955259 + }, + "prevControl": { + "x": 7.047440006692617, + "y": 1.9381888147202682 + }, + "nextControl": { + "x": 6.79436274210612, + "y": 1.464703292281539 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.981227322613751, + "y": 0.5945518075448502 + }, + "prevControl": { + "x": 6.2312116363491175, + "y": 0.5973523229006296 + }, + "nextControl": { + "x": 5.702433550914315, + "y": 0.5914285466207009 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.55, + "y": 0.473 + }, + "prevControl": { + "x": 3.3047432255549993, + "y": 0.42453233460507767 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1706850405092595, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.15, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 3.787433087384258, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.9259100603911046, + "maxWaypointRelativePos": 3.6243806616660272, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.732532711848159, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/rt-block.path b/src/main/deploy/pathplanner/paths/rt-block.path index ad1f5b8d0..834917506 100644 --- a/src/main/deploy/pathplanner/paths/rt-block.path +++ b/src/main/deploy/pathplanner/paths/rt-block.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 7.5574063670411995, - "y": 0.9227996254681647 + "y": 0.9227996254681649 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index bb094b88f..bff432071 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,13 +1,13 @@ { - "robotWidth": 0.936625, - "robotLength": 0.78105, + "robotWidth": 0.937, + "robotLength": 0.781, "holonomicMode": true, "pathFolders": [], "autoFolders": [ "COMP", "WIP" ], - "defaultMaxVel": 4.5, + "defaultMaxVel": 4, "defaultMaxAccel": 3.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, @@ -35,4 +35,4 @@ "{\"name\":\"intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.4685,\"y\":0.0},\"size\":{\"width\":0.762,\"length\":0.2065782},\"borderRadius\":0.05,\"strokeWidth\":0.04,\"filled\":false}}", "{\"name\":\"Robot\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"size\":{\"width\":0.762,\"length\":0.61},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" ] -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index cc4b5768e..e6286edce 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -25,7 +25,7 @@ import frc.robot.sensors.LEDSubsystem; import frc.robot.sensors.LEDSubsystem.LEDMode; import frc.robot.sim.SimWrapper; -import frc.robot.subsystems.auto.AutoDriveRotate; +import frc.robot.subsystems.auto.Misc.AutoDriveRotate; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.subsystems.launcher.TurretSubsystem; import frc.robot.util.GetTargetFromPose; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d26151c75..20ce6d2dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,7 +6,6 @@ import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; -import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -27,13 +26,11 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Subsystems.SubsystemConstants; import frc.robot.sensors.LEDSubsystem; import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; -import frc.robot.subsystems.auto.AutoBuilderConfig; -import frc.robot.subsystems.auto.AutoLogic; -import frc.robot.subsystems.auto.AutonomousField; +import frc.robot.subsystems.auto.BLine.BLineAutonomousField; +import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.util.AllianceUtils; import frc.robot.util.BuildInfo; import frc.robot.util.DriveStateNtLogger; @@ -119,14 +116,20 @@ protected Robot() { controls = new Controls(subsystems, m_simWrapper); if (DRIVEBASE_ENABLED) { - AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem, false); - } - AutoLogic.init(subsystems); - if (Robot.isSimulation()) { - robotSim = new RobotSim(subsystems.drivebaseSubsystem); - } else { - robotSim = null; + if (Robot.isSimulation()) { + robotSim = new RobotSim(subsystems.drivebaseSubsystem); + } else { + robotSim = null; + } + + // BLINE STUFF + BLineLogic.init(subsystems); // Handling init and unit test cases + BLineLogic.configure(subsystems); // configure the autobuilder to run autos + BLineLogic.initSmartDashboard(); // Logging + BLineAutonomousField.initSmartDashBoard( // Visualization + () -> "Autos", 0, 0, this::addPeriodic); } + CommandScheduler.getInstance() .onCommandInitialize( command -> DataLogManager.log("Command initialized: " + command.getName())); @@ -143,13 +146,6 @@ protected Robot() { SmartDashboard.putData(CommandScheduler.getInstance()); - if (SubsystemConstants.DRIVEBASE_ENABLED) { - AutoLogic.initCommandsAndPaths(false); - AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); - - AutoLogic.initSmartDashBoard(); - CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); - } WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); logger = new DriveStateSignalLogger(); @@ -281,24 +277,25 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + subsystems.ledSubsystem.setMode(LEDSubsystem.LEDMode.RAINBOW); - if (AutoLogic.getSelectedAuto() != null) { - if (Robot.isSimulation()) { - robotSim.resetFuelSim(); - } - CommandScheduler.getInstance().schedule(AutoLogic.getSelectedAuto()); - double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); - if (subsystems.visionSubsystem != null) { - if (subsystems.visionSubsystem.limelightaOnline) { - supplyRobotYawToLimelight(Hardware.LIMELIGHT_A, initialYaw); - } - if (subsystems.visionSubsystem.limelightbOnline) { - supplyRobotYawToLimelight(Hardware.LIMELIGHT_B, initialYaw); - } - if (subsystems.visionSubsystem.limelightcOnline) { - supplyRobotYawToLimelight(Hardware.LIMELIGHT_C, initialYaw); - } + if (Robot.isSimulation()) { + + robotSim.resetFuelSim(); + } + + CommandScheduler.getInstance().schedule(BLineLogic.handleAutos()); + double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); + if (subsystems.visionSubsystem != null) { + if (subsystems.visionSubsystem.limelightaOnline) { + supplyRobotYawToLimelight(Hardware.LIMELIGHT_A, initialYaw); + } + if (subsystems.visionSubsystem.limelightbOnline) { + supplyRobotYawToLimelight(Hardware.LIMELIGHT_B, initialYaw); + } + if (subsystems.visionSubsystem.limelightcOnline) { + supplyRobotYawToLimelight(Hardware.LIMELIGHT_C, initialYaw); } } } diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 80f6fa662..23040bc19 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -104,7 +104,7 @@ public class CompTunerConstants { private static final double kDriveGearRatio = 5.357142857142857; private static final double kSteerGearRatio = 18.75; - public static final Distance kWheelRadius = Inches.of(1.924); + public static final Distance kWheelRadius = Inches.of(1.983); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java new file mode 100644 index 000000000..fcc1e798b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java @@ -0,0 +1,174 @@ +package frc.robot.subsystems.auto.BLine; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.lib.BLine.Path; +import java.util.List; +import java.util.Optional; +import java.util.function.DoubleSupplier; +import java.util.function.ObjDoubleConsumer; +import java.util.function.Supplier; + +public class BLineAutonomousField { + + private static final double DEFAULT_PLAYBACK_SPEED = 1.0; + private static final double UPDATE_RATE = 0.02; + private static final double ASSUMED_SPEED_MPS = 3.0; + + /* ---------------- NetworkTables init ---------------- */ + + public static void initSmartDashBoard( + Supplier tabName, + int columnIndex, + int rowIndex, + ObjDoubleConsumer addPeriodic) { + + NetworkTableEntry speedMultiplier = + NetworkTableInstance.getDefault().getTable("Autos").getEntry("DisplaySpeed"); + + speedMultiplier.setDouble(DEFAULT_PLAYBACK_SPEED); + + BLineAutonomousField autonomousField = + new BLineAutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); + + addPeriodic.accept( + () -> { + autonomousField.update(BLineLogic.getSelectedAutoName()); + }, + UPDATE_RATE); + } + + /* ---------------- Path data ---------------- */ + + private List paths = List.of(); + private int pathIndex = 0; + private Pose2d startingPose = Pose2d.kZero; + + /* ---------------- Polylines ---------------- */ + + private List currentTranslations = List.of(); + private double currentPathLength = 1e-9; + + /* ---------------- Time / distance ---------------- */ + + private final DoubleSupplier speedMultiplier; + private double lastFPGATime; + private double distanceAlongCurrentPath; + + private Optional lastName = Optional.empty(); + + /* ---------------- Constructors ---------------- */ + public BLineAutonomousField() { + this(() -> 1.0); + } + + public BLineAutonomousField(double speedMultiplier) { + this(() -> speedMultiplier); + } + + public BLineAutonomousField(DoubleSupplier speedMultiplier) { + this.speedMultiplier = speedMultiplier; + } + + /* ---------------- Geometry helpers ---------------- */ + + private static double arcLength(List pts) { + double total = 0; + for (int i = 1; i < pts.size(); i++) { + total += pts.get(i).getDistance(pts.get(i - 1)); + } + return total; + } + + private static Pose2d samplePolyline(List pts, double dist) { + if (pts.isEmpty()) return Pose2d.kZero; + if (pts.size() == 1) return new Pose2d(pts.get(0), Rotation2d.kZero); + + double remaining = Math.max(0, dist); + for (int i = 1; i < pts.size(); i++) { + Translation2d a = pts.get(i - 1); + Translation2d b = pts.get(i); + double segLen = b.getDistance(a); + boolean lastSeg = (i == pts.size() - 1); + + if (remaining <= segLen || lastSeg) { + double t = (segLen > 1e-9) ? Math.min(remaining / segLen, 1.0) : 1.0; + return new Pose2d( + a.interpolate(b, t), new Rotation2d(b.getX() - a.getX(), b.getY() - a.getY())); + } + remaining -= segLen; + } + + Translation2d last = pts.get(pts.size() - 1); + Translation2d prev = pts.get(pts.size() - 2); + return new Pose2d(last, new Rotation2d(last.getX() - prev.getX(), last.getY() - prev.getY())); + } + + private void refreshCurrentPath() { + if (paths.isEmpty()) { + currentTranslations = List.of(); + currentPathLength = 1e-9; + return; + } + currentTranslations = paths.get(pathIndex).getTranslations(); + currentPathLength = Math.max(arcLength(currentTranslations), 1e-9); + } + + /* ---------------- Pose update ---------------- */ + + public Pose2d getUpdatedPose(String autoName) { + if (autoName == null) return Pose2d.kZero; + + double speed = speedMultiplier.getAsDouble(); + double fpgaTime = Timer.getFPGATimestamp(); + + if (lastName.isEmpty() || !lastName.get().equals(autoName)) { + lastName = Optional.of(autoName); + + BLinePath selected = BLineLogic.getSelectedAutoPath(); + paths = (selected != null) ? selected.getAllPaths() : List.of(); + startingPose = BLineLogic.getSelectedAutoStartingPose(); + + pathIndex = 0; + distanceAlongCurrentPath = 0; + lastFPGATime = fpgaTime; + refreshCurrentPath(); + } + + if (paths.isEmpty()) { + return startingPose; + } + + distanceAlongCurrentPath += (fpgaTime - lastFPGATime) * speed * ASSUMED_SPEED_MPS; + lastFPGATime = fpgaTime; + + while (distanceAlongCurrentPath > currentPathLength) { + distanceAlongCurrentPath -= currentPathLength; + pathIndex++; + if (pathIndex >= paths.size()) { + pathIndex = 0; + } + refreshCurrentPath(); + } + + return samplePolyline(currentTranslations, distanceAlongCurrentPath); + } + + /* ---------------- Periodic update ---------------- */ + + public void update(String autoName) { + if (DriverStation.isEnabled()) { + lastName = Optional.empty(); + return; + } + if (autoName == null) return; + + BLineLogic.field.setRobotPose(getUpdatedPose(autoName)); + BLineLogic.fieldPoseStart.setRobotPose(startingPose); + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java new file mode 100644 index 000000000..416ba9b16 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.auto.BLine; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.lib.BLine.Path; +import java.util.ArrayList; +import java.util.List; + +public class BLineAutos { + + private final List paths; + private final Pose2d startingPose; + + public BLineAutos(String... pathNames) { + List loaded = new ArrayList<>(); + for (String name : pathNames) { + try { + loaded.add(new Path(name)); + } catch (Exception e) { + + System.err.println("[BLineAutos] Could not load path: " + name); + } + } + this.paths = List.copyOf(loaded); + + if (!this.paths.isEmpty()) { + Pose2d start = this.paths.get(0).getStartPose(); + this.startingPose = (start != null) ? start : Pose2d.kZero; + } else { + this.startingPose = Pose2d.kZero; + } + } + + public List getPaths() { + return paths; + } + + public Pose2d getStartingPose() { + return startingPose; + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java new file mode 100644 index 000000000..c8a2d531b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -0,0 +1,474 @@ +package frc.robot.subsystems.auto.BLine; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +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.Commands; +import frc.robot.Controls; +import frc.robot.Robot; +import frc.robot.Subsystems; +import frc.robot.lib.BLine.BLineCommands; +import frc.robot.lib.BLine.FollowPath; +import frc.robot.lib.BLine.Path; +import frc.robot.subsystems.auto.Misc.DynamicSendableChooser; +import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; +import frc.robot.util.simulation.RobotSim; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.concurrent.atomic.AtomicBoolean; + +public class BLineLogic { + + private static Subsystems s; + public static Field2d field = new Field2d(); + public static Field2d fieldPoseStart = new Field2d(); + + private static final Pose2d RIGHT_TRENCH_POSE = + new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90)); + private static final Pose2d LEFT_TRENCH_POSE = + new Pose2d(4.013, 7.597, Rotation2d.fromDegrees(90)); + + public enum StartPosition { + TRENCH("Trench", new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90))), + CENTER("Center", new Pose2d(3.600, 4.035, Rotation2d.fromDegrees(0))), + MISC("Misc", new Pose2d()); + + public final String title; + public final Pose2d startPose; + + StartPosition(String title, Pose2d startPose) { + this.title = title; + this.startPose = startPose; + } + } + + public enum TrenchSide { + RIGHT("Right"), + LEFT("Left"); + + public final String title; + + TrenchSide(String title) { + this.title = title; + } + } + + private static final List autos = new ArrayList<>(); + private static final SendableChooser startPositionChooser = + new SendableChooser<>(); + private static final SendableChooser trenchSideChooser = new SendableChooser<>(); + private static final DynamicSendableChooser autoChooser = new DynamicSendableChooser<>(); + private static final SendableChooser gameObjects = new SendableChooser<>(); + + private static final NetworkTableEntry autoDelayEntry = + NetworkTableInstance.getDefault().getTable("Autos").getEntry("Auto Delay"); + + public static final String keys = "RB=Right Bump, LB=Left Bump, LT=Left Trench, RT=Right Trench"; + + private static BLinePath defaultPath; + private static List rebuiltPaths = List.of(); + private static Map> commandsMap = Map.of(); + private static final Map namesToAuto = new HashMap<>(); + + private static boolean pathsInitialized = false; + private static Command bLineLaunching; + private static Command bLineSimLaunching; + + public static FollowPath.Builder pathBuilder; + private static FollowPath.Builder continuingPathBuilder; + + // ========================= MIRRORING ========================= + + public static boolean isMirrored() { + return startPositionChooser.getSelected() == StartPosition.TRENCH + && trenchSideChooser.getSelected() == TrenchSide.LEFT; + } + + // ========================= INIT ========================= + + public static void init(Subsystems subsystems) { + s = subsystems; + registerCommands(); + + if (pathsInitialized) return; + + initializePaths(); + pathsInitialized = true; + } + + public static void unitTestInit() { + s = null; // Explicitly set to null for unit tests + initializePaths(); + } + + private static void initializePaths() { + defaultPath = new BLinePath("default", "Center", "default"); + + rebuiltPaths = + List.of( + defaultPath, + new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), + new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench"), + new BLinePath("BumpNeutral", "RT", "FirstNeutralBump"), + new BLinePath("DoubleBumpNeutral", "RT", "FirstNeutralBump", "SecondNeutralBump"), + new BLinePath("BumpNeutralDepot", "RT", "FirstNeutralBump")); + + autos.clear(); + autos.addAll(rebuiltPaths); + commandsMap = Map.of(0, rebuiltPaths); + + namesToAuto.clear(); + for (List list : commandsMap.values()) { + for (BLinePath auto : list) { + handleStartingPoses(auto); + namesToAuto.put(auto.getDisplayName(), auto); + } + } + } + + public static void handleStartingPoses(BLinePath path) { + switch (path.getStartingPosName()) { + case "RT": + case "LT": + path.setStartPose2d(StartPosition.TRENCH); + break; + case "Center": + path.setStartPose2d(StartPosition.CENTER); + break; + default: + path.setStartPose2d(StartPosition.MISC); + break; + } + } + + public static void configure(Subsystems s) { + pathBuilder = createPathBuilder(s).withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); + continuingPathBuilder = createPathBuilder(s); + } + + private static FollowPath.Builder createPathBuilder(Subsystems s) { + return new FollowPath.Builder( + s.drivebaseSubsystem, + () -> s.drivebaseSubsystem.getState().Pose, + () -> s.drivebaseSubsystem.getState().Speeds, + (speeds) -> + s.drivebaseSubsystem.setControl( + new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))), + new PIDController(3.0, 0.0, 0.0), + new PIDController(5.0, 0.0, 0.0), + new PIDController(2.0, 0.0, 0.0)) + .withDefaultShouldFlip() + .withShouldMirror(BLineLogic::isMirrored); + } + + // ========================= LOGGING ========================= + + public static void initSmartDashboard() { + startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); + for (StartPosition pos : StartPosition.values()) { + startPositionChooser.addOption(pos.title, pos); + } + + trenchSideChooser.setDefaultOption(TrenchSide.RIGHT.title, TrenchSide.RIGHT); + trenchSideChooser.addOption(TrenchSide.LEFT.title, TrenchSide.LEFT); + + gameObjects.setDefaultOption("0", 0); + filterAutos(0); + + SmartDashboard.putData("Selected auto", field); + SmartDashboard.putData("Start pose", fieldPoseStart); + SmartDashboard.putData("Starting Position", startPositionChooser); + SmartDashboard.putData("Trench Side", trenchSideChooser); + SmartDashboard.putData("Auto Mode", gameObjects); + SmartDashboard.putData("Available Auto Variants", autoChooser); + SmartDashboard.putString("Auto Key", keys); + + autoDelayEntry.setDouble(0.0); + + startPositionChooser.onChange( + v -> { + filterAutos(gameObjects.getSelected()); + updateInitialHeading(); + updateFieldDisplay(); + }); + + trenchSideChooser.onChange( + v -> { + updateInitialHeading(); + updateFieldDisplay(); + }); + + autoChooser.onChange( + v -> { + updateInitialHeading(); + updateFieldDisplay(); + }); + + updateFieldDisplay(); + } + + public static void updateFieldDisplay() { + fieldPoseStart.setRobotPose(getSelectedAutoStartingPose()); + } + + static Pose2d getTrenchPose() { + return isMirrored() ? LEFT_TRENCH_POSE : RIGHT_TRENCH_POSE; + } + + public static void filterAutos(int numGameObjects) { + autoChooser.clearOptions(); + + StartPosition selected = startPositionChooser.getSelected(); + if (selected == null) selected = StartPosition.MISC; + + for (BLinePath auto : autos) { + if (selected == StartPosition.MISC) { + autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); + continue; + } + + if (auto.getStartPositionType() == selected) { + autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); + } + } + } + + // ========================= SELECTION METHODS ========================= + + public static String getSelectedAutoName() { + if (autoChooser.getSelected() == null) { + return "Default"; + } + return autoChooser.getSelected(); + } + + public static BLinePath getSelectedAutoPath() { + String selectedName = autoChooser.getSelected(); + if (selectedName == null) return defaultPath; + return namesToAuto.getOrDefault(selectedName, defaultPath); + } + + public static Pose2d getSelectedAutoStartingPose() { + BLinePath selected = getSelectedAutoPath(); + if (selected == null) return Pose2d.kZero; + + if (startPositionChooser.getSelected() == StartPosition.TRENCH) { + return isMirrored() ? LEFT_TRENCH_POSE : RIGHT_TRENCH_POSE; + } + + return selected.getStartPose2d(); + } + + // ========================= AUTO EXECUTION ========================= + public static List getPathsToBuild() { + BLinePath selected = getSelectedAutoPath(); + if (selected == null) { + return List.of(); + } + return selected.getAllPaths(); + } + + public static Command getSelectedAuto() { + if (s == null) { + // Unit test mode - return empty command + return Commands.none(); + } + + double delay = autoDelayEntry.getDouble(0.0); + BLinePath selected = getSelectedAutoPath(); + + if (selected == null) { + return Commands.none(); + } + + s.drivebaseSubsystem.resetRotation(selected.getPath().getInitialModuleDirection()); + + List commands = new ArrayList<>(); + commands.add(Commands.waitSeconds(delay)); + + List paths = getPathsToBuild(); + for (int i = 0; i < paths.size(); i++) { + boolean resetPose = (i == 0); + commands.add(buildPath(paths.get(i), resetPose)); + } + + return Commands.sequence(commands.toArray(new Command[0])); + } + + public static List getBLinePaths() { + return rebuiltPaths; + } + + public static List getBLinePathsNames() { + List pathsNames = new ArrayList<>(); + for (BLinePath path : getBLinePaths()) { + pathsNames.addAll(path.getDisplayingNames()); + } + return pathsNames; + } + + public static Command buildSingleNeutralTrenchAuto() { + return Commands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralTrench"), true), + launcherCommand()); + } + + public static Command buildDoubleNeutralTrenchAuto() { + return BLineCommands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralTrench"), true), + launcherCommand(4.5), + buildPath(new Path("SecondNeutralTrench"), false), + launcherCommand()); + } + + public static Command buildSingleNeutralBumpAuto() { + return Commands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralBump"), true), + launcherCommand()); + } + + public static Command buildDoubleNeutralBumpAuto() { + return BLineCommands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralBump"), true), + buildPath(new Path("BumpToTrench"), false), + buildPath(new Path("SecondNeutralBump"), false), + launcherCommand()); + } + + public static Command buildSingleNeutralBumpDepotAuto() { + return BLineCommands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralBump"), true), + buildPath(new Path("BumpDepot"), false), + launcherCommand()); + } + + public static Command buildDefaultAuto() { + return BLineCommands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("Default"), true), + launcherCommand()); + } + + private static Command buildPath(Path path, boolean resetPose) { + if (s == null || pathBuilder == null || continuingPathBuilder == null) { + return Commands.none(); + } + return (resetPose ? pathBuilder : continuingPathBuilder).build(path); + } + + public static Command handleAutos() { + switch (getSelectedAutoName()) { + case "TrenchNeutral": + return buildSingleNeutralTrenchAuto(); + case "DoubleTrenchNeutral": + return buildDoubleNeutralTrenchAuto(); + case "BumpNeutral": + return buildSingleNeutralBumpAuto(); + case "DoubleBumpNeutral": + return buildDoubleNeutralBumpAuto(); + case "BumpNeutralDepot": + return buildSingleNeutralBumpDepotAuto(); + default: + return buildDefaultAuto(); + } + } + + private static void updateInitialHeading() { + BLinePath selected = getSelectedAutoPath(); + if (selected == null || selected.getPath() == null) { + SmartDashboard.putNumber("Initial Heading(Deg)", 0.0); + return; + } + Pose2d start = selected.getPath().getStartPose(); + SmartDashboard.putNumber("Initial Heading(Deg)", Math.round(start.getRotation().getDegrees())); + } + + // ========================= COMMANDS ========================= + + public static Command intakeCommand() { + return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) + .withName("Auto Intake Command"); + } + + public static Command launcherCommand(double timeout) { + if (s == null || Robot.isSimulation()) return RobotSim.launch(s, timeout); + return Commands.parallel( + Commands.runOnce(() -> s.flywheels.resetFuelCheck()), + s.launcherSubsystem.launcherAimCommand(), + Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) + .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) + .withTimeout(timeout) + .andThen(s.launcherSubsystem.rawStowCommand()) + .withName("Auto Launcher Command"); + } + + public static Command launcherCommand() { + if (s == null) return Commands.none(); + return Commands.parallel( + Commands.runOnce(() -> s.flywheels.resetFuelCheck()), + s.launcherSubsystem.launcherAimCommand(), + Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) + .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) + .withName("Auto Launcher Command"); + } + + public static Command stowCommand() { + if (s == null) return Commands.none(); + return s.launcherSubsystem.rawStowCommand(); + } + + public static Command climbCommand() { + return Commands.none().withName("Auto Climb Command"); + } + + public static void cancelCommand() { + if (s == null) return; + if (Robot.isSimulation()) { + CommandScheduler.getInstance().cancel(bLineSimLaunching); + } else { + CommandScheduler.getInstance().cancel(bLineLaunching); + } + } + + private static void registerCommands() { + if (s == null) return; // Skip registration during unit tests + + AtomicBoolean launchAllowed = new AtomicBoolean(true); + + if (s.launcherSubsystem != null && s.indexerSubsystem != null) { + if (Robot.isSimulation()) { + bLineSimLaunching = RobotSim.launch(s, 30); + FollowPath.registerEventTrigger( + "launch", + Commands.runOnce(() -> launchAllowed.set(true)) + .andThen(bLineSimLaunching.onlyWhile(launchAllowed::get)) + .andThen(Commands.print("LAUNCH FINISHED"))); + } else { + bLineLaunching = launcherCommand(); + FollowPath.registerEventTrigger("launch", bLineLaunching); + } + } + + FollowPath.registerEventTrigger("intake", intakeCommand()); + FollowPath.registerEventTrigger("climb", climbCommand()); + FollowPath.registerEventTrigger( + "cancel", Commands.runOnce(() -> launchAllowed.set(false)).andThen(stowCommand())); + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java new file mode 100644 index 000000000..574149c20 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems.auto.BLine; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.lib.BLine.Path; +import frc.robot.subsystems.auto.BLine.BLineLogic.StartPosition; +import java.util.ArrayList; +import java.util.List; + +public class BLinePath { + + private final String displayName; + private final String startingPosName; + private final boolean vision; + private final Path path; + private final List allPaths; + private final List displayedPaths; // Make it final + private StartPosition positionType; + private Pose2d startPose; + + public BLinePath(String displayName, String startingPosName, String... displayedPathNames) { + this(displayName, startingPosName, false, displayedPathNames); + } + + public BLinePath( + String displayName, String startingPosName, boolean vision, String... displayedPathNames) { + this.displayName = displayName; + this.startingPosName = startingPosName; + this.vision = vision; + + List loaded = new ArrayList<>(); + List displayedList = new ArrayList<>(); + + for (String name : displayedPathNames) { + loaded.add(new Path(name)); + displayedList.add(name); + } + + this.allPaths = List.copyOf(loaded); + this.displayedPaths = List.copyOf(displayedList); // Make it immutable + this.path = this.allPaths.isEmpty() ? null : this.allPaths.get(0); + this.startPose = this.path != null ? this.path.getStartPose() : new Pose2d(); + } + + public String getDisplayName() { + return displayName; + } + + public List getDisplayingNames() { + return displayedPaths; + } + + public String getStartingPosName() { + return startingPosName; + } + + public Pose2d getStartPose2d() { + if (positionType == StartPosition.TRENCH) { + return BLineLogic.getTrenchPose(); + } + return startPose; + } + + public Pose2d setStartPose2d(StartPosition pos) { + this.positionType = pos; + this.startPose = pos.startPose; + return getStartPose2d(); + } + + public boolean isVision() { + return vision; + } + + public Path getPath() { + return path; + } + + public List getAllPaths() { + return allPaths; + } + + public StartPosition getStartPositionType() { + return positionType; + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/AutoDriveRotate.java b/src/main/java/frc/robot/subsystems/auto/Misc/AutoDriveRotate.java similarity index 99% rename from src/main/java/frc/robot/subsystems/auto/AutoDriveRotate.java rename to src/main/java/frc/robot/subsystems/auto/Misc/AutoDriveRotate.java index 345fc3600..e874f2f28 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoDriveRotate.java +++ b/src/main/java/frc/robot/subsystems/auto/Misc/AutoDriveRotate.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.Misc; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; diff --git a/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java b/src/main/java/frc/robot/subsystems/auto/Misc/DynamicSendableChooser.java similarity index 99% rename from src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java rename to src/main/java/frc/robot/subsystems/auto/Misc/DynamicSendableChooser.java index 854828f63..2979f8896 100644 --- a/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java +++ b/src/main/java/frc/robot/subsystems/auto/Misc/DynamicSendableChooser.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.Misc; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoBuilderConfig.java similarity index 98% rename from src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoBuilderConfig.java index 302d56e38..6149da678 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoBuilderConfig.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoPath.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoPath.java similarity index 95% rename from src/main/java/frc/robot/subsystems/auto/AutoPath.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoPath.java index 707931a70..becce8ded 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoPath.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoPath.java @@ -1,11 +1,11 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.auto.AutoLogic.StartPosition; +import frc.robot.subsystems.auto.PathPlanner.PathPlannerLogic.StartPosition; public class AutoPath { diff --git a/src/main/java/frc/robot/subsystems/auto/AutonomousField.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutonomousField.java similarity index 87% rename from src/main/java/frc/robot/subsystems/auto/AutonomousField.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutonomousField.java index 50f311a52..b60d06690 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutonomousField.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; @@ -14,7 +14,7 @@ import java.util.function.ObjDoubleConsumer; import java.util.function.Supplier; -public class AutonomousField { +public class PathPlannerAutonomousField { private static final double DEFAULT_PLAYBACK_SPEED = 1.0; private static final double UPDATE_RATE = 0.05; @@ -31,15 +31,12 @@ public static void initSmartDashBoard( speedMultiplier.setDouble(DEFAULT_PLAYBACK_SPEED); - AutonomousField autonomousField = - new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); - - SmartDashboard.putData("Selected auto", autonomousField.getField()); - SmartDashboard.putData("Start pose", autonomousField.getStartPose()); + PathPlannerAutonomousField autonomousField = + new PathPlannerAutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); addPeriodic.accept( () -> { - autonomousField.update(AutoLogic.getSelectedAutoName()); + autonomousField.update(PathPlannerLogic.getSelectedAutoName()); SmartDashboard.putNumber( "Est. Time (s)", Math.round(autonomousField.autoTotalTime() * 100.0) / 100.0); }, @@ -69,15 +66,15 @@ public static void initSmartDashBoard( /* ---------------- Constructors ---------------- */ - public AutonomousField() { + public PathPlannerAutonomousField() { this(() -> 1.0); } - public AutonomousField(double speedMultiplier) { + public PathPlannerAutonomousField(double speedMultiplier) { this(() -> speedMultiplier); } - public AutonomousField(DoubleSupplier speedMultiplier) { + public PathPlannerAutonomousField(DoubleSupplier speedMultiplier) { this.speedMultiplier = speedMultiplier; } diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlannerAutos.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutos.java similarity index 98% rename from src/main/java/frc/robot/subsystems/auto/PathPlannerAutos.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutos.java index b01246bbd..dea4244e8 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlannerAutos.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutos.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.RobotConfig; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java similarity index 90% rename from src/main/java/frc/robot/subsystems/auto/AutoLogic.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java index 7b7295aa8..be5b150c5 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java @@ -1,8 +1,4 @@ -package frc.robot.subsystems.auto; - -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Radians; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -20,15 +16,16 @@ import frc.robot.Controls; import frc.robot.Robot; import frc.robot.Subsystems; +import frc.robot.subsystems.auto.Misc.DynamicSendableChooser; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; -import frc.robot.util.simulation.FuelSim; +import frc.robot.util.simulation.RobotSim; import java.io.IOException; import java.util.HashMap; import java.util.List; import java.util.Map; import org.json.simple.parser.ParseException; -public class AutoLogic { +public class PathPlannerLogic { private static Subsystems s; @@ -123,6 +120,7 @@ private static void initPaths() { new AutoPath("RT-Neutral", "RT-Neutral"), new AutoPath("RT-DoubleNeutral", "RT-DoubleNeutral"), new AutoPath("RT-BLOCK", "RT-BLOCK"), + new AutoPath("test", "BLINECOMPARE"), new AutoPath("LT-BLOCK", "LT-BLOCK")); rebuiltPaths = physicalRebuiltPaths; @@ -274,19 +272,7 @@ public static Command launcherCommand() { } public static Command launcherSimCommand() { - return Commands.sequence( - AutoDriveRotate.autoRotate( - s.drivebaseSubsystem, () -> 0, () -> 0, () -> 0), // SIM PURPOSES ONLY - Commands.run( - () -> - FuelSim.getInstance() - .launchFuel( - MetersPerSecond.of(6), - Radians.of(s.hood.getHoodPosition()), - Radians.of(s.turretSubsystem.getTurretPosition() + Math.PI), - Meters.of(1.45))) - .withTimeout(3)) - .withName("Auto Launcher Sim Command"); + return RobotSim.launch(s, 2); } public static Command intakeCommand() { diff --git a/src/main/java/frc/robot/util/UnitTestHelpers.java b/src/main/java/frc/robot/util/UnitTestHelpers.java new file mode 100644 index 000000000..8585942f8 --- /dev/null +++ b/src/main/java/frc/robot/util/UnitTestHelpers.java @@ -0,0 +1,108 @@ +package frc.robot.util; + +import com.google.gson.Gson; +import com.google.gson.JsonObject; +import com.google.gson.JsonParser; +import edu.wpi.first.util.struct.parser.ParseException; +import frc.robot.subsystems.auto.BLine.BLineLogic; +import java.io.File; +import java.io.FileReader; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; + +public class UnitTestHelpers { + public static final String PATHS_PATH = "src/main/deploy/autos/paths"; + + // @BeforeEach + public static void setup() { + BLineLogic.unitTestInit(); + } + + // ==================== METHODS ==================== + public static File[] getAutosFiles() { + File subfolder = new File(PATHS_PATH); + + if (!subfolder.exists() || !subfolder.isDirectory()) { + System.out.println("WARNING: Autos folder does not exist at: " + subfolder.getAbsolutePath()); + return null; + } + + File[] files = subfolder.listFiles(); + if (files == null || files.length == 0) { + System.out.println("WARNING: Autos folder is empty"); + return null; + } + + return files; + } + + public static String getFilenamWithoutExtension(File file) { + return file.getName().split("\\.")[0]; + } + + public static List getValidFileNames(File[] files) { + List validFileNames = new ArrayList<>(); + for (File file : files) { + validFileNames.add(getFilenamWithoutExtension(file)); + } + return validFileNames; + } + + public static List getAllJsonFiles(File dir) { + List jsonFiles = new ArrayList<>(); + collectJsonFiles(dir, jsonFiles); + return jsonFiles; + } + + public static void collectJsonFiles(File dir, List jsonFiles) { + if (!dir.exists() || !dir.isDirectory()) { + return; + } + + File[] files = dir.listFiles(); + if (files == null) return; + + for (File file : files) { + if (file.isFile() && file.getName().endsWith(".json")) { + jsonFiles.add(file); + } else if (file.isDirectory()) { + collectJsonFiles(file, jsonFiles); + } + } + } + + // Source - https://stackoverflow.com/a/68606076 + // Posted by Aman Garg + // Retrieved 2026-06-28, License - CC BY-SA 4.0 + + @SuppressWarnings("unchecked") + public static Object getObjectFromJsonFile(String jsonData, Class classObject) { + Gson gson = new Gson(); + JsonParser parser = new JsonParser(); + JsonObject object = (JsonObject) parser.parse(jsonData); + return gson.fromJson(object, classObject); + } + + // Source - https://stackoverflow.com/a/42440060 + // Posted by Ankur Mahajan + // Retrieved 2026-06-28, License - CC BY-SA 3.0 + + public static JSONObject JSONtoObject(File file) + throws ParseException, org.json.simple.parser.ParseException { + try { + JSONParser parser = new JSONParser(); + // Use JSONObject for simple JSON and JSONArray for array of JSON. + JSONObject data = + (JSONObject) + parser.parse(new FileReader(file.getAbsolutePath())); // path to the JSON file. + return data; + // String json = data.toJSONString(); + } catch (IOException e) { + e.printStackTrace(); + return null; + } + } +} diff --git a/src/main/java/frc/robot/util/simulation/FuelSim.java b/src/main/java/frc/robot/util/simulation/FuelSim.java index 73338d6b3..54e420687 100644 --- a/src/main/java/frc/robot/util/simulation/FuelSim.java +++ b/src/main/java/frc/robot/util/simulation/FuelSim.java @@ -577,7 +577,7 @@ public void launchFuel( spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); } - spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + // spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); } protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { diff --git a/src/main/java/frc/robot/util/simulation/RobotSim.java b/src/main/java/frc/robot/util/simulation/RobotSim.java index a27f98cbb..e3d961a7a 100644 --- a/src/main/java/frc/robot/util/simulation/RobotSim.java +++ b/src/main/java/frc/robot/util/simulation/RobotSim.java @@ -2,6 +2,10 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Subsystems; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import frc.robot.util.simulation.FuelSim.Hub; @@ -9,8 +13,8 @@ public class RobotSim { FuelSim fuelSim; public static final double UPDATE_S = 0.02; // 20 ms update rate - public static final double SIM_ROBOT_WIDTH_M = 0.8; - public static final double SIM_ROBOT_LENGTH_M = 0.8; + public static final double SIM_ROBOT_WIDTH_M = 0.937; + public static final double SIM_ROBOT_LENGTH_M = 0.781; public static final double SIM_ROBOT_BUMPER_HEIGHT = 0.7; static DoublePublisher scorePublisher; @@ -29,7 +33,7 @@ public RobotSim(CommandSwerveDrivetrain drive) { SIM_ROBOT_BUMPER_HEIGHT, () -> drive.getState().Pose, () -> drive.getState().Speeds); - fuelSim.registerIntake(0.1, 0.2, 0.1, 0.746, () -> true); + fuelSim.registerIntake(0.1, 2, 0.1, 0.746, () -> true); scorePublisher = NetworkTableInstance.getDefault() .getTable("Fuel Simulation") @@ -64,4 +68,17 @@ public void updateFuelSim() { scorePublisher.accept(score); fuelHeld.accept(fuelsHeld); } + + public static Command launch(Subsystems s, double timeout) { + return Commands.run( + () -> + FuelSim.getInstance() + .launchFuel( + Units.MetersPerSecond.of(6), + Units.Radians.of(s.hood.getHoodPosition()), + Units.Radians.of(s.turretSubsystem.getTurretPosition() + Math.PI), + Units.Meters.of(1.45))) + .withTimeout(timeout) + .withName("Auto Launcher Sim Command"); + } } diff --git a/src/test/java/AutoFilePathsTest.java b/src/test/java/AutoFilePathsTest.java new file mode 100644 index 000000000..f52397cee --- /dev/null +++ b/src/test/java/AutoFilePathsTest.java @@ -0,0 +1,37 @@ +import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.subsystems.auto.BLine.BLinePath; +import frc.robot.util.UnitTestHelpers; +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class AutoFilePathsTest { + @Test + public void validatePathFiles() { + UnitTestHelpers.setup(); + File[] files = UnitTestHelpers.getAutosFiles(); + if (files == null) return; + + List validFileNames = UnitTestHelpers.getValidFileNames(files); + List invalidPaths = new ArrayList<>(); + + for (BLinePath auto : BLineLogic.getBLinePaths()) { + for (String pathName : auto.getDisplayingNames()) { + if (!validFileNames.contains(pathName)) { + invalidPaths.add( + "Auto '" + auto.getDisplayName() + "' references missing path: " + pathName); + } + } + } + + if (!invalidPaths.isEmpty()) { + System.out.println("WARNING: Autos reference invalid path files:"); + for (String invalid : invalidPaths) { + System.out.println(" - " + invalid); + } + } else { + System.out.println("All autos only reference valid path files"); + } + } +} diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java deleted file mode 100644 index ca88d3e05..000000000 --- a/src/test/java/AutosTest.java +++ /dev/null @@ -1,51 +0,0 @@ -import static org.junit.jupiter.api.Assertions.assertFalse; - -import com.pathplanner.lib.commands.PathPlannerAuto; -import frc.robot.generated.CompTunerConstants; -import frc.robot.subsystems.auto.AutoBuilderConfig; -import frc.robot.subsystems.auto.AutoLogic; -import frc.robot.subsystems.auto.AutoPath; -import java.io.IOException; -import java.util.Dictionary; -import java.util.Enumeration; -import java.util.Hashtable; -import org.json.simple.parser.ParseException; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -class AutosTest { - - @BeforeEach - void setup() { - - AutoBuilderConfig.buildAuto(CompTunerConstants.createDrivetrain(), true); - } - - @Test - void validateFileName() throws IOException, ParseException { - AutoLogic.initCommandsAndPaths(true); - - Dictionary pathDictionary = new Hashtable<>(); - for (AutoPath path : AutoLogic.getAutos()) { // Every auto in the auto chooser - pathDictionary.put( - path.getAutoName(), false); // Putting each auto from the auto chooser in dictionary - } - - // We expect >= 1 paths, otherwise its possible we forgot to call - // AutoLogic::initCommandsAndPaths(). - if (pathDictionary.isEmpty()) { - throw new IllegalStateException( - "No auto paths found. Did you forget to call AutoLogic::initCommandsAndPaths()?"); - } - - Enumeration k = pathDictionary.keys(); - while (k.hasMoreElements()) { // As long as there are more autos in the list - String key = k.nextElement(); - // Next entry in the dictionary - // Asserts false if file name does not exist - - assertFalse( - PathPlannerAuto.getPathGroupFromAutoFile(key).isEmpty(), "No file path matches" + key); - } - } -} diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java new file mode 100644 index 000000000..1ce958465 --- /dev/null +++ b/src/test/java/JSONComparisionTest.java @@ -0,0 +1,47 @@ +import edu.wpi.first.util.struct.parser.ParseException; +import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.util.UnitTestHelpers; +import java.io.File; +import java.util.HashMap; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class JSONComparisionTest { + private final int MINIMUM_FILE_COUNT = 2; + + @Test + public void compareJSONContents() + throws ParseException, ParseException, org.json.simple.parser.ParseException { + BLineLogic.unitTestInit(); + File deployDir = new File(UnitTestHelpers.PATHS_PATH); + + if (!deployDir.exists() || !deployDir.isDirectory()) { + System.out.println("WARNING: Deploy directory not found"); + return; + } + + List allJsonFiles = UnitTestHelpers.getAllJsonFiles(deployDir); + + if (allJsonFiles.size() < MINIMUM_FILE_COUNT) { + System.out.println("WARNING: Not enough files to compare"); + return; + } + + HashMap Hashes = new HashMap<>(); + for (File file : allJsonFiles) { + Object parsedFile = UnitTestHelpers.JSONtoObject(file); + + int hash = parsedFile.hashCode(); + + if (!Hashes.containsKey(hash)) { + + Hashes.put(hash, file); + } else { + System.out.println( + Hashes.get(parsedFile.hashCode()).getName() + + " has duplicate file contents with " + + file.getName()); + } + } + } +} diff --git a/src/test/java/UniqueAutoNamesTest.java b/src/test/java/UniqueAutoNamesTest.java new file mode 100644 index 000000000..f08707a8a --- /dev/null +++ b/src/test/java/UniqueAutoNamesTest.java @@ -0,0 +1,40 @@ +import frc.robot.util.UnitTestHelpers; +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class UniqueAutoNamesTest { + @Test + public void validateUniqueAutoNames() { + UnitTestHelpers.setup(); + File[] files = UnitTestHelpers.getAutosFiles(); + if (files == null) return; + + List invalidNames = new ArrayList<>(); + + for (File file : files) { + String filename = UnitTestHelpers.getFilenamWithoutExtension(file); + + if (filename.startsWith(" ")) { + invalidNames.add(file.getName() + " (contains leading spaces)"); + } + if (filename.endsWith(" ")) { + invalidNames.add(file.getName() + " (contains trailing spaces)"); + } + + if (filename.matches(".*[0-9]$")) { + invalidNames.add(file.getName() + " (ends with number)"); + } + } + + if (!invalidNames.isEmpty()) { + System.out.println("WARNING: Invalid auto names:"); + for (String name : invalidNames) { + System.out.println(" - " + name); + } + } else { + System.out.println("All auto names are valid"); + } + } +} diff --git a/src/test/java/UnusedFilesTest.java b/src/test/java/UnusedFilesTest.java new file mode 100644 index 000000000..16f4cd8a8 --- /dev/null +++ b/src/test/java/UnusedFilesTest.java @@ -0,0 +1,34 @@ +import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.util.UnitTestHelpers; +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class UnusedFilesTest { + @Test + public void unusedFileNames() { + UnitTestHelpers.setup(); + File[] files = UnitTestHelpers.getAutosFiles(); + if (files == null) return; + + List validPathNames = BLineLogic.getBLinePathsNames(); + List unusedFiles = new ArrayList<>(); + + for (File file : files) { + String filename = UnitTestHelpers.getFilenamWithoutExtension(file); + if (!validPathNames.contains(filename)) { + unusedFiles.add(file.getName()); + } + } + + if (!unusedFiles.isEmpty()) { + System.out.println("WARNING: Unused BLine Paths found:"); + for (String file : unusedFiles) { + System.out.println(" - " + file); + } + } else { + System.out.println("All files in 'autos' folder are used"); + } + } +} diff --git a/vendordeps/BLine-Lib.json b/vendordeps/BLine-Lib.json new file mode 100644 index 000000000..6c804c00f --- /dev/null +++ b/vendordeps/BLine-Lib.json @@ -0,0 +1,20 @@ +{ + "fileName": "BLine-Lib.json", + "name": "BLine-Lib", + "version": "v0.9.1", + "frcYear": "2026", + "uuid": "4b7270e9-4e8d-4e7b-8cf0-5805f12c3c7d", + "mavenUrls": [ + "https://jitpack.io" + ], + "jsonUrl": "https://bline-metrics.edan-liahovetsky.workers.dev/vendor/BLine-Lib.json", + "javaDependencies": [ + { + "groupId": "com.github.edanliahovetsky", + "artifactId": "BLine-Lib", + "version": "v0.9.1" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file