diff --git a/README.md b/README.md index 919f6da..5e62afa 100644 --- a/README.md +++ b/README.md @@ -27,26 +27,35 @@ This is a guide to using and updating 5690's season code for our 2026 Command Ro All devices connected to the CAN bus along with their corresponding CAN IDs -| Device | CAN ID | -| ----------------------- | ------ | -| Front Right Drive Motor | 1 | -| Front Left Drive Motor | 2 | -| Rear Left Drive Motor | 3 | -| Rear Right Drive Motor | 4 | -| Front Right Turn Motor | 5 | -| Front Left Turn Motor | 6 | -| Rear Left Turn Motor | 7 | -| Rear Right Turn Motor | 8 | -| Pigeon Gyro | 13 | +| Device | CAN ID | +| -------------------------- | ------ | +| Front Right Drive Motor | 1 | +| Front Left Drive Motor | 10 | +| Rear Left Drive Motor | 14 | +| Rear Right Drive Motor | 2 | +| Front Right Turn Motor | 62 | +| Front Left Turn Motor | 11 | +| Rear Left Turn Motor | 15 | +| Rear Right Turn Motor | 3 | +| Pigeon Gyro | 13 | +| Turret Turning Motor | 18 | +| Turret Shooter Motor | 5 | +| Shooter Hood Motor | 4 | +| Agitator Motor | 9 | +| Roller Motor | 12 | +| First Intake Deploy Motor | 13 | +| Second Intake Deploy Motor | 8 | +| Intake Motor | 7 | +| Climber Motor | 17 | ## Network Map All devices connected to the robot's local network along with each device's assigned IP address -| Device | IP | -| ------- | ---------- | -| Gateway | 10.56.90.1 | -| RoboRio | 10.56.90.2 | +| Device | IP | +| ------------------ | ----------- | +| Gateway | 10.56.90.1 | +| RoboRio | 10.56.90.2 | | Vision Coprocessor | 10.56.90.10 | ## Button Bindings diff --git a/simgui-ds.json b/simgui-ds.json index 37ec5eb..1a435f9 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,7 +1,7 @@ { - "Keyboard 0 Settings": { + "System Joysticks": { "window": { - "visible": true + "enabled": false } }, "keyboardJoysticks": [ @@ -103,5 +103,6 @@ "guid": "78696e70757401000000000000000000", "useGamepad": true } - ] + ], + "zeroDisconnectedJoysticks": false } diff --git a/src/main/deploy/pathplanner/autos/Left Backwards Auto.auto b/src/main/deploy/pathplanner/autos/Left Backwards Auto.auto new file mode 100644 index 0000000..e11868c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Backwards Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Straight back" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Forward Auto.auto b/src/main/deploy/pathplanner/autos/Left Forward Auto.auto new file mode 100644 index 0000000..af51163 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Forward Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Straight Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto new file mode 100644 index 0000000..50c2c88 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto @@ -0,0 +1,94 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Side To Center Path" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.4 + } + }, + { + "type": "named", + "data": { + "name": "Deploy Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Side Center Return" + } + }, + { + "type": "named", + "data": { + "name": "Retract Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto new file mode 100644 index 0000000..440a1ea --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New New Auto.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Backwards Auto.auto b/src/main/deploy/pathplanner/autos/Right Backwards Auto.auto new file mode 100644 index 0000000..963159f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Backwards Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Straight Back" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Forward Auto.auto b/src/main/deploy/pathplanner/autos/Right Forward Auto.auto new file mode 100644 index 0000000..8992651 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Forward Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Straight Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto new file mode 100644 index 0000000..95166c8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Side To Center" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.4 + } + }, + { + "type": "named", + "data": { + "name": "Deploy Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Side Center Return" + } + }, + { + "type": "named", + "data": { + "name": "Retract Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto new file mode 100644 index 0000000..18f2ba1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Uh Oh Forward Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Example Auto.auto rename to src/main/deploy/pathplanner/autos/Uh Oh Forward Auto.auto diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index ac5f521..a6fa85a 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side Center Return.path b/src/main/deploy/pathplanner/paths/Left Side Center Return.path new file mode 100644 index 0000000..59960ad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Side Center Return.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.4548644793152645, + "y": 5.438844507845934 + }, + "prevControl": null, + "nextControl": { + "x": 7.3963978126485985, + "y": 7.3955807259465205 + }, + "isLocked": false, + "linkedName": "Left Center Intake Location" + }, + { + "anchor": { + "x": 3.922610556348074, + "y": 7.340827389443652 + }, + "prevControl": { + "x": 7.170213980028528, + "y": 7.10793152639087 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Auto Start Position" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 88.18635612332106 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.59114027035312 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -62.80891001618222 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path new file mode 100644 index 0000000..f6acac8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.922610556348074, + "y": 7.340827389443652 + }, + "prevControl": null, + "nextControl": { + "x": 5.64345221112696, + "y": 7.379643366619116 + }, + "isLocked": false, + "linkedName": "Left Auto Start Position" + }, + { + "anchor": { + "x": 7.4548644793152645, + "y": 5.438844507845934 + }, + "prevControl": { + "x": 6.885563480741797, + "y": 6.758587731811697 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Center Intake Location" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5219917012448098, + "rotationDegrees": 87.03163957112189 + }, + { + "waypointRelativePos": 0.8144989339019161, + "rotationDegrees": -62.3466986889636 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -62.80891001618222 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 91.59114027035312 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Straight Forward.path b/src/main/deploy/pathplanner/paths/Left Straight Forward.path new file mode 100644 index 0000000..93894da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Straight Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.338366666666666 + }, + "prevControl": null, + "nextControl": { + "x": 5.0, + "y": 7.338366666666666 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.648744444444444, + "y": 7.338366666666666 + }, + "prevControl": { + "x": 4.648744444444444, + "y": 7.338366666666666 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Straight back.path b/src/main/deploy/pathplanner/paths/Left Straight back.path new file mode 100644 index 0000000..80a0060 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Straight back.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.348111111111112 + }, + "prevControl": null, + "nextControl": { + "x": 3.5439444444444446, + "y": 7.3675999999999995 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.037233333333333, + "y": 7.348111111111112 + }, + "prevControl": { + "x": 3.592666666666667, + "y": 7.3675999999999995 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Side Center Return.path b/src/main/deploy/pathplanner/paths/Right Side Center Return.path new file mode 100644 index 0000000..3988ff2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Side Center Return.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.416048502139798, + "y": 2.4888302425106996 + }, + "prevControl": null, + "nextControl": { + "x": 6.295437391028688, + "y": 1.3000080202884772 + }, + "isLocked": false, + "linkedName": "Right Center Intake Location" + }, + { + "anchor": { + "x": 3.8579172610556345, + "y": 0.8197432239657632 + }, + "prevControl": { + "x": 6.14786170550008, + "y": 0.7320432239657635 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Auto Start Position" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -88.52805548427246 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -112.83365417791754 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 61.78264414527067 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Side To Center.path b/src/main/deploy/pathplanner/paths/Right Side To Center.path new file mode 100644 index 0000000..adbbc32 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Side To Center.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8579172610556345, + "y": 0.8197432239657632 + }, + "prevControl": null, + "nextControl": { + "x": 7.190517261055635, + "y": 0.8197432239657632 + }, + "isLocked": false, + "linkedName": "Right Auto Start Position" + }, + { + "anchor": { + "x": 7.416048502139798, + "y": 2.4888302425106996 + }, + "prevControl": { + "x": 6.76317072436202, + "y": 1.2902635758440317 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Center Intake Location" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2746887966804946, + "rotationDegrees": -92.14563506328979 + }, + { + "waypointRelativePos": 0.7526970954356819, + "rotationDegrees": 59.821891467790174 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 61.78264414527067 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -112.83365417791754 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Straight Back.path b/src/main/deploy/pathplanner/paths/Right Straight Back.path new file mode 100644 index 0000000..2b41d0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Straight Back.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.807044444444445, + "y": 0.6829111111111104 + }, + "prevControl": null, + "nextControl": { + "x": 3.2808444444444445, + "y": 0.6634222222222216 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6474555555555552, + "y": 0.6829111111111104 + }, + "prevControl": { + "x": 3.290588888888889, + "y": 0.6829111111111104 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Straight Forward.path b/src/main/deploy/pathplanner/paths/Right Straight Forward.path new file mode 100644 index 0000000..aa1b78e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Straight Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.663837375178317, + "y": 0.6341888888888878 + }, + "prevControl": null, + "nextControl": { + "x": 4.663837375178316, + "y": 0.6341888888888878 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.288200000000001, + "y": 0.6341888888888878 + }, + "prevControl": { + "x": 4.288200000000001, + "y": 0.6341888888888879 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 8e445b7..bfb85f8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,23 +9,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 50.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, - "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, + "driveWheelRadius": 0.076, + "driveGearing": 4.5, + "maxDriveSpeed": 4.0, + "driveMotorType": "NEO", + "driveCurrentLimit": 80.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.294, + "flModuleY": 0.294, + "frModuleX": 0.294, + "frModuleY": -0.294, + "blModuleX": -0.294, + "blModuleY": 0.294, + "brModuleX": -0.294, + "brModuleY": -0.294, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0cc2936..44cda3e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,14 +3,13 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import java.util.HashMap; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -19,6 +18,8 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.Measure; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; @@ -29,27 +30,14 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; - -import java.lang.reflect.Field; -import java.util.HashMap; - -import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.DistanceUnit; -import edu.wpi.first.units.PerUnit; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Per; import edu.wpi.first.units.measure.Time; -import edu.wpi.first.units.measure.Velocity; import frc.robot.utils.ShootingEntry; /** @@ -68,7 +56,7 @@ public static final class DriveConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds - public static final LinearVelocity kMaxSpeed = MetersPerSecond.of(2.4); + public static final LinearVelocity kMaxSpeed = MetersPerSecond.of(4.0); public static final LinearAcceleration kMaxAcceleration = MetersPerSecondPerSecond.of(10.0); public static final AngularVelocity kMaxAngularSpeed = RadiansPerSecond.of(2 * Math.PI); @@ -76,9 +64,9 @@ public static final class DriveConstants { .of(4 * Math.PI); // Chassis configuration - public static final double kTrackWidth = Units.inchesToMeters(26.5); + public static final double kTrackWidth = Units.inchesToMeters(23.149606); // Distance between centers of right and left wheels on robot - public static final double kWheelBase = Units.inchesToMeters(26.5); + public static final double kWheelBase = Units.inchesToMeters(23.149606); // Distance between front and back wheels on robot public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( new Translation2d(kWheelBase / 2, kTrackWidth / 2), @@ -130,15 +118,17 @@ public static final class ModuleConstants { // more teeth will result in a robot that drives faster). public static final int kDrivingMotorPinionTeeth = 14; + public static final int kSpurGearTeeth = 21; // Calculations required for driving motor conversion factors and feed forward public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; public static final double kWheelDiameterMeters = 0.0762; public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; - // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 + // 45 teeth on the wheel's bevel gear, 21 teeth on the first-stage spur gear, 14 // teeth on the bevel pinion - public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDrivingMotorReduction = (45.0 * kSpurGearTeeth) + / (kDrivingMotorPinionTeeth * 15); public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; @@ -154,8 +144,8 @@ public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecond = 2 * Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = 2 * Math.PI; public static final double kPXController = 1; public static final double kPYController = 1; @@ -170,6 +160,7 @@ public static final class AutoConstants { // Auto Names public static final String kExampleAutoName = "Example Auto"; + public static final Time kShootTime = Seconds.of(3.5); } public static final class NeoMotorConstants { @@ -188,8 +179,9 @@ public static final class VisionConstants { // Confidence of encoder readings for vision; should be tuned public static final double kEncoderConfidence = 0.15; - public static final Transform3d kRobotToCamOne = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + public static final Transform3d kRobotToCamOne = new Transform3d( + new Translation3d(Inches.of(-3.854), Inches.of(-4.358), Inches.of(20.585)), + new Rotation3d(0, 180 - 23, 0)); // These are not final numbers public static final Transform3d kRobotToCamTwo = new Transform3d( @@ -203,8 +195,8 @@ public static final class VisionConstants { public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); public static final Distance kCameraTwoZ = Inches.of(18.0); - public static final Translation3d kTurretCenterOfRotation = new Translation3d(Meters.of(0.0), - Meters.of(0.0), + public static final Translation3d kTurretCenterOfRotation = new Translation3d(Inches.of(-6.25), + Inches.of(6.151), Inches.of(18)); public static final Angle kCameraTwoPitch = Degrees.of(15.0); @@ -225,6 +217,8 @@ public static final class NumericalConstants { public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); public static final Angle kNoRotation = Radians.of(0.0); public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.807); + public static final Angle kHalfRotation = Degrees.of(180); + public static final AngularVelocity kNoRotations = RPM.of(0.0); } public static final class TurretConstants { @@ -232,10 +226,10 @@ public static final class TurretConstants { public static final Angle kMinAngle = Rotations.of(0.1); public static final Angle kMaxAngle = Rotations.of(0.854); - public static final int kPositionBufferLength = 4000; + public static final int kPositionBufferLength = 300; public static final Time kEncoderReadingDelay = Seconds.of(0.005); - public static final Time kEncoderReadInterval = Seconds.of(0.01); + public static final Time kEncoderReadInterval = Seconds.of(0.05); public static final double kP = 1.5; public static final double kI = 0.0; @@ -267,15 +261,23 @@ public static final class TurretConstants { public static final Angle kOvershootAmount = Degrees.of(10.0); - public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); + public static final Translation2d kTurretOffset = new Translation2d(Inches.of(-6.25), + Inches.of(6.151)); + + public static final Angle kTurretAngularOffset = Radians + .of(Math.atan2(kTurretOffset.getY(), kTurretOffset.getX())); + + public static final Distance kTurretCenterDistanceFromRobotCenter = Meters + .of(Math.sqrt(Math.pow(kTurretOffset.getX(), 2.0) + + Math.pow(kTurretOffset.getY(), 2.0))); public static final Angle kTurretAngleTolerance = Degrees.of(2.0); - // TODO: Change to real numbers - public static Angle kNonAimTurretAngle = Degrees.of(25.0); + public static Angle kNonAimTurretAngle = Degrees.of(0.0); public static int kTurretMotorAmpLimit = 10; + public static final Angle kTurretTorwardsFront = Degrees.of(180); - public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.605); + public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.629); } public static final class ShooterConstants { @@ -286,9 +288,12 @@ public static final class ShooterConstants { public static final double kHoodI = 0.0; public static final double kHoodD = 0.0; - public static final double kShooterP = 0.1; + public static final double kShooterP = 0.0001; public static final double kShooterI = 0.0; public static final double kShooterD = 0.0; + public static final double kShooterFF = 0.0019; + + public static final AngularVelocity kShooterVelocityTolerance = RPM.of(50); // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part // NOTE: Need to use 14D so the result is a double, otherwise you end up with @@ -298,32 +303,36 @@ public static final class ShooterConstants { // NOTE: gear ration commented out for now is it isn't used // public static final double kHoodGearRatio = (62D / 25) * (14D / 218); public static final int kHoodSmartCurrentLimit = 20; - public static final Angle kFeedAngle = Degrees.of(90.0); + public static final Angle kFeedAngle = Degrees.of(25.0); public static final AngularVelocity kPlaceholderWheelVelocity = RPM.of(2000); public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); + public static final Distance kHubRobotTurretOffset = Inches.of(47); + public static final ShootingEntry[] kShootingEntries = { - new ShootingEntry(Meters.of(0.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(1.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(2.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + new ShootingEntry(Inches.of(30).plus(kHubRobotTurretOffset), RPM.of(3519), null, + Inches.of(101.9), + Seconds.of(0.812), + Degrees.of(0)), + new ShootingEntry(Inches.of(59).plus(kHubRobotTurretOffset), RPM.of(3565), null, + Inches.of(102.335), + Seconds.of(0.822), + Degrees.of(0)), + new ShootingEntry(Inches.of(89).plus(kHubRobotTurretOffset), RPM.of(3975), null, + Inches.of(120.62), Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(3.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(4.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(5.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), + Degrees.of(0)), + new ShootingEntry(Inches.of(122).plus(kHubRobotTurretOffset), RPM.of(4375), null, + Inches.of(134.055333), + Seconds.of(1.217), + Degrees.of(0)), + new ShootingEntry(Inches.of(148).plus(kHubRobotTurretOffset), RPM.of(4600), null, + Inches.of(147.348333), + Seconds.of(1.322), + kFeedAngle) }; public static final Angle kHoodTolerence = Degrees.of(2.0); @@ -336,22 +345,40 @@ public static final class ShooterConstants { public static final double kHoodDegreeConversionFactor = kHoodMaxAbsolutePosition / 30; // TODO: Change to real numbers - public static AngularVelocity kNonAimShooterVelocity = RPM.of(500); - public static Angle kNonAimHoodAngle = Degrees.of(15.0); - public static AngularVelocity kFeedingWheelVelocity = RPM.of(60); - public static Angle kHoodFeedingPosition = Degrees.of(25.0); - public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); + public static final AngularVelocity kNonAimShooterVelocity = RPM.of(2000); + public static final Angle kNonAimHoodAngle = Degrees.of(15.0); + public static final AngularVelocity kFeedingWheelVelocity = RPM.of(4000); + public static final Angle kHoodFeedingPosition = Degrees.of(25.0); + public static final Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); + + public static final Angle kHoodStartingAngle = Degrees.of(3.0); + public static final AngularVelocity kShooterStartVelocity = RPM.of(0.0); + public static final Angle kDefaultHoodPosition = Degrees.of(3.0); + + public static final Time kRampTime = Seconds.of(0.4); + + // Absolute encoder wraps backwards, so it doesn't read -0.001, it reads 0.999. + // This is the min rotational amount where we can reasonably assume that the + // hood has just gone backwards a little too far, beyond the zero of the encoder + public static final Angle kWrapBackMin = Rotations.of(0.9); + public static final Time kAutoShootTime = Seconds.of(4.0); + public static final AngularVelocity kDefaultShooterVelocity = RPM.of(4000); } public static final class StagingConstants { + public static final int kAgitationAmpLimit = 4; + public static int kFeedIntoHoodMotor = 16; - public static double kFeedIntoHoodSpeed = 0.10; + public static double kFeedIntoHoodSpeed = 0.5; + public static double kReverseFeedSpeed = -0.1; public static final int kAgitationMotorId = 9; - public static final double kAgitationSpeed = -0.15; + public static final double kAgitationSpeed = -0.75; + public static double kReverseAgitationSpeed = 0.1; public static final int kRollerMotorId = 12; - public static final double kRollerSpeed = 0.25; + public static final double kRollerSpeed = 0.95; + public static double kReverseRollingSpeed = -0.1; } public static final class Fixtures { @@ -385,16 +412,11 @@ public static enum FieldLocations { } // Placeholders - public static final Angle kBlueLeftSideFeedHeading = Degrees.of(40); - public static final Angle kBlueRightSideFeedHeading = Degrees.of(160); + public static final Angle kFeedOffset = Degrees.of(12); - // Placeholders - public static final Angle kRedLeftSideFeedHeading = Degrees.of(-40); - public static final Angle kRedRightSideFeedHeading = Degrees.of(-160); - - public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout - .loadField(AprilTagFields.k2026RebuiltAndymark) - .getTagPose(3).get().toPose2d(); + public static final Translation2d kRedHubAprilTag = AprilTagFieldLayout + .loadField(AprilTagFields.k2026RebuiltWelded) // TODO: Change to normal field + .getTagPose(3).get().toPose2d().getTranslation(); } public static final class IntakeConstants { @@ -416,15 +438,29 @@ public static final class IntakeConstants { public static final int kIntakeMotorId = 7; // 10 teeth on pinion, 20 teeth on rack - public static final Angle kDeployRotations = Rotations.of(9.5); + public static final Angle kDeployRotations = Rotations.of(9.6); public static final Angle kRetractRotations = Rotations.of(0.0); - public static final Angle kMaxExtension = Rotations.of(9.5); + public static final Angle kMaxExtension = Rotations.of(9.6); public static final Angle kMinExtension = Rotations.of(0.0); - public static final int kDeployMotorCurrentLimit = 40; + public static final int kDeployMotorCurrentLimit = 60; public static final int kIntakeMotorCurrentLimit = 80; - public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2000); + public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2200); + } + + public static final class ClimberConstants { + public static final int kMotorCanId = 17; + + public static final double kUpVelocity = 0.2; + public static final double kDownVelocity = -0.1; + + public static final double klimitMaxExtension = 3.0; + public static final double kLimitMinExtension = 0.0; + // TODO : Get the Constants for max and minimum distance for the climber + + public static final double kMaxExtension = klimitMaxExtension - 0.1; + public static final double kMinExtension = kLimitMinExtension + 0.1; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b0deed2..5c88499 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,8 +16,8 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); - addPeriodic(m_robotContainer.pushTurretEncoderReading(), - Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); + // addPeriodic(m_robotContainer.pushTurretEncoderReading(), + // Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); } @Override @@ -43,12 +43,13 @@ public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { - CommandScheduler.getInstance().schedule(m_autonomousCommand); + m_autonomousCommand.schedule(); } } @Override public void autonomousPeriodic() { + m_robotContainer.teleopPeriodic(); } @Override @@ -60,6 +61,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + + m_robotContainer.teleopInit(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6e0b03c..7acaedf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,202 +7,304 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; - +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import dev.doglog.DogLog; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; 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.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.Fixtures; -import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.OIConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.commands.AimCommandFactory; +import frc.robot.commands.CommandFactory; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.*; public class RobotContainer { - - private final IntakeSubsystem m_intake = new IntakeSubsystem(); - - private final CommandXboxController m_driverController = new CommandXboxController( - OIConstants.kDriverControllerPort); - - private String m_autoSelected; - private final SendableChooser m_chooser = new SendableChooser<>(); - - private final TurretSubsystem m_turret = new TurretSubsystem(); - private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - private final DriveSubsystem m_drive = new DriveSubsystem(m_turret::getRotationAtTime); - - AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret, m_shooter); - Field2d m_field; - - // For getting data points for the lookup table - Angle commandedShooterAngle; - AngularVelocity commandedWheelVelocity; - - public RobotContainer() { - m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); - SmartDashboard.putData("Auto Choices", m_chooser); - - NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); - // SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); - // SmartDashboard.putNumber("Turret angle in degrees", 0.0); - - // Configure the button bindings - configureBindings(); - - // Configure default commands - m_drive.setDefaultCommand( - // The left stick controls translation of the robot. - // Turning is controlled by the X axis of the right stick. - new RunCommand( - () -> m_drive.drive( - -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), - true), - m_drive)); - - m_field = m_drive.getField(); - } - - private void configureBindings() { - // m_driverController.a() - // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - - // m_driverController.b() - // .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle - // in degrees", 0.0)), - // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); - - // m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); - - // System.out.println("Bindings configured"); - // m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); - - // m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - - // m_driverController.x().whileTrue( - // m_aimFactory.Shoot(ShooterConstants.kFeedingWheelVelocity) - // .finallyDo(() -> m_aimFactory.Shoot(RPM.of(0.0)))); - - // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - - m_driverController.x().onTrue(DeployIntake()); - - m_driverController.a().onTrue(retractIntake()); - - m_driverController.b().whileTrue(spinIntake()); - - m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - - } - - public Command getAutonomousCommand() { - m_autoSelected = m_chooser.getSelected(); - - System.out.print(m_autoSelected); - - return new PathPlannerAuto(m_autoSelected); - } - - public Runnable pushTurretEncoderReading() { - return () -> { - m_turret.pushCurrentEncoderReading(); - }; - } - - public Command feedPosition(Alliance alliance) { - return new RunCommand(() -> { - - }, m_drive, m_turret); - } - - public Command spinIntake() { - return new RunCommand(() -> { - m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); - }).finallyDo(m_intake::stopIntake); - } - - public Command retractIntake() { - return new InstantCommand(() -> { - m_intake.retractIntake(); - }); - } - - public Command outTake() { - return new RunCommand(() -> { - m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); - }); - } - - public Command DeployIntake() { - return new InstantCommand(() -> { - m_intake.deployIntake(); - }); - - } - - public Command SpinIntake() { - return new InstantCommand(() -> { - m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); - }); - } - - public void teleopPeriodic() { - m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); - - TargetSolution solution = m_aimFactory.GetHubAimSolution(); - - Pose2d robotPose = m_drive.getPose(); - - Distance xDist = Meters.of(solution.distance().in(Meters) - * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureX()); - Distance yDist = Meters.of(solution.distance().in(Meters) - * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureY()); - - Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); - - m_field.getObject("targetPose").setPose(targetPose); - } - - public void periodic() { - commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); - commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); - - // System.out.println(m_drive.getRobotLocation()); - } - - private Angle getSmartdashBoardRequestedShooterAngle() { - return Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); - } - - private AngularVelocity getSmartdashboardRequestedWheelSpeed() { - return RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); - } - - private Angle getSmartdashBoardRequestedTurretAngle() { - return Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)); - } + private final CommandXboxController m_driverController = new CommandXboxController( + OIConstants.kDriverControllerPort); + + private String m_autoSelected; + + private final TurretSubsystem m_turret = new TurretSubsystem(); + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + private final DriveSubsystem m_drive; + private boolean m_intakeOut = false; + + CommandFactory m_commandFactory; + Field2d m_field; + + // For getting data points for the lookup table + Angle commandedShooterAngle; + AngularVelocity commandedWheelVelocity; + + DoubleSubscriber m_hoodAngleGetter = DogLog.tunable("Hood Angle (in degrees)", + ShooterConstants.kHoodStartingAngle); + DoubleSubscriber m_shooterVelocityGetter = DogLog.tunable("Motor Velocity (in RPM)", + ShooterConstants.kShooterStartVelocity); + + BooleanSubscriber m_zeroGyroGetter = DogLog.tunable("Zero Gyro", false); + + SendableChooser m_sendable = new SendableChooser<>(); + + public RobotContainer() { + m_drive = new DriveSubsystem(m_turret::getRotationAtTime); + m_commandFactory = new CommandFactory(); + m_commandFactory.SetSubsystems(m_drive, m_turret, m_shooter); + + NamedCommands.registerCommand("Deploy Intake", + m_commandFactory.DeployIntake()); + NamedCommands.registerCommand("Retract Intake", + m_commandFactory.RetractIntake()); + NamedCommands.registerCommand("Aim", m_commandFactory.AutoAimAtHubCommand()); + NamedCommands.registerCommand("Shoot", + m_commandFactory.ShootCommand().alongWith(m_commandFactory.RunAllStager()) + .finallyDo(() -> { + m_commandFactory.StopStaging(); + m_commandFactory.StopShoot(); + }).raceWith(new WaitCommand(AutoConstants.kShootTime))); + + SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); + SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); + SmartDashboard.putNumber("Turret angle in degrees", 0.0); + + m_sendable.addOption("Right Forward Auto", "Right Forward Auto"); + m_sendable.addOption("Left Forward Auto", "Left Forward Auto"); + m_sendable.addOption("Right Backwards Auto", "Right Backwards Auto"); + m_sendable.addOption("Left Backwards Auto", "Left Backwards Auto"); + m_sendable.addOption("Left Side Neutral Auto", "Left Side Neutral Auto"); + m_sendable.addOption("Right Side Neutral Auto", "Right Side Neutral Auto"); + m_sendable.setDefaultOption("Right Forward Auto", "Right Forward Auto"); + m_sendable.addOption("Simple Shoot Auto", "Simple Shoot Auto"); + + // Configure the button bindings + configureBindings(); + + // Configure default commands + // m_drive.setDefaultCommand( + // // The left stick controls translation of the robot. + // // Turning is controlled by the X axis of the right stick. + // new RunCommand( + // () -> m_drive.drive( + // -MathUtil.applyDeadband(m_driverController.getLeftY(), + // OIConstants.kDriveDeadband), + // -MathUtil.applyDeadband(m_driverController.getLeftX(), + // OIConstants.kDriveDeadband), + // -MathUtil.applyDeadband(m_driverController.getRightX(), + // OIConstants.kDriveDeadband), + // true), + // m_drive)); + + m_field = m_drive.getField(); + SmartDashboard.putData(m_sendable); + } + + private void configureBindings() { + // m_driverController.a() + // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + + // m_driverController.b() + // .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle + // in degrees", 0.0)), + // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); + + // m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); + + // System.out.println("Bindings configured"); + // m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); + + // m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + + // m_driverController.x().whileTrue( + // m_aimFactory.Shoot(ShooterConstants.kFeedingWheelVelocity) + // .finallyDo(() -> m_aimFactory.Shoot(RPM.of(0.0)))); + + // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + + // m_driverController.rightBumper().whileTrue(m_aimFactory.AimCommand(false)); + // m_driverController.leftBumper().whileTrue(m_aimFactory.AimCommand(true)); + + // m_driverController.a().whileTrue(m_aimFactory.RunAllStager()); + + // m_driverController.y().onTrue(new InstantCommand(() -> { + // double shooterVelocity = m_shooterVelocityGetter.get(); + // m_aimFactory.ShootAtVelocity(RPM.of(shooterVelocity)); + // System.out.println("Shooting at velocity of " + shooterVelocity + " RPM."); + // }).andThen(new + // WaitCommand(ShooterConstants.kRampTime)).andThen(m_aimFactory.RunAllStager()) + // .finallyDo(m_aimFactory::StopShoot)); + + m_driverController.leftBumper().whileTrue(m_commandFactory.AimCommand(true)) + .onFalse(m_commandFactory.StopAimCommand()); + m_driverController.rightBumper().whileTrue(m_commandFactory.AimCommand(false)) + .onFalse(m_commandFactory.StopAimCommand()); + + m_driverController.rightTrigger().whileTrue(m_commandFactory.RunAllStager()) + .onTrue(Commands.waitUntil(m_shooter::AtWheelVelocityTarget).andThen( + m_commandFactory.ShootCommand().until( + () -> m_driverController.rightTrigger() + .getAsBoolean() == false))) + .onFalse(m_commandFactory.StopShootCommand() + .alongWith(m_commandFactory.StopStagingCommand())); + + m_driverController.a().onTrue(new InstantCommand(m_drive::ZeroGyro)); + + m_driverController.x().whileTrue(m_commandFactory.MoveTurretToFront()); + m_driverController.y().onTrue(m_commandFactory.ReverseStager()) + .onFalse(m_commandFactory.StopStagingCommand()); + + // m_driverController.povUp().whileTrue(m_commandFactory.ClimbUpCommand()); + // m_driverController.povDown().whileTrue(m_commandFactory.ClimbDownCommand()); + + // m_driverController.leftTrigger() + // .onTrue(m_commandFactory.DeployIntake().alongWith(m_commandFactory.SpinIntake())) + // .onFalse(m_commandFactory.RetractIntake().alongWith(m_commandFactory.StopIntake())); + + m_driverController.leftTrigger() + .onTrue(new ConditionalCommand( + m_commandFactory.DeployIntake() + .alongWith(m_commandFactory.SpinIntake()), + m_commandFactory.RetractIntake() + .alongWith(m_commandFactory.StopIntake()), + () -> { + m_intakeOut = !m_intakeOut; + return m_intakeOut; + })); + + // m_driverController.povUp() + // .whileTrue(m_commandFactory.ClimbDownCommand().finallyDo(m_commandFactory::StopClimb)); + // m_driverController.povDown() + // .whileTrue(m_commandFactory.ClimbUpCommand().finallyDo(m_commandFactory::StopClimb)); + + // m_driverController.x().onTrue(new InstantCommand(() -> { + // // double hoodAngle = m_hoodAngleGetter.get(); + // // m_aimFactory.MoveHoodToAngle(Degrees.of(hoodAngle)); + // })); + + // m_driverController.a().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(15))); + + // m_driverController.b().onTrue(m_aimFactory.ShootCommand()).onFalse(m_aimFactory.StopShoot()); + + } + + public Command getAutonomousCommand() { + m_autoSelected = m_sendable.getSelected(); + + DogLog.log("Auto Selected", m_autoSelected); + + return new PathPlannerAuto(m_autoSelected); + } + + public Runnable pushTurretEncoderReading() { + return () -> { + m_turret.pushCurrentEncoderReading(); + }; + } + + public Command feedPosition(Alliance alliance) { + return new RunCommand(() -> { + + }, m_drive, m_turret); + } + + public void teleopPeriodic() { + DogLog.log("In Teleop Periodic Robotcontainer", true); + m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); + + // double solutionStart = Timer.getFPGATimestamp(); + // TargetSolution solution = m_commandFactory.GetHubAimSolution(); + // double solutionEnd = Timer.getFPGATimestamp(); + + // Pose2d robotPose = m_drive.getPose(); + + // Distance xDist = Meters.of(solution.distance().in(Meters) + // * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))) + // .plus(robotPose.getMeasureX()); + // Distance yDist = Meters.of(solution.distance().in(Meters) + // * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))) + // .plus(robotPose.getMeasureY()); + + // Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); + + // m_field.getObject("targetPose").setPose(targetPose); + + double start = Timer.getFPGATimestamp(); + m_commandFactory.periodic(); + double end = Timer.getFPGATimestamp(); + + if (m_zeroGyroGetter.get()) { + m_drive.ZeroGyro(); + } + + DogLog.log("Drivetrain command", + m_drive.getCurrentCommand() == null ? "null" : m_drive.getCurrentCommand().toString()); + DogLog.log("Turret Command", + m_turret.getCurrentCommand() == null ? "null" + : m_turret.getCurrentCommand().toString()); + DogLog.log("Shooter command", m_shooter.getCurrentCommand() == null ? "null" + : m_shooter.getCurrentCommand().toString()); + DogLog.log("Time for command factory periodic in ms", (end - start) * 1000.0); + DogLog.log("In Teleop Periodic Robotcontainer", false); + } + + public void periodic() { + // commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in + // rotations per second", 0.0)); + // commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood + // angle in degrees", 0.0)); + + // DogLog.log("At Shooter Velocity Target", m_shooter.AtWheelVelocityTarget()); + + // System.out.println(m_drive.getRobotLocation()); + } + + private Angle getSmartdashBoardRequestedShooterAngle() { + return Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); + } + + private AngularVelocity getSmartdashboardRequestedWheelSpeed() { + return RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); + } + + private Angle getSmartdashBoardRequestedTurretAngle() { + return Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)); + } + + public void teleopInit() { + // Configure default commands + m_drive.setDefaultCommand( + // The left stick controls translation of the robot. + // Turning is controlled by the X axis of the right stick. + new RunCommand( + () -> m_drive.drive( + -MathUtil.applyDeadband(m_driverController.getLeftY(), + OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getLeftX(), + OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getRightX(), + OIConstants.kDriveDeadband), + true), + m_drive)); + } } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java similarity index 62% rename from src/main/java/frc/robot/commands/AimCommandFactory.java rename to src/main/java/frc/robot/commands/CommandFactory.java index d8397e2..1795cda 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -1,16 +1,12 @@ package frc.robot.commands; -import java.lang.annotation.Target; -import java.lang.reflect.Array; import java.util.ArrayList; import java.util.function.Supplier; +import dev.doglog.DogLog; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -30,24 +26,23 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.Constants.DriveConstants; +import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.Constants.Fixtures; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; -import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.*; -import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.ShootingEntry; import frc.robot.utils.TargetSolution; import frc.robot.utils.UtilityFunctions; -public class AimCommandFactory { +public class CommandFactory { private DriveSubsystem m_drive; private TurretSubsystem m_turret; @@ -55,52 +50,124 @@ public class AimCommandFactory { private boolean m_isAiming = false; - private AngularVelocity m_wheelVelocity = RPM.of(60); + private AngularVelocity m_wheelVelocity = RPM.of(4000); private Translation2d m_lockedTag; private StagingSubsystem m_stager = new StagingSubsystem(); + private IntakeSubsystem m_intake = new IntakeSubsystem(); + private ClimberSubsystem m_climber = new ClimberSubsystem(); - public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { - m_drive = drive; - m_turret = turret; - m_shooter = shooter; + TargetSolution m_solution; + + public CommandFactory() { + // m_drive = drive; + // m_turret = turret; + // m_shooter = shooter; } - public Command AimCommand(Supplier isFeedingLeftSide) { + public Command AimCommand(boolean isFeedingLeftSide) { return new RunCommand(() -> { Aim(isFeedingLeftSide); m_isAiming = true; - }, m_drive, m_turret).finallyDo(() -> { - m_isAiming = false; - m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; + }, m_turret, m_shooter); + } + + public void StopAim() { + m_isAiming = false; + m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); + } + + public void SetSubsystems(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { + m_drive = drive; + m_turret = turret; + m_shooter = shooter; + } + + public Command StopAimCommand() { + return new InstantCommand(this::StopAim); + } + + public void periodic() { + DogLog.log("In periodic command factor", true); + m_solution = GetHubAimSolution(); + + m_wheelVelocity = m_solution.wheelSpeed(); + + DogLog.log("Turret Rotation in deg", m_turret.getRotation().in(Degrees)); + DogLog.log("RPM target", m_wheelVelocity.in(RPM)); + DogLog.log("In periodic command factor", false); + } + + public void AimTurretToFront() { + m_turret.moveToAngle(TurretConstants.kTurretTorwardsFront); + } + + public Command AimTurretToFrontCommand() { + return new InstantCommand(this::AimTurretToFrontCommand); + } + + public Command MoveHoodToDefaultPosition() { + return new InstantCommand(() -> { + m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); }); } - private void Aim(Supplier isFeedingLeftSide) { + public Command MoveTurretToFront() { + return new InstantCommand(() -> { + m_turret.moveToAngle(TurretConstants.kTurretTorwardsFront); + }); + } + + public void AutoAimAtHub() { + TargetSolution solution; + + if (m_solution == null) { + solution = GetHubAimSolution(); + } else { + solution = m_solution; + } + + m_wheelVelocity = solution.wheelSpeed(); + MoveTurretToHeading(solution.hubAngle(), false); + } + + public Command AutoAimAtHubCommand() { + return new InstantCommand(this::AutoAimAtHub); + } + + private void Aim(boolean isFeedingLeftSide) { Fixtures.FieldLocations location = m_drive.getRobotLocation(); switch (location) { case AllianceSide: { - TargetSolution solution = GetHubAimSolution(); - MoveTurretToHeading(solution.hubAngle()); - m_shooter.MoveHoodToPosition(solution.hoodAngle()); + TargetSolution solution; - m_drive.moveByAngle(solution.phi()); + if (m_solution == null) { + solution = GetHubAimSolution(); + } else { + solution = m_solution; + } + MoveTurretToHeading(solution.hubAngle().minus (solution.phi()), true); + // DogLog.log("Range from hub (meters)", solution.distance().in(Meters)); + // System.out.println(solution.phi()); + m_shooter.MoveHoodToPosition(solution.hoodAngle()); m_wheelVelocity = solution.wheelSpeed(); break; } case NeutralSide: { // Heading changes 180 degrees depending on which alliance you are on - Angle offset = DriverStation.getAlliance().get() == Alliance.Blue ? Degrees.of(0) : Degrees.of(180); - Angle absHeading = isFeedingLeftSide.get() ? offset.minus(Degrees.of(50)) - : offset.plus(Degrees.of(50)); + Angle offset = DriverStation.getAlliance().get() == Alliance.Red ? Degrees.of(0) : Degrees.of(180); + Angle absHeading = isFeedingLeftSide ? offset.minus(Fixtures.kFeedOffset) + : offset.plus(Fixtures.kFeedOffset); + + absHeading = UtilityFunctions.WrapAngle(absHeading); m_shooter.MoveHoodToPosition(ShooterConstants.kHoodFeedingPosition); m_wheelVelocity = ShooterConstants.kFeedingWheelVelocity; - MoveTurretToHeading(absHeading); + MoveTurretToHeading(absHeading, true); break; } case OpponentSide: { @@ -114,7 +181,7 @@ private void Aim(Supplier isFeedingLeftSide) { public Command AimHoodToPositionCommand(Angle angle) { return new RunCommand(() -> { m_shooter.MoveHoodToPosition(angle); - }).until(m_shooter::AtTarget); + }).until(m_shooter::AtHoodTarget); } public Command AimTurretRelativeToRobot(Angle angle) { @@ -123,39 +190,99 @@ public Command AimTurretRelativeToRobot(Angle angle) { }, m_turret).until(m_turret::atTarget); } - public Command ShootCommand() { - return new ConditionalCommand(new RunCommand(this::Shoot, m_shooter), - AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) - .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) - .andThen(new RunCommand(this::Shoot, m_shooter)), - () -> m_isAiming).alongWith(new RunCommand(() -> { - m_stager.Agitate(); - m_stager.Feed(); - m_stager.Roll(); - }, m_stager)).finallyDo(m_shooter::Stop); - } - public Command RunAllStager() { - return new RunCommand(() -> { + return new InstantCommand(() -> { m_stager.Agitate(); m_stager.Feed(); m_stager.Roll(); - }, m_stager).finallyDo(() -> { - m_stager.StopAgitate(); - m_stager.StopFeed(); - m_stager.StopRoll(); + }, m_stager); + } + + public Command StopStagingCommand() { + return new InstantCommand(() -> { + StopStaging(); }); } + public void StopStaging() { + m_stager.StopAgitate(); + m_stager.StopFeed(); + m_stager.StopRoll(); + } + private void Shoot() { + // System.out.println(m_wheelVelocity + " is wheel velocity"); m_shooter.Spin(m_wheelVelocity); } + public Command ShootCommand() { + return new RunCommand(() -> { + Shoot(); + }).finallyDo(this::StopShoot); + } + + public void ShootAtVelocity(AngularVelocity velocity) { + m_shooter.Spin(velocity); + } + + public Command StopShootCommand() { + return new InstantCommand(() -> { + StopShoot(); + m_wheelVelocity = NumericalConstants.kNoRotations; + }); + } + + public void StopShoot() { + m_shooter.Stop(); + } + + public Command StopIntake() { + return new InstantCommand(() -> { + m_intake.stopIntake(); + }); + } + + public Command RetractIntake() { + return new InstantCommand(() -> { + m_intake.retractIntake(); + }).andThen(StopIntake()); + } + + public Command OutTake() { + return new InstantCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); + }); + } + + public Command DeployIntake() { + return new InstantCommand(() -> { + m_intake.deployIntake(); + }).alongWith(SpinIntake()); + } + + public Command SpinIntake() { + return new InstantCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); + }); + } + public TargetSolution GetHubAimSolution() { Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; - Translation2d turretTranslation = m_drive.getPose().getTranslation().plus(TurretConstants.kTurretOffset); + Pose2d robotPose = m_drive.getPose(); + + Distance turretX = TurretConstants.kTurretCenterDistanceFromRobotCenter + .times(Math.cos(robotPose.getRotation().getMeasure() + .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))) + .plus(robotPose.getTranslation().getMeasureX()); + + Distance turretY = TurretConstants.kTurretCenterDistanceFromRobotCenter + .times(Math.sin(robotPose.getRotation().getMeasure() + .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))) + .plus(robotPose.getTranslation().getMeasureY()); + + Translation2d turretTranslation = new Translation2d(turretX, turretY); Translation2d translationToHub = hubPosition.minus(turretTranslation); @@ -173,17 +300,21 @@ public TargetSolution GetHubAimSolution() { public Command MoveTurretToHeadingCommand(Angle heading) { return new RunCommand(() -> { - MoveTurretToHeading(heading); + MoveTurretToHeading(heading, true); }, m_turret); } - public Command MoveHoodToAbsoluteCommand(Angle angle) { + public Command MoveHoodToAngleCommand(Angle angle) { return new InstantCommand(() -> { - System.out.println("Move Hood to position " + angle); - m_shooter.MoveHoodToPosition(angle); + MoveHoodToAngle(angle); }); } + public void MoveHoodToAngle(Angle angle) { + // System.out.println("Move Hood to angle " + angle.in(Degrees) + " degrees."); + m_shooter.MoveHoodToPosition(angle); + } + public Command PointAtHub(boolean isRed) { return new RunCommand(() -> { Translation2d hubPosition = isRed ? Fixtures.kRedAllianceHub : Fixtures.kBlueAllianceHub; @@ -194,18 +325,25 @@ public Command PointAtHub(boolean isRed) { Angle angle = Radians.of(Math.atan2(dy, dx)); - MoveTurretToHeading(angle); + MoveTurretToHeading(angle, true); System.out.println(angle); }).finallyDo(m_drive::disableFaceHeading); } - public void MoveTurretToHeading(Angle heading) { + public void MoveTurretToHeading(Angle heading, boolean moveDrivetrain) { + // Weird bug with red side position data + Angle offset = DriverStation.getAlliance().get() == Alliance.Red && Robot.isReal() + ? Constants.NumericalConstants.kHalfRotation + : Constants.NumericalConstants.kNoRotation; + + heading = heading.plus(offset); + Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading)); // Angle[] currentRange = getCurrentTurretRange(); - Angle[] currentRange = TurretConstants.kUnrestrictedAngles; + Angle[] currentRange = getCurrentTurretRange(); if (withinAngles(currentRange, robotRelativeTurretAngle)) { m_turret.moveToAngle(robotRelativeTurretAngle); @@ -215,17 +353,22 @@ public void MoveTurretToHeading(Angle heading) { // The overshoot is negative if the robot has to move in a negative direction; // same for positive - Angle overshoot = robotRelativeTurretAngle.minus(closest).in(Degrees) < 0.0 - ? TurretConstants.kOvershootAmount - : TurretConstants.kOvershootAmount.times(-1.0); - closest = closest.plus(overshoot); + if (moveDrivetrain) { + Angle overshoot = UtilityFunctions.angleDiff(robotRelativeTurretAngle, closest).in(Degrees) < 0.0 + ? TurretConstants.kOvershootAmount + : TurretConstants.kOvershootAmount.times(-1.0); + + closest = closest.plus(overshoot); - Angle driveTarget = heading.minus(closest); + Angle driveTarget = heading.minus(closest); - // System.out.println(); - m_drive.moveToAngle(driveTarget); - m_turret.moveToAngle(closest); + // System.out.println(); + m_drive.moveToAngle(driveTarget); + m_turret.moveToAngle(closest); + } else { + m_turret.moveToAngle(closest); + } } } @@ -238,10 +381,56 @@ public Command PointTurretToFixture(Pose2d fixture) { Angle angle = Radians.of(Math.atan2(dy, dx)).minus(Radians.of(robotPose.getRotation().getRadians())); - MoveTurretToHeading(angle); + MoveTurretToHeading(angle, true); }, m_turret); } + public Command ReverseStager() { + return new InstantCommand(() -> { + m_stager.reverseAgitater(); + m_stager.reverseRoller(); + m_stager.reverseFeeder(); + }); + } + + public void ClimbUp() { + m_climber.climbUp(); + } + + public void ClimbDown() { + m_climber.climbDown(); + } + + public void StopClimb() { + m_climber.Stop(); + } + + public Command MoveTurretToRobotRelativeHeadingCommand(Angle angle) { + return new InstantCommand(() -> { + m_turret.moveToAngle(angle); + }); + } + + public Command ClimbUpCommand() { + // return + // MoveTurretToRobotRelativeHeadingCommand(TurretConstants.kTurretTorwardsFront) + // .alongWith(Commands.waitUntil(m_turret::atTarget)) + + return (new RunCommand(this::ClimbUp)) + // .until(m_climber::atMax) // TODO: put this back + .finallyDo(this::StopClimb); + } + + public Command ClimbDownCommand() { + // return + // MoveTurretToRobotRelativeHeadingCommand(TurretConstants.kTurretTorwardsFront) + // .alongWith(Commands.waitUntil(m_turret::atTarget)) + + return (new RunCommand(this::ClimbDown)) + // .until(m_climber::atMin) TODO: put this back + .finallyDo(this::StopClimb); + } + // Aims the camera at april tags within range public Command IdleCameraAim() { @@ -302,11 +491,11 @@ private static Angle angleFromTranslation(Translation2d reference, Translation2d } private static boolean withinRange(Angle min, Angle max, Angle a) { - a = UtilityFunctions.WrapAngle(a); - min = UtilityFunctions.WrapAngle(min); - max = UtilityFunctions.WrapAngle(max); + Angle a1 = UtilityFunctions.WrapAngle(a); + Angle min1 = UtilityFunctions.WrapAngle(min); + Angle max1 = UtilityFunctions.WrapAngle(max); - return a.gt(min) && a.lt(max); + return a1.gt(min1) && a1.lt(max1); } private static Angle getClosestAngle(Angle a, Angle... others) { @@ -364,10 +553,13 @@ private static Translation2d getClosestAngleApriltag(Angle referenceAngle, Trans } private static int getFirstEntryIndex(Distance distance) { + DogLog.log("In shooting entry search function", true); int low = 0; int high = ShooterConstants.kShootingEntries.length; int mid = 0; + int i = 0; + while (low < high) { mid = (low + high) / 2; ShootingEntry midEntry = ShooterConstants.kShootingEntries[mid]; @@ -377,6 +569,13 @@ private static int getFirstEntryIndex(Distance distance) { } else { high = mid; } + + i++; + + if (i > 20) { + System.err.println("Shooting entry loop has exceeded 20 iterations."); + return 1; + } } ShootingEntry closestEntry = ShooterConstants.kShootingEntries[mid]; @@ -397,6 +596,8 @@ private static int getFirstEntryIndex(Distance distance) { } } + DogLog.log("In shooting entry search function", false); + return previousEntryIndex; } @@ -444,9 +645,21 @@ private static TargetSolution getInterpolatedShootingParameters(Distance distanc secondEntry.distance().in(Meters), firstEntry.shooterAngle().in(Radians), secondEntry.shooterAngle().in(Radians), distance.in(Meters))); + DogLog.log("First Entry", firstEntry.toString()); + DogLog.log("Second Entry", secondEntry.toString()); + + // DogLog.log("Last entry", firstEntry.toString()); + // DogLog.log("Next entry ", secondEntry.toString()); + return new TargetSolution(hoodAngle, wheelSpeed, phi, distance, turretAngle); } + public Command AutoIntakeOut() { + return new InstantCommand(() -> { + + }); + } + public Command Aim(Angle turretAngle, Angle hoodAngle) { return new InstantCommand(() -> { m_turret.moveToAngle(turretAngle); @@ -455,9 +668,9 @@ public Command Aim(Angle turretAngle, Angle hoodAngle) { } public Command Shoot(AngularVelocity shooterWheelVelocity) { - return new InstantCommand(() -> { + return new RunCommand(() -> { m_shooter.Spin(shooterWheelVelocity); - }); + }).finallyDo(m_shooter::Stop); } // TODO: Make this better @@ -467,7 +680,7 @@ public Command Shoot(AngularVelocity shooterWheelVelocity) { // hitting the side of the robot or other balls currently being held in the // robot private Angle[] getCurrentTurretRange() { - if (m_shooter.GetHoodAngle().lt(ShooterConstants.kTurretAngleRestrictiveShooterAngle)) { + if (m_shooter.GetHoodAngle().gt(ShooterConstants.kTurretAngleRestrictiveShooterAngle)) { return TurretConstants.kRestrictedAngles; } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..8c603d7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotations; + +import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.RelativeEncoder; +import frc.robot.Constants.ClimberConstants; + +public class ClimberSubsystem extends SubsystemBase { + SparkMax m_climbMotor = new SparkMax(ClimberConstants.kMotorCanId, MotorType.kBrushless); + + RelativeEncoder m_relativeEncoder = m_climbMotor.getEncoder(); + + private final SparkLimitSwitch m_minLimitSwitch = m_climbMotor.getReverseLimitSwitch(); + private final SparkLimitSwitch m_maxLimitSwitch = m_climbMotor.getForwardLimitSwitch(); + + public double GetPosition() { + return m_relativeEncoder.getPosition(); + } + + public void climbUp() { + + // if (!atMax()) { + if (true) { + m_climbMotor.set(ClimberConstants.kUpVelocity); + } else { + Stop(); + } + } + + public void climbDown() { + + // if (!atMin()) { // TODO: Put this back + if (true) { + m_climbMotor.set(ClimberConstants.kDownVelocity); + } else { + Stop(); + } + } + + public boolean atMax() { + return GetPosition() >= ClimberConstants.kMaxExtension; + } + + public boolean atMin() { + return GetPosition() <= ClimberConstants.kMinExtension; + } + + public void Stop() { + m_climbMotor.set(0.0); + } + + public Command ZeroCommand() { + return new InstantCommand(() -> { + m_relativeEncoder.setPosition(0); + }); + } + + @Override + public void periodic() { + if (m_minLimitSwitch.isPressed()) { + m_relativeEncoder.setPosition(ClimberConstants.kLimitMinExtension); + } else if (m_maxLimitSwitch.isPressed()) { + m_relativeEncoder.setPosition(ClimberConstants.kLimitMinExtension); + } + + // SmartDashboard.putNumber("Climber Position", GetPosition()); + // SmartDashboard.putData("Zero", ZeroCommand()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index bff4d06..2198446 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -15,9 +15,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; @@ -48,6 +45,8 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import dev.doglog.DogLog; + import static edu.wpi.first.units.Units.*; import java.util.Optional; @@ -135,10 +134,12 @@ public DriveSubsystem(Function turretPositionSupplier) { } AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> drive(speeds), // Method that will drive the robot given ROBOT RELATIVE + () -> getPose(), // Robot pose supplier + (Pose2d pose) -> resetOdometry(pose), // Method to reset odometry (will be called if your auto has a + // starting pose) + () -> getRobotRelativeSpeeds(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> drive(speeds, "Path planner"), // Method that will drive the robot given ROBOT + // RELATIVE // ChassisSpeeds. Also optionally outputs individual module // feedforwards new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for @@ -231,7 +232,9 @@ public void disableFaceHeading() { @Override public void periodic() { + DogLog.log("In periodic drive subsystem", true); // Update the odometry in the periodic block + double start = Timer.getFPGATimestamp(); if (Robot.isSimulation()) { ChassisSpeeds chassisSpeed = DriveConstants.kDriveKinematics.toChassisSpeeds( @@ -242,11 +245,11 @@ public void periodic() { m_simPidgey.setSupplyVoltage(RobotController.getBatteryVoltage()); m_simPidgey.setRawYaw( - getHeading().in(Degrees) + Radians.of(chassisSpeed.omegaRadiansPerSecond).in(Degrees) + getGyroHeading().in(Degrees) + Radians.of(chassisSpeed.omegaRadiansPerSecond).in(Degrees) * DriveConstants.kPeriodicInterval.in(Seconds)); m_odometry.update( - new Rotation2d(getHeading()), + new Rotation2d(getGyroHeading()), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -275,7 +278,18 @@ public void periodic() { SmartDashboard.putData(m_field); - SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); + double end = Timer.getFPGATimestamp(); + + DogLog.log("Drivetrain periodic time (ms)", (end - start) * 1000.0); + + DogLog.log("In periodic drive subsystem", false); + + // SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); + + // DogLog.log("X dist to april tag in meters", + // getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getX()); + // DogLog.log("Y dist to april tag in meters", + // getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getY()); } /** @@ -295,13 +309,17 @@ public SwerveModulePosition[] getModulePositions() { }; } + public void ZeroGyro() { + pidgey.setYaw(NumericalConstants.kNoRotation); + } + /** * Resets the odometry to the specified pose. * * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition( + m_poseEstimator.resetPosition( new Rotation2d(pidgey.getYaw().getValue()), new SwerveModulePosition[] { m_frontLeft.getPosition(), @@ -352,23 +370,45 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ // System.out.println("Target " + m_targetAutoAngle + ", Current" + // getHeading()); - final var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(ChassisSpeeds.discretize( + // final var swerveModuleStates = + // DriveConstants.kDriveKinematics.toSwerveModuleStates(ChassisSpeeds.discretize( + // fieldRelative + // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, + // rotDelivered, + // new Rotation2d(getHeading())) + // : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered), + // timeElapsed)); + // SwerveDriveKinematics.desaturateWheelSpeeds( + // swerveModuleStates, DriveConstants.kMaxSpeed.magnitude()); + + var speeds = ChassisSpeeds.discretize( fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, + rotDelivered, new Rotation2d(getHeading())) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered), - timeElapsed)); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.kMaxSpeed.magnitude()); + timeElapsed); + + drive(speeds, "Joystick runner"); - m_frontLeft.setDesiredState(swerveModuleStates[0]); - m_frontRight.setDesiredState(swerveModuleStates[1]); - m_rearLeft.setDesiredState(swerveModuleStates[2]); - m_rearRight.setDesiredState(swerveModuleStates[3]); + // m_frontLeft.setDesiredState(swerveModuleStates[0]); + // m_frontRight.setDesiredState(swerveModuleStates[1]); + // m_rearLeft.setDesiredState(swerveModuleStates[2]); + // m_rearRight.setDesiredState(swerveModuleStates[3]); } - public void drive(ChassisSpeeds speeds) { - setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds)); + public void drive(ChassisSpeeds speeds, String caller) { + var states = DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds); + + DogLog.log("First commanded motor speeds", states[0]); + DogLog.log("Caller", caller); + + SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeed.magnitude()); + + m_frontLeft.setDesiredState(states[0]); + m_frontRight.setDesiredState(states[1]); + m_rearLeft.setDesiredState(states[2]); + m_rearRight.setDesiredState(states[3]); } /** @@ -419,7 +459,6 @@ public void zeroHeading() { */ public Angle getHeading() { return pidgey.getYaw().getValue(); - // TODO: Don't use this code // return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ec9385c..f8ec577 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -59,9 +59,9 @@ public IntakeSubsystem() { .kV(IntakeConstants.kFFIn); // Apparently there is no absolute encoder :( // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_deploy1Config.idleMode(IdleMode.kCoast); - m_deploy2Config.idleMode(IdleMode.kCoast); // TODO: change to brake - m_intakeConfig.idleMode(IdleMode.kBrake); + m_deploy1Config.idleMode(IdleMode.kBrake); + m_deploy2Config.idleMode(IdleMode.kBrake); // TODO: change to brake + m_intakeConfig.idleMode(IdleMode.kCoast); m_deploy1Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); m_deploy2Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); @@ -157,18 +157,20 @@ public void periodic() { // if (m_maxLimitSwitch2.isPressed()) // System.out.println("Max Two"); - DogLog.log("Deploy motor 1: canID 13 position", - m_deploy1RelativeEncoder.getPosition()); - DogLog.log("Deploy motor 2: canID 8 position", - m_deploy2RelativeEncoder.getPosition()); + // DogLog.log("Deploy motor 1: canID 13 position", + // m_deploy1RelativeEncoder.getPosition()); + // DogLog.log("Deploy motor 2: canID 8 position", + // m_deploy2RelativeEncoder.getPosition()); - DogLog.log("Deploy motor can ID 8 setpoint", m_deploy2ClosedLoopController.getSetpoint()); - DogLog.log("Deploy motor can ID 13 setpoint", m_deploy1ClosedLoopController.getSetpoint()); + // DogLog.log("Deploy motor can ID 8 setpoint", + // m_deploy2ClosedLoopController.getSetpoint()); + // DogLog.log("Deploy motor can ID 13 setpoint", + // m_deploy1ClosedLoopController.getSetpoint()); - DogLog.log("Min limit one", m_minLimitSwitch1.isPressed()); - DogLog.log("Min limit two", m_minLimitSwitch2.isPressed()); + // DogLog.log("Min limit one", m_minLimitSwitch1.isPressed()); + // DogLog.log("Min limit two", m_minLimitSwitch2.isPressed()); - DogLog.log("Max limit one", m_maxLimitSwitch1.isPressed()); - DogLog.log("Max limit two", m_maxLimitSwitch2.isPressed()); + // DogLog.log("Max limit one", m_maxLimitSwitch1.isPressed()); + // DogLog.log("Max limit two", m_maxLimitSwitch2.isPressed()); } } diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index c92206f..ca35797 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -9,8 +9,10 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.units.measure.Distance; import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -30,7 +32,7 @@ import frc.robot.Robot; public class MAXSwerveModule { - private final SparkMax m_drivingSpark; + private final SparkFlex m_drivingSpark; private final SparkMax m_turningSpark; private final RelativeEncoder m_drivingEncoder; @@ -53,7 +55,7 @@ public class MAXSwerveModule { * Encoder. */ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { - m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless); + m_drivingSpark = new SparkFlex(drivingCANId, MotorType.kBrushless); m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); m_drivingEncoder = m_drivingSpark.getEncoder(); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 6a5ee2d..883bab5 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,45 +1,56 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.*; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import dev.doglog.DogLog; + +import com.revrobotics.spark.config.SparkMaxConfig; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Rotations; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.utils.UtilityFunctions; public class ShooterSubsystem extends SubsystemBase { SparkMax m_shooterMotor = new SparkMax(ShooterConstants.kShooterMotorId, MotorType.kBrushless); - SparkMax m_hoodMotor = new SparkMax(ShooterConstants.kHoodMotorId, MotorType.kBrushless); + // SparkMax m_hoodMotor = new SparkMax(ShooterConstants.kHoodMotorId, + // MotorType.kBrushless); - AbsoluteEncoder m_absoluteEncoder = m_hoodMotor.getAbsoluteEncoder(); + // AbsoluteEncoder m_absoluteEncoder = m_hoodMotor.getAbsoluteEncoder(); + RelativeEncoder m_shooterRelativeEncoder = m_shooterMotor.getEncoder(); private final SparkClosedLoopController m_shooterClosedLoopController = m_shooterMotor.getClosedLoopController(); - private final SparkClosedLoopController m_hoodClosedLoopController = m_hoodMotor.getClosedLoopController(); + // private final SparkClosedLoopController m_hoodClosedLoopController = + // m_hoodMotor.getClosedLoopController(); private final SparkMaxConfig m_shooterConfig = new SparkMaxConfig(); private final SparkMaxConfig m_hoodConfig = new SparkMaxConfig(); Angle m_targetAngle = Degrees.of(0.0); + AngularVelocity m_targetVelocity = RPM.of(0); public ShooterSubsystem() { m_shooterConfig.closedLoop .p(ShooterConstants.kShooterP) .i(ShooterConstants.kShooterI) - .d(ShooterConstants.kShooterD); + .d(ShooterConstants.kShooterD).feedForward.kV(ShooterConstants.kShooterFF); + m_shooterConfig.idleMode(IdleMode.kCoast); m_hoodConfig.closedLoop .p(ShooterConstants.kHoodP) .i(ShooterConstants.kHoodI) @@ -55,12 +66,13 @@ public ShooterSubsystem() { m_hoodConfig.encoder.positionConversionFactor(1); m_shooterMotor.configure(m_shooterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - m_hoodMotor.configure(m_hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // m_hoodMotor.configure(m_hoodConfig, ResetMode.kResetSafeParameters, + // PersistMode.kPersistParameters); } - // Position between 0 and .55 + // Position between 0 and .55. Disabled hood motor public void MoveHoodToPosition(Angle angle) { - System.out.println("Move hood to position: " + angle); + // System.out.println("Move hood to position: " + angle); // get target absolute encoder position. 0 starts in hood min, hood max is .55 // (30 degrees of movement) @@ -69,35 +81,48 @@ public void MoveHoodToPosition(Angle angle) { System.out.println("Hood target position out of bounds. Target: " + targetPosition); return; } - var curentPosition = m_absoluteEncoder.getPosition(); - System.out.println("current hood: " + curentPosition); - if (curentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { - System.out.println("Hood position icorrect for safe movement. Pos: " + curentPosition); - return; - } - m_hoodClosedLoopController.setSetpoint(targetPosition, ControlType.kPosition); + // var currentPosition = m_absoluteEncoder.getPosition(); + + // if (currentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { + // System.out.println("Hood position incorrect for safe movement. Pos: " + + // currentPosition); + // return; + // } + + // m_hoodClosedLoopController.setSetpoint(targetPosition, + // ControlType.kPosition); } public void Spin(AngularVelocity shootSpeedVelocity) { + m_targetVelocity = shootSpeedVelocity; m_shooterClosedLoopController.setSetpoint(shootSpeedVelocity.in(RPM), ControlType.kVelocity); } public void Stop() { - Spin(RPM.of(0)); + m_shooterClosedLoopController.setSetpoint(0, ControlType.kVelocity); } public Angle GetHoodAngle() { - return Degrees.of(m_absoluteEncoder.getPosition() / ShooterConstants.kHoodDegreeConversionFactor); + // return Degrees.of(m_absoluteEncoder.getPosition() / + // ShooterConstants.kHoodDegreeConversionFactor); + return NumericalConstants.kNoRotation; } - public boolean AtTarget() { - return UtilityFunctions.angleDiff(GetHoodAngle(), m_targetAngle).abs(Degrees) < ShooterConstants.kHoodTolerence - .in(Degrees); + public boolean AtHoodTarget() { + // return m_hoodClosedLoopController.isAtSetpoint(); + return true; + } + + public boolean AtWheelVelocityTarget() { + return RPM.of(m_shooterClosedLoopController.getSetpoint()).minus(RPM.of(m_shooterRelativeEncoder.getVelocity())) + .abs(RPM) < ShooterConstants.kShooterVelocityTolerance.in(RPM); + + // return true; } @Override public void periodic() { - + DogLog.log("Motor velocity setpoint", m_shooterClosedLoopController.getSetpoint()); } } diff --git a/src/main/java/frc/robot/subsystems/StagingSubsystem.java b/src/main/java/frc/robot/subsystems/StagingSubsystem.java index 3087a1f..8908a22 100644 --- a/src/main/java/frc/robot/subsystems/StagingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/StagingSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -11,6 +12,9 @@ public class StagingSubsystem extends SubsystemBase { SparkMax m_agitationMotor = new SparkMax(StagingConstants.kAgitationMotorId, MotorType.kBrushless); SparkMax m_rollerMotor = new SparkMax(StagingConstants.kRollerMotorId, MotorType.kBrushless); + public StagingSubsystem() { + } + public void Agitate() { m_agitationMotor.set(StagingConstants.kAgitationSpeed); } @@ -37,4 +41,16 @@ public void StopFeed() { public void StopRoll() { m_rollerMotor.stopMotor(); } + + public void reverseAgitater() { + m_agitationMotor.set(StagingConstants.kReverseAgitationSpeed); + } + + public void reverseRoller() { + m_rollerMotor.set(StagingConstants.kReverseRollingSpeed); + } + + public void reverseFeeder() { + m_feedIntoHoodMotor.set(StagingConstants.kReverseFeedSpeed); + } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index e7d8101..54b6daf 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -24,6 +24,9 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -31,6 +34,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; import frc.robot.utils.TurretPosition; @@ -66,7 +70,6 @@ public class TurretSubsystem extends SubsystemBase { public TurretSubsystem() { m_config.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); m_config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_config.smartCurrentLimit(TurretConstants.kSmartCurrentLimit); m_config.idleMode(IdleMode.kBrake); m_config.absoluteEncoder.inverted(true); m_config.inverted(true); @@ -101,7 +104,7 @@ public void moveToAngle(Angle angle) { angle = angle.plus(TurretConstants.kAngularDistanceToFrontOfRobot); angle = UtilityFunctions.WrapAngle(angle); - System.out.println(angle + "is commanded angle for turret"); + // System.out.println(angle + "is commanded angle for turret"); if (angle.gt(TurretConstants.kMaxAngle)) { System.out @@ -137,18 +140,30 @@ public TurretPosition getRotationAtTime(double timestamp) { return m_positionBuffer.getAngleAtTime(timestamp); // return new TurretPosition(getRotation(), RotationsPerSecond.of(0.0), // timestamp); + + // return new TurretPosition(NumericalConstants.kNoRotation, + // NumericalConstants.kNoRotations, timestamp); } @Override public void periodic() { + DogLog.log("In periodic turret subsystem", true); + double start = Timer.getFPGATimestamp(); + m_positionBuffer.pushElement( UtilityFunctions.WrapAngle(getRotation()), RPM.of(m_absoluteEncoder.getVelocity()), TurretConstants.kEncoderReadingDelay.in(Seconds)); - DogLog.log("Turret rotation relative to front of robot", getRotation().in(Degrees)); - DogLog.log("Turret rotation relative to turret zero", UtilityFunctions - .WrapAngle(getRotation().minus(TurretConstants.kAngularDistanceToFrontOfRobot)).in(Degrees)); + double end = Timer.getFPGATimestamp(); + + DogLog.log("Turret periodic time (ms)", (end - start) * 1000.0); + DogLog.log("In periodic turret subsystem", false); + + // DogLog.log("Turret rotation relative to front of robot", + // getRotation().in(Degrees)); + // DogLog.log("Turret rotation relative to turret zero", UtilityFunctions + // .WrapAngle(getRotation().minus(TurretConstants.kAngularDistanceToFrontOfRobot)).in(Degrees)); } // Connected to another periodic loop that runs quicker than 0.02 seconds @@ -175,7 +190,6 @@ public void simulationPeriodic() { } public boolean atTarget() { - return UtilityFunctions.angleDiff(m_targetAngle, getRotation()) - .abs(Degrees) < TurretConstants.kTurretAngleTolerance.in(Degrees); + return m_turretClosedLoopController.isAtSetpoint(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java index 15e291a..c4b1040 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -1,5 +1,6 @@ package frc.robot.utils; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.units.measure.Angle; @@ -35,6 +36,8 @@ public TurretPosition getAngleAtTime(double requestedTime) { TurretPosition midpointTurretPosition = null; double timeAtMidpoint = 0.0; + int i = 0; + while (low < high) { try { midpointTurretPosition = m_positions.get(midpoint); @@ -51,6 +54,15 @@ public TurretPosition getAngleAtTime(double requestedTime) { } midpoint = low + (high - low) / 2; + + i++; + + // System.out.println("Turret position at loop " + i); + + if (i > 20) { + System.err.println("Turret position loop has exceeded 20 iterations."); + return new TurretPosition(Degrees.of(40), RPM.of(3500), requestedTime); + } } // Linearly UtilityFunctions.interpolate velocity if we aren't on the first/last diff --git a/src/main/java/frc/robot/utils/RingBuffer.java b/src/main/java/frc/robot/utils/RingBuffer.java index f15f4ac..d683c8f 100644 --- a/src/main/java/frc/robot/utils/RingBuffer.java +++ b/src/main/java/frc/robot/utils/RingBuffer.java @@ -30,9 +30,7 @@ public void push(T element) throws Exception { } } - synchronized (m_elements) { - m_elements[getTrueIndex(m_head++)] = element; - } + m_elements[getTrueIndex(m_head++)] = element; } public T pop() throws Exception { @@ -41,22 +39,18 @@ public T pop() throws Exception { throw new Exception("Cannot pop from empty ring buffer."); } - synchronized (m_elements) { - return (T) m_elements[getTrueIndex(m_tail++)]; - } + return (T) m_elements[getTrueIndex(m_tail++)]; } public T get(int index) throws Exception { - synchronized (m_elements) { - int trueIndex = m_tail + index; - if (trueIndex > m_head) { - System.out.println("Index " + index + " is out of bounds of the ring buffer."); + int trueIndex = m_tail + index; + if (trueIndex > m_head) { + System.out.println("Index " + index + " is out of bounds of the ring buffer."); - throw new Exception("Index " + index + " out of bounds."); - } - - return (T) m_elements[getTrueIndex(trueIndex)]; + throw new Exception("Index " + index + " out of bounds."); } + + return (T) m_elements[getTrueIndex(trueIndex)]; } public int getLength() { diff --git a/src/main/java/frc/robot/utils/ShootingEntry.java b/src/main/java/frc/robot/utils/ShootingEntry.java index 14b19e1..df4fa97 100644 --- a/src/main/java/frc/robot/utils/ShootingEntry.java +++ b/src/main/java/frc/robot/utils/ShootingEntry.java @@ -1,5 +1,11 @@ package frc.robot.utils; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -7,7 +13,14 @@ import edu.wpi.first.units.measure.Time; public record ShootingEntry(Distance distance, AngularVelocity wheelVelocity, LinearVelocity muzzleVelocity, - Distance maxHeight, - Time timeOfFlight, Angle shooterAngle) { + Distance maxHeight, + Time timeOfFlight, Angle shooterAngle) { + @Override + public String toString() { + return "Distance (meters): " + distance.in(Meters) + ", Angular Wheel Velocity (RPM): " + wheelVelocity.in(RPM) + + ", Muzzle Velocity (MPS): " + muzzleVelocity + ", Max height (Meters): " + + maxHeight.in(Meters) + "Time of Flight (seconds): " + timeOfFlight.in(Seconds) + + ", Shooter Angle (Degrees): " + shooterAngle.in(Degrees); + } } diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java index 1e8c7eb..9b94904 100644 --- a/src/main/java/frc/robot/utils/UtilityFunctions.java +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -27,7 +27,7 @@ public static double interpolate(double x1, double x2, double y1, double y2, dou double dy = y2 - y1; double dx = x - x1; - return (dx / Math.abs(dsx) > 0.0 ? dsx : NumericalConstants.kEpsilon) * dy + y1; + return (Math.abs(dsx) > 0.0 ? dx / dsx : NumericalConstants.kEpsilon) * dy + y1; } public static Angle angleDiff(Angle a1, Angle a2) { diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 24ec10b..809e5ae 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -11,6 +11,7 @@ import org.photonvision.PhotonPoseEstimator; import org.photonvision.targeting.PhotonTrackedTarget; +import dev.doglog.DogLog; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation3d; @@ -20,11 +21,13 @@ import edu.wpi.first.math.numbers.N3; import frc.robot.Constants.VisionConstants; import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.Timer; + import static edu.wpi.first.units.Units.*; public class Vision { - PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); + // PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); Optional> m_turretPositionSupplier; @@ -46,22 +49,31 @@ public Vision(Optional> turretPositionSupplier, } public void periodic() { - Optional visionEstimationCameraOne = Optional.empty(); + // Enabled Camera One - for (var result : m_camera1.getAllUnreadResults()) { - visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + // Optional visionEstimationCameraOne = Optional.empty(); - if (visionEstimationCameraOne.isEmpty()) { - visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - } + // for (var result : m_camera1.getAllUnreadResults()) { + // visionEstimationCameraOne = + // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), m_poseEstimatorOne); + // if (visionEstimationCameraOne.isEmpty()) { + // visionEstimationCameraOne = + // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + // } - visionEstimationCameraOne.ifPresent(estimation -> { - m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), - estimation.timestampSeconds, getCurrentStdDevs())); - }); - } + // updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), + // m_poseEstimatorOne); + + // visionEstimationCameraOne.ifPresent(estimation -> { + // m_visionConsumer.accept(new + // VisionEstimation(estimation.estimatedPose.toPose2d(), + // estimation.timestampSeconds, getCurrentStdDevs())); + // }); + // } + + DogLog.log("In periodic vision subsystem", true); + double start = Timer.getFPGATimestamp(); Optional visionEstimationCameraTwo = Optional.empty(); for (var result : m_camera2.getAllUnreadResults()) { @@ -70,7 +82,8 @@ public void periodic() { cameraTransform = getTurretCameraTransform(result.getTimestampSeconds()); if (cameraTransform == null) { - System.out.println("Turret exceeded max velocity valid for reading april tags."); + // System.out.println("Turret exceeded max velocity valid for reading april + // tags."); break; } @@ -91,6 +104,11 @@ public void periodic() { estimation.timestampSeconds, getCurrentStdDevs())); }); } + + double end = Timer.getFPGATimestamp(); + + DogLog.log("Vision periodic loop time (ms)", (end - start) * 1000.0); + DogLog.log("In periodic vision subsystem", false); } private void updateEstimationStdDevs( @@ -139,6 +157,53 @@ private void updateEstimationStdDevs( } } + private void updateEstimationStdDevsLessStable( + Optional estimatedPose, List targets, + PhotonPoseEstimator poseEstimator) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) + continue; + numTags++; + avgDist += tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else + // Less stable, so simply doubles the standard deviation to trust camera less + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)).times(2.0); + curStdDevs = estStdDevs; + } + } + } + private Transform3d getTurretCameraTransform(double estimationTime) { if (m_turretPositionSupplier.isEmpty()) return VisionConstants.kRobotToCamTwo; diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d35e593..a5af3e9 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.3", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index e1fc8bb..fbb5c80 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-3" + "version": "v2026.2.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-3" + "version": "v2026.2.2" } ] } \ No newline at end of file