diff --git a/build.gradle b/build.gradle index ac1ca9e..12ace0a 100644 --- a/build.gradle +++ b/build.gradle @@ -57,6 +57,10 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + implementation "edu.wpi.first.epilogue:epilogue-runtime-java:2025.3.2" + annotationProcessor "edu.wpi.first.epilogue:epilogue-processor-java:2025.3.2" + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..8d4d057 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,102 @@ +{ + "System Joysticks": { + "window": { + "enabled": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000" + } + ] +} diff --git a/src/main/deploy/pathplanner/autos/High shoot inner pick 3.auto b/src/main/deploy/pathplanner/autos/High shoot inner pick 3.auto new file mode 100644 index 0000000..a2d7242 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/High shoot inner pick 3.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "take 3" + } + }, + { + "type": "path", + "data": { + "pathName": "score high 2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Longhot pick 3.auto b/src/main/deploy/pathplanner/autos/Longhot pick 3.auto new file mode 100644 index 0000000..b9a5339 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Longhot pick 3.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "high goal shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "take 3" + } + }, + { + "type": "named", + "data": { + "name": "stop intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Longshot 2" + } + }, + { + "type": "named", + "data": { + "name": "longshot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..fa4a3f3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "wait 5 seconds" + } + }, + { + "type": "named", + "data": { + "name": "high goal shoot" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/high low take 3.auto b/src/main/deploy/pathplanner/autos/high low take 3.auto new file mode 100644 index 0000000..0f4deaa --- /dev/null +++ b/src/main/deploy/pathplanner/autos/high low take 3.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "take 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Low goal setup 2" + } + }, + { + "type": "path", + "data": { + "pathName": "low goal finish" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto new file mode 100644 index 0000000..a72f949 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "high goal shoot" + } + }, + { + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "take 3 flat" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "high goal shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "high shoot flat 2" + } + }, + { + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "path", + "data": { + "pathName": "all the way out 1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/there and back again high shoot.auto b/src/main/deploy/pathplanner/autos/there and back again high shoot.auto new file mode 100644 index 0000000..c67d19e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/there and back again high shoot.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "all the way out 1" + } + }, + { + "type": "path", + "data": { + "pathName": "football otherside intake 2" + } + }, + { + "type": "path", + "data": { + "pathName": "back again 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"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,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,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,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,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,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,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,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,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,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,true,true],[true,true,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,false,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,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,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,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,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,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,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,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,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,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,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,true,true],[true,true,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,false,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,true,true],[true,true,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,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,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,true,true,true,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of take 3 flat.path b/src/main/deploy/pathplanner/paths/Copy of take 3 flat.path new file mode 100644 index 0000000..73f3603 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of take 3 flat.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.571468431122449, + "y": 0.768713177235181 + }, + "prevControl": null, + "nextControl": { + "x": 2.8711705695423007, + "y": 0.8517906782938883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 0.9189798206609033 + }, + "prevControl": { + "x": 2.8546664756846116, + "y": 0.6689868103988942 + }, + "nextControl": { + "x": 2.8630504165726527, + "y": 1.790136649285827 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 7.559346286257558 + }, + "prevControl": { + "x": 2.907278601544784, + "y": 7.278351222216316 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3315587734241905, + "rotationDegrees": -85.69322225973595 + } + ], + "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": -88.78832381236471 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.8782176560898 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Longshot 2.path b/src/main/deploy/pathplanner/paths/Longshot 2.path new file mode 100644 index 0000000..665c0a1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Longshot 2.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.029548910017559, + "y": 2.6727494447605586 + }, + "prevControl": null, + "nextControl": { + "x": 5.029548910017559, + "y": 2.6727494447605586 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.053577929308689, + "y": 1.7699595577663172 + }, + "prevControl": { + "x": 3.8630799422428623, + "y": 1.8427224055175209 + }, + "nextControl": { + "x": 2.2440759163745154, + "y": 1.6971967100151135 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2932306675721814, + "y": 2.1005889667957645 + }, + "prevControl": { + "x": 2.4605355879869943, + "y": 1.9148228137834542 + }, + "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": 133.5222600525553 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -148.9317723414291 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Low goal setup 2.path b/src/main/deploy/pathplanner/paths/Low goal setup 2.path new file mode 100644 index 0000000..8a00061 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Low goal setup 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9788515852769675, + "y": 2.6782297740524776 + }, + "prevControl": null, + "nextControl": { + "x": 3.9513717109767534, + "y": 2.9267148973666197 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.893244351311953, + "y": 0.5683707634839654 + }, + "prevControl": { + "x": 3.142398730089056, + "y": 0.5478257997167371 + }, + "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.47602754348356574 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -154.99751127064283 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/all the way out 1.path b/src/main/deploy/pathplanner/paths/all the way out 1.path new file mode 100644 index 0000000..ad0ce8d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/all the way out 1.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6767644557823125, + "y": 0.6788865099611276 + }, + "prevControl": null, + "nextControl": { + "x": 2.5585178267677686, + "y": 0.8830250850321845 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.929469220724004, + "y": 1.5431300868561715 + }, + "prevControl": { + "x": 6.6013347303206995, + "y": 0.6222762542517009 + }, + "nextControl": { + "x": 8.485788056470975, + "y": 1.9288503100333383 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.85379084669582, + "y": 3.849962418002915 + }, + "prevControl": { + "x": 9.641671316964286, + "y": 3.448848298038523 + }, + "nextControl": { + "x": 10.065910376427352, + "y": 4.251076537967306 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.4638453595724, + "y": 6.89932390366861 + }, + "prevControl": { + "x": 12.4638453595724, + "y": 6.89932390366861 + }, + "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": -179.38320323473243 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.6352770022539 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/back again 3.path b/src/main/deploy/pathplanner/paths/back again 3.path new file mode 100644 index 0000000..3c07019 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/back again 3.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 15.35347956147128, + "y": 6.794882015303399 + }, + "prevControl": null, + "nextControl": { + "x": 13.291499635568512, + "y": 7.536082513362487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.053852344509231, + "y": 4.823725249028183 + }, + "prevControl": { + "x": 12.688312333998129, + "y": 6.57735371673813 + }, + "nextControl": { + "x": 9.051899219509231, + "y": 2.675809721209913 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.3744191873216245, + "y": 0.8991113186318737 + }, + "prevControl": { + "x": 7.723385112977601, + "y": 1.4241678814355665 + }, + "nextControl": { + "x": 5.626398384673274, + "y": 0.6079599825792492 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8807644709707976, + "y": 0.6066645408136047 + }, + "prevControl": { + "x": 3.906724520169048, + "y": 0.5968894254103005 + }, + "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": 178.81963121952444 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.3933329278013 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/football otherside intake 2.path b/src/main/deploy/pathplanner/paths/football otherside intake 2.path new file mode 100644 index 0000000..29200ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/football otherside intake 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 13.506267462342079, + "y": 6.9238565962099115 + }, + "prevControl": null, + "nextControl": { + "x": 14.50626746234208, + "y": 6.9238565962099115 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.412414965986393, + "y": 6.8083109359815355 + }, + "prevControl": { + "x": 14.412414965986393, + "y": 6.8083109359815355 + }, + "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": -179.30072257922677 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.76827221562849 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/high shoot flat 2.path b/src/main/deploy/pathplanner/paths/high shoot flat 2.path new file mode 100644 index 0000000..f8fd0cd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/high shoot flat 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8983837078668273, + "y": 2.199665418467734 + }, + "prevControl": null, + "nextControl": { + "x": 3.8983837078668278, + "y": 2.199665418467734 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.619250239158163, + "y": 0.6240752920091648 + }, + "prevControl": { + "x": 1.9235272396719103, + "y": 0.5585276862481111 + }, + "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": -179.1356387852817 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -88.28325926782887 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/low goal finish.path b/src/main/deploy/pathplanner/paths/low goal finish.path new file mode 100644 index 0000000..d30ffa1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/low goal finish.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8964236364188527, + "y": 0.6204256256073871 + }, + "prevControl": null, + "nextControl": { + "x": 2.4294008898202137, + "y": 0.6153008078231296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5683840500485906, + "y": 0.6204256256073871 + }, + "prevControl": { + "x": 2.197550337099125, + "y": 0.6510321762633635 + }, + "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/score high 2.path b/src/main/deploy/pathplanner/paths/score high 2.path new file mode 100644 index 0000000..57b3c80 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/score high 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.978519421161321, + "y": 2.7376871507531586 + }, + "prevControl": null, + "nextControl": { + "x": 3.84197650924759, + "y": 2.5282688296680176 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5968078079446062, + "y": 0.6112199344023327 + }, + "prevControl": { + "x": 1.863816030994844, + "y": 0.8564793673544928 + }, + "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": -178.75297650895942 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -148.72053264780297 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/take 3 flat.path b/src/main/deploy/pathplanner/paths/take 3 flat.path new file mode 100644 index 0000000..0ffb4ca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/take 3 flat.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.571468431122449, + "y": 0.768713177235181 + }, + "prevControl": null, + "nextControl": { + "x": 2.8711705695423007, + "y": 0.8517906782938883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 0.9189798206609033 + }, + "prevControl": { + "x": 2.8546664756846116, + "y": 0.6689868103988942 + }, + "nextControl": { + "x": 2.8630504165726527, + "y": 1.790136649285827 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 2.17686106017928 + }, + "prevControl": { + "x": 2.907278601544784, + "y": 1.8958659961380382 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3315587734241905, + "rotationDegrees": -85.69322225973595 + } + ], + "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": -88.78832381236471 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.8782176560898 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/take 3.path b/src/main/deploy/pathplanner/paths/take 3.path new file mode 100644 index 0000000..17dd9c3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/take 3.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6015235433759782, + "y": 0.6497773688686587 + }, + "prevControl": null, + "nextControl": { + "x": 2.3003049992963907, + "y": 0.6643084286264951 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9091277234265154, + "y": 0.8755345900405657 + }, + "prevControl": { + "x": 2.944079035349721, + "y": 0.3675726922113227 + }, + "nextControl": { + "x": 2.869788703095269, + "y": 1.4472648721572674 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.980649981950894, + "y": 2.6848905275845403 + }, + "prevControl": { + "x": 2.965196582294745, + "y": 2.484849458220909 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.384529386712096, + "rotationDegrees": -89.44080869749423 + } + ], + "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": -149.21236118610597 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.91420959211533 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..f5c5ca6 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.051, + "driveGearing": 7.03, + "maxDriveSpeed": 4.54, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.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, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1a811b6..21c3ef0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,24 +4,94 @@ package frc.robot; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.config.VisionConfig; +import frc.robot.subsystems.Vision; +import java.util.List; +import org.photonvision.targeting.PhotonPipelineResult; -// we'll do nothing in this file too? +@Logged public class Robot extends TimedRobot { private Command m_autonomousCommand; - - private final RobotContainer m_robotContainer; + @Logged private final RobotContainer m_robotContainer; + @Logged private Field2d field = new Field2d(); public Robot() { m_robotContainer = new RobotContainer(); + + Epilogue.bind(this); } @Override public void robotPeriodic() { // loop continuously runs as long as the robot is active CommandScheduler.getInstance().run(); + Command selectedAuto = m_robotContainer.getAutonomousCommand(); + if (selectedAuto != null) { + SmartDashboard.putString("Selected Auto", selectedAuto.getName()); + } else { + SmartDashboard.putString("Selected Auto", "None"); + } + + // Updates the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE_STRATEGY + // (not in use) + VisionConfig.photonPoseEstimatorLeft.setReferencePose( + m_robotContainer.drivetrain.getState().Pose); + VisionConfig.photonPoseEstimatorRight.setReferencePose( + m_robotContainer.drivetrain.getState().Pose); + + // Puts the pose data from one camera into a list + + List results = Vision.leftCameraApril.getAllUnreadResults(); + + // If there is pose data from the cameras, get the latest estimated pose and update the 'vision' + // photon pose estimator + // If there is no multi tag result and the distance from the camera to the target is greater + // than + // 4 meters, return + // Otherwise, add the latest vision pose estimate to a filter with the odometry pose estimate + // and set + // the guessed pose from that to the current pose + if (!results.isEmpty()) { + PhotonPipelineResult result = results.get(results.size() - 1); + VisionConfig.photonPoseEstimatorLeft + .update(result) + .ifPresent( + (pose) -> { + if (result.multitagResult.isEmpty() + && result.targets.get(0).bestCameraToTarget.getTranslation().getNorm() > 4) { + return; + } + m_robotContainer.drivetrain.addVisionMeasurement( + pose.estimatedPose.toPose2d(), pose.timestampSeconds); + // System.out.println((pose.estimatedPose.getX(), pose.estimatedPose.getY()); + }); + } else { + } + results = Vision.rightCameraApril.getAllUnreadResults(); + + if (!results.isEmpty()) { + PhotonPipelineResult result = results.get(results.size() - 1); + VisionConfig.photonPoseEstimatorRight + .update(result) + .ifPresent( + (pose) -> { + if (result.multitagResult.isEmpty() + && result.targets.get(0).bestCameraToTarget.getTranslation().getNorm() > 4) { + return; + } + m_robotContainer.drivetrain.addVisionMeasurement( + pose.estimatedPose.toPose2d(), pose.timestampSeconds); + }); + + field.setRobotPose(m_robotContainer.m_odometry.getEstimatedPosition()); + } } @Override @@ -41,6 +111,7 @@ public void disabledExit() { @Override public void autonomousInit() { + RobotContainer.zeroPigeon(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index defe7f6..0b4072b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,18 +4,347 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import frc.robot.config.CANMappings; +import frc.robot.config.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; +@Logged public class RobotContainer { + private double MaxSpeed = + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = + RotationsPerSecond.of(0.75) + .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = + new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.2) + .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType( + SwerveModule.DriveRequestType + .OpenLoopVoltage); // Use open-loop control for drive motors + private final SwerveRequest.RobotCentric intakeDrive = + new SwerveRequest.RobotCentric() + .withDeadband(MaxSpeed * 0.2) + .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.RobotCentric autoDrive = + new SwerveRequest.RobotCentric() + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); + + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + private final Telemetry logger = new Telemetry(MaxSpeed); + private final SendableChooser autoChooser; + private CommandXboxController controller = new CommandXboxController(0); + public CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private Intake intake = new Intake(); + private Shooter shooter = new Shooter(); + private Pivot pivot = new Pivot(drivetrain, shooter, intake); + private final SwerveRequest.FieldCentricFacingAngle m_default = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType( + SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle + + public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); + Translation2d m_frontLeftLocation = + new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(10.875)); + Translation2d m_frontRightLocation = + new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(-10.875)); + Translation2d m_backLeftLocation = + new Translation2d(Units.inchesToMeters(-10.875), Units.inchesToMeters(10.875)); + Translation2d m_backRightLocation = + new Translation2d(Units.inchesToMeters(-10.855), Units.inchesToMeters(-10.875)); + public SwerveDrivePoseEstimator m_odometry = + new SwerveDrivePoseEstimator( + new SwerveDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation), + pigeon2.getRotation2d(), + new SwerveModulePosition[] { + drivetrain.getModule(0).getPosition(true), + drivetrain.getModule(1).getPosition(true), + drivetrain.getModule(2).getPosition(true), + drivetrain.getModule(3).getPosition(true) + }, + new Pose2d(0.0, 0.0, new Rotation2d())); + static Field2d m_field = new Field2d(); + // links xbox controller to controls public RobotContainer() { + + CommandScheduler.getInstance().registerSubsystem(drivetrain); + + SmartDashboard.putData("Field", m_field); + + SmartDashboard.putData("Field", m_field); + + PathPlannerLogging.setLogActivePathCallback( + (poses) -> { + m_field.getObject("path").setPoses(poses); + }); + + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + configureBindings(); } - private void configureBindings() {} + public void updateTelemetry() { + m_field.setRobotPose(drivetrain.getState().Pose); + } + + @Logged Pose2d estimatedPosition = m_odometry.getEstimatedPosition(); + @Logged private double goalAngle = drivetrain.getState().ModuleTargets[1].angle.getRotations(); + @Logged private double actualAngle = drivetrain.getState().ModuleStates[1].angle.getRotations(); + + private void configureBindings() { + final SwerveRequest.Idle idle = new SwerveRequest.Idle(); + RobotModeTriggers.disabled() + .whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true)); + drivetrain.registerTelemetry(logger::telemeterize); + + InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); + map.put(Units.inchesToMeters(59.0), 0.18); + map.put(Units.inchesToMeters(76.5), 0.155); + map.put(Units.inchesToMeters(96.5), 0.142); + map.put(Units.inchesToMeters(125.5), 0.13); + map.put(Units.inchesToMeters(169.5), 0.12); + map.put(Units.inchesToMeters(210.5), 0.118); + + NamedCommands.registerCommand("wait 5 seconds", Commands.waitSeconds(5)); + + NamedCommands.registerCommand( + "idle", + Commands.runOnce(() -> intake.stopIntake(), intake) + .withDeadline(Commands.waitSeconds(5)) + .alongWith( + Commands.runOnce(() -> shooter.stopShooter(), shooter) + .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot))) + .withDeadline(Commands.waitSeconds(5))); + + NamedCommands.registerCommand( + "high goal shoot", + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + .withDeadline(Commands.waitSeconds(5)) + .alongWith(Commands.run(() -> System.out.println("Pivot going"))) + .withTimeout(1) + .andThen( + Commands.run(() -> shooter.shoot(), shooter) + .alongWith(Commands.run(() -> System.out.println("Shooter going"))) + // Timeout applied to shooter command only + .alongWith( + Commands.run(() -> intake.intake(), intake) + .alongWith(Commands.run(() -> System.out.println("Intake going"))))) + .withDeadline(Commands.waitSeconds(5))); + + NamedCommands.registerCommand( + "intake", + Commands.run(() -> intake.intake(), intake) + .withDeadline(Commands.waitSeconds(5)) + .alongWith( + Commands.run((() -> System.out.println("Intake intaking off ground"))) + .alongWith( + Commands.run(() -> shooter.stopShooter(), shooter) + .alongWith(Commands.run(() -> System.out.println("Shooter stopped"))) + .alongWith( + Commands.run(() -> pivot.lowScore(0.31), pivot) + .alongWith( + Commands.run( + () -> + System.out.println("Pivot to intake position")))))) + .withDeadline(Commands.waitSeconds(5))); + + // Default commands + pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot)); + shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter(), shooter)); + intake.setDefaultCommand(Commands.run(() -> intake.stopIntake(), intake)); + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> + drive + .withVelocityX( + -controller.getLeftY() + * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-controller.getLeftX() * MaxSpeed) // Drive left with negative X + .withRotationalRate( + -controller.getRightX() + * MaxAngularRate))); // Drive counterclockwise with negative X + + // Testing - Commented out until need to be used + // Pivot to angle based off distance + // controller + // .povUp() + // .whileTrue( + // (Commands.run( + // () -> + // pivot.setPivotAngleRot( + // map.get( + // drivetrain + // .getState() + // .Pose + // .getTranslation() + // .getDistance(pivot.getCosmicConverterTranslation(false))))))); + // controller + // .povUp() + // .whileTrue( + // (Commands.run( + // () -> + // pivot.setPivotAngleRot( + // map.get(Units.inchesToMeters(132)))))); + + // Zero pivot + controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); + // + // + // Shoot + // controller + // .rightTrigger() + // .whileTrue( + // Commands.run((() -> intake.runKicker(-0.7))) + // .withTimeout(0.5) + // .andThen( + // Commands.run(() -> shooter.shoot()) + // .alongWith(Commands.run(() -> intake.intake())))); + // // Intake + // controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); + + // Specialized commands + // auto align with inner cosmic converter and raise pivot + controller + .leftBumper() + .toggleOnTrue( + pivot + .getCosmicConverter(controller.rightTrigger(), true) + .alongWith(Commands.run(() -> System.out.println("inner")))); + // auto align with outer cosmic converter and raise pivot + controller + .leftTrigger() + .toggleOnTrue( + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + .alongWith(Commands.run(() -> System.out.println("Pivot going"))) + .withTimeout(1) + .andThen( + Commands.run(() -> shooter.shoot(), shooter) + .alongWith(Commands.run(() -> System.out.println("Shooter going"))) + // Timeout applied to shooter command only + .alongWith( + Commands.run(() -> intake.intake(), intake) + .alongWith(Commands.run(() -> System.out.println("Intake going"))))) + .withDeadline(Commands.waitSeconds(5))); + // Intake + controller + .a() + .toggleOnTrue( + Commands.run(() -> intake.intake()) + .alongWith(pivot.lowScore(0.31)) + .alongWith(Commands.run(() -> System.out.println("intake"))) + .alongWith( + Commands.runOnce(() -> drivetrain.removeDefaultCommand()) + .andThen( + Commands.run( + () -> + drivetrain.setDefaultCommand( + drivetrain.applyRequest( + () -> + intakeDrive + .withVelocityX( + -controller.getLeftY() + * MaxSpeed) // Drive forward with + // negative Y + // (forward) + .withVelocityY( + -controller.getLeftX() + * MaxSpeed) // Drive left with negative + // X + .withRotationalRate( + -controller.getRightX() + * MaxAngularRate))))))); // Drive + // counterclockwise with + // negative X) + // counterclockwise + // with negative + // X)))); + // Outtake + controller + .b() + .toggleOnTrue( + Commands.run(() -> intake.outtake()) + .alongWith(Commands.run(() -> System.out.println("outtake")))); + controller.povRight().onTrue(Commands.runOnce(() -> zeroPigeon())); + // Low score + controller + .x() + .toggleOnTrue( + pivot + .lowScore(0.5) + .until(controller.rightTrigger()) + .andThen( + pivot + .lowScore(0.05) + .alongWith(Commands.run(() -> intake.outtake())) + .alongWith(Commands.run(() -> System.out.println("low"))))); + controller.povDown().toggleOnTrue(Commands.run(() -> pivot.movePivot(0.05), pivot)); + } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return (Commands.run(() -> pivot.setPivotAngleRot(0.1525), pivot)) + .withTimeout(2) + .andThen( + Commands.run(() -> shooter.shoot(), shooter) + .alongWith(Commands.run(() -> intake.intake(), intake))) + .withDeadline(Commands.waitSeconds(10)); + // .withDeadline(Commands.waitSeconds(10)); + // return Commands.waitSeconds(12).andThen( + // Commands.run(() -> drivetrain.setControl(autoDrive.withVelocityX(1))) + // .withTimeout(5) + // .andThen( + // Commands.run( + // () -> + // drivetrain.setControl( + // (drive + // .withVelocityX( + // -controller.getLeftY() + // * MaxSpeed) // Drive forward with negative Y + // // (forward) + // .withVelocityY( + // -controller.getLeftX() + // * MaxSpeed) // Drive left with negative X + // .withRotationalRate( + // -controller.getRightX() + // * MaxAngularRate)))))); // Drive)))); + } + + public static void zeroPigeon() { + Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); + pigeon.reset(); } } diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 0000000..8874e67 --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,145 @@ +package frc.robot; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + + /* Set up the module state Mechanism2d telemetry */ + for (int i = 0; i < 4; ++i) { + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = + driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = + driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = + driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = + driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = + driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = + driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = + driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = + new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; + + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } + + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); + + /* Telemeterize each module state to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + } + } +} diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java deleted file mode 100644 index 0f45f97..0000000 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ /dev/null @@ -1,75 +0,0 @@ -package frc.robot.commands; - -import com.ctre.phoenix6.swerve.SwerveModule; -import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Pivot; - -public class DrivetrainCommand extends Command { - public static enum Position { - OUTER_COSMIC_CONVERTER, - INNER_COSMIC_CONVERTER - } - - private DrivetrainCommand.Position state; - private CommandSwerveDrivetrain drivetrain; - private Pose2d robotPose; - private Translation2d diff; - private Rotation2d targetRotation; - private Translation2d cosmicConverter; - - // Initialize the facing angle request - private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = - new SwerveRequest.FieldCentricFacingAngle() - .withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - - public DrivetrainCommand(CommandSwerveDrivetrain drivetrain, DrivetrainCommand.Position state) { - this.drivetrain = drivetrain; - this.state = state; - - addRequirements(drivetrain); - } - - @Override - public void initialize() { - switch (state) { - case OUTER_COSMIC_CONVERTER: - cosmicConverter = Pivot.getLocation(0); - drivetrain.setControl( - m_faceAngle - .withVelocityX(0) - .withVelocityY(0) - // Set the desired direction in Radians - .withTargetDirection( - new Rotation2d( - Math.atan2( - cosmicConverter.getX() - drivetrain.getState().Pose.getX(), - cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); - break; - - case INNER_COSMIC_CONVERTER: - cosmicConverter = Pivot.getLocation(1); - drivetrain.setControl( - m_faceAngle - .withVelocityX(0) - .withVelocityY(0) - // Set the desired direction in Radians - .withTargetDirection( - new Rotation2d( - Math.atan2( - cosmicConverter.getX() - drivetrain.getState().Pose.getX(), - cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); - break; - } - } - - @Override - public void end(boolean interrupted) { - drivetrain.setControl(new SwerveRequest.Idle()); - } -} diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java deleted file mode 100644 index a8fef87..0000000 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.config.IntakeConfig; -import frc.robot.subsystems.Intake; - -public class IntakeCommand extends Command { - public static enum Speeds { - INTAKE, - OUTTAKE_SCORE, - SHOOT - } - - private Speeds speed; - Intake intake; - - public IntakeCommand(Intake intake, Speeds speed) { - this.intake = intake; - this.speed = speed; - addRequirements(intake); - } - - @Override - public void initialize() { - switch (speed) { - case INTAKE: - intake.runKicker(IntakeConfig.K_KICKER_INTAKE_VELOCITY); - intake.runInitial(IntakeConfig.K_INITIAL_INTAKE_VELOCITY); - break; - - case OUTTAKE_SCORE: - intake.runKicker(IntakeConfig.K_KICKER_OUTTAKE_VELOCITY); - intake.runInitial(IntakeConfig.K_INITIAL_OUTTAKE_VELOCITY); - break; - - case SHOOT: - intake.runKicker(IntakeConfig.K_KICKER_INTAKE_VELOCITY); - intake.runInitial(IntakeConfig.K_KICKER_INTAKE_VELOCITY); - break; - - default: - intake.stopIntake(); - break; - } - } - - @Override - public void end(boolean interrupted) { - intake.stopIntake(); - } -} diff --git a/src/main/java/frc/robot/commands/PivotCommand.java b/src/main/java/frc/robot/commands/PivotCommand.java deleted file mode 100644 index acffdc7..0000000 --- a/src/main/java/frc/robot/commands/PivotCommand.java +++ /dev/null @@ -1,74 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.config.PivotConfig; -import frc.robot.subsystems.Pivot; - -public class PivotCommand extends Command { - public static enum Positions { - INTAKE_GROUND, - INTAKE_STAR_SPIRE, - OUTTAKE_SCORE, - INNER_HIGH_SHOOT, - OUTER_HIGH_SHOOT, - IDLE - } - - private Positions pose; - Pivot pivot; - - public PivotCommand(Pivot pivot, Positions pose) { - this.pivot = pivot; - this.pose = pose; - addRequirements(pivot); - } - - @Override - public void initialize() { - switch (pose) { - case INTAKE_GROUND: - pivot.setPivotAngle( - new Rotation2d(Units.degreesToRadians(PivotConfig.PIVOT_GROUND_INTAKE_ANGLE))); - break; - - case INTAKE_STAR_SPIRE: - pivot.setPivotAngle( - new Rotation2d(Units.degreesToRadians(PivotConfig.PIVOT_STAR_SPIRE_INTAKE_ANGLE))); - break; - - case OUTTAKE_SCORE: - pivot.setPivotAngle( - new Rotation2d(Units.degreesToRadians(PivotConfig.PIVOT_OUTTAKE_ANGLE))); - break; - - case IDLE: - pivot.setPivotAngle(new Rotation2d((Units.degreesToRadians(PivotConfig.PIVOT_IDLE_ANGLE)))); - break; - - case INNER_HIGH_SHOOT: - pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(1))); - break; - - case OUTER_HIGH_SHOOT: - pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(0))); - break; - - default: - pivot.stopPivot(); - break; - } - } - - @Override - public boolean isFinished() { - // The command is finished when the pivot reaches its target angle within tolerance. - return pivot.pivotAtSetpoint(); - } - - @Override - public void end(boolean interrupted) { - pivot.stopPivot(); - } -} diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java deleted file mode 100644 index 9dd06db..0000000 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.config.ShooterConfig; -import frc.robot.subsystems.Shooter; - -public class ShooterCommand extends Command { - public static enum Positions { - SHOOT, - IDLE - } - - private Positions pose; - Shooter shooter; - - public ShooterCommand(Shooter shooter, Positions pose) { - this.shooter = shooter; - this.pose = pose; - addRequirements(shooter); - } - - @Override - public void initialize() { - switch (pose) { - case SHOOT: - shooter.shoot(ShooterConfig.K_TOP_AND_BOTTOM_SHOOTER_VELOCITY); - break; - - case IDLE: - shooter.stopShooter(); - break; - - default: - shooter.stopShooter(); - break; - } - } - - @Override - public void end(boolean interrupted) { - shooter.stopShooter(); - } -} diff --git a/src/main/java/frc/robot/config/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index f6e5e47..569cf28 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -1,10 +1,11 @@ package frc.robot.config; public class CANMappings { - public static final int K_PIVOT_LEFT_ID = 1; - public static final int K_PIVOT_RIGHT_ID = 2; - public static final int K_INITIAL_INTAKE_ID = 3; - public static final int K_KICKER_INTAKE_ID = 4; - public static final int K_TOP_SHOOTER_ID = 5; - public static final int K_BOTTOM_SHOOTER_ID = 6; + public static final int K_PIVOT_LEFT_ID = 15; + public static final int K_PIVOT_RIGHT_ID = 16; + public static final int K_INITIAL_INTAKE_ID = 4; + public static final int K_KICKER_INTAKE_ID = 1; + public static final int K_TOP_SHOOTER_ID = 2; + public static final int K_BOTTOM_SHOOTER_ID = 3; + public static final int PIGEON_CAN_ID = 0; } diff --git a/src/main/java/frc/robot/config/IntakeConfig.java b/src/main/java/frc/robot/config/IntakeConfig.java index 40c623f..296c478 100644 --- a/src/main/java/frc/robot/config/IntakeConfig.java +++ b/src/main/java/frc/robot/config/IntakeConfig.java @@ -1,8 +1,8 @@ package frc.robot.config; public class IntakeConfig { - public static final double K_INITIAL_INTAKE_STATOR_CURRENT_LIMIT = 120.0; - public static final double K_KICKER_INTAKE_STATOR_CURRENT_LIMIT = 120.0; + public static final double K_INITIAL_INTAKE_STATOR_CURRENT_LIMIT = 80.0; + public static final double K_KICKER_INTAKE_STATOR_CURRENT_LIMIT = 80.0; public static final double K_INITIAL_INTAKE_SUPPLY_CURRENT_LIMIT = 70.0; public static final double K_KICKER_INTAKE_SUPPLY_CURRENT_LIMIT = 70.0; diff --git a/src/main/java/frc/robot/config/PivotConfig.java b/src/main/java/frc/robot/config/PivotConfig.java index e943121..62d50d0 100644 --- a/src/main/java/frc/robot/config/PivotConfig.java +++ b/src/main/java/frc/robot/config/PivotConfig.java @@ -1,16 +1,16 @@ package frc.robot.config; public class PivotConfig { - public static final double K_PIVOT_ANGLE_TOLERANCE = 0.0006; // in rotations + public static final double K_PIVOT_ANGLE_TOLERANCE = 0.005; // in rotations - public static final double K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT = 120.0; + public static final double K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT = 80.0; public static final double K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT = 70.0; public static final double K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY = 3000.0; public static final double K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION = 500.0; public static final double K_LEFT_AND_RIGHT_PIVOT_JERK = 0.0; - public static final double K_LEFT_AND_RIGHT_PIVOT_P = 0.0; + public static final double K_LEFT_AND_RIGHT_PIVOT_P = 14; public static final double K_LEFT_AND_RIGHT_PIVOT_I = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_D = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_S = 0.0; @@ -25,4 +25,6 @@ public class PivotConfig { public static final double PIVOT_GROUND_INTAKE_ANGLE = 0.0; public static final double PIVOT_OUTTAKE_ANGLE = 0.0; public static final double PIVOT_IDLE_ANGLE = 0.0; + + public static double ANGLE_ADD = 0.0; } diff --git a/src/main/java/frc/robot/config/ShooterConfig.java b/src/main/java/frc/robot/config/ShooterConfig.java index bf29013..1854abc 100644 --- a/src/main/java/frc/robot/config/ShooterConfig.java +++ b/src/main/java/frc/robot/config/ShooterConfig.java @@ -1,7 +1,7 @@ package frc.robot.config; public class ShooterConfig { - public static final double K_TOP_AND_BOTTOM_SHOOTER_STATOR_CURRENT_LIMIT = 120.0; + public static final double K_TOP_AND_BOTTOM_SHOOTER_STATOR_CURRENT_LIMIT = 80.0; public static final double K_TOP_AND_BOTTOM_SHOOTER_SUPPLY_CURRENT_LIMIT = 70.0; public static final double K_TOP_AND_BOTTOM_SHOOTER_MAX_CRUISE_VELOCITY = 3000.0; diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 9329c5d..28f0a07 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -27,7 +27,7 @@ public class TunerConstants { .withKI(0) .withKD(0.5) .withKS(0.1) - .withKV(2.66) + .withKV(2.49) .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -40,6 +40,7 @@ public class TunerConstants { private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors + // This affects the PID/FF gains for the drive motors private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; // The type of motor used for the drive motor @@ -55,7 +56,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(60); // changed from 120 // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -67,7 +68,7 @@ public class TunerConstants { // Swerve azimuth does not require much torque output, so we can set a relatively // low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(30)) // changed (originally 60) .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs @@ -79,20 +80,20 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.96); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.54); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.125; + private static final double kCoupleRatio = 0; - private static final double kDriveGearRatio = 5.357142857142857; - private static final double kSteerGearRatio = 21.428571428571427; + private static final double kDriveGearRatio = 7.03; + private static final double kSteerGearRatio = 26.09; private static final Distance kWheelRadius = Inches.of(2); - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; + private static final boolean kInvertLeftSide = true; + private static final boolean kInvertRightSide = false; - private static final int kPigeonId = 15; + private static final int kPigeonId = 0; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -134,48 +135,48 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 9; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.3349609375); + private static final int kFrontLeftDriveMotorId = 51; + private static final int kFrontLeftSteerMotorId = 50; + private static final int kFrontLeftEncoderId = 52; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.484619140625); private static final boolean kFrontLeftSteerMotorInverted = false; - private static final boolean kFrontLeftEncoderInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(10.5); - private static final Distance kFrontLeftYPos = Inches.of(10.5); + private static final Distance kFrontLeftXPos = Inches.of(10.875); + private static final Distance kFrontLeftYPos = Inches.of(10.875); // Front Right - private static final int kFrontRightDriveMotorId = 3; - private static final int kFrontRightSteerMotorId = 4; - private static final int kFrontRightEncoderId = 10; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.265625); + private static final int kFrontRightDriveMotorId = 21; + private static final int kFrontRightSteerMotorId = 20; + private static final int kFrontRightEncoderId = 22; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.072509765625); private static final boolean kFrontRightSteerMotorInverted = false; - private static final boolean kFrontRightEncoderInverted = true; + private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(10.5); - private static final Distance kFrontRightYPos = Inches.of(-10.5); + private static final Distance kFrontRightXPos = Inches.of(10.875); + private static final Distance kFrontRightYPos = Inches.of(-10.875); // Back Left - private static final int kBackLeftDriveMotorId = 5; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 11; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.41845703125); + private static final int kBackLeftDriveMotorId = 41; + private static final int kBackLeftSteerMotorId = 40; + private static final int kBackLeftEncoderId = 42; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.184326171875); private static final boolean kBackLeftSteerMotorInverted = false; - private static final boolean kBackLeftEncoderInverted = true; + private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-10.5); - private static final Distance kBackLeftYPos = Inches.of(10.5); + private static final Distance kBackLeftXPos = Inches.of(-10.875); + private static final Distance kBackLeftYPos = Inches.of(10.875); // Back Right - private static final int kBackRightDriveMotorId = 7; - private static final int kBackRightSteerMotorId = 8; - private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.43408203125); + private static final int kBackRightDriveMotorId = 31; + private static final int kBackRightSteerMotorId = 30; + private static final int kBackRightEncoderId = 32; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.284423828125); private static final boolean kBackRightSteerMotorInverted = false; - private static final boolean kBackRightEncoderInverted = true; + private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-10.5); - private static final Distance kBackRightYPos = Inches.of(-10.5); + private static final Distance kBackRightXPos = Inches.of(-10.875); + private static final Distance kBackRightYPos = Inches.of(-10.875); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> diff --git a/src/main/java/frc/robot/config/VisionConfig.java b/src/main/java/frc/robot/config/VisionConfig.java index bf34e01..209d863 100644 --- a/src/main/java/frc/robot/config/VisionConfig.java +++ b/src/main/java/frc/robot/config/VisionConfig.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator; public class VisionConfig { @@ -77,11 +79,24 @@ public class VisionConfig { public static final AprilTagFieldLayout FIELD_LAYOUT = new AprilTagFieldLayout(APRIL_TAG_LIST, Units.inchesToMeters(648), Units.inchesToMeters(324)); public static final PhotonPoseEstimator.PoseStrategy STRATEGY = - PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; + PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY; public static final Transform3d LEFT_CAMERA_POSITION = - new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); + new Transform3d( + Units.inchesToMeters(9.25), + Units.inchesToMeters(10.5), + Units.inchesToMeters(7.5), + new Rotation3d(0.0, 0.0, 0.0)); public static final Transform3d RIGHT_CAMERA_POSITION = - new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); + new Transform3d( + Units.inchesToMeters(9.25), + Units.inchesToMeters(-10.5), + Units.inchesToMeters(7.5), + new Rotation3d(0.0, 0.0, 0.0)); public static final Transform3d REAR_CAMERA_POSITION = new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); + Optional visionEst = Optional.empty(); + public static PhotonPoseEstimator photonPoseEstimatorLeft = + new PhotonPoseEstimator(FIELD_LAYOUT, STRATEGY, LEFT_CAMERA_POSITION); + public static PhotonPoseEstimator photonPoseEstimatorRight = + new PhotonPoseEstimator(FIELD_LAYOUT, STRATEGY, RIGHT_CAMERA_POSITION); } diff --git a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java new file mode 100644 index 0000000..37dbfc4 --- /dev/null +++ b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java @@ -0,0 +1,16 @@ +package frc.robot.loggers; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +@CustomLoggerFor(CommandSwerveDrivetrain.class) +public class CommandSwerveDrivetrainLogger extends ClassSpecificLogger { + public CommandSwerveDrivetrainLogger() { + super(CommandSwerveDrivetrain.class); + } + + @Override + protected void update(EpilogueBackend backend, CommandSwerveDrivetrain drivetrain) {} +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index d044773..1f37096 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -4,10 +4,17 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; @@ -35,6 +42,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; + private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds(); /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = @@ -111,6 +120,37 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); + } + + private void configureAutoBuilder() { + try { + var config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + () -> getState().Pose, // Supplier of current robot pose + this::resetPose, // Consumer for seeding pose against auto + () -> getState().Speeds, // Supplier of current robot speeds + // Consumer of ChassisSpeeds and feedforwards to drive the robot + (speeds, feedforwards) -> + setControl( + m_pathApplyRobotSpeeds + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), + new PPHolonomicDriveController( + // PID constants for translation + new PIDConstants(1, 0, 0), + // PID constants for rotation + new PIDConstants(1, 0, 0)), + config, + // Assume the path needs to be flipped for Red vs Blue, this is normally the case + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this // Subsystem for requirements + ); + } catch (Exception ex) { + DriverStation.reportError( + "Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + } } /** @@ -169,8 +209,7 @@ public CommandSwerveDrivetrain( /** * Returns a command that applies the specified control request to this swerve drivetrain. * - *

