diff --git a/src/main/deploy/pathplanner/autos/BlueCenter3_v2.auto b/src/main/deploy/pathplanner/autos/BlueCenter3_v2.auto deleted file mode 100644 index bdd51fe..0000000 --- a/src/main/deploy/pathplanner/autos/BlueCenter3_v2.auto +++ /dev/null @@ -1,69 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BlueCenter3_1v2" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "gripperOpen" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "BlueCenter3_2v2" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "gripperOpen" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "gripperToGroundIntake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BlueCenter3_v2FLIPPED.auto b/src/main/deploy/pathplanner/autos/BlueCenter3_v2FLIPPED.auto deleted file mode 100644 index e4fe504..0000000 --- a/src/main/deploy/pathplanner/autos/BlueCenter3_v2FLIPPED.auto +++ /dev/null @@ -1,69 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BlueCenter3_1v2FLIPPED" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "gripperOpen" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "BlueCenter3_2v2FLIPPED" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "gripperOpen" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "gripperToGroundIntake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LEFTBlueSource3.auto b/src/main/deploy/pathplanner/autos/LEFTBlueSource3.auto deleted file mode 100644 index 6f0ac2d..0000000 --- a/src/main/deploy/pathplanner/autos/LEFTBlueSource3.auto +++ /dev/null @@ -1,138 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "LEFTBlueSource3_1v2" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "dropOffCoral" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "LEFTBlueSource3_2" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "waitUntilBucketHasCoral" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "LEFTBlueSource3_3" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "dropOffCoral" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "LEFTBlueSource3_4" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "waitUntilBucketHasCoral" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "LEFTBlueSource3_5" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - }, - { - "type": "named", - "data": { - "name": "dropOffCoral" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BlueSource3.auto b/src/main/deploy/pathplanner/autos/LeftSourceCenterStart.auto similarity index 53% rename from src/main/deploy/pathplanner/autos/BlueSource3.auto rename to src/main/deploy/pathplanner/autos/LeftSourceCenterStart.auto index a39efba..a47771d 100644 --- a/src/main/deploy/pathplanner/autos/BlueSource3.auto +++ b/src/main/deploy/pathplanner/autos/LeftSourceCenterStart.auto @@ -7,7 +7,19 @@ { "type": "path", "data": { - "pathName": "BlueSource3_1v2" + "pathName": "LeftCenter1" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCenter2" } }, { @@ -15,15 +27,15 @@ "data": { "commands": [ { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.3 + "name": "driveToCoral" } }, { - "type": "named", + "type": "wait", "data": { - "name": "dropOffCoral" + "waitTime": 2.0 } } ] @@ -32,7 +44,19 @@ { "type": "path", "data": { - "pathName": "BlueSource3_2" + "pathName": "LeftCenter3" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCenter4" } }, { @@ -42,13 +66,13 @@ { "type": "named", "data": { - "name": "waitUntilBucketHasCoral" + "name": "driveToCoral" } }, { "type": "wait", "data": { - "waitTime": 5.0 + "waitTime": 2.0 } } ] @@ -57,7 +81,19 @@ { "type": "path", "data": { - "pathName": "BlueSource3_3" + "pathName": "LeftCenter5" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCenter6" } }, { @@ -65,15 +101,15 @@ "data": { "commands": [ { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.3 + "name": "driveToCoral" } }, { - "type": "named", + "type": "wait", "data": { - "name": "dropOffCoral" + "waitTime": 2.0 } } ] @@ -82,7 +118,19 @@ { "type": "path", "data": { - "pathName": "BlueSource3_4" + "pathName": "LeftCenter7" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCenter8" } }, { @@ -92,13 +140,13 @@ { "type": "named", "data": { - "name": "waitUntilBucketHasCoral" + "name": "driveToCoral" } }, { "type": "wait", "data": { - "waitTime": 5.0 + "waitTime": 2.0 } } ] @@ -107,7 +155,19 @@ { "type": "path", "data": { - "pathName": "BlueSource3_5" + "pathName": "LeftCenter9" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCenter10" } }, { @@ -115,19 +175,31 @@ "data": { "commands": [ { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.3 + "name": "driveToCoral" } }, { - "type": "named", + "type": "wait", "data": { - "name": "dropOffCoral" + "waitTime": 2.0 } } ] } + }, + { + "type": "path", + "data": { + "pathName": "LeftCenter11" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/LoliPops.auto b/src/main/deploy/pathplanner/autos/LoliPops.auto new file mode 100644 index 0000000..e512853 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LoliPops.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LoliPops1" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops2" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops3" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops4a" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops5" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops6" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops7" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops8" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LoliPopsAltA.auto b/src/main/deploy/pathplanner/autos/LoliPopsAltA.auto new file mode 100644 index 0000000..39dad7e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LoliPopsAltA.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LoliPops1a" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL2" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops2a" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops3" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL2" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops4a" + } + }, + { + "type": "named", + "data": { + "name": "elevatorShootL1" + } + }, + { + "type": "path", + "data": { + "pathName": "LoliPops5" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/drive out.auto b/src/main/deploy/pathplanner/autos/drive out.auto deleted file mode 100644 index 26cd70a..0000000 --- a/src/main/deploy/pathplanner/autos/drive out.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "drive out path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/one coral auto.auto b/src/main/deploy/pathplanner/autos/one coral auto.auto deleted file mode 100644 index 109666d..0000000 --- a/src/main/deploy/pathplanner/autos/one coral auto.auto +++ /dev/null @@ -1,38 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "one coral path" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "dropOffCoral" - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Piece 2.path b/src/main/deploy/pathplanner/paths/4 Piece 2.path deleted file mode 100644 index b1496b3..0000000 --- a/src/main/deploy/pathplanner/paths/4 Piece 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.74, - "y": 5.062 - }, - "prevControl": null, - "nextControl": { - "x": 3.141005974760612, - "y": 5.913581521769496 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.9539959016393442, - "y": 5.829149590163934 - }, - "prevControl": { - "x": 2.6223332995942346, - "y": 5.844806821597002 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "4 Piece", - "idealStartingState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_1v2.path b/src/main/deploy/pathplanner/paths/BlueCenter3_1v2.path deleted file mode 100644 index 0f5161b..0000000 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_1v2.path +++ /dev/null @@ -1,190 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.199488636363637, - "y": 6.5 - }, - "prevControl": null, - "nextControl": { - "x": 3.699460227272727, - "y": 6.333423295454545 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.709431818181818, - "y": 5.216605113636364 - }, - "prevControl": { - "x": 4.557017045454546, - "y": 5.50578125 - }, - "nextControl": { - "x": 2.4989600359687945, - "y": 4.8036206232342735 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.4, - "y": 5.475 - }, - "prevControl": { - "x": 1.3251495714735266, - "y": 5.71353178687421 - }, - "nextControl": { - "x": 1.4871988636363633, - "y": 5.1971164772727265 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.15, - "y": 4.15 - }, - "prevControl": { - "x": 1.675659090909091, - "y": 4.164502840909092 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8472468916518769, - "rotationDegrees": 124.393 - }, - { - "waypointRelativePos": 0.9751332149200745, - "rotationDegrees": 124.393 - }, - { - "waypointRelativePos": 1.486678507992877, - "rotationDegrees": -37.875 - }, - { - "waypointRelativePos": 2.3019538188277044, - "rotationDegrees": -37.875 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.9, - "maxWaypointRelativePos": 0.9516310461192309, - "constraints": { - "maxVelocity": 0.3, - "maxAcceleration": 0.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.9302587176602937, - "maxWaypointRelativePos": 3.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.7289088863892024, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.504043126684636, - "maxWaypointRelativePos": 2.078167115902965, - "constraints": { - "maxVelocity": 0.6, - "maxAcceleration": 0.6, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "armToStow", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToStow" - } - } - }, - { - "name": "intakeThrow", - "waypointRelativePos": 0.7843665768194068, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "intakeThrow" - } - } - }, - { - "name": "gripperToGroundIntake", - "waypointRelativePos": 1.1068616422946926, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "gripperToGroundIntake" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "CenterAuton", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_1v2FLIPPED.path b/src/main/deploy/pathplanner/paths/BlueCenter3_1v2FLIPPED.path deleted file mode 100644 index bc76183..0000000 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_1v2FLIPPED.path +++ /dev/null @@ -1,177 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.199488636363637, - "y": 1.5518 - }, - "prevControl": null, - "nextControl": { - "x": 3.699450869129739, - "y": 1.7183881986707088 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.709431818181818, - "y": 2.8348 - }, - "prevControl": { - "x": 4.557019233630228, - "y": 2.5456302773341464 - }, - "nextControl": { - "x": 2.498956910943921, - "y": 3.247775330733026 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.4, - "y": 2.7518 - }, - "prevControl": { - "x": 1.3251482069536238, - "y": 2.5132686413115817 - }, - "nextControl": { - "x": 1.4872004532676757, - "y": 3.0296830239016117 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.15, - "y": 3.9018 - }, - "prevControl": { - "x": 1.6756591945583104, - "y": 3.887286626064463 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8472468916518769, - "rotationDegrees": -124.393 - }, - { - "waypointRelativePos": 0.9751332149200745, - "rotationDegrees": -124.393 - }, - { - "waypointRelativePos": 1.486678507992877, - "rotationDegrees": 37.875 - }, - { - "waypointRelativePos": 2.3019538188277044, - "rotationDegrees": 37.875 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.850393700787409, - "maxWaypointRelativePos": 0.9516310461192309, - "constraints": { - "maxVelocity": 0.3, - "maxAcceleration": 0.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.9302587176602937, - "maxWaypointRelativePos": 3.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.7289088863892024, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "armToStow", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToStow" - } - } - }, - { - "name": "intakeThrow", - "waypointRelativePos": 0.6951631046119233, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "intakeThrow" - } - } - }, - { - "name": "gripperToGroundIntake", - "waypointRelativePos": 1.1068616422946926, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "gripperToGroundIntake" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "CenterAuton", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_2.path b/src/main/deploy/pathplanner/paths/BlueCenter3_2.path deleted file mode 100644 index 6d16803..0000000 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_2.path +++ /dev/null @@ -1,93 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.598, - "y": 5.565 - }, - "prevControl": null, - "nextControl": { - "x": 2.6227069536414387, - "y": 5.310628311261121 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.348737582780602, - "y": 4.184809602645813 - }, - "prevControl": { - "x": 3.2729366684613646, - "y": 4.4230410476491295 - }, - "nextControl": { - "x": 3.4245384970998387, - "y": 3.9465781576424974 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.6416804635761588, - "y": 4.075848509933775 - }, - "prevControl": { - "x": 2.171957781456099, - "y": 4.359147350990184 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.680387409200978, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 1.2300242130750623, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6643109540636051, - "maxWaypointRelativePos": 1.2183745583038859, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.863, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -171.57303097851954 - }, - "reversed": false, - "folder": "CenterAuton", - "idealStartingState": { - "velocity": 0, - "rotation": 142.12500000000003 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_2v2.path b/src/main/deploy/pathplanner/paths/BlueCenter3_2v2.path deleted file mode 100644 index c0b87c7..0000000 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_2v2.path +++ /dev/null @@ -1,101 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.15, - "y": 4.15 - }, - "prevControl": null, - "nextControl": { - "x": 2.5631306818181816, - "y": 4.104673295454546 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.642, - "y": 3.926 - }, - "prevControl": { - "x": 1.4020924513506725, - "y": 3.9963162008435518 - }, - "nextControl": { - "x": 1.881907548649327, - "y": 3.8556837991564485 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.15, - "y": 3.82 - }, - "prevControl": { - "x": 2.2848409090909088, - "y": 3.80846875 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7779751332149082, - "rotationDegrees": 8.427 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6307277628032338, - "maxWaypointRelativePos": 1.0753655793026016, - "constraints": { - "maxVelocity": 0.6, - "maxAcceleration": 0.6, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "gripperToGroundIntake", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "gripperToGroundIntake" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "CenterAuton", - "idealStartingState": { - "velocity": 0.0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_3.path b/src/main/deploy/pathplanner/paths/BlueCenter3_3.path deleted file mode 100644 index 8c25bcb..0000000 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_3.path +++ /dev/null @@ -1,93 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.642, - "y": 4.076 - }, - "prevControl": null, - "nextControl": { - "x": 2.7385554635761586, - "y": 3.9305670529801318 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.3777938741721854, - "y": 3.8724544701986745 - }, - "prevControl": { - "x": 3.128611612143514, - "y": 3.8926584373901885 - }, - "nextControl": { - "x": 3.6465645695364244, - "y": 3.850662251655628 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.590831953642384, - "y": 2.3978476821192056 - }, - "prevControl": { - "x": 2.208278145695364, - "y": 2.8409561258278133 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.680387409200978, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 1.2300242130750623, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6106007067137723, - "maxWaypointRelativePos": 1.2183745583038859, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.863, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -147.65255650055838 - }, - "reversed": false, - "folder": "CenterAuton", - "idealStartingState": { - "velocity": 0, - "rotation": -171.57 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_3v2.path b/src/main/deploy/pathplanner/paths/BlueCenter3_3v2.path deleted file mode 100644 index 0ad0157..0000000 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_3v2.path +++ /dev/null @@ -1,91 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.15, - "y": 3.82 - }, - "prevControl": null, - "nextControl": { - "x": 2.3346988636363637, - "y": 3.0107414772727275 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.55, - "y": 2.25 - }, - "prevControl": { - "x": 1.301088723016711, - "y": 2.2266938593810917 - }, - "nextControl": { - "x": 1.833659090909091, - "y": 2.2765596590909087 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.809, - "y": 2.883 - }, - "prevControl": { - "x": 3.016619318181818, - "y": 2.106673295454545 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5257548845470648, - "rotationDegrees": 32.35 - }, - { - "waypointRelativePos": 1.101243339254005, - "rotationDegrees": 32.35 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "gripperToGroundIntake", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "gripperToGroundIntake" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 58.134 - }, - "reversed": false, - "folder": "CenterAuton", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_4.path b/src/main/deploy/pathplanner/paths/BlueCenter3_4.path deleted file mode 100644 index 79a5f8b..0000000 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_4.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.591, - "y": 2.398 - }, - "prevControl": null, - "nextControl": { - "x": 2.6875554635761585, - "y": 2.2525670529801323 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.7337334437086094, - "y": 3.1605753311258273 - }, - "prevControl": { - "x": 3.1598716887417218, - "y": 2.746523178807945 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.863, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -124.69515353123373 - }, - "reversed": false, - "folder": "CenterAuton", - "idealStartingState": { - "velocity": 0, - "rotation": -147.65300000000002 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSource3_3.path b/src/main/deploy/pathplanner/paths/BlueSource3_3.path deleted file mode 100644 index d7bf886..0000000 --- a/src/main/deploy/pathplanner/paths/BlueSource3_3.path +++ /dev/null @@ -1,96 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.665, - "y": 7.341 - }, - "prevControl": null, - "nextControl": { - "x": 2.5510096989333944, - "y": 5.89074399338601 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.962, - "y": 5.268 - }, - "prevControl": { - "x": 3.7887962162703954, - "y": 5.842456655720645 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7111111111111111, - "rotationDegrees": -59.99999999999999 - } - ], - "constraintZones": [ - { - "name": "Score1", - "minWaypointRelativePos": 0.7986501687289221, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "grabFromFunnel", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "grabFromFunnel" - } - } - }, - { - "name": "armToL4", - "waypointRelativePos": 0.6, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToL4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "reversed": false, - "folder": "SourceAuton", - "idealStartingState": { - "velocity": 0, - "rotation": 126.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index 3824ec7..0000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,107 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.133811475409836, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.6273733459867743, - "y": 7.101294218797317 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.940881147540983, - "y": 6.776178278688524 - }, - "prevControl": { - "x": 6.918200344250221, - "y": 8.122566902037878 - }, - "nextControl": { - "x": 6.965684362114928, - "y": 5.30379806813168 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.133811475409836, - "y": 4.702305327868853 - }, - "prevControl": { - "x": 3.336392441248801, - "y": 5.675964271257023 - }, - "nextControl": { - "x": 1.2649614192324803, - "y": 3.9988486362271214 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.9420081967213114, - "y": 1.453637295081967 - }, - "prevControl": { - "x": 1.0461250953716954, - "y": 2.3108257804407923 - }, - "nextControl": { - "x": 3.2073735951122115, - "y": 0.2429250407285084 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.780020491803279, - "y": 1.453637295081967 - }, - "prevControl": { - "x": 5.528717809671569, - "y": 1.4857556755412125 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.5753698520591644, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 7.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": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_5.path b/src/main/deploy/pathplanner/paths/LEFTBlueSource3_5.path deleted file mode 100644 index 4950b13..0000000 --- a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_5.path +++ /dev/null @@ -1,96 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.665, - "y": 0.711 - }, - "prevControl": null, - "nextControl": { - "x": 2.652198449184467, - "y": 2.126958275441646 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.677, - "y": 2.948 - }, - "prevControl": { - "x": 3.177, - "y": 2.0819745962155616 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.75, - "rotationDegrees": 59.99999999999999 - } - ], - "constraintZones": [ - { - "name": "Score1", - "minWaypointRelativePos": 0.7986501687289221, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "grabFromFunnel", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "grabFromFunnel" - } - } - }, - { - "name": "armToL4", - "waypointRelativePos": 0.6, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToL4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 59.99999999999999 - }, - "reversed": false, - "folder": "SourceAuton", - "idealStartingState": { - "velocity": 0, - "rotation": -126.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/drive out path.path b/src/main/deploy/pathplanner/paths/LeftCenter1.path similarity index 62% rename from src/main/deploy/pathplanner/paths/drive out path.path rename to src/main/deploy/pathplanner/paths/LeftCenter1.path index 7556ff6..9cfa1e6 100644 --- a/src/main/deploy/pathplanner/paths/drive out path.path +++ b/src/main/deploy/pathplanner/paths/LeftCenter1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.227, - "y": 7.5 + "x": 7.22, + "y": 4.026 }, "prevControl": null, "nextControl": { - "x": 6.407636908383329, - "y": 7.5 + "x": 6.22, + "y": 4.026 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.108732876712329, - "y": 7.5 + "x": 5.682, + "y": 4.026 }, "prevControl": { - "x": 5.358732876712329, - "y": 7.5 + "x": 6.682, + "y": 4.026 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,17 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "elevatorToL1", + "waypointRelativePos": 0.5427561837455757, + "endWaypointRelativePos": null, + "command": null + } + ], "globalConstraints": { - "maxVelocity": 4.863, - "maxAcceleration": 4.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +49,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, - "folder": "CenterAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSource3_4.path b/src/main/deploy/pathplanner/paths/LeftCenter10.path similarity index 53% rename from src/main/deploy/pathplanner/paths/BlueSource3_4.path rename to src/main/deploy/pathplanner/paths/LeftCenter10.path index cfc2c12..22f58ed 100644 --- a/src/main/deploy/pathplanner/paths/BlueSource3_4.path +++ b/src/main/deploy/pathplanner/paths/LeftCenter10.path @@ -3,69 +3,65 @@ "waypoints": [ { "anchor": { - "x": 3.962, - "y": 5.268 + "x": 3.896, + "y": 5.061 }, "prevControl": null, "nextControl": { - "x": 3.1199265854866107, - "y": 6.702124460960988 + "x": 3.3960000000000004, + "y": 5.927025403784438 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.665, - "y": 7.341 + "x": 2.753083609271523, + "y": 6.814403973509933 }, "prevControl": { - "x": 1.9138844653576745, - "y": 7.317409262334031 + "x": 3.11965209606183, + "y": 6.474363469316293 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7488888888888889, - "rotationDegrees": 126.0 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { - "name": "armToHome", - "waypointRelativePos": 0.0, + "name": "elevatorToHome", + "waypointRelativePos": 0, "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToHome" - } - } + "command": null + }, + { + "name": "intake", + "waypointRelativePos": 0.500353356890455, + "endWaypointRelativePos": null, + "command": null } ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 126.0 + "velocity": 1.0, + "rotation": 160.0 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCenter11.path b/src/main/deploy/pathplanner/paths/LeftCenter11.path new file mode 100644 index 0000000..fcf7c87 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftCenter11.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.1143089356670064, + "y": 6.510713828835448 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.679, + "y": 4.936 + }, + "prevControl": { + "x": 3.179, + "y": 5.802025403784438 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "elevatorToL1", + "waypointRelativePos": 0.7010600706713782, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/markerTestPath.path b/src/main/deploy/pathplanner/paths/LeftCenter2.path similarity index 60% rename from src/main/deploy/pathplanner/paths/markerTestPath.path rename to src/main/deploy/pathplanner/paths/LeftCenter2.path index d5bb7ea..f4bc062 100644 --- a/src/main/deploy/pathplanner/paths/markerTestPath.path +++ b/src/main/deploy/pathplanner/paths/LeftCenter2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 5.682, + "y": 4.026 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 6.603042218543046, + "y": 4.453580298013245 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0, - "y": 7.0 + "x": 3.2179842715231786, + "y": 7.141287251655629 }, "prevControl": { - "x": 3.0, - "y": 7.0 + "x": 5.179283940397352, + "y": 6.63280215231788 }, "nextControl": null, "isLocked": false, @@ -33,40 +33,35 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "test", - "waypointRelativePos": 0.41844769403824317, + "name": "elevatorToHome", + "waypointRelativePos": 0, "endWaypointRelativePos": null, "command": null }, { - "name": "armToStow", - "waypointRelativePos": 0.6261484098939993, + "name": "intake", + "waypointRelativePos": 0.7844522968197882, "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToStow" - } - } + "command": null } ], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 0.0 + "velocity": 1.0, + "rotation": -179.0 }, "reversed": false, - "folder": "CenterAuton", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Piece 1.path b/src/main/deploy/pathplanner/paths/LeftCenter3.path similarity index 59% rename from src/main/deploy/pathplanner/paths/4 Piece 1.path rename to src/main/deploy/pathplanner/paths/LeftCenter3.path index 521361b..abc7ec0 100644 --- a/src/main/deploy/pathplanner/paths/4 Piece 1.path +++ b/src/main/deploy/pathplanner/paths/LeftCenter3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.04375, - "y": 7.603329918032787 + "x": 2.3, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 7.144413575168995, - "y": 7.510285096677114 + "x": 3.1143089356670064, + "y": 6.510713828835448 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.7401639344262296, - "y": 5.061936475409836 + "x": 3.852, + "y": 5.036 }, "prevControl": { - "x": 4.135758196721312, - "y": 6.104866803278689 + "x": 3.352, + "y": 5.902025403784438 }, "nextControl": null, "isLocked": false, @@ -31,10 +31,17 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "elevatorToL1", + "waypointRelativePos": 0.65, + "endWaypointRelativePos": null, + "command": null + } + ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 6.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +49,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, - "folder": "4 Piece", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_4.path b/src/main/deploy/pathplanner/paths/LeftCenter4.path similarity index 53% rename from src/main/deploy/pathplanner/paths/LEFTBlueSource3_4.path rename to src/main/deploy/pathplanner/paths/LeftCenter4.path index d2cb1fc..8a531b4 100644 --- a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_4.path +++ b/src/main/deploy/pathplanner/paths/LeftCenter4.path @@ -3,69 +3,65 @@ "waypoints": [ { "anchor": { - "x": 3.962, - "y": 2.784 + "x": 3.852, + "y": 5.036 }, "prevControl": null, "nextControl": { - "x": 3.1199299576459087, - "y": 1.3498735590169666 + "x": 3.3520000000000003, + "y": 5.902025403784438 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.665, - "y": 0.711 + "x": 2.753083609271523, + "y": 6.814403973509933 }, "prevControl": { - "x": 1.9138843232467944, - "y": 0.7345922369008364 + "x": 3.11965209606183, + "y": 6.474363469316293 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.804618117229132, - "rotationDegrees": -126.0 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { - "name": "armToHome", - "waypointRelativePos": 0.0, + "name": "elevatorToHome", + "waypointRelativePos": 0, "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToHome" - } - } + "command": null + }, + { + "name": "intake", + "waypointRelativePos": 0.5, + "endWaypointRelativePos": null, + "command": null } ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.5, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": -126.0 + "velocity": 1.0, + "rotation": 160.0 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": 119.99999999999999 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCenter5.path b/src/main/deploy/pathplanner/paths/LeftCenter5.path new file mode 100644 index 0000000..a840871 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftCenter5.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.1143089356670064, + "y": 6.510713828835448 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9388, + "y": 5.08585 + }, + "prevControl": { + "x": 3.4388, + "y": 5.951875403784438 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "elevatorToL1", + "waypointRelativePos": 0.6501766784452314, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCenter6.path b/src/main/deploy/pathplanner/paths/LeftCenter6.path new file mode 100644 index 0000000..151b8ae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftCenter6.path @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9388, + "y": 5.0858 + }, + "prevControl": null, + "nextControl": { + "x": 3.4388000000000005, + "y": 5.951825403784438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.753083609271523, + "y": 6.814403973509933 + }, + "prevControl": { + "x": 3.11965209606183, + "y": 6.474363469316293 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "elevatorToHome", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "intake", + "waypointRelativePos": 0.4989399293286273, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 160.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCenter7.path b/src/main/deploy/pathplanner/paths/LeftCenter7.path new file mode 100644 index 0000000..70ebc85 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftCenter7.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.1143089356670064, + "y": 6.510713828835448 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.069, + "y": 5.161 + }, + "prevControl": { + "x": 3.569, + "y": 6.027025403784438 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "elevatorToL1", + "waypointRelativePos": 0.7180212014134287, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCenter8.path b/src/main/deploy/pathplanner/paths/LeftCenter8.path new file mode 100644 index 0000000..c1aa453 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftCenter8.path @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.069, + "y": 5.161 + }, + "prevControl": null, + "nextControl": { + "x": 3.5690000000000004, + "y": 6.027025403784438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.753083609271523, + "y": 6.814403973509933 + }, + "prevControl": { + "x": 3.11965209606183, + "y": 6.474363469316293 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "elevatorToHome", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "intake", + "waypointRelativePos": 0.5031802120141369, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 160.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCenter9.path b/src/main/deploy/pathplanner/paths/LeftCenter9.path new file mode 100644 index 0000000..5795e90 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftCenter9.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.1143089356670064, + "y": 6.510713828835448 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.896, + "y": 5.061 + }, + "prevControl": { + "x": 3.396, + "y": 5.927025403784438 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "elevatorToL1", + "waypointRelativePos": 0.6685512367491185, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/cycle end.path b/src/main/deploy/pathplanner/paths/LoliPops1.path similarity index 70% rename from src/main/deploy/pathplanner/paths/cycle end.path rename to src/main/deploy/pathplanner/paths/LoliPops1.path index 7ae866b..ad5c32b 100644 --- a/src/main/deploy/pathplanner/paths/cycle end.path +++ b/src/main/deploy/pathplanner/paths/LoliPops1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.259, - "y": 7.16 + "x": 7.220488410596026, + "y": 7.5 }, "prevControl": null, "nextControl": { - "x": 1.7759428791896843, - "y": 6.718219444005195 + "x": 3.8319536423841054, + "y": 6.969722682119204 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.812, - "y": 5.146 + "x": 3.766, + "y": 4.986 }, "prevControl": { - "x": 3.025216145006002, - "y": 5.715044959138366 + "x": 3.421378311258278, + "y": 5.608567880794701 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.25, - "maxAcceleration": 2.5, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 126.648 + "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/one coral path.path b/src/main/deploy/pathplanner/paths/LoliPops1a.path similarity index 64% rename from src/main/deploy/pathplanner/paths/one coral path.path rename to src/main/deploy/pathplanner/paths/LoliPops1a.path index 6d07058..d8295d6 100644 --- a/src/main/deploy/pathplanner/paths/one coral path.path +++ b/src/main/deploy/pathplanner/paths/LoliPops1a.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.242380136986301, - "y": 4.191 + "x": 7.22, + "y": 7.5 }, "prevControl": null, "nextControl": { - "x": 6.242380136986311, - "y": 4.191 + "x": 3.4319412251655623, + "y": 7.332926324503313 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.828, - "y": 4.191 + "x": 3.166, + "y": 4.173 }, "prevControl": { - "x": 6.828, - "y": 4.191 + "x": 1.720449503311258, + "y": 4.049510761589405 }, "nextControl": null, "isLocked": false, @@ -33,20 +33,20 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "armToL4", - "waypointRelativePos": 0.15073115860516673, + "name": "elevatorToL2", + "waypointRelativePos": 0.7528801843318008, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "armToL4" + "name": "elevatorToL2" } } } ], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 0.6, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -54,13 +54,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, - "folder": "CenterAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/added moves.path b/src/main/deploy/pathplanner/paths/LoliPops2.path similarity index 61% rename from src/main/deploy/pathplanner/paths/added moves.path rename to src/main/deploy/pathplanner/paths/LoliPops2.path index 753bbd5..a5da135 100644 --- a/src/main/deploy/pathplanner/paths/added moves.path +++ b/src/main/deploy/pathplanner/paths/LoliPops2.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 7.768032786885246, - "y": 4.690317622950819 + "x": 3.766, + "y": 4.986 }, "prevControl": null, "nextControl": { - "x": 6.710466218147117, - "y": 4.913490891396228 + "x": 1.692528973506839, + "y": 4.860368377484329 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.892930327868852, - "y": 4.1748463114754095 + "x": 1.3147971854304634, + "y": 5.673944536423841 }, "prevControl": { - "x": 6.922774827395629, - "y": 3.9265836517518267 + "x": 1.4184009522945047, + "y": 5.44642253860477 }, "nextControl": { - "x": 6.863091805560741, - "y": 4.423059249467103 + "x": 1.2111934185664222, + "y": 5.901466534242912 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.259, - "y": 7.16 + "x": 3.146, + "y": 4.191 }, "prevControl": { - "x": 2.041099642701508, - "y": 6.595910616150221 + "x": 1.99751405530026, + "y": 4.191 }, "nextControl": null, "isLocked": false, @@ -46,18 +46,18 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.2979821679962358, - "rotationDegrees": 180.0 + "waypointRelativePos": 1.3486682808716712, + "rotationDegrees": 119.99999999999999 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.2016129032258083, + "minWaypointRelativePos": 0.846643109540634, + "maxWaypointRelativePos": 0.9031802120141325, "constraints": { - "maxVelocity": 2.25, - "maxAcceleration": 2.5, + "maxVelocity": 0.5, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -68,7 +68,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 4.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -77,13 +77,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 126.648 + "rotation": 180.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 119.99999999999999 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_1.path b/src/main/deploy/pathplanner/paths/LoliPops2a.path similarity index 53% rename from src/main/deploy/pathplanner/paths/BlueCenter3_1.path rename to src/main/deploy/pathplanner/paths/LoliPops2a.path index bf9bbf2..9fed327 100644 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_1.path +++ b/src/main/deploy/pathplanner/paths/LoliPops2a.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 7.8161423841059605, - "y": 6.603745860927152 + "x": 3.166, + "y": 4.173 }, "prevControl": null, "nextControl": { - "x": 5.709561258278145, - "y": 6.269598509933775 + "x": 2.0201014551470706, + "y": 4.103569999971643 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.93, - "y": 4.933 + "x": 1.215, + "y": 5.874 }, "prevControl": { - "x": 4.734665410240484, - "y": 5.35497294911851 + "x": 1.3186037668640413, + "y": 5.646478002180928 }, "nextControl": { - "x": 3.486754966887417, - "y": 4.700558774834437 + "x": 1.1113962331359588, + "y": 6.101521997819071 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.598, - "y": 5.564983443708609 + "x": 3.146, + "y": 4.191 }, "prevControl": { - "x": 2.004788079470199, - "y": 5.100082781456954 + "x": 1.99751405530026, + "y": 4.191 }, "nextControl": null, "isLocked": false, @@ -46,26 +46,18 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6174334140435842, - "rotationDegrees": -55.607 - }, - { - "waypointRelativePos": 1.03631961259078, - "rotationDegrees": -55.607 - }, - { - "waypointRelativePos": 1.6658595641646528, - "rotationDegrees": 142.12500000000003 + "waypointRelativePos": 1.3486682808716712, + "rotationDegrees": 119.99999999999999 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.7773851590105978, - "maxWaypointRelativePos": 1.2664310954063511, + "minWaypointRelativePos": 0.65, + "maxWaypointRelativePos": 0.8480565371024682, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 0.5, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -76,34 +68,45 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "markerTest", - "waypointRelativePos": 0.17997750281214606, + "name": "elevatorToHomeAndIntake", + "waypointRelativePos": 0.3536866359447039, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "armToStow" + "name": "elevatorToHomeAndIntake" + } + } + }, + { + "name": "waitUntilAndElevatorL1", + "waypointRelativePos": 1.5, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "waitUntilAndElevatorL1" } } } ], "globalConstraints": { - "maxVelocity": 4.863, - "maxAcceleration": 4.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0.0, - "rotation": 142.12500000000003 + "velocity": 0, + "rotation": 180.0 }, "reversed": false, - "folder": "CenterAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_3.path b/src/main/deploy/pathplanner/paths/LoliPops3.path similarity index 51% rename from src/main/deploy/pathplanner/paths/LEFTBlueSource3_3.path rename to src/main/deploy/pathplanner/paths/LoliPops3.path index 921ee89..95d15fe 100644 --- a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_3.path +++ b/src/main/deploy/pathplanner/paths/LoliPops3.path @@ -3,44 +3,55 @@ "waypoints": [ { "anchor": { - "x": 1.665, - "y": 0.711 + "x": 3.146, + "y": 4.191 }, "prevControl": null, "nextControl": { - "x": 2.551005960632313, - "y": 2.1612582904600175 + "x": 2.506105132450331, + "y": 4.112168874172185 }, "isLocked": false, - "linkedName": null + "linkedName": "LoliPops3" }, { "anchor": { - "x": 3.962, - "y": 2.784 + "x": 1.4963990066225168, + "y": 4.026 }, "prevControl": { - "x": 3.788791459029882, - "y": 2.2095447786511095 + "x": 2.1580234066310364, + "y": 4.049104433162813 + }, + "nextControl": { + "x": 0.8347746066139969, + "y": 4.002895566837187 }, - "nextControl": null, "isLocked": false, "linkedName": null - } - ], - "rotationTargets": [ + }, { - "waypointRelativePos": 0.75, - "rotationDegrees": 59.99999999999999 + "anchor": { + "x": 3.146, + "y": 3.861 + }, + "prevControl": { + "x": 2.1290303850205277, + "y": 3.9409122298355586 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LoliPops4" } ], + "rotationTargets": [], "constraintZones": [ { - "name": "Score1", - "minWaypointRelativePos": 0.7986501687289221, - "maxWaypointRelativePos": 1.0, + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8, + "maxWaypointRelativePos": 0.95, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 0.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -52,31 +63,31 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "grabFromFunnel", - "waypointRelativePos": 0, + "name": "elevatorToHomeAndIntake", + "waypointRelativePos": 0.45, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "grabFromFunnel" + "name": "elevatorToHomeAndIntake" } } }, { - "name": "armToL4", - "waypointRelativePos": 0.6, + "name": "waitUntilAndElevatorL2", + "waypointRelativePos": 1.6, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "armToL4" + "name": "waitUntilAndElevatorL2" } } } ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.2, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -84,13 +95,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": 180.0 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -126.0 + "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueCenter3_2v2FLIPPED.path b/src/main/deploy/pathplanner/paths/LoliPops4.path similarity index 54% rename from src/main/deploy/pathplanner/paths/BlueCenter3_2v2FLIPPED.path rename to src/main/deploy/pathplanner/paths/LoliPops4.path index a736ecd..cbdbb3c 100644 --- a/src/main/deploy/pathplanner/paths/BlueCenter3_2v2FLIPPED.path +++ b/src/main/deploy/pathplanner/paths/LoliPops4.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.15, - "y": 3.9018 + "x": 3.146, + "y": 3.861 }, "prevControl": null, "nextControl": { - "x": 2.5631306818181816, - "y": 3.856473295454546 + "x": 2.146, + "y": 3.861 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.642, - "y": 4.1758 + "x": 1.4019660595998582, + "y": 2.4341680463609476 }, "prevControl": { - "x": 1.4020924513506725, - "y": 4.246116200843551 + "x": 1.526583840664379, + "y": 2.6508946221253313 }, "nextControl": { - "x": 1.881907548649327, - "y": 4.105483799156448 + "x": 1.2773482785353374, + "y": 2.217441470596564 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.09, - "y": 4.2318 + "x": 3.296, + "y": 4.026 }, "prevControl": { - "x": 2.2248409090909087, - "y": 4.22026875 + "x": 2.046, + "y": 4.026 }, "nextControl": null, "isLocked": false, @@ -46,18 +46,22 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7779751332149082, - "rotationDegrees": -8.427 + "waypointRelativePos": 0.5, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 0.639225181598074, + "rotationDegrees": -119.99999999999999 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 1.0753655793026016, + "minWaypointRelativePos": 0.9017667844523025, + "maxWaypointRelativePos": 0.9498233215547651, "constraints": { - "maxVelocity": 0.6, - "maxAcceleration": 0.5, + "maxVelocity": 0.5, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -68,20 +72,15 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "gripperToGroundIntake", - "waypointRelativePos": 0, + "name": "intake", + "waypointRelativePos": 0.3396226415094339, "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "gripperToGroundIntake" - } - } + "command": null } ], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -89,13 +88,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, - "folder": "CenterAuton", + "folder": null, "idealStartingState": { - "velocity": 0.0, - "rotation": 0.0 + "velocity": 0, + "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSource3_5.path b/src/main/deploy/pathplanner/paths/LoliPops4a.path similarity index 51% rename from src/main/deploy/pathplanner/paths/BlueSource3_5.path rename to src/main/deploy/pathplanner/paths/LoliPops4a.path index 5c6722c..e7b137b 100644 --- a/src/main/deploy/pathplanner/paths/BlueSource3_5.path +++ b/src/main/deploy/pathplanner/paths/LoliPops4a.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 1.665, - "y": 7.341 + "x": 3.146, + "y": 3.861 }, "prevControl": null, "nextControl": { - "x": 2.6521875, - "y": 5.92503409090909 + "x": 2.0001010761439213, + "y": 3.930423744536579 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.677, - "y": 5.104 + "x": 1.3147971854304634, + "y": 2.3778 }, "prevControl": { - "x": 3.177, - "y": 5.9700254037844385 + "x": 1.418399022788535, + "y": 2.605322876423519 + }, + "nextControl": { + "x": 1.2111953480723918, + "y": 2.1502771235764815 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.296, + "y": 4.026 + }, + "prevControl": { + "x": 2.1475140553002596, + "y": 4.026 }, "nextControl": null, "isLocked": false, @@ -30,17 +46,17 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6711111111111111, - "rotationDegrees": -59.99999999999999 + "waypointRelativePos": 1.3486682808716712, + "rotationDegrees": -119.99999999999999 } ], "constraintZones": [ { - "name": "Score1", - "minWaypointRelativePos": 0.7986501687289221, - "maxWaypointRelativePos": 1.0, + "name": "Constraints Zone", + "minWaypointRelativePos": 0.67, + "maxWaypointRelativePos": 0.8480565371024682, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 0.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -52,31 +68,31 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "grabFromFunnel", - "waypointRelativePos": 0, + "name": "elevatorToHomeAndIntake", + "waypointRelativePos": 0.3, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "grabFromFunnel" + "name": "elevatorToHomeAndIntake" } } }, { - "name": "armToL4", - "waypointRelativePos": 0.6, + "name": "waitUntilAndElevatorL1", + "waypointRelativePos": 1.4504608294930985, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "armToL4" + "name": "waitUntilAndElevatorL1" } } } ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.2, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -84,13 +100,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 180.0 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 126.0 + "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_1v2.path b/src/main/deploy/pathplanner/paths/LoliPops5.path similarity index 67% rename from src/main/deploy/pathplanner/paths/LEFTBlueSource3_1v2.path rename to src/main/deploy/pathplanner/paths/LoliPops5.path index c096711..5bd7f74 100644 --- a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_1v2.path +++ b/src/main/deploy/pathplanner/paths/LoliPops5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.119715909090909, - "y": 2.0518 + "x": 3.296, + "y": 4.026 }, "prevControl": null, "nextControl": { - "x": 6.153790082801841, - "y": 2.310619045102521 + "x": 2.6659147350993377, + "y": 4.221129966887417 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.016, - "y": 2.784 + "x": 1.6198882450331125, + "y": 5.194515728476819 }, "prevControl": { - "x": 5.216, - "y": 2.4375898384862245 + "x": 2.048468543046358, + "y": 4.627918046357614 }, "nextControl": null, "isLocked": false, @@ -31,11 +31,11 @@ "rotationTargets": [], "constraintZones": [ { - "name": "Score1", - "minWaypointRelativePos": 0.65, - "maxWaypointRelativePos": 1.0, + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.0, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 0.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -47,13 +47,13 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "armToL4", - "waypointRelativePos": 0.6, + "name": "elevatorToHomeAndIntake", + "waypointRelativePos": 0.3, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "armToL4" + "name": "elevatorToHomeAndIntake" } } } @@ -67,14 +67,14 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 119.99999999999999 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSource3_1v2.path b/src/main/deploy/pathplanner/paths/LoliPops6.path similarity index 58% rename from src/main/deploy/pathplanner/paths/BlueSource3_1v2.path rename to src/main/deploy/pathplanner/paths/LoliPops6.path index dc1ba8b..2b89ae1 100644 --- a/src/main/deploy/pathplanner/paths/BlueSource3_1v2.path +++ b/src/main/deploy/pathplanner/paths/LoliPops6.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.119715909090909, - "y": 6.0 + "x": 0.6900869205298013, + "y": 6.146109271523178 }, "prevControl": null, "nextControl": { - "x": 6.153790082801841, - "y": 5.741180954897479 + "x": 0.8208402317880796, + "y": 5.187251655629138 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.016, - "y": 5.268 + "x": 3.296, + "y": 4.076 }, "prevControl": { - "x": 5.216, - "y": 5.614410161513775 + "x": 2.315242555573076, + "y": 4.076 }, "nextControl": null, "isLocked": false, @@ -31,11 +31,11 @@ "rotationTargets": [], "constraintZones": [ { - "name": "Score1", - "minWaypointRelativePos": 0.65, - "maxWaypointRelativePos": 1.0, + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.0, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 0.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -45,19 +45,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "armToL4", - "waypointRelativePos": 0.6, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToL4" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -67,14 +55,14 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": -119.99999999999999 + "velocity": 1.0, + "rotation": 180.0 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 100.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_2.path b/src/main/deploy/pathplanner/paths/LoliPops7.path similarity index 55% rename from src/main/deploy/pathplanner/paths/LEFTBlueSource3_2.path rename to src/main/deploy/pathplanner/paths/LoliPops7.path index 35bcbaf..d87fb27 100644 --- a/src/main/deploy/pathplanner/paths/LEFTBlueSource3_2.path +++ b/src/main/deploy/pathplanner/paths/LoliPops7.path @@ -3,44 +3,39 @@ "waypoints": [ { "anchor": { - "x": 5.016, - "y": 2.784 + "x": 3.296, + "y": 4.076 }, "prevControl": null, "nextControl": { - "x": 5.094088983078368, - "y": 2.2673557315867883 + "x": 2.6659147350993377, + "y": 4.2711299668874165 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.665, - "y": 0.711 + "x": 1.6198882450331125, + "y": 5.194515728476819 }, "prevControl": { - "x": 2.364693340150077, - "y": 1.489016138715348 + "x": 2.048468543046358, + "y": 4.627918046357614 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.85, - "rotationDegrees": -126.0 - } - ], + "rotationTargets": [], "constraintZones": [ { - "name": "Score1", + "name": "Constraints Zone", "minWaypointRelativePos": 0.0, "maxWaypointRelativePos": 0.0, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 0.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -50,36 +45,24 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "armToHome", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToHome" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.863, - "maxAcceleration": 3.5, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": -126.0 + "velocity": 1.0, + "rotation": 119.99999999999999 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": 180.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSource3_2.path b/src/main/deploy/pathplanner/paths/LoliPops8.path similarity index 53% rename from src/main/deploy/pathplanner/paths/BlueSource3_2.path rename to src/main/deploy/pathplanner/paths/LoliPops8.path index bff4c48..bd66bc7 100644 --- a/src/main/deploy/pathplanner/paths/BlueSource3_2.path +++ b/src/main/deploy/pathplanner/paths/LoliPops8.path @@ -3,44 +3,39 @@ "waypoints": [ { "anchor": { - "x": 5.016, - "y": 5.268 + "x": 0.6900869205298013, + "y": 6.146109271523178 }, "prevControl": null, "nextControl": { - "x": 5.094092151660752, - "y": 5.784643789483074 + "x": 0.8208402317880796, + "y": 5.187251655629138 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.665, - "y": 7.341 + "x": 3.296, + "y": 3.976 }, "prevControl": { - "x": 2.3646960940343025, - "y": 6.562986337944474 + "x": 2.315242555573076, + "y": 3.976 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.85, - "rotationDegrees": 126.0 - } - ], + "rotationTargets": [], "constraintZones": [ { - "name": "Score1", + "name": "Constraints Zone", "minWaypointRelativePos": 0.0, "maxWaypointRelativePos": 0.0, "constraints": { - "maxVelocity": 1.0, + "maxVelocity": 0.5, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -50,36 +45,24 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "armToHome", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "armToHome" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.863, - "maxAcceleration": 3.5, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 126.0 + "velocity": 1.0, + "rotation": 180.0 }, "reversed": false, - "folder": "SourceAuton", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 100.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/cycle start.path b/src/main/deploy/pathplanner/paths/cycle start.path deleted file mode 100644 index 0e5323b..0000000 --- a/src/main/deploy/pathplanner/paths/cycle start.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.812090163934426, - "y": 5.145850409836064 - }, - "prevControl": null, - "nextControl": { - "x": 3.0486022341182233, - "y": 5.745930884401652 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.2587090163934425, - "y": 7.159784836065573 - }, - "prevControl": { - "x": 1.7898679604513135, - "y": 6.735756842334832 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.25, - "maxAcceleration": 2.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 126.327 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 121.26373169437726 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index e043dd8..a379bcd 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,35 +1,35 @@ { - "robotWidth": 0.762, - "robotLength": 0.8636, + "robotWidth": 0.6604, + "robotLength": 0.7112, "holonomicMode": true, "pathFolders": [ - "4 Piece", - "CenterAuton", - "SourceAuton" + "LeftSourceCenterStart", + "LoliPops", + "RightSourceCenterStart" ], "autoFolders": [], - "defaultMaxVel": 4.863, - "defaultMaxAccel": 4.0, + "defaultMaxVel": 4.0, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 49.619, - "robotMOI": 3.918, + "robotMass": 43.09, + "robotMOI": 3.1344, "robotTrackwidth": 0.546, "driveWheelRadius": 0.049, - "driveGearing": 6.12, - "maxDriveSpeed": 4.863, + "driveGearing": 5.357142857142857, + "maxDriveSpeed": 5.96, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 60.0, + "driveCurrentLimit": 40.0, "wheelCOF": 0.96, - "flModuleX": 0.3048, - "flModuleY": 0.254, - "frModuleX": 0.3048, - "frModuleY": -0.254, - "blModuleX": -0.305, - "blModuleY": 0.254, - "brModuleX": -0.305, - "brModuleY": -0.254, + "flModuleX": 0.2286, + "flModuleY": 0.2032, + "frModuleX": 0.2286, + "frModuleY": -0.2032, + "blModuleX": -0.2286, + "blModuleY": 0.2032, + "brModuleX": -0.2286, + "brModuleY": -0.2032, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ea4304b..db84519 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,13 +7,17 @@ import java.io.IOException; import java.nio.file.Path; import java.util.HashMap; +import java.util.HashSet; import java.util.Map; +import java.util.Set; + import dev.doglog.DogLog; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -22,6 +26,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Unit; import edu.wpi.first.wpilibj.Filesystem; @@ -36,14 +41,13 @@ import java.rmi.MarshalException; import java.text.CollationElementIterator; import java.util.ArrayList; +import java.util.Arrays; import java.util.Collections; import java.util.List; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.util.Units; -import frc.robot.util.ArmPath; -import frc.robot.util.ArmPoint; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -54,24 +58,123 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - // public static double shootAlgaeDistance = 2; // m from center of field - public static double shootAlgaeDriveSpeed = 2.5; // max 5.81 m/s + + public static class CoralConstants { + public static final double indexerCurrentLimit = 30; + public static final String canBus = "canivore1"; + public static final double deployCurrentLimit = 65; + public static final double deployRampRate = 0.03; + public static final double deployOffset = 0.25-0.1; + public static final double homePositionIntake = deployOffset + 0.07; // deployOffset + 0.02 + public static final double deployPositionIntake = deployOffset + 0.325; // 0.27 for algae + public static final double transferPositionIntake = deployPositionIntake - 0.15; + // public static final double elevatorPositionIntake = homePositionIntake + 0.1; + public static final double forwardLimitDeploy = deployPositionIntake; + public static final double reverseLimitDeploy = homePositionIntake; + public static final double kPDeploy = 12; // 12 for 25:1; 25 for 75 : 1 + public static final double kIDeploy = 0; + public static final double kDDeploy = 0; + public static final double kGDeploy = 0.42; // 0.39 for 25:1; 0.07 for 75:1 + public static final double intakeCurrentLimit = 65; + public static final double deployGearRatio = 25.0; + public static final double deploySensorRatio = deployGearRatio; + public static final double intakeGearRatio = 5.0; + public static final double indexerGearRatio = 20.0; + public static final int shooterID = 1; + public static final int indexerID = 2; + public static final int elevatorLeftID = 3; + public static final int elevatorRightID = 4; + public static final int deployID = 5; + public static final int intakeID = 6; + public static final int intakeSensorID = 7; + public static final int indexerSensorID = 8; + public static final double intakeSensorTriggerDistance = 0.09; + public static final double indexerSensorTriggerDistance = 0.02; + public static final double indexerTransferVoltage = 16; + public static final double intakeTransferVoltage = 16; + public static final double intakingVoltage = 16; + // public static final double shooterTransferVoltage = 2; + public static final double shooterRewindVoltage = -1.5; + // public static final double elevatorTransferPosition = 2.9; + public static final double deployTolerance = 0.06; + public static final double autoShootVoltageTransfer = 1; + public static enum coralState { + empty, + coralInRange, + coralInIntake, + coralInIndexer + } + public static enum elevatorLevel { + home(0, 2, 0), + l1(1,13.85, 1.45), + l1upper(1,l1.height + 5, 1.45), + l1inside(1, l1.height + 5, 2.5), + l2(2,22.0, 3.5), + transfer(0, 3.6, 2), + panic(0, 7, 1), + visionClear(0, 5, 0); + + elevatorLevel(int level, double height, double shootVoltage) { + this.height = height; + this.level = level; + this.shootVoltage = shootVoltage; + } + public static elevatorLevel fromLevel(int level) { + for (elevatorLevel elev : elevatorLevel.values()) { + if (elev.level == level) { + return elev; + } + } + return home; + } + public final double shootVoltage; + public final double height; + public final int level; + + } + + + public static final int pulleyTeeth = 12; + public static final double pulleyToothWidth = 5.0; // mm + public static final double pulleyCircumferenceMillimeters = pulleyTeeth * pulleyToothWidth; // mm + public static final double pulleyCircumferenceInches = pulleyCircumferenceMillimeters / 25.4; // inches + public static final double elevatorSensorRatio = 1/pulleyCircumferenceInches; // rot/inch + public static final double kP = 0.70; //1.05 + public static final double kI = 0.00; + public static final double kD = 0.0; + public static final double kG = 0.42; //0.385 + public static final double kS = 0.0; + public static final double elevatorCurrentLimit = 50; + public static final double elevatorRampRate = 0.05; + public static final double elevatorTolerance = 0.15; // in + public static final double homePos = 1.5; // in + public static final double forwardLimit = 24; + public static final double reverseLimit = 0; + + public static final double shooterCurrentLimit = 40; + + + } public static class ClimbConstants { - public static final int kLeftID = 61; - public static final int kRightID = 62; - public static final double currentLimit = 105; - // public static final double ampTriggeredCurrentLimit = 2; - public static final double power = 0.3; - public static final double deployPosition = -70.5; // rot - // public static final double climbedPosition = -18; // rot - public static final double rampRate = 0.2; + public static final double gearRatio = 125.0; + public static final double pulleyDiameter = 2.5; // inches + public static final double inchesPerRotation = pulleyDiameter * 2.0 * Math.PI / gearRatio; + public static final int winchId = 61; + public static final double currentLimit = 20; + public static final double power = 0.7; + public static final double deployPositionInches = -10.5; + public static final double climbedPositionInches = 32; + public static final double deployPosition = deployPositionInches / inchesPerRotation; // rot of kraken + public static final double homePosition = -0; // rot of kraken + public static final double climbedPosition = climbedPositionInches / inchesPerRotation; // rot of kraken + public static final double rampRate = 0.08; public static final double kP = 1.2; public static final double kI = 0.0; public static final double kD = 0.0; public static final String canBus = "canivore1"; - public static final double servoOpenPosition = 1.0; - public static final double servoCloseposition = 0.0; + // public static final double servoOpenPosition = 1.0; + // public static final double servoCloseposition = 0.0; } public static class OperatorConstants { public static final int kDriverControllerPort = 0; @@ -102,17 +205,18 @@ public static class OperatorConstants { static { // Key: cardinal joystick distance // Value: % max speed - joystickMap.put(0.00, 0.00); - joystickMap.put(0.07, 0.05); - joystickMap.put(0.18, 0.1); - joystickMap.put(0.29, 0.15); - joystickMap.put(0.40, 0.2); - joystickMap.put(0.50, 0.3); - joystickMap.put(0.60, 0.50); - joystickMap.put(0.70, 0.65); - joystickMap.put(0.80, 0.80); - joystickMap.put(0.90, 1.00); - joystickMap.put(1.00, 1.00); + joystickMap.put(0.0, 0.0); + joystickMap.put(0.1, 0.02); + joystickMap.put(0.2, 0.05); + joystickMap.put(0.3, 0.1); + joystickMap.put(0.4, 0.15); + joystickMap.put(0.5, 0.21); + joystickMap.put(0.6, 0.30); + joystickMap.put(0.7, 0.45); + joystickMap.put(0.8, 0.62); + joystickMap.put(0.9, 0.85); + joystickMap.put(0.95, 1.00); + joystickMap.put(1.0, 1.00); joystickMap.put(-0.07, -0.10); joystickMap.put(-0.18, -0.15); @@ -127,12 +231,26 @@ public static class OperatorConstants { } } +public static class AutoDropoff { + public static final double robotThickness = Units.inchesToMeters(11+3.125); + public static final double distanceToAutoDrive = Units.feetToMeters(4); // distance between station and bumpers + public static final double L1waitToHome = 1.5; // s + public static final double loopPeriodSecs = 0.02; + public static final ProfiledPIDController driveController = + new ProfiledPIDController( + 15, 0, 0.1, new TrapezoidProfile.Constraints(Constants.Vision.MAX_VELOCITY,Constants.Vision.MAX_ACCELARATION), loopPeriodSecs); //10, 0, 0 + public static final ProfiledPIDController thetaController = + new ProfiledPIDController( + 7, 0,0, new TrapezoidProfile.Constraints(Math.toRadians(Constants.Vision.MAX_VELOCITY_ROTATION), Math.toRadians(Constants.Vision.MAX_ACCELARATION_ROTATION)), loopPeriodSecs); //3, 10, 0 +} public static class Vision { + public static boolean DOGLOG_ENABLED = false; public static final boolean USE_VISION = true; + public static final boolean USE_QUESTNAV = true; public static final boolean USE_BUTTON_BOARD = true; @@ -140,14 +258,14 @@ public static class Vision { public static final String kCameraNameFront = "LeftCamera"; public static final Transform3d kRobotToCamFront = - new Transform3d(new Translation3d(Units.inchesToMeters(14 - 5.03), Units.inchesToMeters((12 - 5.56)), Units.inchesToMeters(17.00)), - new Rotation3d(Math.toRadians(-0), Math.toRadians(20), Math.toRadians(15))); //TODO: determine XYZ + new Transform3d(new Translation3d(-Units.inchesToMeters(8.503), Units.inchesToMeters((6.281)), Units.inchesToMeters(8.216)), + new Rotation3d(Math.toRadians(-0), Math.toRadians(-15), Math.toRadians(180+35))); //TODO: determine XYZ public static final String kCameraNameBack = "RightCamera"; public static final Transform3d kRobotToCamBack = - new Transform3d(new Translation3d(Units.inchesToMeters(14 - 5.03), -Units.inchesToMeters((12 - 5.56)), Units.inchesToMeters(17.00)), - new Rotation3d(Math.toRadians(-0), Math.toRadians(20), Math.toRadians(-15))); //TODO: determine XYZ + new Transform3d(new Translation3d(-Units.inchesToMeters(8.503), -Units.inchesToMeters((6.281)), Units.inchesToMeters(8.216)), + new Rotation3d(Math.toRadians(-0), Math.toRadians(-15), Math.toRadians(180-35))); //TODO: determine XYZ // The layout of the AprilTags on the field public static final AprilTagFieldLayout kTagLayout = @@ -197,6 +315,28 @@ public static class Vision { public static final double kCoralCenterUprightHeight = 0.225425; //in meters public static final double kCoralCenterFallenHeight = 0.0508; //in meters //Testboard Dims. + // public static final double kLimeLightHeight = 0.120; + // public static final double kLimeLightXOffset = 0; + // public static final double kLimeLightYOffset = 0; + // public static final double kLimeLightAOD = -15.0; + + //NERDSwerve Dims. + // public static final double kLimeLightHeight = 0.18; + // public static final double kLimeLightXOffset = 0; + // public static final double kLimeLightYOffset = 0.14; + // public static final double kLimeLightAOD = -15.0; + + //Comp. Dims. + public static final double kLimeLightHeight = 1.02997; + public static final double kLimeLightXOffset = 0; + public static final double kLimeLightYOffset = -0.0007366; + public static final double kLimeLightAOD = -40.0; + + public static boolean kCoralTargeted = false; + public static boolean kCoralInRange = false; + // public static boolean kCoralAutoTarget = true; + + public static final boolean USE_LIMELIGHT = true; @@ -204,11 +344,11 @@ public static class Vision { static { // reefLevelOffsetsMap.put(ReefLevel.L1Inside, new Transform2d(Units.inchesToMeters(24), 0, new Rotation2d(Math.toRadians(0)))); - reefLevelOffsetsMap.put(ReefLevel.L1Top, new Transform2d(Units.inchesToMeters(26), 0, new Rotation2d(Math.toRadians(180)))); - reefLevelOffsetsMap.put(ReefLevel.L1, new Transform2d(Units.inchesToMeters(26), 0, new Rotation2d(Math.toRadians(180)))); - reefLevelOffsetsMap.put(ReefLevel.L2, new Transform2d(Units.inchesToMeters(25.5), 0, new Rotation2d(Math.toRadians(180)))); - reefLevelOffsetsMap.put(ReefLevel.L3, new Transform2d(Units.inchesToMeters(28.5), 0, new Rotation2d(Math.toRadians(180)))); - reefLevelOffsetsMap.put(ReefLevel.L4, new Transform2d(Units.inchesToMeters(25.5), 0, new Rotation2d(Math.toRadians(180)))); + reefLevelOffsetsMap.put(ReefLevel.L1Top, new Transform2d(Units.inchesToMeters(26), 0, new Rotation2d(Math.toRadians(0)))); + reefLevelOffsetsMap.put(ReefLevel.L1, new Transform2d(Units.inchesToMeters(26), 0, new Rotation2d(Math.toRadians(0)))); + reefLevelOffsetsMap.put(ReefLevel.L2, new Transform2d(Units.inchesToMeters(23), 0, new Rotation2d(Math.toRadians(0)))); + reefLevelOffsetsMap.put(ReefLevel.L3, new Transform2d(Units.inchesToMeters(28.5), 0, new Rotation2d(Math.toRadians(0)))); + reefLevelOffsetsMap.put(ReefLevel.L4, new Transform2d(Units.inchesToMeters(25.5), 0, new Rotation2d(Math.toRadians(0)))); } @@ -220,6 +360,11 @@ public static class Vision { coralStationOffSetsMap.put(CoralStations.RIGHT, new Transform2d(Units.inchesToMeters(16.5), 0, new Rotation2d(Math.toRadians(180)))); } + + public static final Set nonReefTagFiducialIDs = new HashSet<>(Set.of(1, 2, 3, 4, 5, 12, 13, 14, 15, 16)); + + public static final boolean QUEST_ENABLED = false; + } public static final double gyroP = 2; @@ -238,302 +383,27 @@ public static class Vision { // } + -public static class BucketConstants { - public static final int leftSensorPort = 1; - public static final int rightSensorPort = 2; - - public static final int timesForBucketToTestPositive = 8; // number of consecutive loops a reading must be within detected coral/algae distance in order to test positive - public static final double coralDistance = 0.12; // maximum distance of distance sensor readings in order to consider coral to be detected - // public static final double algaeDistance = 0.05; // maximum distance of distance sensor readings in order to consider algae to be detected - -} - public final class ArmGains { - public static final double shoulderP = 200.0; //TODO CHANGE SOME OF THIS LATER //52.0 - public static final double shoulderI = 0.0; - public static final double shoulderD = 40.0; - public static final double elbowP = 200.0;//20.0 - public static final double elbowI = 0.0; - public static final double elbowD = 40.0; - public static final double wristP = 18.0; //20.0 - public static final double wristG = 0.0; //20.0 - public static final double wristI = 0.0; - public static final double wristD = 0.0; - public static final double wristVelocity = 3; - public static final double wristAcceleration = 2; - public static final double gripperP = 28.0; // 10.0 - public static final double gripperI = 0.0; - public static final double gripperD = 0.0; - public static final double shoulderS = 0.0; - public static final double shoulderG = 0.6; // 0.25 - public static final double shoulderV = 0.0; - public static final double shoulderA = 0.0; - public static final double elbowS = 0.0; - public static final double elbowG = 0.6;//0.3 - public static final double elbowV = 0.0; - public static final double elbowA = 0.0; - public static final double cruiseVelocity = 0.5; - public static final double cruiseAcceleration = 2.0; - } - public static class ArmVelocityGains{ - public static final double shoulderP = 10.0; //TODO CHANGE SOME OF THIS LATER //52.0 - public static final double shoulderI = 0.0; - public static final double shoulderD = 0.0; - public static final double elbowP = shoulderP*0.4;//20.0 - public static final double elbowI = 0.0; - public static final double elbowD = 0.0; - public static final double shoulderS = 0.0; - public static final double shoulderG = 0.6; // 0.25 - public static final double shoulderV = 0.0; - public static final double shoulderA = 0.0; - public static final double elbowS = 0.0; - public static final double elbowG = 0.6;//0.3 - public static final double elbowV = 0.0; - public static final double elbowA = 0.0; - - public static final double lookAheadDistance = 15.0; - public static final double lookAheadDistanceBeforeInflecting = 1; - public static final double endDistance = 13.0; - public static final double linearApproximationTime = 0.2; // seconds - public static final double velocity = 65;// does not seem to have any effect - public static final double maxMotorVelocity = 3.0; - public static final double arcRadius = 1; - public static final int arcPoints = 10; - public static final double interpolationDistance = 0.5; // inches - public static final double interpolationAngle = 0.2; // deg - } - - - public static class ArmConstants { - - public static final double elbowSlewRate = 4; // accel in rot/s/s - public static final double shoulderSlewRate = 4; - - - public static final int shoulderMotorLeftPort = 41; - public static final int shoulderMotorRightPort = 42; - public static final int elbowMotorLeftPort = 43; - public static final int elbowMotorRightPort = 44; - public static final int wristMotorPort = 45; - public static final int gripperMotorPort = 46; - public static final String armCanBus = "canivore1"; - - public static final double currentLimitShoulder = 30.0;//25 - public static final double currentLimitElbow = 30.0;//25 - public static final double currentLimitWrist = 60.0; //35 - public static final double gripperCurrentLimitDefault = 10.0;//10 - public static final double gripperPowerDefault = 0.1; - - - //shoulder true offset: 34.513 deg below forward horizontal - //shoulder gearbox: 75:1 - //shoulder stage 0: 36:26 - public static final double shoulderGearRatio = 125.0*36.0/26.0; - - // elbow true offset: 122.198 deg above forward horizontal - // elbow gearbox: 75:1 - //elbow stage 0: 38:26 - // elbow stage 1: 50:50 - public static final double elbowGearRatio = 75.0*38.0/26.0; - - //wrist up/down gearbox: 25:1 - //wrist up/down stage 0: 50:50 - //wrist up/down stage 1: 50:50 - //wrist up/down stage 2: 35:50 - public static final double wristGearRatio = 25.0*50.0/50.0*35.0/50.0; - public static final double gripperGearRatio = 5.0*5.0 / 3.0; - public static final double baseStageLength = 23.158; - public static final double secondStageLength = 25.475; - public static final double totalStageLength = baseStageLength + secondStageLength; - - - public static final double shoulderRadPerRot = shoulderGearRatio; - public static final double elbowRadPerRot = elbowGearRatio; - public static final double wristRadPerRot = wristGearRatio; - public static final double gripperRadPerRot = gripperGearRatio; - public static final double gripperOffset = 0; - public static final double shoulderOffset = -0.476 / 2.0 / Math.PI; // radians, fwd = 0 - public static final double elbowOffset = 2.032 / 2.0 / Math.PI; // negative of measurement - public static final double wristOffset = 4.949 / 2.0 / Math.PI; // - - /** wrist flip belting ratio between elbow and the wrist */ - public static final double wristToElbowRatio = 1.0/(35.0 / 50.0); - public static final double rightServoOffset = 0.0; - public static final double leftServoOffset = 0.0; - - - } - - public static class ArmSetpoints { - - public static final int setPointCount = 14; - public static final Translation2d home = new Translation2d(8.5, 11.5);//new Translation2d(15.65, Rotation2d.fromDegrees(60)); //safest home and also closest possible distance arm is allowed to get to central joint - public static final double homeWrist = Units.degreesToRadians(110); - /** - * contains a list of endpoints (0, 0) in arm coordinates = (6.4, 22.0) in bumper-relative coordinates - * @home 0 - * @hotel 13 (position right before going to home) - * - * **Reef** - * @L1 1 - * @L2 2 (-2.2, 31.1) from bumper @ 55 degrees to horizontal - * @L3 3 (-2.2, 47.0) from bumper @ 55 degrees to horizontal - * @L4 4 (-2.0, 71.9) from bumper @ 90 degrees to horizontal - * @L2.5Algae 5 (-8, 35.8) from bumper @ 0 degrees to horizontal - * @L3.5Algae 6 (-8, 51.7) from bumper @ 0 degrees to horizontal - * - * **Coral Intake** - * @grabFromFunnelPreparePosition 7 - * @groundintake 8 - * - * **Algae dropoff** - * @AlgaeBargePrepare 9 - * @AlgaeBargeThrow 10 - * @processor 12 - * - * ** Climb ** - * @climbPosition 11 - */ - public static ArmPoint[] armSetPoints = new ArmPoint[ArmSetpoints.setPointCount]; - static{ - armSetPoints[0] = new ArmPoint(home, homeWrist); - - // armSetPoints[1] = new ArmPoint(home.rotateBy(Rotation2d.fromDegrees(65)), Units.degreesToRadians(180)); - // armSetPoints[2] = new ArmPoint(home.rotateBy(Rotation2d.fromDegrees(35)), Units.degreesToRadians(150)); - // armSetPoints[3] = new ArmPoint(new Translation2d(-8, 27), Units.degreesToRadians(145)); - // armSetPoints[4] = new ArmPoint(new Translation2d(ArmConstants.totalStageLength, Rotation2d.fromDegrees(90)), Units.degreesToRadians(120)); - - - double dropoffDistanceFromBumper = -5.0; - Translation2d gripperOffset = new Translation2d(9.9, -2.0); - // Translation2d gripperCoralOffset = gripperOffset.plus(new Translation2d(-1.9, 9.0)); - // Translation2d gripperCoralOffsetInverted = gripperCoralOffset.plus(new Translation2d(0.0, -11.875)); - Translation2d gripperAlgaeOffset = gripperOffset.plus(new Translation2d(8.0, 0.0)); - - armSetPoints[1] = new ArmPoint(home, Units.degreesToRadians(110)); - armSetPoints[2] = new ArmPoint(new Translation2d(-10.7, 26), Units.degreesToRadians(180 + 105));//new ArmPoint(new Translation2d(-8.6, 20.1), Units.degreesToRadians(180 + 145)).add(new Translation2d(dropoffDistanceFromBumper, 0)).withGripperOffset(gripperCoralOffsetInverted); - armSetPoints[3] = new ArmPoint(new Translation2d(-10.7, 27.5), Units.degreesToRadians(105));//new ArmPoint(new Translation2d(-12.6, 28.0), Units.degreesToRadians(125)).add(new Translation2d(dropoffDistanceFromBumper, 0)).withGripperOffset(gripperCoralOffset); - armSetPoints[4] = new ArmPoint(new Translation2d(ArmConstants.totalStageLength, Rotation2d.fromDegrees(95)), Units.degreesToRadians(115));//new ArmPoint(new Translation2d(-8.4, 49.9), Units.degreesToRadians(150)).add(new Translation2d(dropoffDistanceFromBumper, 0)).withGripperOffset(gripperCoralOffset); - - armSetPoints[5] = new ArmPoint(new Translation2d(-13.4, 20.5), Units.degreesToRadians(180)).add(new Translation2d(dropoffDistanceFromBumper, 0)).withGripperOffset(gripperAlgaeOffset); - armSetPoints[6] = new ArmPoint(new Translation2d(-13.4, 36.5), Units.degreesToRadians(180)).add(new Translation2d(dropoffDistanceFromBumper, 0)).withGripperOffset(gripperAlgaeOffset); - - armSetPoints[7] = new ArmPoint(home, Units.degreesToRadians(223.5)).rotateElbowBy(Rotation2d.fromDegrees(-30)); - armSetPoints[8] = new ArmPoint(new Translation2d(32.2, -14.6), true, 0.0); - - armSetPoints[9] = new ArmPoint(new Translation2d(20, Rotation2d.fromDegrees(90)), Units.degreesToRadians(80)); - armSetPoints[10] = new ArmPoint(new Translation2d(ArmConstants.totalStageLength, Rotation2d.fromDegrees(90)), Units.degreesToRadians(80)); - - armSetPoints[11] = new ArmPoint(new Translation2d(-21.5, 0.1), Units.degreesToRadians(270)); // (-21.5, 0.1) for legal position // (-33, 30) for test position - - armSetPoints[12] = new ArmPoint(home.rotateBy(Rotation2d.fromDegrees(110)), Units.degreesToRadians(180 + 20)); - - armSetPoints[13] = new ArmPoint(home, Units.degreesToRadians(200)); - - for (int i = 0; i < armSetPoints.length; i++) { - if (armSetPoints[i].position.getNorm() > ArmConstants.totalStageLength) { - armSetPoints[i] = new ArmPoint(new Translation2d(ArmConstants.totalStageLength, armSetPoints[i].position.getAngle()), armSetPoints[i].inBend, armSetPoints[i].wrist); - SmartDashboard.putBoolean("armpoint limit trip", true); - System.out.println("armsetpoint trip " + i); - } else if (armSetPoints[i].position.getNorm() < ArmSetpoints.home.getNorm()) { - armSetPoints[i] = new ArmPoint(new Translation2d(ArmSetpoints.home.getNorm(), armSetPoints[i].position.getAngle()), armSetPoints[i].inBend, armSetPoints[i].wrist); - SmartDashboard.putBoolean("armpoint limit trip", true); - System.out.println("armsetpoint trip " + i); - } - System.out.println("Arm Setpoint " + i + " " + armSetPoints[i].position.toString()); - } - } - - /** list of dunk setpoints for reef dropoff - * @L1 1 - * @L2 2 - * @L3 3 - * @L4 4 - */ - public static ArmPoint[] armSetPointsDunkAuto = new ArmPoint[5]; // autodunks +public static class LEDConstants { + public static final double scrollSpeed = 40; + public static final double numOfSteps = 3.0; + public static final int kPort = 0; + public static final int kLength = 125; + public static final double blinkSeconds = 1.0; + public static InterpolatingDoubleTreeMap driveToPoseDistanceMap = new InterpolatingDoubleTreeMap(); static { - armSetPointsDunkAuto[0] = armSetPoints[0]; - armSetPointsDunkAuto[1] = armSetPoints[1]; - armSetPointsDunkAuto[2] = armSetPoints[2].addToWristFlip(Units.degreesToRadians(-45)).add(new Translation2d(-9, new Rotation2d(armSetPoints[2].wrist + (Math.PI*0.5)))); - armSetPointsDunkAuto[3] = armSetPoints[3].add(new Translation2d(-9, new Rotation2d(armSetPoints[3].wrist - (Math.PI*0.2)))).addToWristFlip(Units.degreesToRadians(90)); - armSetPointsDunkAuto[4] = armSetPoints[4].addToWristFlip(Units.degreesToRadians(60)).add(new Translation2d(-2, -4)); + // Key: cardinal joystick distance + // Value: % max speed + driveToPoseDistanceMap.put(0.02, 0.00); + driveToPoseDistanceMap.put(0.021, 0.25); + driveToPoseDistanceMap.put(0.05, 0.35); + driveToPoseDistanceMap.put(0.1, 0.45); + driveToPoseDistanceMap.put(0.2, 0.55); + driveToPoseDistanceMap.put(0.5, 0.65); + driveToPoseDistanceMap.put(1.0, 0.80); + driveToPoseDistanceMap.put(3.0, 1.0); } - public static ArmPoint[] armSetPointsMicroAdjust = new ArmPoint[5]; // manual dunks - static { - armSetPointsMicroAdjust[0] = armSetPoints[0]; - armSetPointsMicroAdjust[1] = armSetPoints[1]; - armSetPointsMicroAdjust[2] = armSetPoints[2].add(new Translation2d(0, -5)); - armSetPointsMicroAdjust[3] = armSetPoints[3].add(new Translation2d(-9, new Rotation2d(armSetPoints[3].wrist - (Math.PI*0.3)))).addToWristFlip(Units.degreesToRadians(45)); - armSetPointsMicroAdjust[4] = armSetPoints[4].addToWristFlip(Units.degreesToRadians(60)); - - } - - - - - /** - * contains a list of intermediate points from first index to second index - */ - @SuppressWarnings("unchecked") - public static List[][] intermediatePoints = new List[ArmSetpoints.setPointCount][ArmSetpoints.setPointCount]; - static{ - // intermediatePoints[0][1] = (List) List.of(new ArmPoint(new Translation2d(22.43, 30.52)), new ArmPoint(new Translation2d(47.519, 10.114))); -intermediatePoints[0][8] = (List) List.of((new ArmPoint(new Translation2d(34.5, 16.5), true))); -intermediatePoints[4][8] = (List) List.of((new ArmPoint(new Translation2d(34.5, 16.5), true))); - -// intermediatePoints[6][5] = List.of(armSetPoints[6].withWrist(Units.degreesToRadians(45))); - // intermediatePoints[1][4] = (List) List.of(new ArmPoint(new Translation2d(30.0, 24.0))); - - for (int i = 0; i < ArmSetpoints.setPointCount; i++){ - for (int j = 0; j < i; j++){ - if (intermediatePoints[i][j] == null && intermediatePoints[j][i] != null){ - intermediatePoints[i][j] = new ArrayList<>(); - intermediatePoints[i][j].addAll(intermediatePoints[j][i]); - Collections.reverse(intermediatePoints[i][j]); - } else if (intermediatePoints[i][j] != null && intermediatePoints[j][i] == null){ - intermediatePoints[j][i] = new ArrayList<>(); - intermediatePoints[j][i].addAll(intermediatePoints[i][j]); - Collections.reverse(intermediatePoints[j][i]); - } else if (intermediatePoints[i][j] == null && intermediatePoints[j][i] == null) { - intermediatePoints[i][j] = List.of(); - intermediatePoints[j][i] = List.of(); - } - } - } - } - - - // /** - // * contains a full path between two points - // */ - // public static ArmPath[][] armPaths = new ArmPath[ArmSetpoints.setPointCount][ArmSetpoints.setPointCount]; - // static { - // for (int i = 0; i < ArmSetpoints.setPointCount; i++){ - // for (int j = 0; j < ArmSetpoints.setPointCount; j++){ - // if (i != j){ - // if (intermediatePoints[i][j].size() != 0) { - // armPaths[i][j] = new ArmPath(intermediatePoints[i][j], armSetPoints[i], armSetPoints[j]); - // } else { - // armPaths[i][j] = new ArmPath(armSetPoints[i], armSetPoints[j]); - // } - // // System.out.println("Created arm path" + i + " " + j); - // // System.out.println(armPaths[i][j]); - // } else { // set path to just the endpoint - // armPaths[i][j] = new ArmPath(List.of(armSetPoints[i])); - // } - // } - // } - // } - - } - - - public static class LEDConstants { - public static final double scrollSpeed = 40; - public static final double numOfSteps = 3.0; - public static final int kPort = 3; - public static final int kLength = 120; - public static final double blinkSeconds = 1.0; - } +} } diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 6aecdb3..896cd79 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -12,12 +12,22 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogOutput; +import frc.robot.Constants.AutoDropoff; +import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.Vision; +import frc.robot.util.AllianceFlipUtil; import java.util.ArrayList; import java.util.Arrays; import java.util.HashMap; import java.util.List; import java.util.Map; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import com.fasterxml.jackson.databind.node.POJONode; + import dev.doglog.DogLog; @@ -243,4 +253,56 @@ public enum CoralStations { } +// public static Pose2d getReefPoseOffset(DoubleSupplier offsetScalar, int reefFace) { +// Pose2d facePose = Reef.centerFaces[reefFace]; +// // Transform2d offsetVector = new Transform2d(, , 0.0); +// Translation2d offsetVectorTranslation2d = new Translation2d(offsetScalar.getAsDouble(), Rotation2d.fromDegrees(180 - (60 * reefFace))); +// Transform2d offsetVector = new Transform2d(offsetVectorTranslation2d, new Rotation2d(0)); +// Pose2d drivePose = facePose.plus(offsetVector); +// return drivePose; +// } + + public static int getClosestFace(Supplier robotPose) { + int minIndex = 0; + double minDistance = Double.MAX_VALUE; + Transform2d currentFacePose = new Transform2d(0.0, 0.0, new Rotation2d(0.0)); + double currentDistance = 0.0; + for (int i=0; i<6; i++) { + currentFacePose = Reef.centerFaces[i].minus(robotPose.get()); + currentDistance = currentFacePose.getTranslation().getNorm(); + if(currentDistance < minDistance) { + minDistance = currentDistance; + minIndex = i; + } + } + return minIndex; + } + + /** + * gets only L2 pose + * @param robotPose + * @return + */ + public static Pose2d getClosestPole(Supplier robotPose) { + int minIndex = 0; + double minDistance = Double.MAX_VALUE; + Transform2d currentFacePose = new Transform2d(0.0, 0.0, new Rotation2d(0.0)); + double currentDistance = 0.0; + for (int i=0; i<12; i++) { + currentFacePose = Reef.branchPositions2d.get(i).get(ReefLevel.L2).minus(robotPose.get()); + currentDistance = currentFacePose.getTranslation().getNorm(); + if(currentDistance < minDistance) { + minDistance = currentDistance; + minIndex = i; + } + } + return AllianceFlipUtil.apply(FieldConstants.Reef.branchPositions.get(minIndex).get(ReefLevel.L2).toPose2d()).plus(Constants.Vision.reefLevelOffsetsMap.get(ReefLevel.L2)); + + // return Reef.branchPositions2d.get(minIndex).get(ReefLevel.L2); + } + + public static boolean getCloseEnoughForAutoDrive(Supplier robotPose) { + return robotPose.get().getTranslation().getDistance(Reef.center) < AutoDropoff.distanceToAutoDrive + Reef.faceLength + AutoDropoff.robotThickness; + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/QuestNav/NerdQuestNav.java b/src/main/java/frc/robot/QuestNav/NerdQuestNav.java new file mode 100644 index 0000000..7e69b1e --- /dev/null +++ b/src/main/java/frc/robot/QuestNav/NerdQuestNav.java @@ -0,0 +1,364 @@ +package frc.robot.QuestNav; + +import static edu.wpi.first.units.Units.Degrees; + +import java.util.Optional; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.FloatArraySubscriber; +import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/** Add your docs here. */ +public class NerdQuestNav { + private boolean initializedPosition = false; + public static boolean isActive = false; + private String networkTableRoot = "questnav"; + private NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); + private NetworkTable networkTable; + private Transform3d robotToQuest; + private Pose3d initPose = new Pose3d(); + private Transform3d softResetTransform = new Transform3d(); + private Pose3d softResetPose = new Pose3d(); + private double xScale = 1.0; + + private IntegerEntry miso; + private IntegerPublisher mosi; + + private IntegerSubscriber frameCount; + private DoubleSubscriber timestamp; + private FloatArraySubscriber position; + private FloatArraySubscriber quaternion; + private FloatArraySubscriber eulerAngles; + private DoubleSubscriber battery; + private double startTimestamp; + + private ChassisSpeeds velocity; + private Pose3d previousPose; + private double previousTime; + private final double TIMESTAMP_DELAY = 0.002; + + private long previousFrameCount; + + private Translation2d _calculatedOffsetToRobotCenter = new Translation2d(); + private int _calculatedOffsetToRobotCenterCount = 0; + + private final SwerveRequest.ApplyRobotSpeeds m_ApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); + + /** Subscriber for heartbeat requests */ + private DoubleSubscriber heartbeatRequestSub; + /** Publisher for heartbeat responses */ + private DoublePublisher heartbeatResponsePub; + /** Last processed heartbeat request ID */ + private double lastProcessedHeartbeatId = 0; + + public enum QuestCommand { + RESET(1); + + public final int questRequestCode; + + private QuestCommand(int command) { + this.questRequestCode = command; + } + + public int getQuestRequest() { + return questRequestCode; + } + } + + public NerdQuestNav(Transform3d robotToQuest) { + super(); + this.robotToQuest = robotToQuest; + setupNetworkTables(networkTableRoot); + setupInitialTimestamp(); + } + + public NerdQuestNav(Transform3d robotToQuest, String networkTableRoot) { + super(); + this.robotToQuest = robotToQuest; + this.networkTableRoot = networkTableRoot; + setupNetworkTables(networkTableRoot); + setupInitialTimestamp(); + } + + /** Process heartbeat requests from Quest and respond with the same ID */ + public void processHeartbeat() { + double requestId = heartbeatRequestSub.get(); + // Only respond to new requests to avoid flooding + if (requestId > 0 && requestId != lastProcessedHeartbeatId) { + heartbeatResponsePub.set(requestId); + lastProcessedHeartbeatId = requestId; + } + } + + private void setupInitialTimestamp() { + startTimestamp = timestamp.get(); + } + + private void setupNetworkTables(String root) { + //PubSubOption periodic = PubSubOption.periodic(0); + //PubSubOption sPeriodic = PubSubOption.periodic(30); + networkTable = networkTableInstance.getTable(root); + miso = networkTable.getIntegerTopic("miso").getEntry(0); + mosi = networkTable.getIntegerTopic("mosi").publish(); + frameCount = networkTable.getIntegerTopic("frameCount").subscribe(0); + timestamp = networkTable.getDoubleTopic("timestamp").subscribe(0.0); + position = networkTable.getFloatArrayTopic("position").subscribe(new float[3]); + quaternion = networkTable.getFloatArrayTopic("quaternion").subscribe(new float[4]); + eulerAngles = networkTable.getFloatArrayTopic("eulerAngles").subscribe(new float[3]); + battery = networkTable.getDoubleTopic("battery").subscribe(0.0); + heartbeatRequestSub = networkTable.getDoubleTopic("heartbeat/quest_to_robot").subscribe(0.0); + heartbeatResponsePub = networkTable.getDoubleTopic("heartbeat/robot_to_quest").publish(); + } + + public Translation3d getRawPosition() { + return new Translation3d(position.get()[2], -position.get()[0] * xScale, position.get()[1]); + } + + private Translation3d rotateAxes(Translation3d raw, Rotation3d rotation) { + return raw.rotateBy(rotation); + } + + private Translation3d correctWorldAxis(Translation3d rawPosition) { + return rotateAxes(rawPosition, robotToQuest.getRotation()); + } + + public Rotation3d getRawRotation() { + float[] euler = eulerAngles.get(); + return new Rotation3d(Degrees.of(euler[2]), Degrees.of(euler[0]), Degrees.of(-euler[1])); + } + + public Optional getRobotPose() { + if (RobotBase.isReal()) { + Pose3d pose = new Pose3d(getPosition(), getRotation()); + return Optional.of(pose); + } else { + return Optional.empty(); + } + } + + // @Override + // public List getObservations() { + // List observations = new ArrayList<>(); + // double calib = getConfidence(); + // if (isActive) { + // observations.add(new PoseObservation(getCaptureTime(), 0, 0, VecBuilder.fill(calib, calib, calib*0.01), + // new Pose3d(getPosition(), getRotation()))); + // } + // return observations; + // } + + public Translation3d getProcessedPosition() { + Translation3d correctedWorldAxis = correctWorldAxis(getRawPosition()); + Translation3d offsetCorrection = correctedWorldAxis + .plus(robotToQuest.getTranslation()) + .plus(robotToQuest.getTranslation().times(-1).rotateBy(new Rotation3d(0, 0, getRawRotation().getZ()))); + Translation3d rotatedAxis = rotateAxes(offsetCorrection, initPose.getRotation()); + Translation3d hardResetTransformation = rotatedAxis.plus(initPose.getTranslation()); + return hardResetTransformation; + } + + public Translation3d getPosition() { + Translation3d hardResetTransform = getProcessedPosition(); + Translation3d softResetTransformation = rotateAxes(hardResetTransform.minus(softResetPose.getTranslation()), + softResetTransform.getRotation()).plus(softResetPose.getTranslation()) + .plus(softResetTransform.getTranslation()); + return softResetTransformation; + } + + public Rotation3d getProcessedRotation() { + return getRawRotation().plus(initPose.getRotation()); + } + + public Rotation3d getRotation() { + return getProcessedRotation().plus(softResetTransform.getRotation()); + } + + public double getConfidence() { + if (RobotBase.isReal()) { + return 0.01; + } else { + return Double.MAX_VALUE; + } + } + + public double getCaptureTime() { + double t = RobotController.getFPGATime() / 1E6 - TIMESTAMP_DELAY; + SmartDashboard.putNumber("Quest Timestamp", t); + updateFrameCount(); + return t; + } + + public boolean isActive() { + double t = timestamp.get(); + boolean simulation = RobotBase.isSimulation(); + boolean disabled = DriverStation.isDisabled(); + double frame = frameCount.get(); + double previousFrame = previousFrameCount; + if (t == 0 || simulation || previousFrame == frame) { + isActive = false; + return false; + } + isActive = true; + return initializedPosition; + } + + public void updateFrameCount() { + previousFrameCount = frameCount.get(); + } + + public boolean processQuestCommand(QuestCommand command) { + if (miso.get() == 99) { + return false; + } + mosi.set(command.getQuestRequest()); + return true; + } + + private void resetQuestPose() { + processQuestCommand(QuestCommand.RESET); + } + + public void softReset(Pose3d pose) { + softResetTransform = new Transform3d(pose.getTranslation().minus(getProcessedPosition()), + pose.getRotation().minus(getProcessedRotation())); + softResetPose = new Pose3d(getProcessedPosition(), getProcessedRotation()); + } + + public void hardReset(Pose3d pose) { + initPose = pose; + resetQuestPose(); + } + + public static Trigger isQuestOn() { + return new Trigger(() -> isActive); + } + + public void resetPose(Pose3d pose) { + initializedPosition = true; + softReset(pose); + } + + public void resetPose() { + initializedPosition = true; + resetQuestPose(); + } + + public void cleanUpQuestCommand() { + if (miso.get() == 99) { + mosi.set(0); + } + } + + public int fiducialId() { + return 0; + } + + // private void updateVelocity() { + // if (previousPose == null) { + // previousPose = getRobotPose().get(); + // previousTime = timestamp.get(); + // return; + // } + // double currentTime = timestamp.get(); + // double deltaTime = currentTime - previousTime; + // if (deltaTime == 0) { + // return; + // } + // velocity = new ChassisSpeeds( + // (getPosition().getX() - previousPose.getTranslation().getX()) / deltaTime, + // (getPosition().getY() - previousPose.getTranslation().getY()) / deltaTime, + // (getRotation().getZ() - previousPose.getRotation().getZ()) / deltaTime); + // previousTime = currentTime; + // previousPose = getRobotPose().get(); + + // } + + // public ChassisSpeeds getVelocity() { + // if (null != velocity) { + // return velocity; + // } + // return new ChassisSpeeds(); + // } + + // @Override + // public void update() { + // if (RobotBase.isReal()) { + // cleanUpQuestCommand(); + // updateVelocity(); + // SmartDashboard.putBoolean("Reset Pose", false); + + // Pose2d currPose = getRobotPose().get().toPose2d(); + // SmartDashboard.putNumberArray("Quest POSE", new double[] { + // currPose.getX(), currPose.getY(), currPose.getRotation().getDegrees() + // }); + + // ChassisSpeeds velocity = getVelocity(); + // SmartDashboard.putNumberArray("Velocity", new double[] { velocity.vxMetersPerSecond, + // velocity.vyMetersPerSecond, velocity.omegaRadiansPerSecond }); + // } + // } + + private Translation2d calculateOffsetToRobotCenter() { + Pose3d currentPose = getRobotPose().get(); + Pose2d currentPose2d = currentPose.toPose2d(); + + Rotation2d angle = currentPose2d.getRotation(); + Translation2d displacement = currentPose2d.getTranslation(); + + double x = ((angle.getCos() - 1) * displacement.getX() + angle.getSin() * displacement.getY()) + / (2 * (1 - angle.getCos())); + double y = ((-1 * angle.getSin()) * displacement.getX() + (angle.getCos() - 1) * displacement.getY()) + / (2 * (1 - angle.getCos())); + + return new Translation2d(x, y); + } + + public Command determineOffsetToRobotCenter(CommandSwerveDrivetrain drivetrain, double speed) { + resetPose(); + return Commands.repeatingSequence( + Commands.run(() -> { + drivetrain.setControl( + m_ApplyRobotSpeeds.withSpeeds(new ChassisSpeeds(0,0, speed))); + }, drivetrain).withTimeout(0.5), + + Commands.runOnce(() -> { + // Update current offset + Translation2d offset = calculateOffsetToRobotCenter(); + + _calculatedOffsetToRobotCenter = _calculatedOffsetToRobotCenter + .times((double) _calculatedOffsetToRobotCenterCount + / (_calculatedOffsetToRobotCenterCount + 1)) + .plus(offset.div(_calculatedOffsetToRobotCenterCount + 1)); + _calculatedOffsetToRobotCenterCount++; + + SmartDashboard.putNumberArray("Quest Calculated Offset to Robot Center", new double[] { + _calculatedOffsetToRobotCenter.getX(), _calculatedOffsetToRobotCenter.getY() }); + + }).onlyIf(() -> getRotation().getMeasureZ().in(Degrees) > 30 || getRotation().getMeasureZ().in(Degrees) < -90)) + .onlyWhile(() -> getRotation().getMeasureZ().in(Degrees) >= 0 || getRotation().getMeasureZ().in(Degrees) < -90); + } +} \ 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 522ba26..2f3d56d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,12 +17,13 @@ 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.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; -import frc.robot.Constants.ArmSetpoints; import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.Vision; +import frc.robot.QuestNav.NerdQuestNav; import org.ironmaple.simulation.SimulatedArena; @@ -31,11 +32,13 @@ public class Robot extends TimedRobot { private final RobotContainer m_robotContainer; + private NerdQuestNav questNav = new NerdQuestNav(new Transform3d()); + /** * This function is run when the robot is first started up and should be used for any * initialization code. */ - private final boolean kUseLimelight = false; + // private final boolean kUseLimelight = false; public Robot() { m_robotContainer = new RobotContainer(); @@ -49,17 +52,17 @@ public void robotInit() { // Pathfinding.setPathfinder(new LocalADStar()); // PathfindingCommand.warmupCommand().schedule(); - if(!Vision.DOGLOG_ENABLED){ - DogLog.setEnabled(false); - }else{ - DogLog.setEnabled(true); - DogLog.setOptions(new DogLogOptions() - .withLogExtras(true) - .withCaptureDs(true) - .withNtPublish(true) - .withCaptureNt(true)); - DogLog.setPdh(new PowerDistribution()); - } + // if(!Vision.DOGLOG_ENABLED){ + // DogLog.setEnabled(false); + // }else{ + // DogLog.setEnabled(true); + // DogLog.setOptions(new DogLogOptions() + // .withLogExtras(true) + // .withCaptureDs(true) + // .withNtPublish(true) + // .withCaptureNt(true)); + // DogLog.setPdh(new PowerDistribution()); + // } } @@ -83,6 +86,8 @@ public void robotPeriodic() { // } // } SmartDashboard.putNumber("Match Time",DriverStation.getMatchTime()); + + questNav.processHeartbeat(); } @Override @@ -118,16 +123,14 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { - // m_robotContainer.arm.setArmPosition(ArmSetpoints.home.plus(new Translation2d(4, 2)), false); -// TODO: remove after testing // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } + // if (m_autonomousCommand != null) { + // m_autonomousCommand.cancel(); + // } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7956a20..e70f111 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,33 +4,46 @@ package frc.robot; import static edu.wpi.first.units.Units.*; +import static frc.robot.Constants.Vision.coralStationOffSetsMap; +import java.lang.reflect.Field; import java.rmi.dgc.Lease; import java.util.Map; import java.util.function.BooleanSupplier; import java.util.function.Supplier; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; +import frc.robot.Constants.AutoDropoff; +import frc.robot.Constants.CoralConstants; import frc.robot.Constants.OperatorConstants; -import frc.robot.commandSequences.ArmActions; +import frc.robot.QuestNav.NerdQuestNav; +import frc.robot.FieldConstants.ReefLevel; +import frc.robot.Constants.CoralConstants.coralState; +import frc.robot.Constants.CoralConstants.elevatorLevel; import frc.robot.commandSequences.Autos; -import frc.robot.commands.GripperCommand; +import frc.robot.commandSequences.SubsystemActions; +import frc.robot.commands.DriveToCoral; +import frc.robot.commands.DriveToCoralAuto; +import frc.robot.commands.DriveToPose; import frc.robot.commands.LEDCommand; import frc.robot.subsystems.PoseEstimatorSubsystem; import frc.robot.subsystems.ScoringProfileSubsystem; import frc.robot.subsystems.Vision; -import frc.robot.commands.ArmCommand; -import frc.robot.commands.ArmCommandPathToPoint; -import frc.robot.subsystems.Gripper; +import frc.robot.util.AllianceFlipUtil; import frc.robot.subsystems.LEDSubsytem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -40,17 +53,19 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.CoralManipulator; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Bucket; import frc.robot.subsystems.Climb; /** @@ -66,19 +81,19 @@ 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 // private final LEDSubsytem m_LedSubsystem = new LEDSubsytem(); - - public Arm arm; - private Gripper gripper; + + private final ProfiledPIDController driveController = AutoDropoff.driveController; + private final ProfiledPIDController thetaController = AutoDropoff.thetaController; + private LEDSubsytem LEDs; - private Bucket bucket; private Climb climb; + private CoralManipulator coralManipulator; + + private NerdQuestNav QuestNav = new NerdQuestNav(new Transform3d(0,0, 0, new Rotation3d(Rotation2d.fromDegrees(90)))); public static BooleanSupplier autoBucketEnabled = () -> true; private BooleanSupplier driveTrainFinishedMoving = () -> false; - private BooleanSupplier gripperHasGamePiece = () -> false; - private BooleanSupplier bucketHasCoral = () -> false; - private Trigger bucketHasCoralTrigger = new Trigger(bucketHasCoral); private Trigger driveTrainFinishedMovingTrigger = new Trigger(driveTrainFinishedMoving); /* Setting up bindings for necessary control of the swerve drive platform */ @@ -93,7 +108,6 @@ public class RobotContainer { // private final Telemetry logger = new Telemetry(MaxSpeed); - // private Trigger armFinishedMoving = new Trigger(() -> arm.finishedMoving); @@ -102,42 +116,34 @@ public class RobotContainer { public final PoseEstimatorSubsystem poseEstimatorSubsystem;// = new PoseEstimatorSubsystem(drivetrain); - // public final ScoringProfileSubsystem scoringSubsystem = new ScoringProfileSubsystem(); - public final ScoringProfileSubsystem scoringSubsystem; - private final PIDController thetaController = - new PIDController( - 3, 0,0);//, new TrapezoidProfile.Constraints(Math.toRadians(540), Math.toRadians(360))); //3, 10, 0 - - + // public final ScoringProfileSubsystem scoringSubsystem; /* Path follower */ private SendableChooser autoChooser; - // private Command armDefaultCommand = new ArmDefaultCommand(arm, () -> (arm.stowing ? 6 : 7)); - // private Command gripperDefaultCommand = new ArmCommandGripperAutoClose(gripper, () -> !arm.stowing, () -> arm.stowing); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + driveController.setTolerance(Constants.Vision.TRANSLATION_TOLERANCE_X, Constants.Vision.VELOCITY_TOLERANCE_X); + thetaController.setTolerance(Constants.Vision.ROTATION_TOLERANCE,Constants.Vision.VELOCITY_TOLERANCE_OMEGA); thetaController.enableContinuousInput(-Math.PI, Math.PI); drivetrain = TunerConstants.createDrivetrain(); - poseEstimatorSubsystem = new PoseEstimatorSubsystem(drivetrain); - scoringSubsystem = new ScoringProfileSubsystem(); - arm = new Arm(); - gripper = new Gripper(); - bucket = new Bucket(); + poseEstimatorSubsystem = new PoseEstimatorSubsystem(drivetrain, QuestNav); + // scoringSubsystem = new ScoringProfileSubsystem(); + // climb = new Climb(); + coralManipulator = new CoralManipulator(); climb = new Climb(); - configureTriggers(); + // configureTriggers(); configureNamedCommands(); - // SignalLogger.setPath("/media/sda1/armLog"); // SignalLogger.start(); configureDefaultCommands(); @@ -147,31 +153,24 @@ public RobotContainer() { configureAutoChooser(); configureLEDs(); + } private void configureNamedCommands(){ - NamedCommands.registerCommand("armToHome", arm.goToHome()); - NamedCommands.registerCommand("grabFromFunnel", ArmActions.grabFromFunnel(arm, gripper)); - NamedCommands.registerCommand("gripperToGroundIntake", ArmActions.groundIntake(arm, gripper)); - NamedCommands.registerCommand("armToL4", ArmActions.armToCoralReef(arm, gripper, () -> 4)); - NamedCommands.registerCommand("dropOffCoral", ArmActions.dunkDropCoral(arm, gripper, () -> 4));//new ArmCommandGripper(gripper, () -> false).alongWith(new ArmCommandPathToPoint(arm, () -> 18))); - NamedCommands.registerCommand("waitUntilBucketHasCoral", new WaitUntilCommand(bucketHasCoral)); - // NamedCommands.registerCommand("gripperOpenThenGroundIntake", new ArmCommandGripper(gripper, () -> false).withTimeout(0.25).andThen((new WaitCommand(1.0).andThen(new ArmCommandGripperGroundPickup(gripper))).raceWith((new ArmCommandPathToPoint(arm, () -> 14))).andThen(new WaitCommand(0.2)).andThen(new ArmCommandPathToPoint(arm, () -> 18)))); - // NamedCommands.registerCommand("armToStow", new ArmCommandPathToPoint(arm, () -> 17)); - // NamedCommands.registerCommand("intakeThrow", new IntakeCommandPower(intake, ()-> IntakeConstants.intakeThrowDeployPower, () -> IntakeConstants.intakePassive).until(() -> intake.getIntakeDeployPosition() < IntakeConstants.intakeThrowPosition) - // .andThen(new IntakeCommandPower(intake, ()-> IntakeConstants.intakeThrowDeployPower, () -> IntakeConstants.intakeThrowPower) - // .withTimeout(0.05)) - // .andThen(new IntakeCommand(intake, ()-> IntakeConstants.home, () -> IntakeConstants.intakeThrowPower).withTimeout(1.0).andThen(() ->intake.stopIntake()))); - // NamedCommands.registerCommand("test", new InstantCommand(()-> System.out.println("test"))); - - - // // coach elmer didn't want us to break the arm - // NamedCommands.registerCommand("armToStowPrint", new InstantCommand(()-> System.out.println("armToStowPrint"))); - // NamedCommands.registerCommand("intakePrepareThrowPrint", new InstantCommand(()-> System.out.println("intakePrepareThrowPrint"))); - // NamedCommands.registerCommand("intakeThrowPrint", new InstantCommand(()-> System.out.println("intakeThrowPrint"))); - // NamedCommands.registerCommand("gripperToGroundIntakePrint", new InstantCommand(()-> System.out.println("gripperToGroundIntakePrint"))); - // NamedCommands.registerCommand("armToL4Print", new InstantCommand(()-> System.out.println("armToL4Print"))); - + NamedCommands.registerCommand("zero elevator and intake", SubsystemActions.resetDeploy(coralManipulator).alongWith(SubsystemActions.resetElevator(coralManipulator))); + NamedCommands.registerCommand("intake", coralManipulator.intakeCommand()); + NamedCommands.registerCommand("waitUntilHasCoral", new WaitUntilCommand(() -> !coralManipulator.getCoralState().equals(coralState.empty))); + NamedCommands.registerCommand("elevatorToL2", coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.l2.height)); + NamedCommands.registerCommand("elevatorShootL2", SubsystemActions.placeCoral(coralManipulator, elevatorLevel.l2)); + NamedCommands.registerCommand("elevatorToL1", coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.l1.height)); + NamedCommands.registerCommand("elevatorShootL1", SubsystemActions.placeCoral(coralManipulator, elevatorLevel.l1)); + NamedCommands.registerCommand("elevatorToHome", coralManipulator.elevatorToHome()); + NamedCommands.registerCommand("elevatorToHomeAndIntake", coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.visionClear.height).alongWith(coralManipulator.intakeCommand())); + NamedCommands.registerCommand("elevatorShootL2", SubsystemActions.placeCoral(coralManipulator, elevatorLevel.l2)); + NamedCommands.registerCommand("driveToCoral", new DriveToCoralAuto(drivetrain, () -> (poseEstimatorSubsystem.coralArrayUpdateReturn().size() > 0) ? poseEstimatorSubsystem.coralArrayUpdateReturn().get(0).getPose() : poseEstimatorSubsystem.getCurrentPose())); + NamedCommands.registerCommand("waitUntilCoralInElevator", new WaitUntilCommand(() -> !coralManipulator.getIndexerSensor() && !coralManipulator.getIntakeSensor())); + NamedCommands.registerCommand("waitUntilAndElevatorL2", new WaitUntilCommand(() -> !coralManipulator.getIndexerSensor() && !coralManipulator.getIntakeSensor()).andThen(coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.l2.height))); + NamedCommands.registerCommand("waitUntilAndElevatorL1", new WaitUntilCommand(() -> !coralManipulator.getIndexerSensor() && !coralManipulator.getIntakeSensor()).andThen(coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.l1.height))); } private void configureDefaultCommands() { @@ -187,7 +186,6 @@ private void configureDefaultCommands() { - // arm.setDefaultCommand(new ArmCommand(arm, () -> 0)); // gripper.setDefaultCommand(new GripperCommand(gripper)); @@ -195,146 +193,77 @@ private void configureDefaultCommands() { } private void configureTriggers() { - bucketHasCoral = () -> bucket.getDetected(); - driveTrainFinishedMoving = () -> poseEstimatorSubsystem.getCurrentPose().getTranslation() - .getDistance(scoringSubsystem.getSelectedBranchPose().getTranslation()) < 1; + // driveTrainFinishedMoving = () -> poseEstimatorSubsystem.getCurrentPose().getTranslation() + // .getDistance(scoringSubsystem.getSelectedBranchPose().getTranslation()) < 1; // || poseEstimatorSubsystem.getCurrentPose().getTranslation().getDistance((scoringSubsystem.getSelectedCoralStationPose().getTranslation()))<1; - gripperHasGamePiece = () -> Bucket.gripperHasGamePiece; - bucketHasCoralTrigger = new Trigger(bucketHasCoral).and(() -> DriverStation.isTeleop()).and(() -> !Bucket.gripperHasGamePiece).and(() -> (arm.getArmPosition().getDistance(ArmSetpoints.home) < 5)); // bucketHasCoralTrigger = new Trigger(bucketHasCoral); } private void configureBindings() { - // if(bucketHasCoral.get()) { - // new InstantCommand(() -> joystick.setRumble(RumbleType.kBothRumble, 1));} - // else { - // new InstantCommand(() -> joystick.setRumble(RumbleType.kBothRumble, 0));} + // Find Quest Offsets + //joystick.leftTrigger().onTrue(QuestNav.determineOffsetToRobotCenter(drivetrain, 0.35)); //0.314 + // climb + joystick.y().onTrue(climb.extend().alongWith(coralManipulator.deployIntake())); + joystick.x().onTrue(climb.returnToHome()); + joystick.a().whileTrue(climb.contract().alongWith(coralManipulator.stopDeploy())).onFalse(climb.stopCommand()); + joystick.b().onTrue(climb.stopCommand()); + // coral manipulator joystick.leftStick().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + joystick.back().whileTrue(SubsystemActions.resetDeploy(coralManipulator).alongWith(SubsystemActions.resetElevator(coralManipulator))); + + joystick.rightBumper().onTrue(SubsystemActions.panicButton(coralManipulator)).onFalse(coralManipulator.intakeToHome().alongWith(coralManipulator.stopIntake()).alongWith(coralManipulator.stopIndexer()).alongWith(coralManipulator.stopShooter()).alongWith(coralManipulator.elevatorToHome())); + + // full auto dropoffs for L2 + joystick.povUp().and(() -> FieldConstants.getCloseEnoughForAutoDrive(() -> drivetrain.getState().Pose)) + .whileTrue(coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.visionClear.height) + .andThen(new DriveToPose(drivetrain, () -> FieldConstants.getClosestPole(() -> drivetrain.getState().Pose))) + .andThen(SubsystemActions.placeCoral(coralManipulator, CoralConstants.elevatorLevel.l2))); + // .and(() -> coralManipulator.getCoralState().equals(coralState.coralInElevator)) + // .onTrue(coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.l2.height)) + // .onFalse(SubsystemActions.placeCoral(coralManipulator, CoralConstants.elevatorLevel.l2)); + // joystick.povUp() + // .whileTrue(SubsystemActions.placeCoral(coralManipulator, CoralConstants.elevatorLevel.l2)).onFalse(coralManipulator.elevatorToHome()); + + // semi auto dropoffs for L1 + joystick.leftBumper() + .whileTrue(driveToLine(() -> AllianceFlipUtil.apply(FieldConstants.Reef.centerFaces[FieldConstants.getClosestFace(() -> drivetrain.getState().Pose)]).plus(Constants.Vision.reefLevelOffsetsMap.get(ReefLevel.L1)), () -> new Translation2d(-joystick.getRightY(), -joystick.getRightX()), () -> FieldConstants.Reef.centerFaces[FieldConstants.getClosestFace(() -> drivetrain.getState().Pose)].getTranslation().minus(FieldConstants.Reef.center).getAngle())) + .and(() -> coralManipulator.getCoralState().equals(coralState.coralInIndexer)) + .whileTrue(coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.l1.height)); + + joystick.povRight() + .whileTrue(SubsystemActions.placeCoral(coralManipulator, CoralConstants.elevatorLevel.l1inside)); + joystick.povLeft() + .whileTrue(SubsystemActions.placeCoral(coralManipulator, CoralConstants.elevatorLevel.l1upper)); + joystick.povDown() + .whileTrue(SubsystemActions.placeCoral(coralManipulator, CoralConstants.elevatorLevel.l1)); + + joystick.povRight().or(joystick.povLeft()).or(joystick.povDown()).or(joystick.povUp()).and(() -> !coralManipulator.getCoralState().equals(coralState.coralInIndexer)).onFalse(coralManipulator.elevatorToHome()); + + //intake commands + joystick.leftTrigger().or(joystick.rightTrigger()) + .onTrue(coralManipulator.setCoralStateCommand(coralState.empty)) + .onTrue(coralManipulator.intakeCommand())//.onlyIf(() -> coralManipulator.getCoralState().equals(coralState.empty))) + .onFalse(coralManipulator.intakeToHome());//.onlyIf(() -> !coralManipulator.getCoralState().equals(coralState.coralInIntake) )); + + Trigger coralInRange = new Trigger(() -> poseEstimatorSubsystem.coralInRange()); + // Trigger coralAutoTarget = new Trigger(() -> Constants.Vision.kCoralAutoTarget); + Trigger coralInList = new Trigger(() -> poseEstimatorSubsystem.coralInList()); + - // * arm testing * - // joystick.leftBumper().onTrue(ArmActions.armToCoralReef(arm, () -> scoringSubsystem.getArmReefTarget())); - // joystick.a().onTrue(ArmActions.grabFromFunnel(arm, gripper)); - // joystick.rightBumper().onTrue(ArmActions.groundIntake(arm, gripper)); - // joystick.leftTrigger().onTrue(ArmActions.dunkDropCoral(arm, gripper, () -> scoringSubsystem.getArmReefTarget())); - // joystick.x().onTrue(ArmActions.removeAlgae(arm, gripper, () -> ((scoringSubsystem.getBranch() / 2 % 2) == 0))); - // joystick.y().onTrue(ArmActions.shootAlgaeBarge(gripper)); - // joystick.rightTrigger().onTrue(ArmActions.armToAlgaeBarge(arm)); - - - // * real competition bindings * - - // home arm - joystick.rightStick().whileTrue(arm.goToHome()) - .whileTrue(gripper.neutralCommand()); - // joystick.y().whileTrue(gripper.spitOutCommand()).onFalse(gripper.neutralCommand()); - - // coral dropoff - //manual dunk - joystick.povLeft().onTrue(ArmActions.dunkCoral(arm, () -> scoringSubsystem.getArmReefTarget(), () -> (joystick.getLeftTriggerAxis() - joystick.getRightTriggerAxis()))) - .onTrue(gripper.coralDefaultCommand()); - joystick.leftBumper().onTrue(gripper.spitOutCommand()) - .onFalse(new WaitCommand(0.4).andThen(gripper.neutralCommand())).onFalse(new WaitCommand(0.2).andThen(arm.goToHome())); - - // autodunk - joystick.povUp().onTrue(ArmActions.dunkDropCoral(arm, gripper, () -> scoringSubsystem.getArmReefTarget())); - - // coral pickup - joystick.povDown().onTrue(ArmActions.grabFromFunnel(arm, gripper)).onTrue(bucket.disableAutoBucket()); - // bucketHasCoralTrigger.onTrue(ArmActions.grabFromFunnel(arm, gripper)); - - // algae pickup - joystick.povRight().onTrue(ArmActions.removeAlgae(arm, gripper, () -> (((scoringSubsystem.getBranch() / 2) % 2) == 0))); - - // algae dropoff (odo-based) - // joystick.y().whileTrue(ArmActions.armToAlgaeBarge(arm, gripper) - // .andThen(new WaitUntilCommand(() -> Math.abs(drivetrain.getState().Pose.getMeasureX().abs(Meter) - FieldConstants.fieldLength/2) < Constants.shootAlgaeDistance)) - // .andThen(ArmActions.shootAlgaeBarge(arm, gripper)) - // .deadlineFor( - // drivetrain.applyRequest(() -> drive.withVelocityX(Constants.shootAlgaeDriveSpeed) - // .withRotationalRate(thetaController.calculate(drivetrain.getState().Pose.getRotation().getRadians(), 0))))) - // .onFalse(arm.goToHome()); - - //algae dropoff driver-based - joystick.y().whileTrue(ArmActions.armToAlgaeBarge(arm, gripper).alongWith(drivetrain.applyRequest(() -> drive.withVelocityX(Constants.shootAlgaeDriveSpeed) - .withRotationalRate(thetaController.calculate(drivetrain.getState().Pose.getRotation().getRadians(), 0))))) - .onFalse(ArmActions.shootAlgaeBarge(arm, gripper).alongWith(drivetrain.applyRequest(() -> drive.withVelocityX(Constants.shootAlgaeDriveSpeed)).withTimeout(0.35))); - - // joystick.y().onTrue(ArmActions.armToProcessor(arm, gripper)); + joystick.rightTrigger().and(coralInRange).and(coralInList).whileTrue(new DriveToCoral(drivetrain, () -> new Pose2d( + poseEstimatorSubsystem.coralArrayUpdateReturn().get(0).getPose().getX() + new Translation2d(-0.381, poseEstimatorSubsystem.coralArrayUpdateReturn().get(0).getPose().getRotation()).getX(), + poseEstimatorSubsystem.coralArrayUpdateReturn().get(0).getPose().getY() + new Translation2d(-0.381, poseEstimatorSubsystem.coralArrayUpdateReturn().get(0).getPose().getRotation()).getY(), + poseEstimatorSubsystem.coralArrayUpdateReturn().get(0).getPose().getRotation()))); - // wrist fix offset - // joystick.back().onTrue(new InstantCommand(() -> arm.addToWristOffset(Units.degreesToRotations(10)))); - // joystick.start().onTrue(new InstantCommand(() -> arm.addToWristOffset(Units.degreesToRotations(-10)))); + + new Trigger(() -> coralManipulator.getCoralState().equals(coralState.coralInIntake)).and(() -> DriverStation.isTeleop()).onTrue(SubsystemActions.transferCoral(coralManipulator)); - // climb - joystick.back().onTrue(new ArmCommand(arm, () -> 11)).onTrue(new WaitCommand(0.3).andThen(climb.deploy())); - joystick.start().and(() -> !climb.climbed()).whileTrue(climb.climb()).onFalse(climb.stopCommand()); - - - /* autodrive TODO: rebind to not conflict with drive stick */ - joystick.b().whileTrue(Autos.getAutoDriveCommandReef(drivetrain, - () -> drivetrain.getState().Pose, - () -> scoringSubsystem.getRobotPoseForSelectedBranch(), - ()->scoringSubsystem.getLevel(), - ()-> false, - ()->-joystick.getRightY(), - ()->-joystick.getRightX(), - ()->-joystick.getLeftX())); - - joystick.rightBumper().whileTrue(Autos.driveAndAutoDropoff(drivetrain, - () -> drivetrain.getState().Pose, - () -> scoringSubsystem.getRobotPoseForSelectedBranch(), - ()->scoringSubsystem.getLevel(), - ()-> false, - ()->-joystick.getRightY(), - ()->-joystick.getRightX(), - ()->-joystick.getLeftX(), arm, gripper, () -> scoringSubsystem.getArmReefTarget())).onFalse(arm.goToHome().alongWith(gripper.neutralCommand())); - - joystick.a().whileTrue(Autos.getAutoDriveCommandStation(drivetrain, - () -> drivetrain.getState().Pose, - () -> scoringSubsystem.getRobotPoseForSelectedCoralStation(), - ()->-joystick.getRightY(), - ()->-joystick.getRightX(), - ()->-joystick.getLeftX())); - - joystick.x().whileTrue(Autos.getAutoDriveCommandReef(drivetrain, - () -> drivetrain.getState().Pose, - () -> scoringSubsystem.getRobotPoseForSelectedAlgae(), - ()->scoringSubsystem.getLevel(), - ()-> false, - ()->-joystick.getRightY(), - ()->-joystick.getRightX(), - ()->-joystick.getLeftX())); - - - - // final Command noBlinkPattern = LEDs.runPattern( - // () -> LEDPattern.steps( - // Map.of( - // 0, - // LEDs.updateStepColor(hasCoral)[0] - // ) - // ) - // // .scrollAtRelativeSpeed(Percent.per(Second).of(Constants.LEDConstants.scrollSpeed)) - // ); - // final Command blinkPattern = LEDs.runPattern( - // () -> LEDPattern.steps( - // Map.of( - // 0, - // LEDs.updateStepColor(hasCoral)[0] - // ) - // ).blink(Seconds.of(Constants.LEDConstants.blinkSeconds)) - // // .scrollAtRelativeSpeed(Percent.per(Second).of(Constants.LEDConstants.scrollSpeed)) - // ); - - // if (detectedCoral.get()) { - // joystick.back().onTrue(blinkPattern); - // } else { - // joystick.back().onTrue(noBlinkPattern); - // } + new Trigger(() -> coralManipulator.getCoralState().equals(coralState.coralInIntake)).and(() -> DriverStation.isAutonomous()).onTrue(SubsystemActions.transferCoralForAuto(coralManipulator)); + // new Trigger(() -> !coralManipulator.getIndexerSensor()).onTrue(coralManipulator.setCoralStateCommand(coralState.coralInIndexer)); } private void configureAutoChooser() { @@ -356,32 +285,37 @@ public Command getAutonomousCommand() { } private void configureLEDs() { LEDs = new LEDSubsytem(); - if((bucketHasCoral.getAsBoolean() || gripperHasGamePiece.getAsBoolean()) && driveTrainFinishedMoving.getAsBoolean()){ - joystick.rightBumper().onFalse(LEDs.runPattern( - () -> LEDPattern.steps( - Map.of( - 0, - LEDs.getPattern(driveTrainFinishedMoving, bucketHasCoral, gripperHasGamePiece) - ) - )//.blink(Seconds.of(Constants.LEDConstants.blinkSeconds)) - )); - - } else { - joystick.rightBumper().onFalse( - LEDs.runPattern( - () -> LEDPattern.steps( - Map.of( - 0, - LEDs.getPattern(driveTrainFinishedMoving, bucketHasCoral, gripperHasGamePiece) - ) - ) - ) - ); - - } - bucketHasCoralTrigger.or(driveTrainFinishedMoving).and(() -> DriverStation.isTeleop()) - .onTrue(LEDs.blink()).onFalse(LEDs.breathe()); - + joystick.b().onTrue(LEDs.runPattern(() -> LEDPattern.solid(new Color(1, 1, 1)))); + // if (!climb.climbed()) { + // LEDs.setDefaultCommand(LEDs.runPattern(() -> LEDPattern.solid(LEDs.getColor(() -> true, () -> joystick.rightBumper().getAsBoolean(), () -> 1.0)))); + // } else { + // LEDs.setDefaultCommand( + // LEDs.runPattern(() -> LEDPattern.rainbow(255, 128).scrollAtRelativeSpeed(Percent.per(Second).of(25))) + // ); + // } } + private Command driveToLine(Supplier targetCenter, Supplier joystickVector, Supplier targetVector) { + // double target = new Translation2d(targetCenter.get().getTranslation().getX() * targetVector.get().getCos(), targetCenter.get().getTranslation().getY() * targetVector.get().getSin()).getNorm(); + return drivetrain.applyRequest(() -> + drive + .withVelocityX(xLimiter.calculate((OperatorConstants.joystickMap.get(joystickVector.get().getX() * targetVector.get().getCos()) * MaxSpeed) + driveController.calculate( + new Translation2d( + drivetrain.getState().Pose.getTranslation().getX() * targetVector.get().plus(Rotation2d.kCCW_90deg).getCos(), + drivetrain.getState().Pose.getTranslation().getY() * targetVector.get().plus(Rotation2d.kCCW_90deg).getSin()).getNorm(), + new Translation2d( + targetCenter.get().getTranslation().getX() * targetVector.get().plus(Rotation2d.kCCW_90deg).getCos(), + targetCenter.get().getTranslation().getY() * targetVector.get().plus(Rotation2d.kCCW_90deg).getSin()).getNorm() + ) * targetVector.get().getSin())) + .withVelocityY(yLimiter.calculate((OperatorConstants.joystickMap.get(joystickVector.get().getY() * targetVector.get().getSin()) * MaxSpeed) + + driveController.calculate( + new Translation2d( + drivetrain.getState().Pose.getTranslation().getX() * targetVector.get().plus(Rotation2d.kCCW_90deg).getCos(), + drivetrain.getState().Pose.getTranslation().getY() * targetVector.get().plus(Rotation2d.kCCW_90deg).getSin()).getNorm(), + new Translation2d( + targetCenter.get().getTranslation().getX() * targetVector.get().plus(Rotation2d.kCCW_90deg).getCos(), + targetCenter.get().getTranslation().getY() * targetVector.get().plus(Rotation2d.kCCW_90deg).getSin()).getNorm() + ) * targetVector.get().getCos())) + .withRotationalRate(zLimiter.calculate(thetaController.calculate(drivetrain.getState().Pose.getRotation().getRadians(), targetVector.get().getRadians())))); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commandSequences/ArmActions.java b/src/main/java/frc/robot/commandSequences/ArmActions.java deleted file mode 100644 index 2a97194..0000000 --- a/src/main/java/frc/robot/commandSequences/ArmActions.java +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commandSequences; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import java.util.function.IntSupplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.commands.ArmCommand; -import frc.robot.commands.ArmCommandPathToPoint; -import frc.robot.commands.ArmInstantCommand; -import frc.robot.commands.GripperCommand; -import frc.robot.commands.WristCommand; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Bucket; -import frc.robot.subsystems.Gripper; - -/** Add your docs here. */ -public class ArmActions { - - /** grab coral from funnel */ - public static Command grabFromFunnel(Arm arm, Gripper gripper) { // has bug where wrist slips while transferring and does not go low enough and messes up sequential scheduling (only when activated by the beambreak) - // System.out.println("grab from funnel"); - // Bucket.gripperHasGamePiece = true; - return new SequentialCommandGroup( - gripper.coralIntakeCommand(), - // new ArmCommand(arm, () -> ArmSetpoints.armSetPoints[7].withWrist(Math.PI)).withTimeout(0.2), - new ArmCommand(arm, () -> ArmSetpoints.armSetPoints[7]).withTimeout(0.6), - new ArmCommand(arm, () -> ArmSetpoints.armSetPoints[0].withWrist(ArmSetpoints.armSetPoints[7].wrist)).withTimeout(0.55), - gripper.coralDefaultCommand(), - new ArmCommand(arm, () -> ArmSetpoints.armSetPoints[7]).withTimeout(0.2), - new ArmCommand(arm, () -> ArmSetpoints.armSetPoints[7].withWrist(Math.PI)).withTimeout(0.2), - arm.goToHome()); - } - - /** move arm to ground intake position and begin intaking */ - public static Command groundIntake(Arm arm, Gripper gripper) { - return new ArmCommandPathToPoint(arm, () -> 8).alongWith(gripper.coralIntakeCommand()); - } - - /** move arm to desired setpoint to drop coral on reef - * @param setPointIndex level on the reef - */ - public static Command armToCoralReef(Arm arm, Gripper gripper, IntSupplier setPointIndex) { - // System.out.println("arm to coral reef"); - return new ArmCommand(arm, setPointIndex).alongWith(gripper.coralDefaultCommand()); - } - - /** tilt wrist downwards and release coral - * @param setPointIndex level on the reef - */ - public static Command dunkDropCoral(Arm arm, Gripper gripper, IntSupplier setPointIndex) { - return - new ArmInstantCommand(arm, () -> ArmSetpoints.armSetPointsDunkAuto[setPointIndex.getAsInt()]).andThen( - new WaitCommand(0.25).andThen(gripper.spitOutCommand())).andThen(new WaitCommand(0.35).andThen(arm.goToHotel().andThen(new WaitCommand(0.7).andThen(gripper.neutralCommand().alongWith(arm.goToHome()))))); - } - - /** tilt wrist downwards manually - * @param setPointIndex level on the reef - @param dunkScalar amount to dunk by set by driver - */ - public static Command dunkCoral(Arm arm, IntSupplier setPointIndex, DoubleSupplier dunkScalar) { - // System.out.println("dunk coral"); - return new ArmCommand(arm, () -> ArmSetpoints.armSetPoints[setPointIndex.getAsInt()].interpolate(ArmSetpoints.armSetPointsMicroAdjust[setPointIndex.getAsInt()], dunkScalar.getAsDouble())); - } - - /** position arm to remove algae while rolling rollers inwards - * @param higherLevel true if the algae is at a higher level (L3.5); false if the algae is at a lower level (L2.5) - */ - public static Command removeAlgae(Arm arm, Gripper gripper, BooleanSupplier higherLevel) { - // System.out.println("remove algae"); - // Bucket.gripperHasGamePiece = true; - return new ArmCommand(arm, () -> higherLevel.getAsBoolean() ? 6 : 5).alongWith(gripper.algaeIntakeCommand()); - } - - /** position arm to drop off algae in barge */ - public static Command armToAlgaeBarge(Arm arm, Gripper gripper) { - return gripper.algaeDefaultCommand().alongWith(new WaitCommand(0.05).andThen(new ArmInstantCommand(arm, () -> 9))); - } - - /** spin rollers to drop off algae in barge */ - public static Command shootAlgaeBarge(Arm arm, Gripper gripper) { - return new SequentialCommandGroup( - new ArmInstantCommand(arm, () -> 10), - new WaitCommand(0.16), - gripper.algaeSpitCommand(), - new WaitCommand(0.1), - arm.goToHome(), - new WaitCommand(0.05), - gripper.neutralCommand()); - } - - public static Command armToProcessor(Arm arm, Gripper gripper) { - return new ArmInstantCommand(arm, () -> 12).alongWith(gripper.algaeDefaultCommand()); - } -} diff --git a/src/main/java/frc/robot/commandSequences/Autos.java b/src/main/java/frc/robot/commandSequences/Autos.java index bc6e01a..23cabdc 100644 --- a/src/main/java/frc/robot/commandSequences/Autos.java +++ b/src/main/java/frc/robot/commandSequences/Autos.java @@ -5,18 +5,12 @@ package frc.robot.commandSequences; import frc.robot.Constants; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; import frc.robot.Constants.Vision; import frc.robot.FieldConstants; import frc.robot.FieldConstants.Reef; import frc.robot.FieldConstants.ReefLevel; import frc.robot.commands.DriveToPose; -import frc.robot.subsystems.Arm; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Gripper; -import frc.robot.util.ArmPoint; - import java.util.function.DoubleSupplier; import java.util.function.IntSupplier; import java.util.function.Supplier; @@ -48,7 +42,10 @@ private Autos() { } - + // public static Command getAutoDriveReefWithOffset(CommandSwerveDrivetrain drivetrain, DoubleSupplier scalar) { + // return new DriveToPose(drivetrain, () -> FieldConstants.getReefPoseOffset( + // scalar, FieldConstants.getClosestFace(() -> drivetrain.getState().Pose))); + // } public static Command getAutoDriveCommandReef( CommandSwerveDrivetrain drive, @@ -67,28 +64,6 @@ public static Command getAutoDriveCommandReef( } - public static Command driveAndAutoDropoff(CommandSwerveDrivetrain drive, - Supplier robotPoseSupplier, - Supplier goalPoseSupplier, - Supplier reefLevelSupplier, - Supplier isBackwardsSupplier, - DoubleSupplier linearFF_X, - DoubleSupplier linearFF_Y, - DoubleSupplier omegaFF, - Arm arm, Gripper gripper, IntSupplier setPointIndex) { - return new ParallelRaceGroup(getAutoDriveCommandReef(drive, - robotPoseSupplier, - goalPoseSupplier, - reefLevelSupplier, - isBackwardsSupplier, - linearFF_X, - linearFF_Y, - omegaFF), - new WaitUntilCommand(() -> goalPoseSupplier.get().getTranslation().getDistance(robotPoseSupplier.get().getTranslation()) < 1.0) - .andThen(ArmActions.armToCoralReef(arm, gripper, setPointIndex))) - .andThen(new WaitUntilCommand(() -> arm.getIsFinishedMoving()).withTimeout(1.0).andThen(ArmActions.dunkDropCoral(arm, gripper, setPointIndex))); - } - public static Command getAutoDriveCommandAlgae( CommandSwerveDrivetrain drive, Supplier robotPoseSupplier, diff --git a/src/main/java/frc/robot/commandSequences/SubsystemActions.java b/src/main/java/frc/robot/commandSequences/SubsystemActions.java new file mode 100644 index 0000000..b495c15 --- /dev/null +++ b/src/main/java/frc/robot/commandSequences/SubsystemActions.java @@ -0,0 +1,156 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commandSequences; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import frc.robot.Constants.CoralConstants; +import frc.robot.Constants.CoralConstants.coralState; +import frc.robot.Constants.CoralConstants.elevatorLevel; +import frc.robot.subsystems.CoralManipulator; + +/** Add your docs here. */ +public class SubsystemActions { + // public static Command placeCoral(CoralManipulator coralManipulator, double target, double shootVoltage) { + // return new SequentialCommandGroup( + // coralManipulator.setElevatorPosition(target), + // new WaitUntilCommand(() -> coralManipulator.elevatorAtTarget()), + // new WaitCommand(0.08), + // coralManipulator.shoot(shootVoltage), + // new WaitCommand(1.0), + // coralManipulator.setCoralStateCommand(coralState.empty)//, + // // new WaitCommand(1.0), + // // elevIndexer.home() + // ); + // } + public static Command panicButton(CoralManipulator coralManipulator) { + return new SequentialCommandGroup( + coralManipulator.setCoralStateCommand(coralState.empty), + coralManipulator.deployIntake(), + coralManipulator.setIntakeVoltage(-5), + coralManipulator.setIndexerVoltage(CoralConstants.indexerTransferVoltage), + coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.panic.height), + coralManipulator.shoot(CoralConstants.elevatorLevel.panic.shootVoltage) + ); + } + public static Command resetDeploy(CoralManipulator coralManipulator) { + return new SequentialCommandGroup( + coralManipulator.setDeployVoltage(-1), + new WaitCommand(0.1), + new WaitUntilCommand(() -> coralManipulator.deployAmpTriggered()), + coralManipulator.resetDeploy(), + coralManipulator.stopDeploy()); + } + public static Command resetElevator(CoralManipulator coralManipulator) { + return new SequentialCommandGroup( + coralManipulator.setElevatorLeftVoltage(-0.2), + new WaitCommand(0.05), + new WaitUntilCommand(() -> coralManipulator.elevatorLeftAmpTriggered()), + coralManipulator.resetElevatorLeft(), + coralManipulator.stopElevatorLeft()).alongWith(new SequentialCommandGroup( + coralManipulator.setElevatorRightVoltage(-0.2), + new WaitCommand(0.05), + new WaitUntilCommand(() -> coralManipulator.elevatorRightAmpTriggered()), + coralManipulator.resetElevatorRight(), + coralManipulator.stopElevatorRight())); + } + public static Command placeCoral(CoralManipulator coralManipulator, elevatorLevel level) { + return new SequentialCommandGroup( + // new WaitUntilCommand(() -> coralManipulator.getCoralState().equals(coralState.coralInElevator)), + coralManipulator.setElevatorPosition(level.height), + new WaitUntilCommand(() -> coralManipulator.elevatorAtTarget()), + new WaitCommand(0.08), + coralManipulator.shoot(level.shootVoltage), + // new WaitCommand(1.0/level.shootVoltage), + coralManipulator.setCoralStateCommand(coralState.empty)); + } + + public static Command transferCoral(CoralManipulator coralManipulator) { + return new SequentialCommandGroup( + coralManipulator.transferIntake(), + new WaitUntilCommand(() -> coralManipulator.deployAtTarget()), + coralManipulator.setIndexerVoltage(CoralConstants.indexerTransferVoltage), + coralManipulator.setIntakeVoltage(CoralConstants.intakeTransferVoltage), + coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.transfer.height), + coralManipulator.shoot(CoralConstants.elevatorLevel.transfer.shootVoltage), + new WaitUntilCommand(() -> !coralManipulator.getIntakeSensor()), + new WaitCommand(0.0), + coralManipulator.retractIntake(), + new WaitUntilCommand(() -> coralManipulator.getIndexerSensor()), + new WaitUntilCommand(() -> !coralManipulator.getIndexerSensor()), + coralManipulator.shoot(CoralConstants.shooterRewindVoltage), + new WaitCommand(0.02), + new WaitUntilCommand(() -> coralManipulator.getIndexerSensor()), + coralManipulator.stopShooter(), + coralManipulator.stopIndexer(), + coralManipulator.stopIntake(), + // coralManipulator.elevatorToHome(), + coralManipulator.setCoralStateCommand(coralState.coralInIndexer) + ).withInterruptBehavior(InterruptionBehavior.kCancelSelf); + + } + + public static Command transferCoralForAuto(CoralManipulator coralManipulator) { + return new SequentialCommandGroup( + coralManipulator.transferIntake(), + new WaitUntilCommand(() -> coralManipulator.deployAtTarget()), + coralManipulator.setIndexerVoltage(CoralConstants.indexerTransferVoltage), + coralManipulator.setIntakeVoltage(CoralConstants.intakeTransferVoltage), + coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.transfer.height), + coralManipulator.shoot(CoralConstants.autoShootVoltageTransfer), + new WaitUntilCommand(() -> !coralManipulator.getIntakeSensor()), + new WaitCommand(0.0), + coralManipulator.retractIntake(), + new WaitUntilCommand(() -> coralManipulator.getIndexerSensor()), + new WaitUntilCommand(() -> !coralManipulator.getIndexerSensor()), + coralManipulator.stopShooter(), + coralManipulator.stopIndexer(), + coralManipulator.stopIntake(), + // coralManipulator.elevatorToHome(), + coralManipulator.setCoralStateCommand(coralState.coralInIndexer) + ).withInterruptBehavior(InterruptionBehavior.kCancelSelf); + + } + + public static Command transferCoralToIndexer(CoralManipulator coralManipulator) { + return new SequentialCommandGroup( + coralManipulator.transferIntake(), + new WaitUntilCommand(() -> coralManipulator.deployAtTarget()), + coralManipulator.setIndexerVoltage(CoralConstants.indexerTransferVoltage), + coralManipulator.setIntakeVoltage(CoralConstants.intakeTransferVoltage), + coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.transfer.height)); + // new WaitUntilCommand(() -> coralManipulator.getIndexerSensor()), + // coralManipulator.setCoralStateCommand(coralState.coralInIndexer), + // coralManipulator.stopIndexer(), + // coralManipulator.stopIntake(), + // coralManipulator.retractIntake()); + } + + public static Command transferCoralToElevator(CoralManipulator coralManipulator) { + return new SequentialCommandGroup( + coralManipulator.setElevatorPosition(CoralConstants.elevatorLevel.transfer.height), + new WaitUntilCommand(() -> coralManipulator.elevatorAtTarget()), + coralManipulator.setIndexerVoltage(CoralConstants.indexerTransferVoltage), + coralManipulator.shoot(CoralConstants.elevatorLevel.transfer.shootVoltage), + new WaitUntilCommand(() -> !coralManipulator.getIndexerSensor()), + coralManipulator.setCoralStateCommand(coralState.coralInIndexer)); + // coralManipulator.stopIndexer(), + // coralManipulator.stopShooter(), + // coralManipulator.elevatorToHome()); + } + +} diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java deleted file mode 100644 index 40175f9..0000000 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.subsystems.Arm; -import frc.robot.util.ArmPoint; - -import java.util.function.IntSupplier; -import java.util.function.Supplier; - -import javax.crypto.SecretKeyFactorySpi; - -import org.opencv.core.Point; - -public class ArmCommand extends Command { - private Arm arm; - private Supplier point; - private Supplier inBend; - private ArmPoint prevPoint = new ArmPoint(new Translation2d()); - /** Creates a new ArmCommand. */ - public ArmCommand(Arm arm, Supplier point) { - this.arm = arm; - this.point = point; - inBend = () -> point.get().inBend; - - addRequirements(arm); - } - public ArmCommand(Arm arm, IntSupplier index) { - this.arm = arm; - this.point = () -> ArmSetpoints.armSetPoints[index.getAsInt()]; - inBend = () -> this.point.get().inBend; - - addRequirements(arm); - } - - - // Called when the command is initially scheduled. - @Override - public void initialize() { - // arm.resetEncoders(); - System.out.println("armcommand init " + point.get().position.toString() + "wrist " + point.get().wrist); - setPosition(); - - } - public void setPosition() { - System.out.println("armcommand refresh - xy:" + point.get().position.toString() + " - wrist:" + point.get().wrist); - arm.setArmPosition(point.get().position, inBend.get()); - arm.setWristTarget(point.get().wrist); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // arm.getArmPosition(); - if ((!point.get().position.equals(prevPoint.position)) || !(point.get().wrist == prevPoint.wrist)) { - setPosition(); - prevPoint = point.get(); - } - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - // arm.setElbowPosition(-ArmConstants.elbowOffset); - // arm.setShoulderPosition(-ArmConstants.shoulderOffset); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } - - @Override - public InterruptionBehavior getInterruptionBehavior() { - return InterruptionBehavior.kCancelSelf; - } -} diff --git a/src/main/java/frc/robot/commands/ArmCommandPathToPoint.java b/src/main/java/frc/robot/commands/ArmCommandPathToPoint.java deleted file mode 100644 index 2f9fdfc..0000000 --- a/src/main/java/frc/robot/commands/ArmCommandPathToPoint.java +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import static edu.wpi.first.units.Units.Rotation; - -import java.util.ArrayList; -import java.util.List; -import java.util.function.IntSupplier; -import java.util.function.Supplier; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.LEDPattern; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.Constants.ArmVelocityGains; -import frc.robot.subsystems.Arm; -// import frc.robot.subsystems.LEDSubsytem.LEDSubsystem; -import frc.robot.util.ArmPath; -import frc.robot.util.ArmPathplannerUtil; -import frc.robot.util.ArmPoint; -import frc.robot.util.GenPath; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class ArmCommandPathToPoint extends Command { - private Arm arm; - private ArmPath path; - private IntSupplier setPointIntSupplier; - private int setPointIndex = -1; - private Supplier setPoint = null; - private double initialWrist, initialWristTwist; - /** path to the specified armPoint */ // TODO: add inflection point generation and hardstop avoidance to automatic path generation - public ArmCommandPathToPoint(Arm arm, IntSupplier setPointIntSupplier) { - this.setPointIntSupplier = setPointIntSupplier; - this.arm = arm; - addRequirements(arm); - int closestPoint = ArmPathplannerUtil.closestArmPoint(ArmSetpoints.armSetPoints, arm.getArmPosition()); - initialWrist = ArmSetpoints.armSetPoints[closestPoint].wrist; - } - public ArmCommandPathToPoint(Arm arm, Supplier setPoint) { - this.setPoint = setPoint; - this.arm = arm; - addRequirements(arm); - initialWrist = arm.getWristPosition(); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - if (setPoint == null) { - setPointIndex = setPointIntSupplier.getAsInt(); - setPoint = () -> ArmSetpoints.armSetPoints[setPointIndex]; - } - calculatePath(); - - - } - public void calculatePath() { - int closestPoint = ArmPathplannerUtil.closestArmPoint(ArmSetpoints.armSetPoints, arm.getArmPosition()); - - if (closestPoint != setPointIndex && setPointIndex > -1) { - List temp = new ArrayList<>(); - temp.add(new ArmPoint(arm.getArmPosition(), arm.getCurrentInBend(), arm.getWristPosition())); - temp.addAll(ArmSetpoints.intermediatePoints[closestPoint][setPointIndex]); - temp.add(setPoint.get()); - // path = new ArmPath(GenPath.generateSmoothPath(GenPath.generateInflectionPoints(temp), ArmConstants.arcRadius, ArmConstants.arcPoints)); - path = new ArmPath(GenPath.generateInflectionPoints(temp)); - } else { - List temp = new ArrayList<>(); - temp.add(new ArmPoint(arm.getArmPosition(), arm.getCurrentInBend(), arm.getWristPosition())); - temp.add(setPoint.get()); - path = new ArmPath(GenPath.generateInflectionPoints(temp)); - } - - // System.out.println(path.toStringList().toString()); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {//TODO: add wrist interpolation or other way to time wrist movement - // System.out.println("ArmCommandPathToPoint/execute"); - if (ArmPathplannerUtil.CheckArmPosition(path.points, arm.getArmState())) { - } - if (setPointIntSupplier != null) { - if (setPointIndex != setPointIntSupplier.getAsInt()) { - System.out.println("recalculate"); - setPointIndex = setPointIntSupplier.getAsInt(); - setPoint = () -> ArmSetpoints.armSetPoints[setPointIndex]; - calculatePath(); - } - } - // SmartDashboard.putBoolean("Check", false); - if (arm.getIsFinishedMoving()){ - // SmartDashboard.putBoolean("Check", true); - arm.setArmPosition(path.getTranslations().get(path.getTranslations().size()-1), path.points.get(path.getTranslations().size() - 1).inBend); - // LEDSubsystem.runPattern(LEDPattern.solid(new Color(0.0f, 0.0f, 1.0f))); - } else { - ArmPoint nextPoint = ArmPathplannerUtil.getNextPoint(path.points, arm.getArmState()); - Rotation2d direction = nextPoint.position.minus(arm.getArmPosition()).getAngle(); - arm.setVelocity(new Translation2d(direction.getCos(), direction.getSin()).times(ArmVelocityGains.velocity), nextPoint.inBend); - // LEDSubsystem.runPattern(LEDPattern.solid(new Color(0.0f, 1.0f, 0.0f))); - } - - double nextPointIndex = ArmPathplannerUtil.getNextPointIndex(path.points, arm.getArmPosition()); - double pathProgress = nextPointIndex/path.points.size(); - double finalWrist = path.points.get(path.points.size() - 1).wrist; - // if (((pathProgress > 0.8) || arm.finishedMoving)){ - arm.setWristTarget(finalWrist); - // } else { - // arm.setWristPosition((MathUtil.clamp((pathProgress), 0, 1) * (finalWrist-initialWrist)) + initialWrist); - // arm.setWristTwistPosition((MathUtil.clamp((pathProgress), 0, 1) * (finalWristTwist-initialWristTwist)) + initialWristTwist); - // arm.stopWrist(); - - // } - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - arm.stopArm(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ArmInstantCommand.java b/src/main/java/frc/robot/commands/ArmInstantCommand.java deleted file mode 100644 index 3f562e8..0000000 --- a/src/main/java/frc/robot/commands/ArmInstantCommand.java +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.subsystems.Arm; -import frc.robot.util.ArmPoint; - -import java.util.function.IntSupplier; -import java.util.function.Supplier; - -import javax.crypto.SecretKeyFactorySpi; - -import org.opencv.core.Point; - -public class ArmInstantCommand extends InstantCommand { - private Arm arm; - private Supplier point; - private Supplier inBend; - /** Creates a new ArmCommand. */ - public ArmInstantCommand(Arm arm, Supplier point) { - this.arm = arm; - this.point = point; - inBend = () -> point.get().inBend; - - addRequirements(arm); - } - public ArmInstantCommand(Arm arm, IntSupplier index) { - this.arm = arm; - this.point = () -> ArmSetpoints.armSetPoints[index.getAsInt()]; - inBend = () -> this.point.get().inBend; - - addRequirements(arm); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - System.out.println("arm instant command init"); - System.out.println("xy:" + point.get().position.toString()); - System.out.println("wrist:" + point.get().wrist); - arm.setArmPosition(point.get().position, inBend.get()); - arm.setWristTarget(point.get().wrist); - } - - -} diff --git a/src/main/java/frc/robot/commands/AutoScoreCommand.java b/src/main/java/frc/robot/commands/AutoScoreCommand.java deleted file mode 100644 index 5f5bc6d..0000000 --- a/src/main/java/frc/robot/commands/AutoScoreCommand.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.commands; - -import java.util.function.DoubleSupplier; -import java.util.function.IntSupplier; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.FieldConstants.ReefLevel; -import frc.robot.commandSequences.Autos; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.CommandSwerveDrivetrain; - -public class AutoScoreCommand extends SequentialCommandGroup { - public AutoScoreCommand(CommandSwerveDrivetrain drive, - Supplier robotPoseSupplier, - Supplier goalPoseSupplier, - Supplier reefLevelSupplier, - Supplier isBackWordSupplier, - DoubleSupplier linearFF_X, - DoubleSupplier linearFF_Y, - DoubleSupplier omegaFF, Arm arm, IntSupplier reefTarget - ) { - - - addCommands( - new ParallelCommandGroup( - Autos.getAutoDriveCommandReef(drive, robotPoseSupplier, goalPoseSupplier, reefLevelSupplier,isBackWordSupplier, linearFF_X,linearFF_Y, omegaFF), - new WaitUntilCommand(() -> robotPoseSupplier.get().minus(goalPoseSupplier.get()).getTranslation().getNorm() < 1).andThen(new ArmCommand(arm, reefTarget)) - ) - ); - } - -} diff --git a/src/main/java/frc/robot/commands/DriveToCoral.java b/src/main/java/frc/robot/commands/DriveToCoral.java new file mode 100644 index 0000000..6f3d746 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToCoral.java @@ -0,0 +1,275 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; + +import dev.doglog.DogLog; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import frc.robot.Constants; +import frc.robot.FieldConstants; +import frc.robot.Constants.Vision; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.util.AllianceFlipUtil; + +public class DriveToCoral extends Command { + private final CommandSwerveDrivetrain drive; + private final Supplier poseSupplier; + + private boolean running = false; + double loopPeriodSecs = 0.02; +// private final ProfiledPIDController driveController = +// new ProfiledPIDController( +// Constants.Vision.kPXController, Constants.Vision.kIXController, Constants.Vision.kDXController, new TrapezoidProfile.Constraints(Constants.Vision.MAX_VELOCITY,Constants.Vision.MAX_ACCELARATION), loopPeriodSecs); +// private final ProfiledPIDController thetaController = +// new ProfiledPIDController( +// Constants.Vision.kPXController, Constants.Vision.kIThetaController, Constants.Vision.kDThetaController, new TrapezoidProfile.Constraints(Math.toRadians(Constants.Vision.MAX_VELOCITY_ROTATION), Math.toRadians(Constants.Vision.MAX_ACCELARATION_ROTATION)), loopPeriodSecs); + + +private final ProfiledPIDController driveController = + new ProfiledPIDController( + 15, 0, 0.1, new TrapezoidProfile.Constraints(Constants.Vision.MAX_VELOCITY,Constants.Vision.MAX_ACCELARATION), loopPeriodSecs); //10, 0, 0 + private final ProfiledPIDController thetaController = + new ProfiledPIDController( + 0.5, 0,0, new TrapezoidProfile.Constraints(Math.toRadians(Constants.Vision.MAX_VELOCITY_ROTATION), Math.toRadians(Constants.Vision.MAX_ACCELARATION_ROTATION)), loopPeriodSecs); //3, 10, 0 + private double driveErrorAbs; + private double thetaErrorAbs; + private Translation2d lastSetpointTranslation; + private final SwerveRequest.ApplyRobotSpeeds driveToPoseRequest = new SwerveRequest.ApplyRobotSpeeds(); + private Supplier linearFF = () -> Translation2d.kZero; + private DoubleSupplier omegaFF = () -> 0.0; + + /** Drives to the specified pose under full software control. */ + public DriveToCoral(CommandSwerveDrivetrain drive, Supplier poseSupplier) { + this.drive = drive; + this.poseSupplier = poseSupplier; + addRequirements(drive); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** Drives to the specified pose under full software control. */ + public DriveToCoral(CommandSwerveDrivetrain drive, Supplier poseSupplier, + Supplier linearFF, + DoubleSupplier omegaFF) { + this.drive = drive; + this.poseSupplier = poseSupplier; + this.linearFF = linearFF; + this.omegaFF = omegaFF; + addRequirements(drive); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } + + + @Override + public void initialize() { + + driveController.setTolerance(Constants.Vision.TRANSLATION_TOLERANCE_X, Constants.Vision.VELOCITY_TOLERANCE_X); + thetaController.setTolerance(Constants.Vision.ROTATION_TOLERANCE, Constants.Vision.VELOCITY_TOLERANCE_OMEGA); + // Reset all controllers + var currentPose = drive.getState().Pose; + + Constants.Vision.kCoralTargeted = true; + + driveController.reset( + currentPose.getTranslation().getDistance(poseSupplier.get().getTranslation()), + Math.min( + 0.0, + -new Translation2d( + drive.getCurrentRobotChassisSpeeds().vxMetersPerSecond, drive.getCurrentRobotChassisSpeeds().vyMetersPerSecond) + .rotateBy( + poseSupplier + .get() + .getTranslation() + .minus(drive.getState().Pose.getTranslation()) + .getAngle()) + .unaryMinus() + .getX())); + thetaController.reset(currentPose.getRotation().getRadians(), drive.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond); + lastSetpointTranslation = drive.getState().Pose.getTranslation(); + } + + @Override + public void execute() { + running = true; + + // Get current and target pose + var currentPose = drive.getState().Pose; + var targetPose = poseSupplier.get(); + Transform2d error = currentPose.minus(targetPose); + SignalLogger.writeDouble("X", error.getX()); + SignalLogger.writeDouble("Y", error.getY()); + SignalLogger.writeDouble("O", error.getRotation().getDegrees()); + + System.out.println("x: " + error.getX() + "; Y: " + error.getY() + "; O: " + Math.abs((currentPose.getRotation().getRadians()) - (targetPose.getRotation().getRadians()) + Math.PI)); + + // Calculate drive speed + double currentDistance = + currentPose.getTranslation().getDistance(poseSupplier.get().getTranslation()); + double ffScaler = + MathUtil.clamp( + (currentDistance - 0.1) / (0.8 - 0.1), + 0.0, + 1.0); + driveErrorAbs = currentDistance; + + driveController.reset( + lastSetpointTranslation.getDistance(targetPose.getTranslation()), + driveController.getSetpoint().velocity); + + double driveVelocityScalar = + driveController.getSetpoint().velocity * ffScaler + + driveController.calculate(driveErrorAbs, 0.0); + + if (currentDistance < driveController.getPositionTolerance()) driveVelocityScalar = 0.0; + lastSetpointTranslation = + new Pose2d( + targetPose.getTranslation(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + .transformBy( + new Transform2d(new Translation2d(driveController.getSetpoint().position, 0.0), new Rotation2d())) + .getTranslation(); + + // Calculate theta speed + double thetaVelocity = + thetaController.getSetpoint().velocity * ffScaler + + thetaController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + thetaErrorAbs = + Math.abs((currentPose.getRotation().getRadians()) - (targetPose.getRotation().getRadians()) + Math.PI); + if (thetaErrorAbs < thetaController.getPositionTolerance()) thetaVelocity = 0.0; + + + Translation2d driveVelocity = + new Pose2d( + new Translation2d(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + .transformBy(new Transform2d(driveVelocityScalar, 0.0, new Rotation2d())) + .getTranslation(); + + // Scale feedback velocities by input ff + final double linearS = linearFF.get().getNorm() * 6.0; + final double thetaS = Math.abs(omegaFF.getAsDouble()) * 3.0; + if (Vision.DOGLOG_ENABLED){ + DogLog.log("DriveToPose/driveVelocity.X", driveVelocity.getX()); + DogLog.log("DriveToPose/driveVelocity.Y", driveVelocity.getY()); + DogLog.log("DriveToPose/thetaVelocity", thetaVelocity); + DogLog.log("DriveToPose/linearS", linearS); + DogLog.log("DriveToPose/thetaS", thetaS); + } + + + if(linearS >0) + driveVelocity = + driveVelocity.interpolate(linearFF.get().times(TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)), linearS).times(3); + if(thetaS >0 ) + thetaVelocity = + MathUtil.interpolate( + thetaVelocity, omegaFF.getAsDouble() * RotationsPerSecond.of(0.75).in(RadiansPerSecond), thetaS); + if (Vision.DOGLOG_ENABLED){ + + DogLog.log("DriveToPose/driveVelocity.X_I", driveVelocity.getX()); + DogLog.log("DriveToPose/driveVelocity.Y_I", driveVelocity.getY()); + DogLog.log("DriveToPose/thetaVelocity_I", thetaVelocity); + } + + drive.setControl(driveToPoseRequest.withSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(driveVelocity.getX(),driveVelocity.getY(), + thetaVelocity, drive.getState().Pose.getRotation())).withDriveRequestType(DriveRequestType.Velocity)); + // Log data + if (Vision.DOGLOG_ENABLED){ + + DogLog.log("DriveToPose/DistanceMeasured", currentDistance); + DogLog.log("DriveToPose/DistanceSetpoint", driveController.getSetpoint().position); + DogLog.log("DriveToPose/ThetaMeasured", Math.toDegrees(currentPose.getRotation().getRadians())); + DogLog.log("DriveToPose/ThetaSetpoint", Math.toDegrees(thetaController.getSetpoint().position)); + DogLog.log( + "DriveToPose/DriveToPoseSetpoint", + new Pose2d( + lastSetpointTranslation, new Rotation2d(thetaController.getSetpoint().position))); + DogLog.log("DriveToPose/DriveToPoseGoal", targetPose); + DogLog.log("DriveToPose/ThetaErrorAbs", thetaErrorAbs); + DogLog.log("DriveToPose/DriveErrorAbs", driveErrorAbs); + DogLog.log("DriveToPose/DriveTolerance", driveController.getPositionTolerance()); + DogLog.log("DriveToPose/DriveVelocityTolerance", driveController.getVelocityTolerance()); + DogLog.log("DriveToPose/DriveVelocityError", driveController.getVelocityError()); + DogLog.log("DriveToPose/ThetaVelocityTolerance", thetaController.getVelocityTolerance()); + DogLog.log("DriveToPose/ThetaVelocityError", thetaController.getVelocityError()); + + DogLog.log("DriveToPose/DriveControllerAtGoal", driveController.atGoal()); + DogLog.log("DriveToPose/ThetaControllerAtGoal", thetaController.atGoal()); + } + } + + @Override + public void end(boolean interrupted) { + running = false; + Constants.Vision.kCoralTargeted = false; + // Constants.Vision.kCoralAutoTarget = false; + drive.applyRequest(() -> new SwerveRequest.SwerveDriveBrake()); + if (Vision.DOGLOG_ENABLED){ + + DogLog.log("DriveToPose/DriveToPoseSetpoint", new double[] {}); + DogLog.log("DriveToPose/DriveToPoseGoal", new double[] {}); + } + } + + /** Checks if the robot is stopped at the final pose. */ + public boolean atGoal() { + thetaErrorAbs = + Math.abs((drive.getState().Pose.getRotation().getRadians()) - (poseSupplier.get().getRotation().getRadians()) + Math.PI); + //boolean thetaAtGoal = (Math.abs(drive.getState().Pose.getRotation().getDegrees() - poseSupplier.get().getRotation().getDegrees()) < 20); + if (Vision.DOGLOG_ENABLED){ + DogLog.log("DriveToPose/DriveControllerAtGoal1", driveController.atGoal()); + DogLog.log("DriveToPose/ThetaControllerAtGoal1", thetaController.atGoal()); + } + return running && driveController.atGoal() && (thetaErrorAbs < thetaController.getPositionTolerance()); + } + + /** Checks if the robot pose is within the allowed drive and theta tolerances. */ + public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) { + return running + && Math.abs(driveErrorAbs) < driveTolerance + && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians(); + } + + /** Returns whether the command is actively running. */ + public boolean isRunning() { + return running; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return this.atGoal();// || !Constants.Vision.kCoralAutoTarget; + } + + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DriveToCoralAuto.java b/src/main/java/frc/robot/commands/DriveToCoralAuto.java new file mode 100644 index 0000000..88fcc28 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToCoralAuto.java @@ -0,0 +1,275 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; + +import dev.doglog.DogLog; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import frc.robot.Constants; +import frc.robot.FieldConstants; +import frc.robot.Constants.Vision; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.util.AllianceFlipUtil; + +public class DriveToCoralAuto extends Command { + private final CommandSwerveDrivetrain drive; + private final Supplier poseSupplier; + + private boolean running = false; + double loopPeriodSecs = 0.02; +// private final ProfiledPIDController driveController = +// new ProfiledPIDController( +// Constants.Vision.kPXController, Constants.Vision.kIXController, Constants.Vision.kDXController, new TrapezoidProfile.Constraints(Constants.Vision.MAX_VELOCITY,Constants.Vision.MAX_ACCELARATION), loopPeriodSecs); +// private final ProfiledPIDController thetaController = +// new ProfiledPIDController( +// Constants.Vision.kPXController, Constants.Vision.kIThetaController, Constants.Vision.kDThetaController, new TrapezoidProfile.Constraints(Math.toRadians(Constants.Vision.MAX_VELOCITY_ROTATION), Math.toRadians(Constants.Vision.MAX_ACCELARATION_ROTATION)), loopPeriodSecs); + + +private final ProfiledPIDController driveController = + new ProfiledPIDController( + 15, 0, 0.1, new TrapezoidProfile.Constraints(Constants.Vision.MAX_VELOCITY,Constants.Vision.MAX_ACCELARATION), loopPeriodSecs); //10, 0, 0 + private final ProfiledPIDController thetaController = + new ProfiledPIDController( + 7, 0,0, new TrapezoidProfile.Constraints(Math.toRadians(Constants.Vision.MAX_VELOCITY_ROTATION), Math.toRadians(Constants.Vision.MAX_ACCELARATION_ROTATION)), loopPeriodSecs); //3, 10, 0 + private double driveErrorAbs; + private double thetaErrorAbs; + private Translation2d lastSetpointTranslation; + private final SwerveRequest.ApplyRobotSpeeds driveToPoseRequest = new SwerveRequest.ApplyRobotSpeeds(); + private Supplier linearFF = () -> Translation2d.kZero; + private DoubleSupplier omegaFF = () -> 0.0; + + /** Drives to the specified pose under full software control. */ + public DriveToCoralAuto(CommandSwerveDrivetrain drive, Supplier poseSupplier) { + this.drive = drive; + this.poseSupplier = poseSupplier; + addRequirements(drive); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** Drives to the specified pose under full software control. */ + public DriveToCoralAuto(CommandSwerveDrivetrain drive, Supplier poseSupplier, + Supplier linearFF, + DoubleSupplier omegaFF) { + this.drive = drive; + this.poseSupplier = poseSupplier; + this.linearFF = linearFF; + this.omegaFF = omegaFF; + addRequirements(drive); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } + + + @Override + public void initialize() { + + driveController.setTolerance(Constants.Vision.TRANSLATION_TOLERANCE_X, Constants.Vision.VELOCITY_TOLERANCE_X); + thetaController.setTolerance(Constants.Vision.ROTATION_TOLERANCE, Constants.Vision.VELOCITY_TOLERANCE_OMEGA); + // Reset all controllers + var currentPose = drive.getState().Pose; + + Constants.Vision.kCoralTargeted = true; + + driveController.reset( + currentPose.getTranslation().getDistance(poseSupplier.get().getTranslation()), + Math.min( + 0.0, + -new Translation2d( + drive.getCurrentRobotChassisSpeeds().vxMetersPerSecond, drive.getCurrentRobotChassisSpeeds().vyMetersPerSecond) + .rotateBy( + poseSupplier + .get() + .getTranslation() + .minus(drive.getState().Pose.getTranslation()) + .getAngle()) + .unaryMinus() + .getX())); + thetaController.reset(currentPose.getRotation().getRadians(), drive.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond); + lastSetpointTranslation = drive.getState().Pose.getTranslation(); + } + + @Override + public void execute() { + running = true; + + // Get current and target pose + var currentPose = drive.getState().Pose; + var targetPose = poseSupplier.get(); + Transform2d error = currentPose.minus(targetPose); + SignalLogger.writeDouble("X", error.getX()); + SignalLogger.writeDouble("Y", error.getY()); + SignalLogger.writeDouble("O", error.getRotation().getDegrees()); + + System.out.println("x: " + error.getX() + "; Y: " + error.getY() + "; O: " + Math.abs((currentPose.getRotation().getRadians()) - (targetPose.getRotation().getRadians()) + Math.PI)); + + // Calculate drive speed + double currentDistance = + currentPose.getTranslation().getDistance(poseSupplier.get().getTranslation()); + double ffScaler = + MathUtil.clamp( + (currentDistance - 0.1) / (0.8 - 0.1), + 0.0, + 1.0); + driveErrorAbs = currentDistance; + + driveController.reset( + lastSetpointTranslation.getDistance(targetPose.getTranslation()), + driveController.getSetpoint().velocity); + + double driveVelocityScalar = + driveController.getSetpoint().velocity * ffScaler + + driveController.calculate(driveErrorAbs, 0.0); + + if (currentDistance < driveController.getPositionTolerance()) driveVelocityScalar = 0.0; + lastSetpointTranslation = + new Pose2d( + targetPose.getTranslation(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + .transformBy( + new Transform2d(new Translation2d(driveController.getSetpoint().position, 0.0), new Rotation2d())) + .getTranslation(); + + // Calculate theta speed + double thetaVelocity = + thetaController.getSetpoint().velocity * ffScaler + + thetaController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + thetaErrorAbs = + Math.abs((currentPose.getRotation().getRadians()) - (targetPose.getRotation().getRadians()) + Math.PI); + if (thetaErrorAbs < thetaController.getPositionTolerance()) thetaVelocity = 0.0; + + + Translation2d driveVelocity = + new Pose2d( + new Translation2d(), + currentPose.getTranslation().minus(targetPose.getTranslation()).getAngle()) + .transformBy(new Transform2d(driveVelocityScalar, 0.0, new Rotation2d())) + .getTranslation(); + + // Scale feedback velocities by input ff + final double linearS = linearFF.get().getNorm() * 6.0; + final double thetaS = Math.abs(omegaFF.getAsDouble()) * 3.0; + if (Vision.DOGLOG_ENABLED){ + DogLog.log("DriveToPose/driveVelocity.X", driveVelocity.getX()); + DogLog.log("DriveToPose/driveVelocity.Y", driveVelocity.getY()); + DogLog.log("DriveToPose/thetaVelocity", thetaVelocity); + DogLog.log("DriveToPose/linearS", linearS); + DogLog.log("DriveToPose/thetaS", thetaS); + } + + + if(linearS >0) + driveVelocity = + driveVelocity.interpolate(linearFF.get().times(TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)), linearS).times(3); + if(thetaS >0 ) + thetaVelocity = + MathUtil.interpolate( + thetaVelocity, omegaFF.getAsDouble() * RotationsPerSecond.of(0.75).in(RadiansPerSecond), thetaS); + if (Vision.DOGLOG_ENABLED){ + + DogLog.log("DriveToPose/driveVelocity.X_I", driveVelocity.getX()); + DogLog.log("DriveToPose/driveVelocity.Y_I", driveVelocity.getY()); + DogLog.log("DriveToPose/thetaVelocity_I", thetaVelocity); + } + + drive.setControl(driveToPoseRequest.withSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(driveVelocity.getX(),driveVelocity.getY(), + thetaVelocity, drive.getState().Pose.getRotation())).withDriveRequestType(DriveRequestType.Velocity)); + // Log data + if (Vision.DOGLOG_ENABLED){ + + DogLog.log("DriveToPose/DistanceMeasured", currentDistance); + DogLog.log("DriveToPose/DistanceSetpoint", driveController.getSetpoint().position); + DogLog.log("DriveToPose/ThetaMeasured", Math.toDegrees(currentPose.getRotation().getRadians())); + DogLog.log("DriveToPose/ThetaSetpoint", Math.toDegrees(thetaController.getSetpoint().position)); + DogLog.log( + "DriveToPose/DriveToPoseSetpoint", + new Pose2d( + lastSetpointTranslation, new Rotation2d(thetaController.getSetpoint().position))); + DogLog.log("DriveToPose/DriveToPoseGoal", targetPose); + DogLog.log("DriveToPose/ThetaErrorAbs", thetaErrorAbs); + DogLog.log("DriveToPose/DriveErrorAbs", driveErrorAbs); + DogLog.log("DriveToPose/DriveTolerance", driveController.getPositionTolerance()); + DogLog.log("DriveToPose/DriveVelocityTolerance", driveController.getVelocityTolerance()); + DogLog.log("DriveToPose/DriveVelocityError", driveController.getVelocityError()); + DogLog.log("DriveToPose/ThetaVelocityTolerance", thetaController.getVelocityTolerance()); + DogLog.log("DriveToPose/ThetaVelocityError", thetaController.getVelocityError()); + + DogLog.log("DriveToPose/DriveControllerAtGoal", driveController.atGoal()); + DogLog.log("DriveToPose/ThetaControllerAtGoal", thetaController.atGoal()); + } + } + + @Override + public void end(boolean interrupted) { + running = false; + Constants.Vision.kCoralTargeted = false; + // Constants.Vision.kCoralAutoTarget = false; + drive.applyRequest(() -> new SwerveRequest.SwerveDriveBrake()); + if (Vision.DOGLOG_ENABLED){ + + DogLog.log("DriveToPose/DriveToPoseSetpoint", new double[] {}); + DogLog.log("DriveToPose/DriveToPoseGoal", new double[] {}); + } + } + + /** Checks if the robot is stopped at the final pose. */ + public boolean atGoal() { + thetaErrorAbs = + Math.abs((drive.getState().Pose.getRotation().getRadians()) - (poseSupplier.get().getRotation().getRadians()) + Math.PI); + //boolean thetaAtGoal = (Math.abs(drive.getState().Pose.getRotation().getDegrees() - poseSupplier.get().getRotation().getDegrees()) < 20); + if (Vision.DOGLOG_ENABLED){ + DogLog.log("DriveToPose/DriveControllerAtGoal1", driveController.atGoal()); + DogLog.log("DriveToPose/ThetaControllerAtGoal1", thetaController.atGoal()); + } + return running && driveController.atGoal() && (thetaController.atGoal()); + } + + /** Checks if the robot pose is within the allowed drive and theta tolerances. */ + public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) { + return running + && Math.abs(driveErrorAbs) < driveTolerance + && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians(); + } + + /** Returns whether the command is actively running. */ + public boolean isRunning() { + return running; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return this.atGoal(); + } + + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java index 83d360f..1031dd5 100644 --- a/src/main/java/frc/robot/commands/DriveToPose.java +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -35,6 +35,7 @@ import frc.robot.Constants; import frc.robot.FieldConstants; +import frc.robot.Constants.AutoDropoff; import frc.robot.Constants.Vision; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -45,7 +46,7 @@ public class DriveToPose extends Command { private final Supplier poseSupplier; private boolean running = false; - double loopPeriodSecs = 0.02; + double loopPeriodSecs = AutoDropoff.loopPeriodSecs; // private final ProfiledPIDController driveController = // new ProfiledPIDController( // Constants.Vision.kPXController, Constants.Vision.kIXController, Constants.Vision.kDXController, new TrapezoidProfile.Constraints(Constants.Vision.MAX_VELOCITY,Constants.Vision.MAX_ACCELARATION), loopPeriodSecs); @@ -54,12 +55,8 @@ public class DriveToPose extends Command { // Constants.Vision.kPXController, Constants.Vision.kIThetaController, Constants.Vision.kDThetaController, new TrapezoidProfile.Constraints(Math.toRadians(Constants.Vision.MAX_VELOCITY_ROTATION), Math.toRadians(Constants.Vision.MAX_ACCELARATION_ROTATION)), loopPeriodSecs); -private final ProfiledPIDController driveController = - new ProfiledPIDController( - 15, 0, 0.1, new TrapezoidProfile.Constraints(Constants.Vision.MAX_VELOCITY,Constants.Vision.MAX_ACCELARATION), loopPeriodSecs); //10, 0, 0 - private final ProfiledPIDController thetaController = - new ProfiledPIDController( - 7, 0,0, new TrapezoidProfile.Constraints(Math.toRadians(Constants.Vision.MAX_VELOCITY_ROTATION), Math.toRadians(Constants.Vision.MAX_ACCELARATION_ROTATION)), loopPeriodSecs); //3, 10, 0 + private final ProfiledPIDController driveController = AutoDropoff.driveController; + private final ProfiledPIDController thetaController = AutoDropoff.thetaController; private double driveErrorAbs; private double thetaErrorAbs; private Translation2d lastSetpointTranslation; @@ -135,7 +132,7 @@ public void execute() { MathUtil.clamp( (currentDistance - 0.1) / (0.8 - 0.1), 0.0, - 1.0); + 0.0); driveErrorAbs = currentDistance; driveController.reset( @@ -264,6 +261,7 @@ public boolean isFinished() { return this.atGoal(); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java deleted file mode 100644 index 94978f7..0000000 --- a/src/main/java/frc/robot/commands/GripperCommand.java +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import java.util.function.DoubleSupplier; -import java.util.function.IntSupplier; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.Constants.ArmConstants; -import frc.robot.subsystems.Bucket; -import frc.robot.subsystems.Gripper; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class GripperCommand extends InstantCommand { - /** Creates a new GripperComand. */ - private Gripper gripper; - private double power; - private double currentLimit; - - public GripperCommand(Gripper gripper, double power, double currentLimit) { - // Use addRequirements() here to declare subsystem dependencies. - this.gripper = gripper; - this.power = power; - this.currentLimit = currentLimit; - - addRequirements(gripper); - } - public GripperCommand(Gripper gripper, double power) { - // Use addRequirements() here to declare subsystem dependencies. - this.gripper = gripper; - this.power = power; - this.currentLimit = ArmConstants.gripperCurrentLimitDefault; - - addRequirements(gripper); - } - public GripperCommand(Gripper gripper) { - // Use addRequirements() here to declare subsystem dependencies. - this.gripper = gripper; - this.power = ArmConstants.gripperPowerDefault; - this.currentLimit = ArmConstants.gripperCurrentLimitDefault; - - addRequirements(gripper); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - gripper.setGripperPower(power); - - if (currentLimit != gripper.getCurrentLimit()) { - gripper.setCurrentLimit(currentLimit); - } - if (power < 0) { - // Bucket.gripperHasGamePiece = false; - } - } - -} diff --git a/src/main/java/frc/robot/commands/LEDCommand.java b/src/main/java/frc/robot/commands/LEDCommand.java index 1148a18..23b905f 100644 --- a/src/main/java/frc/robot/commands/LEDCommand.java +++ b/src/main/java/frc/robot/commands/LEDCommand.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; -import frc.robot.subsystems.Arm; import frc.robot.subsystems.LEDSubsytem; import frc.robot.subsystems.PoseEstimatorSubsystem; import frc.robot.subsystems.ScoringProfileSubsystem; diff --git a/src/main/java/frc/robot/commands/SetStowing.java b/src/main/java/frc/robot/commands/SetStowing.java deleted file mode 100644 index 3836856..0000000 --- a/src/main/java/frc/robot/commands/SetStowing.java +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.subsystems.Arm; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class SetStowing extends InstantCommand { - private Arm arm; - private boolean stowing; - public SetStowing(Arm arm, boolean stowing) { - this.arm = arm; - this.stowing = stowing; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - arm.setStowing(stowing); - // SmartDashboard.putBoolean("stowing", stowing); - - } -} diff --git a/src/main/java/frc/robot/commands/WristCommand.java b/src/main/java/frc/robot/commands/WristCommand.java deleted file mode 100644 index 040ca76..0000000 --- a/src/main/java/frc/robot/commands/WristCommand.java +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Arm; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class WristCommand extends Command { - private Arm arm; - private Supplier position; - /** Creates a new ArmCommandWrist. */ - public WristCommand(Arm arm, Supplier position) { - this.arm = arm; - this.position = position; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - arm.setWristTarget(position.get()); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 78c4fa7..d33afc5 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -19,17 +20,22 @@ // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. - // The steer motor uses any SwerveModule.SteerRequestType control request with the + + // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0) - .withKS(0).withKV(0.124).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + .withKP(100).withKI(0).withKD(0.0) + .withKS(0.1).withKV(0.124).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + // private static final Slot0Configs driveGains = new Slot0Configs() + // .withKP(0.1).withKI(0).withKD(0) + // .withKS(0).withKV(0.124); + private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(5.26885).withKI(0).withKD(0) - .withKS(4.53832).withKV(0.124).withKA(0.89048); + .withKP(5.26885).withKI(0).withKD(0) + .withKS(4.53832).withKV(0.124).withKA(0.89048); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -44,7 +50,7 @@ public class TunerConstants { private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; // The stator current at which the wheels start to slip; @@ -72,20 +78,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.21); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.96); // 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.5714285714285716; + private static final double kCoupleRatio = 3.125; - private static final double kDriveGearRatio = 6.122448979591837; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); + private static final double kDriveGearRatio = 5.357142857142857; + private static final double kSteerGearRatio = 18.75; + private static final Distance kWheelRadius = Inches.of(1.95); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - public static final int kPigeonId = 25; + private static final int kPigeonId = 25; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -127,45 +133,45 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 15; private static final int kFrontLeftSteerMotorId = 16; private static final int kFrontLeftEncoderId = 23; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.39111328125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.043701171875); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(12); - private static final Distance kFrontLeftYPos = Inches.of(10); + private static final Distance kFrontLeftXPos = Inches.of(9); + private static final Distance kFrontLeftYPos = Inches.of(8); // Front Right private static final int kFrontRightDriveMotorId = 13; private static final int kFrontRightSteerMotorId = 14; private static final int kFrontRightEncoderId = 22; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.2900390625); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.43701171875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(12); - private static final Distance kFrontRightYPos = Inches.of(-10); + private static final Distance kFrontRightXPos = Inches.of(9); + private static final Distance kFrontRightYPos = Inches.of(-8); // Back Left private static final int kBackLeftDriveMotorId = 17; private static final int kBackLeftSteerMotorId = 18; private static final int kBackLeftEncoderId = 24; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.469482421875); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.401611328125); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-12); - private static final Distance kBackLeftYPos = Inches.of(10); + private static final Distance kBackLeftXPos = Inches.of(-9); + private static final Distance kBackLeftYPos = Inches.of(8); // Back Right private static final int kBackRightDriveMotorId = 11; private static final int kBackRightSteerMotorId = 12; private static final int kBackRightEncoderId = 21; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.115234375); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.140380859375); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-12); - private static final Distance kBackRightYPos = Inches.of(-10); + private static final Distance kBackRightXPos = Inches.of(-9); + private static final Distance kBackRightYPos = Inches.of(-8); public static final SwerveModuleConstants FrontLeft = diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java deleted file mode 100644 index b520f3f..0000000 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ /dev/null @@ -1,663 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Volts; - -import java.io.Console; -import java.util.ArrayList; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -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.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.compound.Diff_DutyCycleOut_Position; -import com.ctre.phoenix6.controls.compound.Diff_MotionMagicVoltage_Position; -import com.ctre.phoenix6.controls.compound.Diff_PositionVoltage_Velocity; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.ControlModeValue; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmGains; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.Constants.ArmVelocityGains; -import frc.robot.commands.ArmCommand; -import frc.robot.commands.ArmInstantCommand; -import frc.robot.util.ArmPoint; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; - -/** - * Subsystem for the 3-jointed arm with a shoulder, elbow, and wrist. - * - * The shoulder and elbow consists of 2 motors each while the wrist consists - * of a single motor. - */ -public class Arm extends SubsystemBase { - - private TalonFX shoulderLeft, shoulderRight, elbowLeft, elbowRight, wrist; - - private boolean wristStopped = true; - private TalonFXConfiguration shoulderConfig = new TalonFXConfiguration(); - public boolean stowing = false; - private SlewRateLimiter shoulderLimiter = new SlewRateLimiter(ArmConstants.shoulderSlewRate); - private SlewRateLimiter elbowLimiter = new SlewRateLimiter(ArmConstants.elbowSlewRate); - private double wristOffset = ArmConstants.wristOffset; - private TalonFXConfiguration wristConfig = new TalonFXConfiguration(); - - public double wristTarget = ArmSetpoints.homeWrist; - public double elbowTarget = ArmConstants.shoulderOffset; - public double shoulderTarget = ArmConstants.elbowOffset; - private Translation2d targetPosition = new Translation2d(); - - /** - * Create a new Arm. - */ - public Arm() { - shoulderLeft = new TalonFX(ArmConstants.shoulderMotorLeftPort, ArmConstants.armCanBus); - shoulderRight = new TalonFX(ArmConstants.shoulderMotorRightPort, ArmConstants.armCanBus); - elbowLeft = new TalonFX(ArmConstants.elbowMotorLeftPort, ArmConstants.armCanBus); - elbowRight = new TalonFX(ArmConstants.elbowMotorRightPort, ArmConstants.armCanBus); - wrist = new TalonFX(ArmConstants.wristMotorPort, ArmConstants.armCanBus); - - TalonFXConfiguration elbowConfig = new TalonFXConfiguration(); - - shoulderConfig.CurrentLimits = new CurrentLimitsConfigs() - .withStatorCurrentLimit(ArmConstants.currentLimitShoulder) - .withStatorCurrentLimitEnable(true); - shoulderConfig.Feedback = new FeedbackConfigs() - .withFeedbackRotorOffset(ArmConstants.shoulderOffset) - .withSensorToMechanismRatio(ArmConstants.shoulderRadPerRot); - shoulderConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(0.03); - shoulderConfig.Slot0 = new Slot0Configs() - .withKP(ArmGains.shoulderP) - .withKI(ArmGains.shoulderI) - .withKD(ArmGains.shoulderD) - .withKG(ArmGains.shoulderG) - .withGravityType(GravityTypeValue.Arm_Cosine); - shoulderConfig.Slot1 = new Slot1Configs() - .withKP(ArmVelocityGains.shoulderP) - .withKI(ArmVelocityGains.shoulderI) - .withKD(ArmVelocityGains.shoulderD) - .withKG(ArmVelocityGains.shoulderG) - .withGravityType(GravityTypeValue.Arm_Cosine); - shoulderConfig.MotionMagic = new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(ArmGains.cruiseVelocity) - .withMotionMagicAcceleration(ArmGains.cruiseAcceleration); - shoulderLeft - .getConfigurator() - .apply(shoulderConfig.withMotorOutput(new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast))); - - shoulderRight - .getConfigurator() - .apply(shoulderConfig.withMotorOutput(new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast))); - - - elbowConfig.CurrentLimits = new CurrentLimitsConfigs() - .withStatorCurrentLimit(ArmConstants.currentLimitElbow) - .withStatorCurrentLimitEnable(true); - elbowConfig.Feedback = new FeedbackConfigs() - .withFeedbackRotorOffset(ArmConstants.elbowOffset) - .withSensorToMechanismRatio(ArmConstants.elbowRadPerRot); - elbowConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(0.03); - elbowConfig.Slot0 = new Slot0Configs() - .withKP(ArmGains.elbowP) - .withKI(ArmGains.elbowI) - .withKD(ArmGains.elbowD) - .withKG(ArmGains.elbowG) - .withGravityType(GravityTypeValue.Arm_Cosine); - elbowConfig.Slot1 = new Slot1Configs() - .withKP(ArmVelocityGains.elbowP) - .withKI(ArmVelocityGains.elbowI) - .withKD(ArmVelocityGains.elbowD) - .withKG(ArmVelocityGains.elbowG) - .withGravityType(GravityTypeValue.Arm_Cosine); - - elbowConfig.MotionMagic = new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(ArmGains.cruiseVelocity) - .withMotionMagicAcceleration(ArmGains.cruiseAcceleration); - elbowLeft - .getConfigurator() - .apply(elbowConfig.withMotorOutput(new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast))); - - elbowRight - .getConfigurator() - .apply(elbowConfig.withMotorOutput(new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast))); - - wristConfig.CurrentLimits = new CurrentLimitsConfigs() - .withStatorCurrentLimit(ArmConstants.currentLimitWrist) - .withStatorCurrentLimitEnable(true); - wristConfig.Feedback = new FeedbackConfigs() - .withFeedbackRotorOffset(ArmConstants.wristOffset) - .withSensorToMechanismRatio(ArmConstants.wristRadPerRot); - wristConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(0.1); - wristConfig.Slot0 = new Slot0Configs() - .withKP(ArmGains.wristP) - .withKI(ArmGains.wristI) - .withKD(ArmGains.wristD); - wristConfig.MotionMagic = new MotionMagicConfigs().withMotionMagicAcceleration(ArmGains.wristAcceleration).withMotionMagicCruiseVelocity(ArmGains.wristVelocity); - wrist - .getConfigurator() - .apply(wristConfig.withMotorOutput(new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast))); - - resetOffsets(); - - } - - public boolean getIsFinishedMoving() { - return targetPosition.getDistance(getArmPosition()) < 1; - } - public void addToWristOffset(double addTo) { - // wristOffset += addTo; - // wrist.getConfigurator().apply(wristConfig.withFeedback(new FeedbackConfigs() - // .withFeedbackRotorOffset(wristOffset) - // .withSensorToMechanismRatio(ArmConstants.wristRadPerRot))); - wrist.setPosition(wrist.getPosition().getValueAsDouble()+addTo); - // return wristOffset; - } - - /** - * Reset velocity limiters for shoulder and elbow. - */ - public void resetVelocityLimiters() { - shoulderLimiter.reset(getShoulderVelocity()); - elbowLimiter.reset(getElbowVelocity()); - } - - /** - * Get shoulder velocity. - * - * @return Averaged shoulder velocity in rotations per second. - */ - public double getShoulderVelocity() { - return (shoulderLeft.getVelocity().getValueAsDouble() + shoulderRight.getVelocity().getValueAsDouble())/2; - } - - /** - * Get elbow velocity. - * - * @return Averaged elbow velocity in rotations per second. - */ - public double getElbowVelocity() { - return (elbowLeft.getVelocity().getValueAsDouble() + elbowRight.getVelocity().getValueAsDouble())/2; - } - - // public void setBrakeMode(boolean BrakeModeEnabled) { - // if (BrakeModeEnabled) { - // shoulderLeft - // .getConfigurator() - // .apply(shoulderConfig.withMotorOutput(new MotorOutputConfigs() - // .withInverted(InvertedValue.CounterClockwise_Positive) - // .withNeutralMode(NeutralModeValue.Brake))); - // shoulderRight - // .getConfigurator() - // .apply(shoulderConfig.withMotorOutput(new MotorOutputConfigs() - // .withInverted(InvertedValue.Clockwise_Positive) - // .withNeutralMode(NeutralModeValue.Brake))); - // } else { - // shoulderLeft - // .getConfigurator() - // .apply(shoulderConfig.withMotorOutput(new MotorOutputConfigs() - // .withInverted(InvertedValue.CounterClockwise_Positive) - // .withNeutralMode(NeutralModeValue.Coast))); - // shoulderRight - // .getConfigurator() - // .apply(shoulderConfig.withMotorOutput(new MotorOutputConfigs() - // .withInverted(InvertedValue.Clockwise_Positive) - // .withNeutralMode(NeutralModeValue.Coast))); - // } - // } - - /** - * Set arm to offset positions. - * The offset positions are - *
    - *
  • {@link Constants.ArmConstants.elbowOffset} - *
  • {@link Constants.ArmConstants.shoulderOffset} - *
  • {@link Constants.ArmConstants.wristOffset} - *
- */ - public void resetOffsets() { - elbowRight.setPosition(Constants.ArmConstants.elbowOffset); - elbowLeft.setPosition(Constants.ArmConstants.elbowOffset); - shoulderRight.setPosition(Constants.ArmConstants.shoulderOffset); - shoulderLeft.setPosition(Constants.ArmConstants.shoulderOffset); - wrist.setPosition(Constants.ArmConstants.wristOffset); - } - - /** - * Obtain the 2d position of the wrist. - * The 2d plane is the plane in which the arm's movement is constrained. - * - * @return The arm position. - */ - public Translation2d getArmPosition() { - Translation2d jointPos = new Translation2d( - Math.cos(getShoulderPosition()) * ArmConstants.baseStageLength, - Math.sin(getShoulderPosition()) * ArmConstants.baseStageLength); - Translation2d jointToEndPos = new Translation2d( - Math.cos(getElbowPosition()) * ArmConstants.secondStageLength, - Math.sin(getElbowPosition()) * ArmConstants.secondStageLength); - // SmartDashboard.putNumber("arm x position", jointPos.plus(jointToEndPos).getX()); - // SmartDashboard.putNumber("arm y position", jointPos.plus(jointToEndPos).getY()); - return jointPos.plus(jointToEndPos); - // return ArmSetpoints.armSetPoints[4].position; - } - - /** - * Get the current state of the arm. - * - * @return the current state of the arm. - */ - public ArmPoint getArmState() { - return new ArmPoint(getArmPosition(), getCurrentInBend(), getWristPosition()); - } - - /** - * Move the arm to a specified position by rotating the shoulder and elbow. - * - * @param position position to move the wrist to. - * @param inBend a flag to choose which orientation of the arm to use. - * * false = bend convex side facing negative rotation direction - * * true = bend convex side facing positive rotation direction - */ - public void setArmPosition(Translation2d position, boolean inBend) { // rotates the two base stages - targetPosition = position; - - double distance = MathUtil.clamp(position.getNorm(), ArmSetpoints.home.getNorm(), ArmConstants.baseStageLength + ArmConstants.secondStageLength); - - double BaseAngleArmDiff = Math.acos(((distance * distance) - + (ArmConstants.baseStageLength * ArmConstants.baseStageLength) - - (ArmConstants.secondStageLength * ArmConstants.secondStageLength)) - / (2 * distance * ArmConstants.baseStageLength)); - double SecondAngleArmDiff = Math.acos(((distance * distance) - - (ArmConstants.baseStageLength * ArmConstants.baseStageLength) - + (ArmConstants.secondStageLength * ArmConstants.secondStageLength)) - / (2 * distance * ArmConstants.secondStageLength)); - double shoulderPosition = position.getAngle().getRadians() + (BaseAngleArmDiff * (inBend ? 1 : -1)); - double elbowPosition = position.getAngle().getRadians() + (SecondAngleArmDiff * (inBend ? -1 : 1)); - shoulderTarget = shoulderPosition; - elbowTarget = elbowPosition; - setShoulderPosition(shoulderPosition); - setElbowPosition(elbowPosition); - - resetVelocityLimiters(); - // smart dashboard - // SmartDashboard.putNumber("shouldertarget", shoulderPosition); - // SmartDashboard.putNumber("elbowtarget", elbowPosition); - // SmartDashboard.putNumber("shoulderLeftPosition error", shoulderPosition - getShoulderPosition()); - // SmartDashboard.putNumber("elbow Left Position error", elbowPosition - getElbowPosition()); - } - - /** - * Set the velocity of the arm by setting the shoulder and elbow velocities. - * - * @param velocity vector defining desired velocity of the wrist. - * @param inBend a flag to choose which orientation of the arm to use. - */ - public void setVelocity(Translation2d velocity, boolean inBend){ - Translation2d position = getArmPosition().plus(velocity.times(ArmVelocityGains.linearApproximationTime)); - // SmartDashboard.putNumber("velocity x", velocity.getX()); - // SmartDashboard.putNumber("velocity y", velocity.getY()); - double distance = MathUtil.clamp(position.getNorm(), ArmSetpoints.home.getNorm(), ArmConstants.baseStageLength + ArmConstants.secondStageLength); - double currentMaxV = 30; - - double BaseAngleArmDiff = Math.acos(((distance * distance) - + (ArmConstants.baseStageLength * ArmConstants.baseStageLength) - - (ArmConstants.secondStageLength * ArmConstants.secondStageLength)) - / (2 * distance * ArmConstants.baseStageLength)); - double SecondAngleArmDiff = Math.acos(((distance * distance) - - (ArmConstants.baseStageLength * ArmConstants.baseStageLength) - + (ArmConstants.secondStageLength * ArmConstants.secondStageLength)) - / (2 * distance * ArmConstants.secondStageLength)); - double shoulderPosition = position.getAngle().getRadians() + (BaseAngleArmDiff * (inBend ? 1 : -1)); - double elbowPosition = position.getAngle().getRadians() + (SecondAngleArmDiff * (inBend ? -1 : 1)); - double shoulderVelocity = (shoulderPosition - getShoulderPosition())/ArmVelocityGains.linearApproximationTime; - double elbowVelocity =(elbowPosition - getElbowPosition())/ArmVelocityGains.linearApproximationTime; - // SmartDashboard.putNumber("shoulder target velocity", shoulderVelocity); - // SmartDashboard.putNumber("elbow target velocity", elbowVelocity); - // SmartDashboard.putNumber("target pos x", position.getX()); - // SmartDashboard.putNumber("target pos y", position.getY()); - currentMaxV = Math.max(Math.abs(shoulderVelocity), Math.abs(elbowVelocity)); - // SmartDashboard.putNumber("current max v", currentMaxV); - // if(currentMaxV > ArmConstants.maxMotorVelocity){ - // shoulderVelocity = shoulderVelocity * (ArmConstants.maxMotorVelocity/currentMaxV); - // elbowVelocity = elbowVelocity * (ArmConstants.maxMotorVelocity/currentMaxV); - // } - // if(shoulderPosition < ArmConstants.shoulderOffset *(2*Math.PI)){ - // // SmartDashboard.putBoolean("shoulder soft limit triggered", true); - - // setShoulderVelocity(0); - // shoulderLimiter.reset(0); - // } else { - // SmartDashboard.putBoolean("shoulder soft limit triggered", false); - setShoulderVelocity(shoulderLimiter.calculate(shoulderVelocity)); - // } - // if(Math.abs(elbowPosition - shoulderPosition) > Math.abs(ArmConstants.elbowOffset - ArmConstants.shoulderOffset) * (2*Math.PI)){ - // SmartDashboard.putBoolean("elbow soft limit triggered", true); - // setElbowVelocity((shoulderVelocity)); - // elbowLimiter.reset(shoulderVelocity); - // } else { - // SmartDashboard.putBoolean("elbow soft limit triggered", false); - setElbowVelocity(elbowLimiter.calculate(elbowVelocity)); - // } - } - - /** - * Request the elbow PIDs to target a position. - * - * @param position position to target in radians. - */ - public void setElbowPosition(double position) { - // position -= getShoulderLeftPosition() * (1.0 - ArmConstants.virtual4BarGearRatio); - position /= (2d * Math.PI); - - // SmartDashboard.putNumber("elbow position set raw", position); - // if ((Math.abs(position - elbowLeft.getPosition().getValueAsDouble()) + Math.abs(position - - // elbowRight.getPosition().getValueAsDouble())) < 0.14 && (Math.abs(position - - // (ArmConstants.elbowOffset/Math.PI/2.0)) < 0.01)) { - // elbowLeft.setControl(new DutyCycleOut(0)); - // elbowRight.setControl(new DutyCycleOut(0)); - // } else { - elbowLeft.setControl( - new PositionVoltage(position).withPosition(position).withSlot(0)); - elbowRight.setControl( - new PositionVoltage(position).withPosition(position).withSlot(0)); - // } - // SmartDashboard.putNumber("elbow Left Position error", position - elbowLeft.getPosition().getValueAsDouble()); - // SmartDashboard.putNumber("elbow Right Position error", position - elbowRight.getPosition().getValueAsDouble()); - } - - /** - * Request the wrist to stop moving. - */ - public void stopWrist() { - wristStopped = true; - } - - /** - * Get the wrist position relative to the wrist joint. - * This takes into account the belting ratio between the wrist and elbow. - * - * @return the wrist position in radians. - */ - public double getWristPosition(){ - double wristPosition = (wrist.getPosition().getValueAsDouble() * (2d * Math.PI)); - // TODO: Is this subtraction 'safe'? Multiplication can be wrong for - // unrestricted angular domains. - wristPosition -= getElbowPosition() * (ArmConstants.wristToElbowRatio - 1.0); - // SmartDashboard.putNumber("wrist flip position", wristPosition); - return wristPosition; - } - - /** - * Request the wrist to move to a certain position. - * This method enables wrist movement if it was requested - * to stop previously. - * - * @param position the wrist position in radians. - */ - public void setWristTarget(double position) { - wristTarget = position; - wristStopped = false; - // wristFinishedMoving = false; - // ratio = oo: wrist change amount -oo - // ratio = 1: wrist change amount 0 - // ratio = 0: wrist change amount 1 - // SmartDashboard.putNumber("wrist flip position set raw", position); - } - - /** - * Update wrist to requested movement. - * If the wrist was requested to stop, its motor will be stopped. - * Otherwise, the wrist will be set to the last requested target position. - */ - public void updateWristSetpoints() { - if (wristStopped) { - wrist.stopMotor(); - } else { - double flipPosition = wristTarget; - // TODO: Same question in getWristTarget() - flipPosition += getElbowPosition() * (ArmConstants.wristToElbowRatio - 1.0); - flipPosition /= (2d*Math.PI); - wrist.setControl(new MotionMagicVoltage(flipPosition).withPosition(flipPosition).withSlot(0)); - } - } - - /** - * Get elbow position. - * - * @return Averaged elbow position in radians. - */ - public double getElbowPosition() { - double elbowPose = (elbowLeft.getPosition().getValueAsDouble() + elbowRight.getPosition().getValueAsDouble())/2 * (2d * Math.PI); - // SmartDashboard.putNumber("elbow position", elbowPose); - // SmartDashboard.putNumber("elbow adjustment factor", shoulderLeft.getPosition()*24.0/42.0); - // SmartDashboard.putNumber("elbow to shoulder", elbowPose - shoulderLeft.getPosition()); - return elbowPose; - // + ((ArmConstants.virtual4BarGearRatio - 1) * (getShoulderPosition() - ArmConstants.shoulderOffset)); - } - - public Command goToHome() { - return new ArmInstantCommand(this, () -> 0); - } - public Command goToHotel() { - return new ArmInstantCommand(this, () -> 13); - } - - /** - * Set the speed of the shoulder. - * - * @param power the speed of the motors from -1 to +1. - */ - public void setShoulderPower(double power) { - shoulderLeft.set(-power); - shoulderRight.set(power); - } - - /** - * Set the amp limit of the shoulder motors. - * - * @param amplimit the amp limit in Amps. - */ - public void setShoulderAmpLimit(double amplimit) { - shoulderConfig.CurrentLimits = new CurrentLimitsConfigs() - .withStatorCurrentLimit(amplimit); - shoulderLeft.getConfigurator().apply(shoulderConfig); - shoulderRight.getConfigurator().apply(shoulderConfig); - } - - - /** - * Request the shoulder PIDs to target a position. - * - * @param position position to target in radians. - */ - public void setShoulderPosition(double position) { - position /= (2d * Math.PI); - - //position = MathUtil.clamp(position, -0.1, 2.5); - - // SmartDashboard.putNumber("shoulder position set raw", position); - // if ((Math.abs(position - shoulderLeft.getPosition().getValueAsDouble()) - // + Math.abs( - // position - shoulderRight.getPosition().getValueAsDouble())) - // < 0.1 - // && Math.abs(position - (ArmConstants.shoulderOffset / Math.PI / 2.0)) < 0.01) { - // shoulderLeft.setControl(new DutyCycleOut(0)); - // shoulderRight.setControl(new DutyCycleOut(0)); - // } else { - shoulderLeft.setControl( - new PositionVoltage(position).withPosition(position).withSlot(0)); - shoulderRight.setControl( - new PositionVoltage(position).withPosition(position).withSlot(0)); - // SmartDashboard.putNumber("shoulder target pos", position); - // SmartDashboard.putNumber("shoulder Left Position error", position - shoulderLeft.getPosition().getValueAsDouble()); - // SmartDashboard.putNumber("shoulder Right Position error", position - shoulderRight.getPosition().getValueAsDouble()); - //} - } - - /** - * Get shoulder position. - * - * @return averaged shoulder position in radians. - */ - public double getShoulderPosition() { - double position = (shoulderLeft.getPosition().getValueAsDouble() + shoulderRight.getPosition().getValueAsDouble())/2 * (2d * Math.PI); - // SmartDashboard.putNumber("shoulder position", position); - - return position; - } - - /** - * Request the shoulder PIDs to target a velocity. - * - * @param velocity velocity to target in rotations per second. - */ - public void setShoulderVelocity(double velocity) { - shoulderLeft.setControl(new VelocityVoltage(velocity).withSlot(1)); - shoulderRight.setControl(new VelocityVoltage(velocity).withSlot(1)); - // SmartDashboard.putNumber("shoulder target velocity limited", velocity); - } - - /** - * Request the elbow PIDs to target a velocity. - * - * @param velocity velocity to target in rotations per second. - */ - public void setElbowVelocity(double velocity) { - elbowLeft.setControl(new VelocityVoltage(velocity).withSlot(1)); - elbowRight.setControl(new VelocityVoltage(velocity).withSlot(1)); - // SmartDashboard.putNumber("elbow target velocity limited", velocity); - } - - /** - * Request the wrist PID to target a velocity. - * - * @param velocity velocity to target in rotations per second. - */ - public void setWristVelocity(double velocity) { - wrist.setControl(new VelocityVoltage(velocity)); - } - - /** - * Get whether the arm is currently bent inwards. - * - * @return true if bent inwards. - * false = bend convex side facing negative rotation direction - * true = bend convex side facing positive rotation direction - */ - public boolean getCurrentInBend() { - return getElbowPosition() - getShoulderPosition() < 0; - } - - /** - * Stop the shoulder and elbow motors. - */ - public void stopArm() { - elbowLeft.stopMotor(); - elbowRight.stopMotor(); - shoulderLeft.stopMotor(); - shoulderRight.stopMotor(); - } - - /** - * Set whether the arm's default state is the stow position. - * - * @param Stowing set true if the arm is stowing. - */ - public void setStowing(boolean Stowing) { - stowing = Stowing; - } - - /** - * Get whether the wrist has reached near its target position. - * - * @return true if the wrist is near its target position. - */ - // public boolean wristFinishedMoving() { - // TODO: This doesn't work for unrestricted angular domains. - // return Math.abs(getWristPosition() - wristTarget) < 0.2; - // } - - @Override - public void periodic() { - updateWristSetpoints(); - // getShoulderPosition(); - // getElbowPosition(); - - // SignalLogger.writeDouble("shoulder Left amps", shoulderLeft.getStatorCurrent().getValueAsDouble()); - // SignalLogger.writeDouble("shoulder right amps", shoulderRight.getStatorCurrent().getValueAsDouble()); - - // SmartDashboard.putNumber("shoulder Left amps", shoulderLeft.getStatorCurrent().getValueAsDouble()); - // SmartDashboard.putNumber("shoulder right amps", shoulderRight.getStatorCurrent().getValueAsDouble()); - // getWristPosition(); - // getWristTwistPosition(); - // SmartDashboard.putNumber("wrist flip output", wrist.getClosedLoopOutput().getValueAsDouble()); - // SmartDashboard.putNumber("wrist flip amp", wrist.getStatorCurrent().getValueAsDouble()); - // SmartDashboard.putNumber("wrist Twist output", wristTwist.getClosedLoopOutput().getValueAsDouble()); - // SmartDashboard.putNumber("wrist Twist amp", wristTwist.getStatorCurrent().getValueAsDouble()); - // SmartDashboard.putNumber("wrist twist pos", getWristTwistPosition()); - // SmartDashboard.putNumber("wrist flip pos", getWristPosition()); - SignalLogger.writeDouble("shoulder error", shoulderTarget - getShoulderPosition()); - SignalLogger.writeDouble("elbow error", elbowTarget - getElbowPosition()); - SignalLogger.writeDouble("wrist error", wristTarget - getWristPosition()); - SmartDashboard.putNumber("arm pose x", getArmPosition().getX()); - SmartDashboard.putNumber("arm pose y", getArmPosition().getY()); - SignalLogger.writeDouble("elbow pos", getElbowPosition()); - SignalLogger.writeDouble("shoulder pos", getShoulderPosition()); - SignalLogger.writeDouble("wrist flip pos", getWristPosition()); - SmartDashboard.putNumber("elbow pos", getElbowPosition()); - SmartDashboard.putNumber("shoulder pos", getShoulderPosition()); - SmartDashboard.putNumber("wrist flip pos", getWristPosition()); - // SmartDashboard.putNumber("shoulder velocity", getShoulderVelocity()); - // SmartDashboard.putNumber("elbow velocity", getElbowVelocity()); - // SmartDashboard.putNumber("left elbow amp", elbowLeft.getDutyCycle().getValueAsDouble()); - SignalLogger.writeDouble("right shoulder amp", shoulderRight.getStatorCurrent().getValueAsDouble()); - SignalLogger.writeDouble("left shoulder amp", shoulderLeft.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("right shoulder amp", shoulderRight.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("left shoulder amp", shoulderLeft.getStatorCurrent().getValueAsDouble()); - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/Bucket.java b/src/main/java/frc/robot/subsystems/Bucket.java deleted file mode 100644 index 01f1126..0000000 --- a/src/main/java/frc/robot/subsystems/Bucket.java +++ /dev/null @@ -1,106 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.Meters; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.configs.CANrangeConfiguration; -import com.ctre.phoenix6.configs.FovParamsConfigs; -import com.ctre.phoenix6.configs.ProximityParamsConfigs; -import com.ctre.phoenix6.hardware.CANrange; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotContainer; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.BucketConstants; - -public class Bucket extends SubsystemBase { - /** Creates a new Bucket. */ - private double[] prevDistancesLeft = new double[BucketConstants.timesForBucketToTestPositive]; - private double[] prevDistancesRight = new double[BucketConstants.timesForBucketToTestPositive]; - private int i = 0; - - public static boolean gripperHasGamePiece = false; - private CANrange sensorLeft; - private CANrange sensorRight; - private CANrangeConfiguration sensorConfig; - - public Bucket() { - sensorLeft = new CANrange(BucketConstants.leftSensorPort, ArmConstants.armCanBus); - sensorRight = new CANrange(BucketConstants.rightSensorPort, ArmConstants.armCanBus); - sensorConfig = new CANrangeConfiguration() - .withFovParams(new FovParamsConfigs().withFOVRangeX(27).withFOVRangeY(27)) - .withProximityParams(new ProximityParamsConfigs().withProximityThreshold(Meters.of(BucketConstants.coralDistance)).withMinSignalStrengthForValidMeasurement(2500)); - - sensorLeft.getConfigurator().apply(sensorConfig); - sensorRight.getConfigurator().apply(sensorConfig); - } - - @Override - public void periodic() { - refreshSensors(); - SignalLogger.writeBoolean("bucket detected", getDetected()); - SmartDashboard.putBoolean("bucket detected", getDetected()); - SmartDashboard.putNumber("bucket left sensor dist", sensorLeft.getDistance().getValueAsDouble()); - SmartDashboard.putNumber("bucket right sensor dist", sensorRight.getDistance().getValueAsDouble()); - SmartDashboard.putNumberArray("prevdistancesleft",prevDistancesLeft); - SmartDashboard.putBoolean("leftdetectedarray tripped", getLeftCoralDetected()); - SmartDashboard.putBoolean("left considered detected", sensorLeft.getIsDetected().getValue()); - // This method will be called once per scheduler run - } - public void refreshSensors() { - i++; - if (i >= BucketConstants.timesForBucketToTestPositive) { - i = 0; - } - - prevDistancesLeft[i] = sensorLeft.getIsDetected().getValue() ? sensorLeft.getDistance().getValueAsDouble() : 2.0; - - prevDistancesRight[i] = sensorRight.getIsDetected().getValue() ? sensorRight.getDistance().getValueAsDouble() : 2.0; - - } - public boolean getSensorNotLagging() { - for (int j = 0; j < BucketConstants.timesForBucketToTestPositive - 1; j++){ - double currentArrayLeft = prevDistancesLeft[j]; - double currentArrayRight = prevDistancesRight[j]; - if(((currentArrayLeft > prevDistancesLeft[j+1]) || (currentArrayLeft < prevDistancesLeft[j+1])) - && ((currentArrayRight > prevDistancesRight[j+1]) || (currentArrayRight < prevDistancesRight[j+1]))){ - return true; - } - } - return false; - } - - public boolean getLeftCoralDetected() { - for (double distance : prevDistancesLeft) { - if (distance > BucketConstants.coralDistance) { - return false; - } - } - return true; - } - public boolean getRightCoralDetected() { - for (double distance : prevDistancesRight) { - if (distance > BucketConstants.coralDistance) { - return false; - } - } - return true; - } - public boolean getDetected() { - return (getLeftCoralDetected() || getRightCoralDetected()); - } - public Command disableAutoBucket() { - return new Command() { - @Override - public void initialize() { - gripperHasGamePiece = true; - } - }; - } -} diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java index 0a0e81d..7c7c2d6 100644 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ b/src/main/java/frc/robot/subsystems/Climb.java @@ -26,51 +26,54 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Constants; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmGains; import frc.robot.Constants.ClimbConstants; -import frc.robot.commands.GripperCommand; public class Climb extends SubsystemBase { - private TalonFX climbLeft, climbRight; - private TalonFXConfiguration climbConfig = new TalonFXConfiguration(); - private boolean ampTriggered, ampTriggerStarted = false; + private TalonFX winch; + private TalonFXConfiguration winchConfig = new TalonFXConfiguration(); // private double startTime, toAmpTriggerStartTime = 0; /** Creates a new Gripper. */ public Climb() { - climbLeft = new TalonFX(ClimbConstants.kLeftID, ClimbConstants.canBus); - climbRight = new TalonFX(ClimbConstants.kRightID, ClimbConstants.canBus); - climbConfig.CurrentLimits = new CurrentLimitsConfigs() + winch = new TalonFX(ClimbConstants.winchId, ClimbConstants.canBus); + winchConfig.CurrentLimits = new CurrentLimitsConfigs() .withStatorCurrentLimit(ClimbConstants.currentLimit) .withStatorCurrentLimitEnable(true); - climbConfig.Feedback = new FeedbackConfigs() + winchConfig.Feedback = new FeedbackConfigs() .withFeedbackRotorOffset(0) .withSensorToMechanismRatio(1); - climbConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(ClimbConstants.rampRate); - climbConfig.Slot0 = new Slot0Configs() + winchConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(ClimbConstants.rampRate); + winchConfig.Slot0 = new Slot0Configs() .withKP(ClimbConstants.kP) .withKI(ClimbConstants.kI) .withKD(ClimbConstants.kD); - climbLeft + winch .getConfigurator() - .apply(climbConfig.withMotorOutput(new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake))); - climbRight - .getConfigurator() - .apply(climbConfig.withMotorOutput(new MotorOutputConfigs() + .apply(winchConfig.withMotorOutput(new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake))); + resetPosition(); } - + public Command extend() { + return new InstantCommand(() -> setPosition(ClimbConstants.deployPosition)); + } + public Command contract() { + return new SequentialCommandGroup( + new InstantCommand(() -> setPower(ClimbConstants.power)), + new WaitUntilCommand(() -> getPosition() > ClimbConstants.climbedPosition), + stopCommand()); + } + public Command returnToHome() { + return new InstantCommand(() -> setPosition(ClimbConstants.homePosition)); + } public void setPower(double power) { - climbLeft.set(power); - climbRight.set(power); + winch.set(power); } // public void setClimb() { // if (ampTriggered) { @@ -83,45 +86,40 @@ public void setPower(double power) { // } // } public void stop() { - climbLeft.stopMotor(); - climbRight.stopMotor(); + winch.stopMotor(); } public double getCurrent() { - return (climbLeft.getStatorCurrent().getValueAsDouble() + climbRight.getStatorCurrent().getValueAsDouble())/2; + return (winch.getStatorCurrent().getValueAsDouble() ); } public void resetPosition() { - climbLeft.setPosition(0); - climbRight.setPosition(0); + winch.setPosition(0); } public void setPosition(double position) { - climbLeft.setControl(new PositionVoltage(position)); - climbRight.setControl(new PositionVoltage(position)); + winch.setControl(new PositionVoltage(position)); } public double getPosition() { - return (climbLeft.getPosition().getValueAsDouble() + climbRight.getPosition().getValueAsDouble())/2; + return (winch.getPosition().getValueAsDouble()); } public void setCurrentLimit(double currentLimit) { - climbConfig.CurrentLimits = new CurrentLimitsConfigs() + winchConfig.CurrentLimits = new CurrentLimitsConfigs() .withStatorCurrentLimit(currentLimit) .withStatorCurrentLimitEnable(true); - climbLeft.getConfigurator().apply(climbConfig); - climbRight.getConfigurator().apply(climbConfig); + winch.getConfigurator().apply(winchConfig); } public boolean climbed() { return false;//Math.abs(getPosition()) < Math.abs(ClimbConstants.climbedPosition); } public double getCurrentLimit() { - return climbConfig.CurrentLimits.StatorCurrentLimit; + return winchConfig.CurrentLimits.StatorCurrentLimit; } public Command climb() { return new InstantCommand(() -> setPower(ClimbConstants.power));//setFOC(ClimbConstants.currentLimit)); } public void setFOC(double current){ - climbLeft.setControl(new TorqueCurrentFOC(current)); - climbRight.setControl(new TorqueCurrentFOC(current)); + winch.setControl(new TorqueCurrentFOC(current)); } // public Command climbSwitchToFOC() { // return new Command() { @@ -164,13 +162,8 @@ public Command stopCommand() { @Override public void periodic() { - SmartDashboard.putNumber("climb pos", getPosition()); - SmartDashboard.putNumber("climb right pos", climbRight.getPosition().getValueAsDouble()); - SmartDashboard.putNumber("climb left pos", climbLeft.getPosition().getValueAsDouble()); - SignalLogger.writeDouble("climb left amp", climbLeft.getStatorCurrent().getValueAsDouble()); - SignalLogger.writeDouble("climb right amp", climbRight.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("climb left amp", climbLeft.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("climb right amp", climbRight.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("winch pos", getPosition()); + SmartDashboard.putNumber("winch amp", winch.getStatorCurrent().getValueAsDouble()); } diff --git a/src/main/java/frc/robot/subsystems/CoralManipulator.java b/src/main/java/frc/robot/subsystems/CoralManipulator.java new file mode 100644 index 0000000..f5d8ae3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CoralManipulator.java @@ -0,0 +1,305 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.configs.CANrangeConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.FovParamsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; +import com.ctre.phoenix6.configs.ProximityParamsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ControlModeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.LEDPattern.GradientType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.Constants.ClimbConstants; +import frc.robot.Constants.CoralConstants; +import frc.robot.Constants.CoralConstants.coralState; + +public class CoralManipulator extends SubsystemBase { + private CoralConstants.coralState coralState = CoralConstants.coralState.empty; + private CoralConstants.elevatorLevel elevatorLevel = CoralConstants.elevatorLevel.home; + private TalonFX elevatorLeft, elevatorRight, shooter, indexer, deploy, intake; + private CANrange intakeSensor, indexerSensor; + private TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); + private TalonFXConfiguration elevatorConfig = new TalonFXConfiguration(); + private TalonFXConfiguration deployConfig = new TalonFXConfiguration(); + private TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); + private TalonFXConfiguration indexerConfig = new TalonFXConfiguration(); + private double targetPositionElevator, targetPositionDeploy = 0; + private CANrangeConfiguration intakeSensorConfig = new CANrangeConfiguration(); + private CANrangeConfiguration indexerSensorConfig = new CANrangeConfiguration(); + + /** Creates a new ElevIndexer. */ + public CoralManipulator() { + shooter = new TalonFX(CoralConstants.shooterID, CoralConstants.canBus); + indexer = new TalonFX(CoralConstants.indexerID, CoralConstants.canBus); + elevatorLeft = new TalonFX(CoralConstants.elevatorLeftID, CoralConstants.canBus); + elevatorRight = new TalonFX(CoralConstants.elevatorRightID, CoralConstants.canBus); + deploy = new TalonFX(CoralConstants.deployID, CoralConstants.canBus); + intake = new TalonFX(CoralConstants.intakeID, CoralConstants.canBus); + intakeSensor = new CANrange(CoralConstants.intakeSensorID, CoralConstants.canBus); + indexerSensor = new CANrange(CoralConstants.indexerSensorID, CoralConstants.canBus); + intakeSensorConfig = new CANrangeConfiguration() + .withFovParams(new FovParamsConfigs().withFOVRangeX(27).withFOVRangeY(27)) + .withProximityParams(new ProximityParamsConfigs().withProximityThreshold(CoralConstants.intakeSensorTriggerDistance).withMinSignalStrengthForValidMeasurement(2500)); + intakeSensor.getConfigurator().apply(intakeSensorConfig); + indexerSensorConfig = new CANrangeConfiguration() + .withFovParams(new FovParamsConfigs().withFOVRangeX(7).withFOVRangeY(7)) + .withProximityParams(new ProximityParamsConfigs().withProximityThreshold(CoralConstants.indexerSensorTriggerDistance).withMinSignalStrengthForValidMeasurement(5000)); + indexerSensor.getConfigurator().apply(indexerSensorConfig); + configureDeploy(); + configureIntake(); + configureShooter(); + configureElevator(); + configureIndexer(); + resetMotors(); + } + private void configureIndexer() { + indexerConfig.MotorOutput = new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive).withNeutralMode(NeutralModeValue.Brake); + indexerConfig.OpenLoopRamps = new OpenLoopRampsConfigs().withVoltageOpenLoopRampPeriod(CoralConstants.indexerCurrentLimit); + indexer.getConfigurator().apply(indexerConfig); + } + private void configureIntake() { + intakeConfig.CurrentLimits = new CurrentLimitsConfigs().withStatorCurrentLimit(CoralConstants.intakeCurrentLimit).withStatorCurrentLimitEnable(true); + intakeConfig.OpenLoopRamps = new OpenLoopRampsConfigs().withVoltageOpenLoopRampPeriod(0.05); + intakeConfig.MotorOutput = new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive); + intake.getConfigurator().apply(intakeConfig); + } + private void configureShooter() { + shooterConfig.CurrentLimits = new CurrentLimitsConfigs() + .withStatorCurrentLimit(CoralConstants.shooterCurrentLimit) + .withStatorCurrentLimitEnable(true); + shooterConfig.OpenLoopRamps = new OpenLoopRampsConfigs().withVoltageOpenLoopRampPeriod(0.05); + shooter + .getConfigurator() + .apply(shooterConfig.withMotorOutput(new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake))); + } + private void configureDeploy() { + deployConfig.CurrentLimits = new CurrentLimitsConfigs() + .withStatorCurrentLimit(CoralConstants.deployCurrentLimit) + .withStatorCurrentLimitEnable(true); + deployConfig.Feedback = new FeedbackConfigs().withSensorToMechanismRatio(CoralConstants.deploySensorRatio); + deployConfig.Slot0 = new Slot0Configs() + .withKP(CoralConstants.kPDeploy) + .withKI(CoralConstants.kIDeploy) + .withKD(CoralConstants.kDDeploy) + .withKG(CoralConstants.kGDeploy) + .withGravityType(GravityTypeValue.Arm_Cosine); + deployConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(CoralConstants.deployRampRate); + deployConfig.SoftwareLimitSwitch = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(CoralConstants.forwardLimitDeploy) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(CoralConstants.reverseLimitDeploy); + deployConfig.MotorOutput = new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive).withNeutralMode(NeutralModeValue.Coast); + deploy.getConfigurator().apply(deployConfig); + } + private void configureElevator() { + elevatorConfig.CurrentLimits = new CurrentLimitsConfigs() + .withStatorCurrentLimit(CoralConstants.elevatorCurrentLimit) + .withStatorCurrentLimitEnable(true); + elevatorConfig.Feedback = new FeedbackConfigs().withSensorToMechanismRatio(CoralConstants.elevatorSensorRatio); + elevatorConfig.Slot0 = new Slot0Configs() + .withKP(CoralConstants.kP) + .withKI(CoralConstants.kI) + .withKD(CoralConstants.kD) + .withKG(CoralConstants.kG) + .withKS(CoralConstants.kS) + .withGravityType(GravityTypeValue.Elevator_Static); + elevatorConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(CoralConstants.elevatorRampRate); + elevatorConfig.SoftwareLimitSwitch = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(CoralConstants.forwardLimit) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(CoralConstants.reverseLimit); + elevatorLeft + .getConfigurator() + .apply(elevatorConfig.withMotorOutput(new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake))); + elevatorRight + .getConfigurator() + .apply(elevatorConfig.withMotorOutput(new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake))); + } + + private void resetMotors() { + elevatorLeft.setPosition(0); + elevatorRight.setPosition(0); + deploy.setPosition(CoralConstants.deployOffset); + } + public Command resetDeploy() { + return new InstantCommand(() -> deploy.setPosition(CoralConstants.deployOffset)); + } + public Command resetElevatorLeft() { + return new InstantCommand(() -> elevatorLeft.setPosition(0)); + } + public Command resetElevatorRight() { + return new InstantCommand(() -> elevatorRight.setPosition(0)); + } + public boolean deployAmpTriggered() { + return Math.abs(deploy.getStatorCurrent().getValueAsDouble()) > 15; + } + public coralState getCoralState() { + return coralState; + } + public Command stopDeploy() { + return new InstantCommand(() -> deploy.stopMotor()); + } + public Command stopElevatorLeft() { + return new InstantCommand(() -> elevatorLeft.stopMotor()); + } + public Command stopElevatorRight() { + return new InstantCommand(() -> elevatorRight.stopMotor()); + } + public Command setDeployVoltage(double voltage) { + return new InstantCommand(() -> deploy.setVoltage(voltage)); + } + public boolean getIntakeSensor() { + return intakeSensor.getIsDetected().getValue(); + } + public boolean getIndexerSensor() { + return indexerSensor.getIsDetected().getValue(); + } + public void setCoralState(coralState coralState) { + this.coralState = coralState; + } + public Command setCoralStateCommand(coralState coralState) { + return new InstantCommand(() -> setCoralState(coralState)); + } + public Command setIndexerVoltage(double voltage){ + return new InstantCommand(() -> indexer.setControl(new VoltageOut(voltage))); + } + public Command setIntakeVoltage(double voltage){ + return new InstantCommand(() -> intake.setControl(new VoltageOut(voltage))); + } + public Command stopIntake() { + return new InstantCommand(() -> intake.stopMotor()); + } + public Command stopIndexer() { + return new InstantCommand(() -> indexer.stopMotor()); + } + private void setDeployPosition(double position) { + deploy.setControl(new PositionVoltage(position)); + targetPositionDeploy = position; + } + public Command deployIntake() { + return new InstantCommand(() -> setDeployPosition(CoralConstants.deployPositionIntake)); + } + public Command transferIntake() { + return new InstantCommand(() -> setDeployPosition(CoralConstants.transferPositionIntake)); + } + public Command retractIntake() { + return new InstantCommand(() ->setDeployPosition(CoralConstants.homePositionIntake)); + } + + + public void setShooter(double voltage) { + shooter.setControl(new VoltageOut(voltage)); + } + public void setElevatorControl(ControlRequest request) { + elevatorLeft.setControl(request); + elevatorRight.setControl(request); + } + public Command setElevatorLeftVoltage(double voltage) { + return new InstantCommand(() -> elevatorLeft.setVoltage(voltage)); + } + public Command setElevatorRightVoltage(double voltage) { + return new InstantCommand(() -> elevatorRight.setVoltage(voltage)); + } + + public boolean elevatorLeftAmpTriggered() { + return Math.abs(elevatorLeft.getStatorCurrent().getValueAsDouble()) > 5; + } + public boolean elevatorRightAmpTriggered() { + return Math.abs(elevatorRight.getStatorCurrent().getValueAsDouble()) > 5; + } + public void setElevPosition(double position) { + targetPositionElevator = position; + setElevatorControl(new PositionVoltage(position).withSlot(0)); + } + public boolean elevatorAtTarget() { + return Math.abs((elevatorLeft.getPosition().getValueAsDouble() + elevatorRight.getPosition().getValueAsDouble())/2 - targetPositionElevator) < CoralConstants.elevatorTolerance; + } + public boolean deployAtTarget() { + return Math.abs(deploy.getPosition().getValueAsDouble() - targetPositionDeploy) < CoralConstants.deployTolerance; + } + public Command setElevatorPosition(double position) { + return new InstantCommand(() -> setElevPosition(position)); + } + public Command elevatorToHome() { + return new SequentialCommandGroup( + setElevatorPosition(CoralConstants.homePos), + stopShooter(), + new WaitCommand(0.5), + stopElevator()); + } + + public Command shoot(double voltage) { + return new InstantCommand(() -> + shooter.setControl(new VoltageOut(voltage)) + ); + } + public Command stopShooter() { + return new InstantCommand(() -> shooter.stopMotor()); + } + public Command stopElevator() { + return new InstantCommand(() -> elevatorLeft.stopMotor()).alongWith(new InstantCommand(() -> elevatorRight.stopMotor())); + } + public Command intakeCommand() { + return deployIntake().alongWith(setIntakeVoltage(CoralConstants.intakingVoltage)); + } + public Command intakeToHome() { + return retractIntake().alongWith(stopIntake()); + } + + @Override + public void periodic() { + if (getIndexerSensor()) { + setCoralState(coralState.coralInIndexer); + } else if (getIntakeSensor()) { + setCoralState(coralState.coralInIntake); + } + SmartDashboard.putNumber("elev left pos", elevatorLeft.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("elev right pos", elevatorRight.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("elevator left amp", elevatorLeft.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("elevator right amp", elevatorRight.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("shooter pos", shooter.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("shooter amp", shooter.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("deploy pose", deploy.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("deploy error", targetPositionDeploy - deploy.getPosition().getValueAsDouble()); + SmartDashboard.putString("coralState", getCoralState().name()); + SmartDashboard.putBoolean("intake detected", getIntakeSensor()); + SmartDashboard.putNumber("deploy amp", deploy.getStatorCurrent().getValueAsDouble()); + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java deleted file mode 100644 index 080fc85..0000000 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ /dev/null @@ -1,112 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.ctre.phoenix6.configs.CANrangeConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.ProximityParamsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANrange; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmGains; -import frc.robot.commands.GripperCommand; - -public class Gripper extends SubsystemBase { - private TalonFX gripper; - private TalonFXConfiguration gripperConfig = new TalonFXConfiguration(); - /** Creates a new Gripper. */ - public Gripper() { - gripper = new TalonFX(ArmConstants.gripperMotorPort, ArmConstants.armCanBus); - gripperConfig.CurrentLimits = new CurrentLimitsConfigs() - .withStatorCurrentLimit(ArmConstants.gripperCurrentLimitDefault) - .withStatorCurrentLimitEnable(true); - gripperConfig.Feedback = new FeedbackConfigs() - .withFeedbackRotorOffset(ArmConstants.gripperOffset) - .withSensorToMechanismRatio(ArmConstants.gripperRadPerRot); - gripperConfig.ClosedLoopRamps = new ClosedLoopRampsConfigs().withVoltageClosedLoopRampPeriod(0.01); - gripperConfig.Slot0 = new Slot0Configs() - .withKP(ArmGains.gripperP) - .withKI(ArmGains.gripperI) - .withKD(ArmGains.gripperD); - gripperConfig.MotorOutput = new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast); - gripper - .getConfigurator() - .apply(gripperConfig); - } - - public void setGripperPower(double power) { - gripper.set(power); - } - public double getGripperVoltage() { - return gripper.getMotorVoltage().getValueAsDouble(); - } - - public void setCurrentLimit(double currentLimit) { - gripperConfig.CurrentLimits = new CurrentLimitsConfigs() - .withStatorCurrentLimit(currentLimit) - .withStatorCurrentLimitEnable(true); - gripper.getConfigurator().apply(gripperConfig); - } - - public double getCurrentLimit() { - return gripperConfig.CurrentLimits.StatorCurrentLimit; - } - - public Command coralDefaultCommand() { - return new GripperCommand(this, 0.3, 10); - } - public Command coralIntakeCommand() { - return new GripperCommand(this, 1.0, 20); - } - public Command algaeIntakeCommand() { - return new GripperCommand(this, 0.6, 30); - } - public Command algaeDefaultCommand() { - return new GripperCommand(this, 0.5, 15); - } - public Command spitOutCommand() { - return new GripperCommand(this, -1.0, 20); - } - public Command algaeSpitCommand() { - return new GripperCommand(this, -1.0, 40); - } - public Command neutralCommand() { - return new GripperCommand(this, 0.0, 20); - } - - - - // public boolean getAlgaeDetected() { - // for (double distance : prevDistances) { - // if (distance > ArmConstants.algaeDistance) { - // return false; - // } - // } - // return false; - // } - - @Override - public void periodic() { - - - SmartDashboard.putNumber("Gripper velocity", gripper.getVelocity().getValueAsDouble()); // angular velocity (rotations per second) - - } -} diff --git a/src/main/java/frc/robot/subsystems/LEDSubsytem.java b/src/main/java/frc/robot/subsystems/LEDSubsytem.java index 7fd8d33..0f51786 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsytem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsytem.java @@ -1,19 +1,16 @@ // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems; - import static edu.wpi.first.units.Units.Percent; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Seconds; - import java.util.Map; import java.util.function.BooleanSupplier; import java.util.function.Supplier; - import com.ctre.phoenix6.signals.Led1OffColorValue; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; @@ -24,62 +21,94 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; - +import frc.robot.Constants.LEDConstants; public class LEDSubsytem extends SubsystemBase { private static final int kPort = Constants.LEDConstants.kPort; private static final int kLength = Constants.LEDConstants.kLength; private Supplier currentPattern = () -> LEDPattern.solid(new Color(1.0f, 1.0f ,1.0f)); - private final AddressableLED m_led; private final AddressableLEDBuffer m_buffer; - - public LEDSubsytem() { m_led = new AddressableLED(kPort); m_buffer = new AddressableLEDBuffer(kLength); m_led.setLength(kLength); m_led.start(); } - @Override public void periodic() { // Periodically send the latest LED color data to the LED strip for it to display - - m_led.setData(m_buffer); } - - public Color getPattern(BooleanSupplier driveTrainFinishedMoving, BooleanSupplier bucketHasCoral, BooleanSupplier gripperHasGamePiece){ - boolean hasGamePiece = bucketHasCoral.getAsBoolean() || gripperHasGamePiece.getAsBoolean(); - Color color = (hasGamePiece) ? new Color(1.0f, 1.0f, 1.0f) : new Color(1.0f, 1.0f, 1.0f); // GRB - boolean flashing = hasGamePiece ? driveTrainFinishedMoving.getAsBoolean() : false; - // if (flashing) { - // return LEDPattern.solid(color).blink(Time.ofBaseUnits(0.05, Second)); + public Color getColor(BooleanSupplier bucketHasCoral, Supplier autoDrive, Supplier distanceToReef){ + // boolean hasGamePiece = bucketHasCoral.get() || gripperHasGamePiece.get(); + Color color = new Color(); + // boolean flashing = hasGamePiece ? driveTrainFinishedMoving.get() : false; + // if (l1.get() == true && l2.get() == false && l3.get() == false && l4.get() == false) { + // color = new Color(0.0f, 1.0f, 0.0f); // red + // } else if (l1.get() == false && l2.get() == true && l3.get() == false && l4.get() == false) { + // color = new Color(0.0f, 1.0f, 1.0f); // magenta + // } else if (l1.get() == false && l2.get() == false && l3.get() == true && l4.get() == false) { + // color = new Color(0.0f, 0.0f, 1.0f); // blue + // } else if (l1.get() == false && l2.get() == false && l3.get() == false && l4.get() == true) { + // color = new Color(1.0f, 0.0f, 0.0f); // green + // } + // switch (bucketHasCoral.get()) { + // case 1: + // color = new Color(0.0f, 1.0f, 0.0f); // red + // break; + // case 2: + // color = new Color(0.0f, 1.0f, 1.0f); // magenta + // break; + // case 3: + // color = new Color(0.0f, 0.0f, 1.0f); // blue + // break; + // case 4: + // color = new Color(1.0f, 0.0f, 0.0f); // green + // break; + // default: + // color = new Color(1.0f, 1.0f, 1.0f); // default color: this case will never happen + // } + if (!autoDrive.get()) { + if (bucketHasCoral.getAsBoolean()) { + color = new Color(1.0f, 0.0f, 0.0f); // green + } else { + color = new Color(0.0f, 1.0f, 0.0f); // red + } + + } else { + double colorRatio = MathUtil.clamp(LEDConstants.driveToPoseDistanceMap.get(distanceToReef.get()), 0, 1); + color = new Color(Math.abs(Math.sin(colorRatio*Math.PI*0.5)), Math.abs(Math.cos(colorRatio*Math.PI*0.5)), 0); + // if (distanceToReef.get() >= 1.0) { + // color = new Color(0.0f, 1.0f, 0.0f); // red + // } else if (distanceToReef.get() >= .3) { + // color = new Color(0.0f, 1.0f, 1.0f); // magenta + // } else if (distanceToReef.get() >= .1) { + // color = new Color(0.0f, 0.0f, 1.0f); // blue // } else { - // return LEDPattern.solid(color); + // color = new Color(1.0f, 0.0f, 0.0f); // green // } + } + + // if (driveTrainFinishedMoving.get()) {} return color; + } // public Color[] updateStepColor(Trigger armFinishedMoving, Trigger driveTrainFinishedMoving, Trigger hasCoral) { // Color step1 = new Color(); // Color step2 = new Color(); - // if(hasCoral.getAsBoolean()) { // // step2 = new Color(1.0f, 0.0f, 0.0f); // cyan // } else { // step2 = new Color(0.0f, 1.0f, 0.0f); // red // } - // // if (detectedCoral.get()) { // // // step3 = new Color(1.0f, 0.0f, 0.0f); // green // // } else { // // step3 = new Color(0.0f, 1.0f, 0.0f); // red // // } - // Color[] colors = {step2}; // return colors; // } - /** * Creates a command that runs a pattern on the entire LED strip. * @@ -89,14 +118,4 @@ public Command runPattern(Supplier pattern) { // currentPattern = pattern; return run(() -> pattern.get().applyTo(m_buffer)); } - public Command blink() { - return new InstantCommand(() -> currentPattern.get().blink(Seconds.of(0.05)).applyTo(m_buffer)); - } - public Command breathe() { - return new InstantCommand(() -> currentPattern.get().breathe(Seconds.of(2.0)).applyTo(m_buffer)); - } -} - - - -// distance between two poses -> ratio of gradient joystick.map() +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java b/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java index 46c4213..3b6c554 100644 --- a/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java @@ -3,6 +3,7 @@ import static frc.robot.Constants.Vision.USE_VISION; import static frc.robot.Constants.Vision.kCameraNameBack; import static frc.robot.Constants.Vision.kCameraNameFront; +import static frc.robot.Constants.Vision.kLimeLightHeight; import static frc.robot.Constants.Vision.kRobotToCamBack; import static frc.robot.Constants.Vision.kRobotToCamFront; @@ -12,27 +13,28 @@ import org.photonvision.EstimatedRobotPose; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import dev.doglog.DogLog; 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.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.util.CoralArrayManager; +import frc.robot.util.CoralObject; +import frc.robot.QuestNav.NerdQuestNav; public class PoseEstimatorSubsystem extends SubsystemBase { @@ -41,9 +43,12 @@ public class PoseEstimatorSubsystem extends SubsystemBase { public Vision visionFront; public Vision visionBack; private static Notifier allNotifier; + private NerdQuestNav QuestNAV; + Pose2d robotPose2d = new Pose2d(); StructPublisher publisher; + StructPublisher publisherQuest; //private final Pigeon2 gyro = new Pigeon2(TunerConstants.kPigeonId); @@ -51,18 +56,22 @@ public class PoseEstimatorSubsystem extends SubsystemBase { //public double targetAngle = 0; //private Rotation2d gyroResetAngle = new Rotation2d(); + private static List corals = new ArrayList<>(); + private static CoralArrayManager coralManager = new CoralArrayManager(); + static boolean coralInRange = false; - private Field2d field = new Field2d(); + private Field2d field = new Field2d(); // Simulation - public PoseEstimatorSubsystem(CommandSwerveDrivetrain driveTrain) { + public PoseEstimatorSubsystem(CommandSwerveDrivetrain driveTrain, NerdQuestNav questNav) { this.driveTrain = driveTrain; if(USE_VISION) { - this.visionFront = new Vision(kCameraNameFront, kRobotToCamFront, driveTrain); - this.visionBack = new Vision(kCameraNameBack, kRobotToCamBack, driveTrain); - + this.visionFront = new Vision(kCameraNameFront, kRobotToCamFront); + this.visionBack = new Vision(kCameraNameBack, kRobotToCamBack); + this.QuestNAV = questNav; + allNotifier = new Notifier(() -> { visionFront.run(); visionBack.run(); @@ -73,6 +82,9 @@ public PoseEstimatorSubsystem(CommandSwerveDrivetrain driveTrain) { publisher = NetworkTableInstance.getDefault() .getStructTopic("Robot Pose AdvScope", Pose2d.struct).publish(); + + publisherQuest = NetworkTableInstance.getDefault() + .getStructTopic("Robot Pose Quest", Pose2d.struct).publish(); } @@ -83,17 +95,19 @@ public void periodic() { // Update pose estimator with drivetrain sensors if(USE_VISION) { Optional visionEstFront = visionFront.getEstimatedRobotPose(); + visionEstFront.ifPresent( est -> { // Change our trust in the measurement based on the tags we can see var estStdDevs = visionFront.getEstimationStdDevs(); - SignalLogger.writeBoolean("front refreshed", true); // printMatrixValues(estStdDevs); driveTrain.addVisionMeasurement( est.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(est.timestampSeconds), estStdDevs); - }); + + + }); Optional visionEstBack = visionBack.getEstimatedRobotPose(); @@ -102,30 +116,82 @@ public void periodic() { est -> { // Change our trust in the measurement based on the tags we can see var estStdDevs = visionBack.getEstimationStdDevs(); - SignalLogger.writeBoolean("front refreshed", true); - + driveTrain.addVisionMeasurement( est.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(est.timestampSeconds), estStdDevs); - + }); + if(Constants.Vision.QUEST_ENABLED){ + Optional visionEstFrontQuest = visionFront.getEstimatedRobotPoseQuest(); + Optional visionEstBackQuest = visionBack.getEstimatedRobotPoseQuest(); + + visionEstFrontQuest.ifPresent( + estQuest -> { + QuestNAV.resetPose(estQuest.estimatedPose); + //QuestNAV.hardReset(estQuest.estimatedPose); + }); + + visionEstBackQuest.ifPresent( + estQuest -> { + QuestNAV.resetPose(estQuest.estimatedPose); + //QuestNAV.hardReset(estQuest.estimatedPose); + }); + + } + if(Robot.isSimulation() ) { visionFront.simulationPeriodic(getCurrentPose()); - // SmartDashboard.putData("Debug Field Front", visionFront.getSimDebugField()); + SmartDashboard.putData("Debug Field Front", visionFront.getSimDebugField()); visionBack.simulationPeriodic(getCurrentPose()); - // SmartDashboard.putData("Debug Field Back", visionBack.getSimDebugField()); + SmartDashboard.putData("Debug Field Back", visionBack.getSimDebugField()); } - if(visionEstFront.isPresent() && Constants.Vision.DOGLOG_ENABLED) { + if(visionEstFront.isPresent()) { DogLog.log("PoseEstimator/VisionEst", visionEstFront.get().estimatedPose.toPose2d()); } - if(visionEstBack.isPresent() && Constants.Vision.DOGLOG_ENABLED) { + if(visionEstBack.isPresent()) { DogLog.log("PoseEstimator/VisionEst", visionEstBack.get().estimatedPose.toPose2d()); - } + } - + //coral code + + if (Constants.Vision.USE_LIMELIGHT) { + + // CoralObject newCoral = newCoral(); + // if (!newCoral.getIgnored() && visionFront.hasTarget()) { + // corals.add(newCoral); + // } + + + + corals = coralArrayUpdateReturn(); + SmartDashboard.putNumber("size", corals.size()); + SmartDashboard.putBoolean("targeting", Constants.Vision.kCoralTargeted); + coralInRange = coralInRange(); + + + + if (corals.size() > 0) { + Pose2d coralPose = corals.get(corals.size() - 1).getPose(); + SmartDashboard.putString("coralPose", getFomattedPose(coralPose)); + } + + // if (corals.size() > 0) { + // int size = corals.size(); + // CoralObject lastCoral = corals.get(size - 1); + // Pose2d lastCoralPose = lastCoral.getPose(); + // SmartDashboard.putNumber("lastCoralX", lastCoralPose.getX()); + // SmartDashboard.putNumber("lastCoralY", lastCoralPose.getY()); + // SmartDashboard.putNumber("size", size); + // } + + // Pose2d coralPose = newCoral.getPose(); + // SmartDashboard.putNumber("coralX", coralPose.getX()); + // SmartDashboard.putNumber("coralY", coralPose.getY()); + } } else { if (allNotifier != null) allNotifier.close(); @@ -138,13 +204,24 @@ public void periodic() { // field.getObject("VisionEstimation").setPoses(); - SmartDashboard.putData("Robot Pose in Field", field); + // SmartDashboard.putData("Robot Pose in Field", field); SmartDashboard.putString("Formatted Pose", getFomattedPose()); - // DogLog.log("PoseEstimator/Pose", getCurrentPose()); - // DogLog.log("PoseEstimator/Formatted Pose", getFomattedPose()); + DogLog.log("PoseEstimator/ODO+Vision Pose", getCurrentPose()); + DogLog.log("PoseEstimator/ODO+Vision Formatted Pose", getFomattedPose()); + + if(Robot.isReal() && QuestNAV.getRobotPose().isPresent()) { + //field.setRobotPose(QuestNAV.getRobotPose().get().toPose2d()); + SmartDashboard.putString("Quest Pose", getFomattedPose(QuestNAV.getRobotPose().get().toPose2d())); + DogLog.log("PoseEstimator/Quest Pose", QuestNAV.getRobotPose().get().toPose2d()); + publisherQuest.set(QuestNAV.getRobotPose().get().toPose2d()); + SmartDashboard.putData("Robot Pose in Field", field); + } } + SmartDashboard.putBoolean("tV", visionFront.hasTarget()); + SmartDashboard.putBoolean("Coral In Range", coralInRange()); + SmartDashboard.putBoolean("Coral In List", coralInList()); } @@ -177,6 +254,102 @@ public void setCurrentPose(Pose2d newPose) { driveTrain.resetPose(newPose); } + public CoralObject newCoral() { + //Rotation2d yaw = gyro.getGyro(); + //Pose2d pose = new Pose2d(0.0,0.0,yaw); + Pose2d pose = getCurrentPose(); + double poseX = pose.getX(); + double poseY = pose.getY(); + Rotation2d yaw = pose.getRotation(); + + SmartDashboard.putNumber("poseX", poseX); + SmartDashboard.putNumber("poseY", poseY); + SmartDashboard.putNumber("yaw", -yaw.getDegrees()); + + double tx = visionFront.getTx(); + double ty = visionFront.getTy(); + double hb = visionFront.getHB(); + boolean upfall = false; + boolean ignored = false; + boolean targeted = false; + + Translation2d offset = new Translation2d(); + //Translation2d offset = new Translation2d(-0.90, new Rotation2d((-yaw.getDegrees() + tx) * Math.PI / 180)); + + //DriverStation.getMatchTime(); + + double boundingHeight = 0.0; + double boundingWidth = 0.0; + + double[] xys = visionFront.getCoordinates(); + if (xys.length != 0) { //debug + boundingHeight = xys[5] - xys[3]; + boundingWidth = xys[2] - xys[0]; + SmartDashboard.putNumber("boundingHeight", boundingHeight); + SmartDashboard.putNumber("boundingWidth", boundingWidth); + } + + double distance = 0.0; + double theta = 0.0; + if (boundingHeight > boundingWidth) { + upfall = false; + ignored = true; + } else if (boundingHeight <= boundingWidth && boundingHeight != 0.0) { + distance = (Constants.Vision.kCoralCenterFallenHeight - kLimeLightHeight) / Math.tan((Constants.Vision.kLimeLightAOD+ty) * (Math.PI / 180)) / Math.cos((tx) * Math.PI / 180); + upfall = true; + ignored = false; + } else { + distance = 0.0; + SmartDashboard.putString("orientation", ""); + ignored = true; + } + if (distance > 0.0) { + Rotation2d coralOrientation = new Rotation2d(theta); + Pose2d coralPose = new Pose2d(distance * Math.cos((yaw.getDegrees()-tx) * (Math.PI / 180)) + poseX + offset.getX(), + distance * Math.sin((yaw.getDegrees()-tx) * (Math.PI / 180)) + poseY + offset.getY(), + yaw); + //Pose2d coralPose = new Pose2d(2 + offset.getX(), 2 + offset.getY(), yaw); + SmartDashboard.putNumber("distance", distance); + ignored = false; + CoralObject newCoral = new CoralObject(coralPose, hb, distance, upfall, targeted, ignored); + return newCoral; + } else { + Pose2d zeroed = new Pose2d(0,0, new Rotation2d(0.0)); + ignored = true; + CoralObject newCoral = new CoralObject(zeroed, hb, distance, upfall, targeted, ignored); + // Pose2d coralPose = new Pose2d(2 + offset.getX(), 2 + offset.getY(), yaw); + // SmartDashboard.putNumber("distance", distance); + // ignored = false; + // CoralObject newCoral = new CoralObject(coralPose, hb, distance, upfall, targeted, ignored); + return newCoral; + } + } + + public List coralArrayUpdateReturn() { + if (!Constants.Vision.kCoralTargeted) { + CoralObject newCoral = newCoral(); + double hb = visionFront.getHB(); + double fps = visionFront.getFPS(); + corals.add(newCoral); + coralManager.distanceAndYawUpdate(corals, getCurrentPose()); + coralManager.expiryFilter(corals, hb, fps); + coralManager.displacementFilter(corals); + coralManager.possibilityFilter(corals); + return corals; + } else { + return coralManager.selectCoral(corals); + } + } + + public boolean coralInRange() { + coralInRange = coralManager.getCoralInRange(corals, getCurrentPose()); + return coralInRange; + } + + public boolean coralInList() { + return (corals.size() > 0); + } + /** * Resets the position on the field to 0,0 0-degrees, with forward being * downfield. This resets diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 538cefe..5c59a1e 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -68,18 +68,18 @@ public class Vision implements Runnable { private Matrix curStdDevs; private String cameraName; + private final NetworkTable llTable; private Optional optionalEstimatedRobotPose = Optional.empty(); + private Optional optionalEstimatedRobotPoseQuest = Optional.empty(); + // Simulation private PhotonCameraSim cameraSim; private VisionSystemSim visionSim; - private CommandSwerveDrivetrain driveTrain; - public Vision(String photonCamName, Transform3d robotToCam, CommandSwerveDrivetrain driveTrain) { + public Vision(String photonCamName, Transform3d robotToCam) { this.cameraName = photonCamName; - this.driveTrain = driveTrain; - camera = new PhotonCamera(photonCamName); @@ -87,8 +87,7 @@ public Vision(String photonCamName, Transform3d robotToCam, CommandSwerveDrivetr photonPoseEstimator = new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - // photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); - + llTable = NetworkTableInstance.getDefault().getTable("limelight"); // Simulation if (Robot.isSimulation()) { @@ -117,6 +116,10 @@ public Vision(String photonCamName, Transform3d robotToCam, CommandSwerveDrivetr public void run(){ optionalEstimatedRobotPose = getEstimatedGlobalPose(this.camera, this.photonPoseEstimator); + if(Constants.Vision.QUEST_ENABLED){ + optionalEstimatedRobotPoseQuest = getEstimatedGlobalPoseQuest(this.camera, this.photonPoseEstimator); + + } } /** @@ -155,6 +158,9 @@ public Optional getEstimatedGlobalPose(PhotonCamera camera, public Optional getEstimatedRobotPose(){ return this.optionalEstimatedRobotPose; } + public Optional getEstimatedRobotPoseQuest(){ + return this.optionalEstimatedRobotPoseQuest; + } /** * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard @@ -175,7 +181,6 @@ private void updateEstimationStdDevs( int numTags = 0; double avgDist = 0; - // photonEstimator.addHeadingData(estimatedPose.get().timestampSeconds, driveTrain.getRotation3d()); // Precalculation - see how many tags we found, and calculate an average-distance metric for (var tgt : targets) { var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); @@ -202,16 +207,13 @@ private void updateEstimationStdDevs( if (numTags == 1 && avgDist > kSingleTagDistanceThreshold) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); else if(numTags == 1 && avgDist < kSingleTagDistanceThreshold) { - if(estimatedPose.get().strategy == PoseStrategy.PNP_DISTANCE_TRIG_SOLVE || - (estimatedPose.get().strategy == PoseStrategy.LOWEST_AMBIGUITY && targets.get(0).getPoseAmbiguity() < kPoseAmbiguityThreshold)) { + if(targets.get(0).getPoseAmbiguity() < kPoseAmbiguityThreshold) { // estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); double xydeviations = kXYStdDev * Math.pow(avgDist, 2) / numTags ; double thetadeviations = kThetaStdDev * Math.pow(avgDist, 2) / numTags ; estStdDevs = VecBuilder.fill(xydeviations, xydeviations, thetadeviations); - if (Constants.Vision.DOGLOG_ENABLED){ DogLog.log("Vision"+cameraName+"/PoseAmbiguity", targets.get(0).getPoseAmbiguity()); DogLog.log("Vision"+cameraName+"/estStdDevs", estStdDevs); - } } else{ estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); @@ -241,7 +243,73 @@ public Matrix getEstimationStdDevs() { return curStdDevs; } + // - LimeLight + + public double getTx() { + return llTable.getEntry("tx").getDouble(0.0); + } + + public double getTy() { + return llTable.getEntry("ty").getDouble(0.0); + } + + public double getTa() { + return llTable.getEntry("ta").getDouble(0.0); + } + + public boolean hasTarget() { + return llTable.getEntry("tv").getDouble(0.0) == 1.0; + } + + public long getID() { + return llTable.getEntry("tid").getInteger(0); + } + + public double[] getRelBotPose() { + NetworkTableEntry relbotpose = llTable.getEntry("targetpose_cameraspace"); + return relbotpose.getDoubleArray(new double[6]); + } + public double[] getBotPose() { + NetworkTableEntry botpose = llTable.getEntry("botpose"); + return botpose.getDoubleArray(new double[6]); + } + + public void setPipelineNumber(int i) { + llTable.getEntry("pipeline").setNumber(i); + } + + public String getObjectClass() { + return llTable.getEntry("tclass").getString("none"); + } + + public double getHB() { + return llTable.getEntry("hb").getDouble(0.0); + } + + public double[] getCoordinates() { + double coords[] = new double[8]; + if (llTable.getEntry("tcornxy").getDoubleArray(new double[1]).length == 8) { + coords = llTable.getEntry("tcornxy").getDoubleArray(new double[1]); + } + return coords; + } + + public double getYaw() { + double[] botpose = getBotPose(); + double yaw = 0.0; + if (botpose.length == 6) { + yaw = botpose[5]; + } else { + yaw = 0.0; + } + return yaw; + } + + public double getFPS() { + double[] hw = llTable.getEntry("hw").getDoubleArray(new double[5]); + return hw[0]; + } // ----- Simulation public void simulationPeriodic(Pose2d robotSimPose) { @@ -258,4 +326,62 @@ public Field2d getSimDebugField() { if (!Robot.isSimulation()) return null; return visionSim.getDebugField(); } + + public Optional getEstimatedGlobalPoseQuest(PhotonCamera camera, PhotonPoseEstimator photonEstimator) { + Optional visionEst = Optional.empty(); + boolean noGood = false; + for (var change : camera.getAllUnreadResults()) { + visionEst = photonEstimator.update(change); + // updateEstimationStdDevs(visionEst, change.getTargets(), photonEstimator); + if(!visionEst.isEmpty() ) { + int numTags = 0; + double avgDist = 0; + + // photonEstimator.addHeadingData(estimatedPose.get().timestampSeconds, driveTrain.getRotation3d()); + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : change.getTargets()) { + if(Constants.Vision.nonReefTagFiducialIDs.contains(tgt.getFiducialId())) { + noGood = true; + break; + } + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(visionEst.get().estimatedPose.toPose2d().getTranslation()); + } + avgDist /= numTags; + if(numTags < 2 || avgDist >= 2.5) { + noGood = true; + } + } + else { + noGood = true; + } + + + + // if (Robot.isSimulation()) { + // visionEst.ifPresentOrElse( + // est -> + // getSimDebugField() + // .getObject("VisionEstimation") + // .setPose(est.estimatedPose.toPose2d()), + // () -> { + // getSimDebugField().getObject("VisionEstimation").setPoses(); + // }); + + // } + } + if(noGood) { + return Optional.empty(); + } + else { + return visionEst; + } + } } diff --git a/src/main/java/frc/robot/util/ArmPath.java b/src/main/java/frc/robot/util/ArmPath.java deleted file mode 100644 index 25f4d16..0000000 --- a/src/main/java/frc/robot/util/ArmPath.java +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.ArrayList; -import java.util.List; - -import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.subsystems.Arm; - -/** an interpolated arm path between a starting point, a list of intermediate points, and an end point */ -public class ArmPath { - public List points = (List) new ArrayList(); - //TODO: add and incorporate optional inflection point - - - /** interpolates linearly betwen points */ - public ArmPath(List points) { - for (int i = 0; i < points.size()-1; i++) { // interpolate between each point - this.points.addAll(ArmPathplannerUtil.interpolateArmPath(points.get(i), points.get(i+1))); - } - this.points.add(points.get(points.size()-1)); - } - - /** also interpolates */ - public ArmPath(List points, ArmPoint end) { - for (int i = 0; i < points.size()-1; i++) { // interpolate between each point - this.points.addAll(ArmPathplannerUtil.interpolateArmPath(points.get(i), points.get(i+1))); - } - this.points.add(end); - } - - /** also interpolates */ - public ArmPath(ArmPoint start, ArmPoint end) { - this.points.addAll(ArmPathplannerUtil.interpolateArmPath(start, end)); - this.points.add(end); - } - - /** interpolates linearly betwen start, points, and end */ - public ArmPath(List points, ArmPoint start, ArmPoint end) { - this.points.addAll(ArmPathplannerUtil.interpolateArmPath(start, points.get(0))); - for (int i = 0; i < points.size()-1; i++) { - this.points.addAll(ArmPathplannerUtil.interpolateArmPath(points.get(i), points.get(i+1))); - } - this.points.addAll(ArmPathplannerUtil.interpolateArmPath(points.get(points.size()-1), end)); - this.points.add(end); - } - - /** returns arm setpoints in a translation2d list */ - public List getTranslations() { - List translations = new ArrayList(); - for (ArmPoint point : points) { - translations.add(point.position); - } - return translations; - } - public List toStringList() { - List Strings = new ArrayList(); - for (ArmPoint point : points) { - Strings.add(point.position + "" + point.inBend); - } - return Strings; - } - - - - -} diff --git a/src/main/java/frc/robot/util/ArmPathplannerUtil.java b/src/main/java/frc/robot/util/ArmPathplannerUtil.java deleted file mode 100644 index 05b7b6e..0000000 --- a/src/main/java/frc/robot/util/ArmPathplannerUtil.java +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.time.chrono.MinguoDate; -import java.util.ArrayList; -import java.util.List; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.Constants.ArmVelocityGains; - -/** Add your docs here. */ -public class ArmPathplannerUtil { - /** chooses next point on a 2d path given current arm position - * @param armPaths list of arm points - * @param position current arm position - */ - public static Rotation2d ArmPathChooser(List armPaths,Translation2d position){ - Translation2d closestVector = new Translation2d(10000, 100000); - for (int i = armPaths.size() - 1; i >= 0; i-- ){ - // SmartDashboard.putNumber("distance pos - target", armPaths.get(i).getDistance(position)); - if (armPaths.get(i).getDistance(position) < ArmVelocityGains.lookAheadDistance){ - Rotation2d angle = armPaths.get(i).minus(position).getAngle(); - // SmartDashboard.putBoolean("on path", true); - // SmartDashboard.putNumber("target point angle", angle.getDegrees()); - return angle; - } - if (armPaths.get(i).getDistance(position) < closestVector.getNorm()){ - closestVector = armPaths.get(i).minus(position); - } - } - // SmartDashboard.putBoolean("on path", false); - // SmartDashboard.putNumber("target point angle", closestVector.getAngle().getDegrees()); - return closestVector.getAngle(); - } - public static ArmPoint getNextPoint(List armPaths, ArmPoint currentPoint) { - ArmPoint closestArmPoint = new ArmPoint(new Translation2d(1000000,1000000)); - for (int i = armPaths.size() - 1; i >= 0; i-- ){ - // SmartDashboard.putNumber("distance pos - target", armPaths.get(i).position.getDistance(position)); - if (armPaths.get(i).position.getDistance(currentPoint.position) < ArmVelocityGains.lookAheadDistance){ - if (armPaths.get(i).inBend == currentPoint.inBend) { - return armPaths.get(i); - } else { - if (armPaths.get(i).position.getDistance(currentPoint.position) < ArmVelocityGains.lookAheadDistanceBeforeInflecting) { - return armPaths.get(i); - } - } - // Rotation2d angle = armPaths.get(i).position.minus(position).getAngle(); - // SmartDashboard.putBoolean("on path", true); - // SmartDashboard.putNumber("target point angle", angle.getDegrees()); - } - if (armPaths.get(i).position.getDistance(currentPoint.position) < closestArmPoint.position.getNorm()){ - closestArmPoint = armPaths.get(i); - } - } - return closestArmPoint; - } - public static int getNextPointIndex(List armPaths, Translation2d position) { - ArmPoint closestArmPoint = new ArmPoint(new Translation2d(1000000,1000000)); - int index = 0; - for (int i = armPaths.size() - 1; i >= 0; i-- ){ - // SmartDashboard.putNumber("distance pos - target", armPaths.get(i).position.getDistance(position)); - if (armPaths.get(i).position.getDistance(position) < ArmVelocityGains.lookAheadDistance){ - Rotation2d angle = armPaths.get(i).position.minus(position).getAngle(); - // SmartDashboard.putBoolean("on path", true); - // SmartDashboard.putNumber("target point angle", angle.getDegrees()); - return i; - } - if (armPaths.get(i).position.getDistance(position) < closestArmPoint.position.getNorm()){ - closestArmPoint = armPaths.get(i); - index = i; - } - } - return index; - } - - - /** checks if the arm is at the end of the path */ - public static boolean CheckArmPosition(List armPaths, ArmPoint position){ - return position.position.getDistance(armPaths.get(armPaths.size()-1).position) < ArmVelocityGains.endDistance && position.inBend == armPaths.get(armPaths.size()-1).inBend; - } - /** interpolates linearly between start and end, includes endpoint but not startpoint */ - public static List interpolateArmPath(ArmPoint start, ArmPoint end){ - List path = (List) new ArrayList(); - // path.add(start); - - //interpolate polarly every 1 degree - Rotation2d step = Rotation2d.fromDegrees(ArmVelocityGains.interpolationAngle); - int pointCount = (int) ((end.position.getAngle().minus(start.position.getAngle())).getDegrees() / ArmVelocityGains.interpolationAngle); - double stepDist = (end.position.getNorm() - start.position.getNorm()) / (pointCount); - - if (pointCount < 0) { - pointCount = -pointCount; - step = step.times(-1); - stepDist *= -1; - } - if (pointCount > 3 || end.position.getDistance(start.position) < 10) { // continue interpolation polarly - for (int i = 1; i <= pointCount; i++){ - boolean inBend = i < pointCount/2 ? start.inBend : end.inBend; - path.add(new ArmPoint(new Translation2d(start.position.getNorm() + (stepDist * i), start.position.getAngle().plus(step.times(i))), inBend)); - } - } else { // switch to linear interpolation - double distance = start.position.getDistance(end.position); - Translation2d stepLinear = end.position.minus(start.position).div(distance).times(ArmVelocityGains.interpolationDistance); - int pointCountLinear = (int)(distance/ArmVelocityGains.interpolationDistance); - for (int i = 1; i <= pointCountLinear; i++){ - boolean inBend = i < pointCountLinear/2 ? start.inBend : end.inBend; - path.add(new ArmPoint(start.position.plus(stepLinear.times(i)), inBend)); - } - } - - path.add(end); - - //TODO: add wrist interpolation or other way to time wrist movement - - return path; - } - - // /** interpolates linearly between all points */ - // public static List interpolateArmPath(List points){ - // List path = (List) new ArrayList(); - // for (int i = 0; i < points.size()+1; i++) { - // path.addAll(interpolateArmPath(points.get(i), points.get(i+1))); - // } - // return path; - // } - - /** returns the closest armPoint to the current armPosition - * @param armPoints possible arm setpoints - * @param armPosition current arm position - */ - public static int closestArmPoint(ArmPoint[] armPoints, Translation2d armPosition) { - Translation2d position = armPosition; - double minDist = 1000000; - int closestPoint = 0; - for (int i = 0; i < armPoints.length; i++) { - ArmPoint point = armPoints[i]; - if (point.position.getDistance(position) < minDist) { - minDist = point.position.getDistance(position); - closestPoint = i; - } - } - // SmartDashboard.putNumber("closest point", closestPoint); - return closestPoint; - } - -} diff --git a/src/main/java/frc/robot/util/ArmPoint.java b/src/main/java/frc/robot/util/ArmPoint.java deleted file mode 100644 index 767a601..0000000 --- a/src/main/java/frc/robot/util/ArmPoint.java +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.ArrayList; -import java.util.List; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.subsystems.Arm; - -/** a single setpoint for an arm position */ -public class ArmPoint { - public Translation2d position; - public boolean inBend = false; - public double wrist = ArmSetpoints.homeWrist; - public ArmPoint(Translation2d point, boolean inBend, double wrist) { - this.position = point; - // if (position.getNorm() > ArmConstants.totalStageLength) { - // position = new Translation2d(ArmConstants.totalStageLength, position.getAngle()); - // } else if (position.getNorm() < ArmSetpoints.home.getNorm()) { - // position = new Translation2d(ArmSetpoints.home.getNorm(), position.getAngle()); - // } - this.inBend = inBend; - this.wrist = wrist; - } - public ArmPoint(Translation2d point, boolean inBend) { - this.position = point; - // if (position.getNorm() > ArmConstants.totalStageLength) { - // position = new Translation2d(ArmConstants.totalStageLength, position.getAngle()); - // } else if (position.getNorm() < ArmSetpoints.home.getNorm()) { - // position = new Translation2d(ArmSetpoints.home.getNorm(), position.getAngle()); - // } - this.inBend = inBend; - } - public ArmPoint(Translation2d point, double wrist) { - this.position = point; - // if (position.getNorm() > ArmConstants.totalStageLength) { - // position = new Translation2d(ArmConstants.totalStageLength, position.getAngle()); - // } else if (position.getNorm() < ArmSetpoints.home.getNorm()) { - // position = new Translation2d(ArmSetpoints.home.getNorm(), position.getAngle()); - // } - this.wrist = wrist; - } - public ArmPoint(Translation2d point) { - this.position = point; - // if (position.getNorm() > ArmConstants.totalStageLength) { - // position = new Translation2d(ArmConstants.totalStageLength, position.getAngle()); - // } else if (position.getNorm() < ArmSetpoints.home.getNorm()) { - // position = new Translation2d(ArmSetpoints.home.getNorm(), position.getAngle()); - // } - } - public static List fromTranslations(List points, boolean inBend) { - List ret = new ArrayList<>(); - for (int i = 0; i < points.size(); i++) { - ret.add(new ArmPoint(points.get(i), inBend)); - } - return ret; - } - public ArmPoint withWrist(double wristPos) { - return new ArmPoint(position, inBend, wristPos); - } - public ArmPoint addToWristFlip(double wristAdd) { - return new ArmPoint(position, inBend, wrist + wristAdd); - } - public ArmPoint add(Translation2d add) { - return new ArmPoint(position.plus(add), inBend, wrist); - } - /** - * Interpolates between two ArmPoints - * @param endPoint position to go to at alpha = 1 - * @param alpha percentage of the way to the end point - * @return the interpolated ArmPoint - */ - public ArmPoint interpolate(ArmPoint endPoint, double alpha) { - return new ArmPoint(position.plus(endPoint.position.minus(position).times(alpha)), alpha < ((ArmConstants.totalStageLength - endPoint.position.getNorm())/(2*ArmConstants.totalStageLength - position.getNorm() - endPoint.position.getNorm())) ? inBend : endPoint.inBend, wrist + (alpha * (endPoint.wrist - wrist))); - } - public ArmPoint rotateBy(Rotation2d rotateBy) { - return new ArmPoint(position.rotateBy(rotateBy), inBend, wrist); - } - public ArmPoint rotateElbowBy(Rotation2d rotateBy) { - double distance = position.getNorm(); - double BaseAngleArmDiff = Math.acos(((distance * distance) - + (ArmConstants.baseStageLength * ArmConstants.baseStageLength) - - (ArmConstants.secondStageLength * ArmConstants.secondStageLength)) - / (2 * distance * ArmConstants.baseStageLength)); - double SecondAngleArmDiff = Math.acos(((distance * distance) - - (ArmConstants.baseStageLength * ArmConstants.baseStageLength) - + (ArmConstants.secondStageLength * ArmConstants.secondStageLength)) - / (2 * distance * ArmConstants.secondStageLength)); - double shoulderPosition = position.getAngle().getRadians() + (BaseAngleArmDiff * (inBend ? 1 : -1)); - double elbowPosition = position.getAngle().getRadians() + (SecondAngleArmDiff * (inBend ? -1 : 1)); - elbowPosition += rotateBy.getRadians(); - Translation2d newPos = new Translation2d(ArmConstants.baseStageLength, new Rotation2d(shoulderPosition)).plus(new Translation2d(ArmConstants.secondStageLength, new Rotation2d(elbowPosition))); - return new ArmPoint(newPos, inBend, wrist); - } - - public ArmPoint withGripperOffset(Translation2d offset) { - return new ArmPoint(position.minus(offset.rotateBy(new Rotation2d(wrist))), inBend, wrist); - } -} diff --git a/src/main/java/frc/robot/util/CoralArrayManager.java b/src/main/java/frc/robot/util/CoralArrayManager.java index 42270d2..24fab5e 100644 --- a/src/main/java/frc/robot/util/CoralArrayManager.java +++ b/src/main/java/frc/robot/util/CoralArrayManager.java @@ -8,9 +8,13 @@ import java.util.List; import java.util.function.DoubleSupplier; -/** Add your docs here. */ +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** Manages a coral list. */ public class CoralArrayManager { + /** Chooses a coral to target and clears out others. */ public List selectCoral(List corals) { ArrayList distances = new ArrayList<>(); int sizeCorals = corals.size(); @@ -53,9 +57,9 @@ public List selectCoral(List corals) { return corals; } - + /** Filters outdated corals from the list. */ public List expiryFilter(List corals, double hb, double fps) { - double expiryFrameDiff = fps * 5; + double expiryFrameDiff = fps * 1; int sizeCorals = corals.size(); for(int i = 0; i < sizeCorals; i++) { @@ -80,4 +84,178 @@ public List expiryFilter(List corals, double hb, doubl return corals; } + + /** Filters out corals that are too close together to be considered distinct, removing the older version. */ + public List displacementFilter(List corals) { + double maxDisplacement = 0.3; + int sizeCorals = corals.size(); + + for (int i = 0; i < sizeCorals - 1; i++) { + for (int j = i + 1; j < sizeCorals; j++) { + CoralObject coral1 = corals.get(i); + CoralObject coral2 = corals.get(j); + + if (!coral1.getIgnored() && !coral2.getIgnored()) { + double poseX1 = coral1.getPose().getX(); + double poseY1 = coral1.getPose().getY(); + + double poseX2 = coral2.getPose().getX(); + double poseY2 = coral2.getPose().getY(); + + double distance = Math.sqrt(Math.pow((poseX2 - poseX1),2) + Math.pow((poseY2 - poseY1), 2)); + + if (distance <= maxDisplacement) { + double hb1 = coral1.getHB(); + double hb2 = coral2.getHB(); + + if (hb1 < hb2) { + CoralObject coralIgnored = coral1; + coralIgnored.setCoralIgnored(true); + corals.set(i, coralIgnored); + } else { + CoralObject coralIgnored = coral2; + coralIgnored.setCoralIgnored(true); + corals.set(j, coralIgnored); + } + } + } else if (coral1.getIgnored()) { + break; + } + } + } + + for (int i = 0; i < sizeCorals; i++) { + CoralObject coralChecked = corals.get(i); + if (coralChecked.getIgnored()) { + corals.remove(i); + i--; + sizeCorals = corals.size(); + } + } + + return corals; + } + + public List possibilityFilter(List corals) { + double fieldXDim = 17.55; + double fieldYDim = 8.05; + + double borderThickness = 0.1; + + int sizeCorals = corals.size(); + + for (int i = 0; i < sizeCorals; i++) { + Pose2d coralPose = corals.get(i).getPose(); + + double coralX = coralPose.getX(); + double coralY = coralPose.getY(); + + if ((borderThickness > coralX) || ((fieldXDim - borderThickness) < coralX) || + (borderThickness > coralY) || ((fieldYDim - borderThickness) < coralY)) { + CoralObject coralIgnored = corals.get(i); + coralIgnored.setCoralIgnored(true); + corals.set(i, coralIgnored); + } + } + + for (int i = 0; i < sizeCorals; i++) { + CoralObject coralChecked = corals.get(i); + if (coralChecked.getIgnored()) { + corals.remove(i); + i--; + sizeCorals = corals.size(); + } + } + + return corals; + } + + /** Updates the distance and angle to each coral in the list. */ + public List distanceAndYawUpdate(List corals, Pose2d pose) { + double poseX = pose.getX(); + double poseY = pose.getY(); + + int sizeCorals = corals.size(); + + for (int i = 0; i < sizeCorals; i++) { + CoralObject coralToUpdate = corals.get(i); + + Pose2d coralPose = coralToUpdate.getPose(); + + double coralX = coralPose.getX(); + double coralY = coralPose.getY(); + + double distance = Math.sqrt(Math.pow((coralX - poseX), 2) + + Math.pow(coralY - poseY, 2)); + + Rotation2d coralYawNew = new Rotation2d(Math.atan2(coralY - poseY, coralX - poseX)); + + Pose2d updatedCoralPose = new Pose2d(coralX, coralY, coralYawNew); + + coralToUpdate.setCoralPose(updatedCoralPose); + coralToUpdate.setCoralDistance(distance); + + corals.set(i, coralToUpdate); + } + + return corals; + } + + /** Detects if a coral is within a certain distance. */ + public boolean getCoralInRange(List corals, Pose2d pose) { + boolean coralInRange = false; + + double poseX = pose.getX(); + double poseY = pose.getY(); + + int sizeCoral = corals.size(); + + double minRange = 1.52; //2 ft in m + + for (int i = 0; i < sizeCoral; i++) { + CoralObject coralChecked = corals.get(i); + + Pose2d coralPose = coralChecked.getPose(); + + double coralX = coralPose.getX(); + double coralY = coralPose.getY(); + + double distance = Math.sqrt(Math.pow((coralX - poseX), 2) + + Math.pow(coralY - poseY, 2)); + + if (distance <= minRange) { + coralInRange = true; + } + } + + return coralInRange; + } + + /** Returns the closest coral in the list. */ + public CoralObject getClosestCoral(List corals) { + ArrayList distances = new ArrayList<>(); + int sizeCorals = corals.size(); + for (int i = 0; i < sizeCorals - 1; i++) { + double distanceNew = corals.get(i).getDistance(); + distances.add(() -> distanceNew); + } + int sizeDistances = distances.size(); + int indexMinDistance = 0; + + for (int i = 1; i < sizeDistances; i++) { + double distanceMinCurrent = distances.get(indexMinDistance).getAsDouble(); + double distanceMinProspective = distances.get(i).getAsDouble(); + + if (distanceMinProspective < distanceMinCurrent) { + indexMinDistance = i; + } + + } + + CoralObject coralClosest = corals.get(indexMinDistance); + + return coralClosest; + } + + } diff --git a/src/main/java/frc/robot/util/GenPath.java b/src/main/java/frc/robot/util/GenPath.java deleted file mode 100644 index 46bbed9..0000000 --- a/src/main/java/frc/robot/util/GenPath.java +++ /dev/null @@ -1,202 +0,0 @@ -package frc.robot.util; -import java.util.ArrayList; -import java.util.List; -import java.util.function.Supplier; - -import com.fasterxml.jackson.databind.introspect.DefaultAccessorNamingStrategy.FirstCharBasedValidator; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; -import frc.robot.Constants.ArmVelocityGains; -import frc.robot.util.ArmPoint; - -public class GenPath { - - /** - * Finds the points along a circular arc for a bend defined by three points. - * - * @param p1 Starting point. - * @param p2 Midpoint where the bend occurs. - * @param p3 Stopping point. - * @param radius Radius of circular arc path. - * @param numPoints Number of points lying between the arc endpoints. - * @return A list of ArmPoint points describing a path along the circular arc. - */ - public static List getArcPoints(Translation2d p1, - Translation2d p2, - Translation2d p3, - double radius, - int numPoints) { - // Translate frame to set p2 as the origin (frame "q") - Translation2d q1 = p1.minus(p2); - Translation2d q3 = p3.minus(p2); - - // Inner bend angles from point p2 - Rotation2d thetaB = q3.getAngle().minus(q1.getAngle()); // inner bend angle - // Divide thetaB by 2 to obtain thetaC (angle from q1 to circle center) - Rotation2d thetaC = new Rotation2d(thetaB.getRadians() / 2.0); - - // Arc endpoints in frame "q" - double a = Math.abs(radius / Math.tan(thetaC.getRadians())); // distance from p2 to arc endpoints - Translation2d qi = fromPolar(a, q1.getAngle()); - Translation2d qf = fromPolar(a, q3.getAngle()); - - // Circle center in frame "q" - double h = Math.abs(radius / Math.sin(thetaC.getRadians())); // distance from p2 to circle center - Rotation2d centerAngle = q1.getAngle().plus(thetaC); - Translation2d qc = fromPolar(h, centerAngle); - - // Angles of arc endpoints from circle center - Rotation2d phiI = (qi.minus(qc)).getAngle(); - Rotation2d phiF = (qf.minus(qc)).getAngle(); - - // Interpolate points on the arc between the endpoints - List ret = new ArrayList<>(); - for (int i = 0; i < numPoints + 2; i++) { - double t = (double) i / (numPoints + 1); - Rotation2d phi = interpolate(phiI, phiF, t); - // Create a point at the given angle and radius, then translate back by qc and p2 - Translation2d q = fromPolar(radius, phi).plus(qc); - Translation2d p = q.plus(p2); - ret.add((p)); - } - return ret; - } - - /** adds inflection points for inBend reversal */ - public static List generateInflectionPoints(List points) { - for (int i = 0; i < points.size() - 1; i++) { - if (points.get(i).inBend != points.get(i+1).inBend) { - Translation2d first = points.get(i).position; - Translation2d second = points.get(i+1).position; - double totalLength = ArmConstants.baseStageLength + ArmConstants.secondStageLength; - double firstToInflection = totalLength - first.getNorm(); - double secondToInflection = totalLength - second.getNorm(); - double fraction = firstToInflection/(firstToInflection + secondToInflection); - Rotation2d angleDiff = second.getAngle().minus(first.getAngle()); - Rotation2d angle = ((angleDiff).times(fraction)).plus(first.getAngle()); - ArmPoint inflectionPoint1 = new ArmPoint(new Translation2d(totalLength, angle.minus(angleDiff.times(0.1).times(fraction))), points.get(i).inBend); - ArmPoint inflectionPoint2 = new ArmPoint(new Translation2d(totalLength, angle.plus(angleDiff.times(0.1).times(1-fraction))), points.get(i+1).inBend); - points.add(i+1, inflectionPoint1); - points.add(i+2, inflectionPoint2); - i+=2; - System.out.println("fraction" + fraction); - System.out.println("anglediff" + angleDiff.getDegrees()); - } - } - int i = 0; - for (ArmPoint point : points) { - System.out.println(point.position +""+ i +""+ point.inBend); - i++; - } - return points; - } - - /** - * Generates a smooth path through the given list of points by replacing each bend with an arc. - * - * @param points A sequence of ArmPoint points. - * @param radius The radius of the arcs. - * @param numPoints The number of points to generate between each pair of arc endpoints. - * @return A list of ArmPoint points representing the smooth path. - */ - public static List generateSmoothPath(List points, - double radius, - int numPoints) { - for (int i = 0; i < points.size() -1; i++) { - if (points.get(i).position.getDistance(points.get(i+1).position) < 0.001 && (points.get(i).inBend == points.get(i).inBend)) { - points.remove(i+1); - i--; - } - } - List ret = new ArrayList<>(); - if (points.isEmpty()) { - return ret; - } - ret.add(points.get(0)); - for (int i = 0; i < points.size() - 2; i++) { - if ((points.get(i).position.getDistance(points.get(i+1).position) > ArmVelocityGains.arcRadius*2) && (points.get(i+1).position.getDistance(points.get(i+2).position) > ArmVelocityGains.arcRadius*2)) { - List arcPoints = ArmPoint.fromTranslations(getArcPoints(points.get(i).position, points.get(i + 1).position, points.get(i + 2).position, radius, numPoints), points.get(i+1).inBend); - ret.addAll(arcPoints); - } else { - ret.add(points.get(i)); - } - } - ret.add(points.get(points.size() - 1)); - return ret; - } - - public static List generateSmoothPath(ArmPoint start, List points, ArmPoint end, - double radius, - int numPoints) { - List temp = new ArrayList<>(); - temp.addAll(points); - temp.add(0, start); - temp.add(end); - List ret = generateSmoothPath(temp, radius, numPoints); - return ret; - } - - /** - * Helper method to create a Translation2d from polar coordinates. - * - * @param r The radius (distance). - * @param angle The angle as a Rotation2d. - * @return A Translation2d point. - */ - public static Translation2d fromPolar(double r, Rotation2d angle) { - double x = r * Math.cos(angle.getRadians()); - double y = r * Math.sin(angle.getRadians()); - return new Translation2d(x, y); - } - - /** - * Helper method to interpolate between two Rotation2d objects. - * - * @param a The starting rotation. - * @param b The ending rotation. - * @param t The interpolation parameter (0.0 to 1.0). - * @return The interpolated Rotation2d. - */ - public static Rotation2d interpolate(Rotation2d a, Rotation2d b, double t) { - double start = a.getRadians(); - double end = b.getRadians(); - // Normalize the difference to be within [-pi, pi] - double diff = end - start; - while (diff < -Math.PI) { - diff += 2 * Math.PI; - } - while (diff > Math.PI) { - diff -= 2 * Math.PI; - } - double interpolated = start + t * diff; - return new Rotation2d(interpolated); - } - - // Example usage of the above methods. - public static void main(String[] args) { - List points = new ArrayList<>(); - points.add(new ArmPoint(new Translation2d(1, 0))); - points.add(new ArmPoint(new Translation2d(2, -1))); - points.add(new ArmPoint(new Translation2d(3, 2))); - points.add(new ArmPoint(new Translation2d(2, 2))); - points.add(new ArmPoint(new Translation2d(4, -1))); - points.add(new ArmPoint(new Translation2d(1, 1))); - points.add(new ArmPoint(new Translation2d(2, 4))); - points.add(new ArmPoint(new Translation2d(2.5, 2.5))); - points.add(new ArmPoint(new Translation2d(3, 4))); - - double radius = 0.2; - int numPoints = 5; - - List smoothPath = generateSmoothPath(points, radius, numPoints); - - // Print the generated smooth path points. - for (ArmPoint point : smoothPath) { - System.out.println("Point: (" + point.position.getX() + ", " + point.position.getY() + ")"); - } - } -} - diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json index ee73968..2ee13f6 100644 --- a/vendordeps/DogLog.json +++ b/vendordeps/DogLog.json @@ -3,7 +3,7 @@ { "groupId": "com.github.jonahsnider", "artifactId": "doglog", - "version": "2025.3.1" + "version": "2025.8.1" } ], "fileName": "DogLog.json", @@ -15,6 +15,6 @@ "https://jitpack.io" ], "cppDependencies": [], - "version": "2025.3.1", + "version": "2025.8.1", "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" } \ No newline at end of file diff --git a/vendordeps/Phoenix6-25.3.1.json b/vendordeps/Phoenix6-25.3.2.json similarity index 92% rename from vendordeps/Phoenix6-25.3.1.json rename to vendordeps/Phoenix6-25.3.2.json index a216d97..29187a8 100644 --- a/vendordeps/Phoenix6-25.3.1.json +++ b/vendordeps/Phoenix6-25.3.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.3.1.json", + "fileName": "Phoenix6-25.3.2.json", "name": "CTRE-Phoenix (v6)", - "version": "25.3.1", + "version": "25.3.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.1" + "version": "25.3.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 1219919..2d7b1d8 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.2.1", + "version": "v2025.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.2.1" + "version": "v2025.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.2.1" + "version": "v2025.3.1" } ] } \ No newline at end of file