// @param request Function returning the request to apply - * + * @param request Function returning the request to apply * @return Command to run */ public Command applyRequest(Supplier requestSupplier) { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index d50fe22..d26aa06 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,10 +4,12 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.CANMappings; import frc.robot.config.IntakeConfig; +@Logged public class Intake extends SubsystemBase { protected TalonFX mInitialIntake; protected TalonFX mKickerIntake; @@ -68,19 +70,37 @@ public Intake() { } // Velocity is rotations per second of motor accounting for SensorToMechanismRatio - public void intake(double velocity) { - mInitialIntake.setControl(new DutyCycleOut(velocity)); - mKickerIntake.setControl(new DutyCycleOut(velocity)); + public void intake(double initialVelocity, double kickerVelocity) { + mInitialIntake.setControl(new DutyCycleOut(initialVelocity)); + mKickerIntake.setControl(new DutyCycleOut(kickerVelocity)); + } + + public void intake() { + mInitialIntake.setControl(new DutyCycleOut(-0.5)); + mKickerIntake.setControl(new DutyCycleOut(-1)); + } + + public void outtake() { + mInitialIntake.setControl(new DutyCycleOut(0.5)); + mKickerIntake.setControl(new DutyCycleOut(0.7)); } public void runKicker(double velocity) { mKickerIntake.setControl(new DutyCycleOut(velocity)); } + public void runKicker() { + mKickerIntake.setControl(new DutyCycleOut(-0.7)); + } + public void runInitial(double velocity) { mInitialIntake.setControl(new DutyCycleOut(velocity)); } + public void runInitial() { + mInitialIntake.setControl(new DutyCycleOut(-0.5)); + } + public void stopIntake() { mInitialIntake.stopMotor(); mKickerIntake.stopMotor(); diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 4a4eb0c..273b456 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -1,24 +1,26 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.CANMappings; import frc.robot.config.PivotConfig; -import frc.robot.config.TunerConstants; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; import java.util.Optional; +import java.util.function.BooleanSupplier; @Logged public class Pivot extends SubsystemBase { @@ -26,15 +28,19 @@ public class Pivot extends SubsystemBase { protected TalonFX mPivotRight; protected Follower follower; protected CommandSwerveDrivetrain drivetrain; + protected Shooter shooter; + protected Intake intake; + private final InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); private double currentAngle; - public Pivot() { + public Pivot(CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake) { mPivotLeft = new TalonFX(CANMappings.K_PIVOT_LEFT_ID); mPivotRight = new TalonFX(CANMappings.K_PIVOT_RIGHT_ID); - drivetrain = TunerConstants.createDrivetrain(); - + this.drivetrain = drivetrain; + this.shooter = shooter; + this.intake = intake; TalonFXConfiguration leftPivotConfig = new TalonFXConfiguration(); TalonFXConfiguration rightPivotConfig = new TalonFXConfiguration(); @@ -87,13 +93,12 @@ public Pivot() { leftPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; rightPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - ; + // leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; mPivotLeft.getConfigurator().apply(leftPivotConfig); mPivotRight.getConfigurator().apply(rightPivotConfig); - follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, true); + // follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, false); } public void setPivotAngle(Rotation2d angleSetpoint) { @@ -101,6 +106,16 @@ public void setPivotAngle(Rotation2d angleSetpoint) { mPivotRight.setControl(follower); } + public void setPivotAngleRot(double rotation) { + mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); + mPivotRight.setControl(new MotionMagicVoltage(rotation)); + } + + public void pivotDefault() { + mPivotLeft.setControl(new MotionMagicVoltage(0.0)); + mPivotRight.setControl(new MotionMagicVoltage(0.0)); + } + public void zeroPivot() { mPivotLeft.setPosition(0.0); mPivotRight.setPosition(0.0); @@ -111,25 +126,16 @@ public void stopPivot() { mPivotRight.stopMotor(); } + public void movePivot(double speed) { + mPivotLeft.setControl(new DutyCycleOut(speed)); + mPivotRight.setControl(new DutyCycleOut(-speed)); + } + public boolean pivotAtSetpoint() { return Math.abs(mPivotLeft.getClosedLoopError().getValueAsDouble()) <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; } - public Rotation2d getHighAngle(Translation2d location) { - // location: the cosmic converter we're shooting on - 1 is blue inner, 2 is blue outer, 3 is red - // inner, 4 is red outer - // want 5-8 calibrations (distance, angle) - InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - map.put(0.0, 0.0); - - double distance = - Math.sqrt( - Math.pow(location.getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(location.getY() - drivetrain.getState().Pose.getY(), 2)); - return Rotation2d.fromDegrees(map.get(distance)); - } - public double getPivotAngleDegrees() { currentAngle = mPivotLeft.getPosition().getValueAsDouble(); currentAngle = currentAngle * 360; @@ -137,47 +143,137 @@ public double getPivotAngleDegrees() { return currentAngle; } - public static int getAlliance() { - Optional alliance = DriverStation.getAlliance(); + private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) + .withHeadingPID(6, 0, 0); // Or OpenLoopDutyCycle - if (alliance.isPresent()) { - if (alliance.get() == DriverStation.Alliance.Blue) { - return 0; + public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + map.put(Units.inchesToMeters(59.0), 0.18); + map.put(Units.inchesToMeters(76.5), 0.155); + map.put(Units.inchesToMeters(96.5), 0.142); + map.put(Units.inchesToMeters(125.5), 0.13); + map.put(Units.inchesToMeters(169.5), 0.12); + map.put(Units.inchesToMeters(210.5), 0.118); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); + } } - if (alliance.get() == DriverStation.Alliance.Red) { - return 1; + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + } } + + final Translation2d cosmicConverterFinal = cosmicConverter; + + Rotation2d heading = drivetrain.getState().Pose.getRotation(); + + // shooter offset in robot frame (meters) + double shooterOffsetX = 0.0; // forward + double shooterOffsetY = Units.inchesToMeters(-1); // right + + // convert to field frame + double shooterX = + drivetrain.getState().Pose.getX() + + shooterOffsetX * heading.getCos() + - shooterOffsetY * heading.getSin(); + + double shooterY = + drivetrain.getState().Pose.getY() + + shooterOffsetX * heading.getSin() + + shooterOffsetY * heading.getCos(); + + return Commands.run( + () -> + drivetrain.setControl( + m_faceAngle.withTargetDirection( + new Rotation2d( + Math.atan2( + cosmicConverterFinal.getY() + - drivetrain.getState().Pose.getY(), + cosmicConverterFinal.getX() + - drivetrain.getState().Pose.getX()) + + Units.degreesToRadians(-3.0)))), + drivetrain) + .alongWith( + run( + () -> + setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance(getCosmicConverterTranslation(false))) + + 0.05))) + .withDeadline( + Commands.waitUntil(complete) + .andThen( + Commands.parallel( + Commands.run((() -> intake.runKicker(0.2)), intake) + .withTimeout(0.25) + .andThen( + Commands.run(() -> shooter.shoot(), shooter) + .withTimeout(1) + .andThen( + Commands.run(() -> intake.intake(), intake) + .alongWith( + Commands.run( + () -> shooter.shoot(), shooter))))) + .until(() -> !complete.getAsBoolean()))); + } else { + System.out.println("no alliance detected: likely causing many errors"); + return Commands.none(); } - System.out.println("no alliance detected: likely causing many errors"); - return -1; } - public static Translation2d getLocation(int innerouter) { - // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // innerouter: 0 - outer, 1 - inner - // getAlliance(): blue - 0, red - 1 - - List locations = - new ArrayList<>( - Arrays.asList( - new Translation2d(4.0, 196.125), - new Translation2d(4.0, 20.5), - new Translation2d(644.0, 196.125), - new Translation2d(644.0, 20.5))); // same order as explained above - - if (innerouter == 1 & Pivot.getAlliance() == 0) { // blue inner - return locations.get(0); - } - if (innerouter == 0 & Pivot.getAlliance() == 0) { // blue outer - return locations.get(1); - } - if (innerouter == 1 & Pivot.getAlliance() == 1) { // red inner - return locations.get(2); - } - if (innerouter == 0 & Pivot.getAlliance() == 1) { // red outer - return locations.get(3); + public Translation2d getCosmicConverterTranslation(boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); + } + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + } + } } - System.out.println("error in getLocation in pivot subsystem"); - return new Translation2d(0.0, 0.0); + return cosmicConverter; + } + + public Command defaults() { + return Commands.run( + () -> + drivetrain.setControl( + m_faceAngle.withTargetDirection(drivetrain.getState().Pose.getRotation()))); + } + + public Command lowScore(double angle) { + return Commands.run(() -> setPivotAngleRot(angle)); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 6372e38..2c850fc 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,10 +4,12 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.CANMappings; import frc.robot.config.ShooterConfig; +@Logged public class Shooter extends SubsystemBase { protected TalonFX mTopShooter; protected TalonFX mBottomShooter; @@ -69,8 +71,13 @@ public Shooter() { // Velocity is rotations per second of motor accounting for SensorToMechanismRatio public void shoot(double velocity) { - mTopShooter.setControl(new DutyCycleOut(velocity)); - mBottomShooter.setControl(new DutyCycleOut(-velocity)); + mTopShooter.setControl(new DutyCycleOut(-velocity)); + mBottomShooter.setControl(new DutyCycleOut(velocity)); + } + + public void shoot() { + mTopShooter.setControl(new DutyCycleOut(-1)); + mBottomShooter.setControl(new DutyCycleOut(1)); } public void stopShooter() { diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 73ae05f..7167fab 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.VisionConfig; import org.photonvision.PhotonCamera; +import org.photonvision.estimation.*; public class Vision extends SubsystemBase { public static final PhotonCamera leftCameraApril = new PhotonCamera(VisionConfig.CAMERA_NAME); diff --git a/tuner-project.json b/tuner-project.json new file mode 100644 index 0000000..959f076 --- /dev/null +++ b/tuner-project.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":52,"Name":"Front Left CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":50,"Name":"Front Left Turn","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":51,"Name":"Front Left Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.0185546875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":50,"ValidatedDriveId":51,"ValidatedEncoderId":52},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":22,"Name":"Front Right CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":20,"Name":"Front Right Turning","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":21,"Name":"Front Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.43505859375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":20,"ValidatedDriveId":21,"ValidatedEncoderId":22},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":42,"Name":"Back Left CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":40,"Name":"Back Left Turn","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":41,"Name":"Back Left Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.31787109375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":40,"ValidatedDriveId":41,"ValidatedEncoderId":42},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":32,"Name":"Back Right CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":30,"Name":"Back Right Turning","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":31,"Name":"Back Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":0.2109375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":30,"ValidatedDriveId":31,"ValidatedEncoderId":32}],"SwerveOptions":{"kSpeedAt12Volts":4.540338742599189,"Gyro":{"Id":0,"Name":"THE PIGEON","Model":"Pigeon 2 vers. S","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":21.75,"HorizontalTrackSizeInches":21.75,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":0,"SwerveModuleConfiguration":{"ModuleBrand":-1,"DriveRatio":7.03,"SteerRatio":26.09,"CouplingRatio":0.0,"CustomName":null},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Custom","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file