diff --git a/build.gradle b/build.gradle index 1611678..14a4943 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/deploy/pathplanner/autos/Copy of Frontface 4.auto b/src/main/deploy/pathplanner/autos/Copy of Frontface 4.auto new file mode 100644 index 0000000..2265a08 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of Frontface 4.auto @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Copy of F4 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Copy of F4 2" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 3" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, + { + "type": "named", + "data": { + "name": "autoL3DeAlgeafy" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "path", + "data": { + "pathName": "F4 4" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 5" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 6" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 7" + } + }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Frontface 4.auto b/src/main/deploy/pathplanner/autos/Frontface 4.auto index 7d8f1e7..dd1c4cc 100644 --- a/src/main/deploy/pathplanner/autos/Frontface 4.auto +++ b/src/main/deploy/pathplanner/autos/Frontface 4.auto @@ -10,18 +10,6 @@ "pathName": "F4 1" } }, - { - "type": "named", - "data": { - "name": "5PieceScore" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0 - } - }, { "type": "path", "data": { @@ -35,27 +23,28 @@ } }, { - "type": "named", + "type": "parallel", "data": { - "name": "5PieceScore" + "commands": [ + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, + { + "type": "named", + "data": { + "name": "autoL3DeAlgeafy" + } + } + ] } }, { "type": "wait", "data": { - "waitTime": 0 - } - }, - { - "type": "named", - "data": { - "name": "autoL3DeAlgeafy" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 + "waitTime": 0.4 } }, { @@ -70,18 +59,6 @@ "pathName": "F4 5" } }, - { - "type": "named", - "data": { - "name": "5PieceScore" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0 - } - }, { "type": "path", "data": { @@ -99,12 +76,6 @@ "data": { "name": "5PieceScore" } - }, - { - "type": "wait", - "data": { - "waitTime": 0 - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Processor 4.auto b/src/main/deploy/pathplanner/autos/Processor 4.auto new file mode 100644 index 0000000..0125733 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Processor 4.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LCS 1" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 2" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 3" + } + }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 4" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 5" + } + }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 6" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 7" + } + }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of F4 1.path b/src/main/deploy/pathplanner/paths/Copy of F4 1.path new file mode 100644 index 0000000..c4ce656 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of F4 1.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.100822368421052, + "y": 0.5515625 + }, + "prevControl": null, + "nextControl": { + "x": 2.4439144736842096, + "y": 1.9563322368421043 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.175, + "y": 4.0 + }, + "prevControl": { + "x": 3.098190789469281, + "y": 3.0724506578918773 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CL41" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "autoIntakeDown", + "waypointRelativePos": 0.22800718132854628, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "autoIntakeDown" + } + } + }, + { + "name": "1stPieceUpFrontCopy", + "waypointRelativePos": 0.55, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "1stPieceUpFrontCopy" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Front 4", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of F4 2.path b/src/main/deploy/pathplanner/paths/Copy of F4 2.path new file mode 100644 index 0000000..855bf99 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of F4 2.path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.175, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.415935075290731, + "y": 4.253293538766084 + }, + "isLocked": false, + "linkedName": "CL41" + }, + { + "anchor": { + "x": 1.4063150104370652, + "y": 5.66 + }, + "prevControl": { + "x": 1.8281250000012157, + "y": 5.179605263154684 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F4 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6871069182390003, + "rotationDegrees": 136.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "5PieceScore", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + }, + { + "name": "ElevatorDown", + "waypointRelativePos": 0.06, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 133.35422167699903 + }, + "reversed": false, + "folder": "Front 4", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F4 1.path b/src/main/deploy/pathplanner/paths/F4 1.path index 4f6d420..87a07e2 100644 --- a/src/main/deploy/pathplanner/paths/F4 1.path +++ b/src/main/deploy/pathplanner/paths/F4 1.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.1548708457619004, + "x": 3.175, "y": 4.358630803339179 }, "prevControl": { - "x": 3.183735977340848, + "x": 3.2038651315789473, "y": 5.3015584349181255 }, "nextControl": null, @@ -32,11 +32,11 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.7, + "minWaypointRelativePos": 0.6, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.1, + "maxAcceleration": 1.15, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -70,8 +70,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 5.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/F4 2.path b/src/main/deploy/pathplanner/paths/F4 2.path index 3ab75bb..a962256 100644 --- a/src/main/deploy/pathplanner/paths/F4 2.path +++ b/src/main/deploy/pathplanner/paths/F4 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.1548708457619004, + "x": 3.175, "y": 4.358630803339179 }, "prevControl": null, "nextControl": { - "x": 2.548703082604006, - "y": 4.839716329654968 + "x": 2.415935075290731, + "y": 4.611924342105263 }, "isLocked": false, "linkedName": "F4 1" @@ -17,11 +17,11 @@ { "anchor": { "x": 1.4063150104370652, - "y": 5.685463358904612 + "y": 5.66 }, "prevControl": { - "x": 1.7815617209633812, - "y": 5.271729806273033 + "x": 1.8281250000012157, + "y": 5.179605263154684 }, "nextControl": null, "isLocked": false, @@ -31,16 +31,16 @@ "rotationTargets": [ { "waypointRelativePos": 0.6871069182390003, - "rotationDegrees": 137.13691041664012 + "rotationDegrees": 136.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.6, + "minWaypointRelativePos": 0.7, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.25, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -52,19 +52,19 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "IntakeOn", + "name": "5PieceScore", "waypointRelativePos": 0, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "IntakeOn" + "name": "5PieceScore" } } }, { "name": "ElevatorDown", - "waypointRelativePos": 0, + "waypointRelativePos": 0.06, "endWaypointRelativePos": null, "command": { "type": "named", @@ -72,11 +72,22 @@ "name": "ElevatorDown" } } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } } ], "globalConstraints": { "maxVelocity": 6.0, - "maxAcceleration": 4.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -84,7 +95,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 137.48955292199918 + "rotation": 133.35422167699903 }, "reversed": false, "folder": "Front 4", diff --git a/src/main/deploy/pathplanner/paths/F4 3.path b/src/main/deploy/pathplanner/paths/F4 3.path index 75c1f32..8529de0 100644 --- a/src/main/deploy/pathplanner/paths/F4 3.path +++ b/src/main/deploy/pathplanner/paths/F4 3.path @@ -4,12 +4,12 @@ { "anchor": { "x": 1.4063150104370652, - "y": 5.685463358904612 + "y": 5.66 }, "prevControl": null, "nextControl": { "x": 1.8998484947882865, - "y": 5.061753805014962 + "y": 5.03629044611035 }, "isLocked": false, "linkedName": "F4 2" @@ -65,7 +65,7 @@ ], "globalConstraints": { "maxVelocity": 6.0, - "maxAcceleration": 4.5, + "maxAcceleration": 4.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -79,7 +79,7 @@ "folder": "Front 4", "idealStartingState": { "velocity": 0, - "rotation": 137.48955292199918 + "rotation": 133.35422167699903 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F4 4.path b/src/main/deploy/pathplanner/paths/F4 4.path index 730ba8c..ddaf64f 100644 --- a/src/main/deploy/pathplanner/paths/F4 4.path +++ b/src/main/deploy/pathplanner/paths/F4 4.path @@ -35,7 +35,7 @@ "minWaypointRelativePos": 0.6, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.1, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/F4 5.path b/src/main/deploy/pathplanner/paths/F4 5.path index 84602f8..580ba53 100644 --- a/src/main/deploy/pathplanner/paths/F4 5.path +++ b/src/main/deploy/pathplanner/paths/F4 5.path @@ -36,7 +36,7 @@ "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 3.0, - "maxAcceleration": 2.52, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -60,7 +60,7 @@ ], "globalConstraints": { "maxVelocity": 6.0, - "maxAcceleration": 5.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/F4 6.path b/src/main/deploy/pathplanner/paths/F4 6.path index 8e2927a..d09b0fd 100644 --- a/src/main/deploy/pathplanner/paths/F4 6.path +++ b/src/main/deploy/pathplanner/paths/F4 6.path @@ -40,7 +40,7 @@ "minWaypointRelativePos": 0.6, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.1, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -52,19 +52,19 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "IntakeOn", + "name": "5PieceScore", "waypointRelativePos": 0, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "IntakeOn" + "name": "5PieceScore" } } }, { "name": "ElevatorDown", - "waypointRelativePos": 0, + "waypointRelativePos": 0.06, "endWaypointRelativePos": null, "command": { "type": "named", @@ -72,6 +72,17 @@ "name": "ElevatorDown" } } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/LCS 1.path b/src/main/deploy/pathplanner/paths/LCS 1.path new file mode 100644 index 0000000..1f304f9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 1.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.946471406470677, + "y": 2.464 + }, + "prevControl": null, + "nextControl": { + "x": 6.564270068533928, + "y": 2.5387007677177387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.179216737944418, + "y": 2.8955872013570754 + }, + "prevControl": { + "x": 5.47509969301644, + "y": 2.8330116867132045 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 1" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "autoIntakeDown", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "Processor4Piece1", + "waypointRelativePos": 0.4021543985637337, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece1" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -60.70863782901576 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 2.path b/src/main/deploy/pathplanner/paths/LCS 2.path new file mode 100644 index 0000000..90b889e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 2.path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.179216737944418, + "y": 2.8955872013570754 + }, + "prevControl": null, + "nextControl": { + "x": 3.8294407894736837, + "y": 2.2546052631578943 + }, + "isLocked": false, + "linkedName": "LCS 1" + }, + { + "anchor": { + "x": 2.021, + "y": 0.7994156169734083 + }, + "prevControl": { + "x": 3.0216578947368418, + "y": 1.790451801186588 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6776729559748446, + "rotationDegrees": -132.7479460740572 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "5PieceScore", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + }, + { + "name": "ElevatorDown", + "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.1, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 2.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -128.6116599476519 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -60.70863782901576 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 3.path b/src/main/deploy/pathplanner/paths/LCS 3.path new file mode 100644 index 0000000..7ed10eb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 3.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.021, + "y": 0.7994156169734083 + }, + "prevControl": null, + "nextControl": { + "x": 2.6443759035353986, + "y": 1.5590942478745697 + }, + "isLocked": false, + "linkedName": "LCS 2" + }, + { + "anchor": { + "x": 3.8146166810247983, + "y": 2.8987628231340046 + }, + "prevControl": { + "x": 3.614706259324243, + "y": 2.6474121427221844 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8317610062893104, + "rotationDegrees": -119.99999999999999 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.35, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Processor4Piece2", + "waypointRelativePos": 0.33, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece2" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -128.6116599476519 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 4.path b/src/main/deploy/pathplanner/paths/LCS 4.path new file mode 100644 index 0000000..63d0d9b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 4.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8213518783932194, + "y": 2.9054980205024266 + }, + "prevControl": null, + "nextControl": { + "x": 3.1078125, + "y": 2.3123355263157896 + }, + "isLocked": false, + "linkedName": "LCS 3" + }, + { + "anchor": { + "x": 2.021, + "y": 0.7994156169734083 + }, + "prevControl": { + "x": 2.372169441998694, + "y": 1.2536507379032424 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "ElevatorDown", + "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.2692998204667869, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -128.6116599476519 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 5.path b/src/main/deploy/pathplanner/paths/LCS 5.path new file mode 100644 index 0000000..ee2357b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 5.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.021, + "y": 0.7994156169734083 + }, + "prevControl": null, + "nextControl": { + "x": 2.5983026315789473, + "y": 1.2708794327655348 + }, + "isLocked": false, + "linkedName": "LCS 2" + }, + { + "anchor": { + "x": 3.9347059868001075, + "y": 2.849571035298859 + }, + "prevControl": { + "x": 3.140956255762699, + "y": 1.9517313826234841 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 4" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8726415094339707, + "rotationDegrees": -119.99999999999999 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.87, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Processor4Piece3", + "waypointRelativePos": 0.6, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -128.6116599476519 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 6.path b/src/main/deploy/pathplanner/paths/LCS 6.path new file mode 100644 index 0000000..f236735 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 6.path @@ -0,0 +1,96 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9347059868001075, + "y": 2.849571035298859 + }, + "prevControl": null, + "nextControl": { + "x": 2.9426705017481174, + "y": 1.9359439480223979 + }, + "isLocked": false, + "linkedName": "LCS 4" + }, + { + "anchor": { + "x": 2.021, + "y": 0.7994156169734083 + }, + "prevControl": { + "x": 2.7345774818798887, + "y": 1.655268742410823 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6053459119496889, + "rotationDegrees": -128.08554313418483 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "ElevatorDown", + "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.33572710951526064, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -128.6116599476519 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 7.path b/src/main/deploy/pathplanner/paths/LCS 7.path new file mode 100644 index 0000000..621ab0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 7.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.021, + "y": 0.7994156169734083 + }, + "prevControl": null, + "nextControl": { + "x": 3.0573014305086144, + "y": 1.5024288465675784 + }, + "isLocked": false, + "linkedName": "LCS 2" + }, + { + "anchor": { + "x": 4.893712143783675, + "y": 2.732663794013762 + }, + "prevControl": { + "x": 4.252796052631578, + "y": 2.225740131578947 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6729559748427736, + "rotationDegrees": -59.99999999999999 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Processor4Piece4", + "waypointRelativePos": 0.6535008976660687, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -128.6116599476519 + }, + "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 0e9a996..7e19274 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,10 +5,11 @@ "pathFolders": [ "2 L4 Auto", "3 L4 Auto", + "4 Piece Processor side", "5 Piece Cs", + "Front 4", "Human Load NonProcessor", - "Lollipop 4", - "Front 4" + "Lollipop 4" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ea31a54..581bd64 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,18 +24,17 @@ * (log replay from a file). */ public final class Constants { - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.REPLAY; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public static final Distance kReefL1 = Meters.of(0.0); public static final Distance kReefL2 = Meters.of(0.5192); // 0.5492 public static final Distance kReefL3 = Meters.of(0.92); // 0.9706 - public static final Distance kReefL4 = Meters.of(1.61); // 1.5975 - public static final Distance kDeAlgaeL2 = Meters.of(0.5); - public static final Distance kDeAlgaeL3 = Meters.of(.9); - public static final Distance kElevatorTroughScorePos = Meters.of(0.08); - public static final Distance kElevatorTrough2ScorePos = Meters.of(0.13); - + public static final Distance kReefL4 = Meters.of(1.59115); // 1.61 // 1.5975 + public static final Distance kDeAlgaeL2 = Meters.of(0.52); + public static final Distance kDeAlgaeL3 = Meters.of(.95); + public static final Distance kElevatorTroughScorePos = Meters.of(0.08); // 0.08 old // 0.20 + public static final Distance kElevatorTrough2ScorePos = Meters.of(0.20); // 0.12 old // public static final Double kElevatorL4FudgeFactor = 1.4; public static final Double kElevatorL23FudgeFactor = 1.5; @@ -52,15 +51,15 @@ public final class Constants { public static final Integer kShooterPigeon2 = 2; // Intake Pivot configurations - public static final Double kIntakePivotkP = 0.0; + public static final Double kIntakePivotkP = 10.0; public static final Double kIntakePivotkI = 0.0; public static final Double kIntakePivotkD = 0.0; - public static final Double kIntakePivotkV = 0.127; + public static final Double kIntakePivotkV = 0.105; public static final Double kIntakePivotkA = 0.0; public static final Double kIntakePivotkS = 0.0; public static final Double kIntakePivotMotionMagicCruiseVelocity = 94.0; // Max speed at 12V - public static final Double kIntakePivotMotionMagicAcceleration = 200.0; - public static final Double kIntakePivotMotionMagicJerk = 0.0; + public static final Double kIntakePivotMotionMagicAcceleration = 400.0; + public static final Double kIntakePivotMotionMagicJerk = 2000.0; public static final Double kIntakePivotForwardSoftLimitThreshold = 30.9; public static final Double kIntakePivotReverseSoftLimitThreshold = 0.0; public static final Boolean kIntakePivotForwardSoftLimitEnable = true; @@ -137,21 +136,24 @@ public final class Constants { public static final Double kShooterVelocitykA = 0.0; public static final Double kShooterVelocitykS = 0.6; - public static final Double kShooterPositionkP = 50.0; + public static final Double kShooterPositionkP = 5.0; // 50.0; public static final Double kShooterPositionkI = 0.0; public static final Double kShooterPositionkD = 0.0; - public static final Double kShooterPositionkV = 0.0959; + public static final Double kShooterPositionkV = 0.1238; // 0.0959; public static final Double kShooterPositionkA = 0.0; - public static final Double kShooterPositionkS = 0.6; + public static final Double kShooterPositionkS = 0.3; // 0.6 public static final Double kShooterMotionMagicAcceleration = 0.0; public static final Double kShooterMotionMagicCruiseVelocity = 0.0; public static final Double kShooterMotionMagicJerk = 0.0; public static final Double kShooterScoreSpeed = 8.0; public static final Double kShooterLength = 8.5; - public static final Double kL1ShooterVelo = 31.0; // 15 + public static final Double kStowShooterVelo = 100.0; + public static final Double kL1ShooterVelo = 40.0; // 38 + public static final Double kL1PostSlideShooterVelo = 30.0; + public static final Double kL1_5ShooterVelo = 50.0; public static final Double kL4ShooterVelo = 60.0; // Only gets to 90 when at 110 - public static final Double kL2andL3ShooterVelo = 60.0; // Goes to around 52 + public static final Double kL2andL3ShooterVelo = 60.0; // Goes to around 52 at 60 // climber configurations public static final Double kClimberkP = 100.0; @@ -187,21 +189,28 @@ public final class Constants { public static final Double kAngulatorL4 = 47.0; public static final Double kAngulatorDeAlgaeL2 = 45.0; public static final Double kAngulatorTravelPos = 4.5; - public static final Double kAngulatorTroughScorePos = -5.0; - public static final Double kAngulatorTrough2ScorePos = -8.0; - public static final Double kAngulatorBargeScorePos = 0.0; + public static final Double kAngulatorTroughScorePos = 0.0; // 0.0 old L1 + public static final Double kAngulatorTrough2ScorePos = 10.0; // -12 old toss up // 10 + public static final Double kAngulatorBargeScorePos = -3.0; public static final Double kAngulatorGroundPickUpPos = 61.5; public static final Double kAngulatorFeederPosition = -20.0; public static final Double kAngulatorFeederPositionTolerance = 2.0; public static final Double kSlideFeederPositionToleranceStopping = 0.005; - public static final Double kFeederkP = 50.0; - public static final Double kFeederkI = 0.0; - public static final Double kFeederkD = 0.0; - public static final Double kFeederkV = 0.0909; - public static final Double kFeederkA = 0.0; - public static final Double kFeederkS = 0.5; + public static final Double kVeloFeederkP = 0.5; + public static final Double kVeloFeederkI = 0.0; + public static final Double kVeloFeederkD = 0.0; + public static final Double kVeloFeederkV = 0.0928; + public static final Double kVeloFeederkA = 0.0; + public static final Double kVeloFeederkS = 0.3; + + public static final Double kPosFeederkP = 50.0; + public static final Double kPosFeederkI = 0.0; + public static final Double kPosFeederkD = 0.0; + public static final Double kPosFeederkV = 0.0909; + public static final Double kPosFeederkA = 0.0; + public static final Double kPosFeederkS = 0.5; // drive configurations public static final Integer DriveMaxSpeed = 1; diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 5d19b5d..6bbda32 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -64,7 +64,7 @@ public static class Reef { Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line public static final Pose2d[] centerFaces = - new Pose2d[6]; // Starting facing the driver station in clockwise order + new Pose2d[12]; // Starting facing the driver station in clockwise order public static final List> branchPositions = new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise @@ -100,39 +100,39 @@ public static class Reef { Units.inchesToMeters(160.375), Units.inchesToMeters(130.144), Rotation2d.fromDegrees(-120)); - // centerFaces[6] = - // new Pose2d( - // Units.inchesToMeters(530.49), - // Units.inchesToMeters(130.17), - // Rotation2d.fromDegrees(300)); - // centerFaces[7] = - // new Pose2d( - // Units.inchesToMeters(546.87), - // Units.inchesToMeters(158.50), - // Rotation2d.fromDegrees(0)); - // centerFaces[8] = - // new Pose2d( - // Units.inchesToMeters(530.49), - // Units.inchesToMeters(186.83), - // Rotation2d.fromDegrees(60)); - // centerFaces[9] = - // new Pose2d( - // Units.inchesToMeters(497.77), - // Units.inchesToMeters(186.83), - // Rotation2d.fromDegrees(120)); - // centerFaces[10] = - // new Pose2d( - // Units.inchesToMeters(481.39), - // Units.inchesToMeters(158.50), - // Rotation2d.fromDegrees(180)); - // centerFaces[11] = - // new Pose2d( - // Units.inchesToMeters(497.77), - // Units.inchesToMeters(130.17), - // Rotation2d.fromDegrees(240)); + centerFaces[6] = + new Pose2d( + Units.inchesToMeters(530.49), + Units.inchesToMeters(130.17), + Rotation2d.fromDegrees(300)); + centerFaces[7] = + new Pose2d( + Units.inchesToMeters(546.87), + Units.inchesToMeters(158.50), + Rotation2d.fromDegrees(0)); + centerFaces[8] = + new Pose2d( + Units.inchesToMeters(530.49), + Units.inchesToMeters(186.83), + Rotation2d.fromDegrees(60)); + centerFaces[9] = + new Pose2d( + Units.inchesToMeters(497.77), + Units.inchesToMeters(186.83), + Rotation2d.fromDegrees(120)); + centerFaces[10] = + new Pose2d( + Units.inchesToMeters(481.39), + Units.inchesToMeters(158.50), + Rotation2d.fromDegrees(180)); + centerFaces[11] = + new Pose2d( + Units.inchesToMeters(497.77), + Units.inchesToMeters(130.17), + Rotation2d.fromDegrees(240)); // Initialize branch positions - for (int face = 0; face < 6; face++) { + for (int face = 0; face < 12; face++) { Map fillRight = new HashMap<>(); Map fillLeft = new HashMap<>(); for (var level : ReefHeight.values()) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 12c0a70..505b31d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,13 +13,14 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -50,7 +51,7 @@ public class Robot extends LoggedRobot { private boolean brakesDisabled = false; public Robot() { - super(0.01); + super(0.02); // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); @@ -125,6 +126,8 @@ public Robot() { // Start AdvantageKit logger Logger.start(); + SignalLogger.enableAutoLogging(false); + // Check for valid swerve config var modules = new SwerveModuleConstants[] { @@ -144,6 +147,7 @@ public Robot() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); + FollowPathCommand.warmupCommand().schedule(); } /** This function is called periodically during all modes. */ @@ -167,11 +171,11 @@ public void robotPeriodic() { /** This function is called once when the robot is disabled. */ @Override public void disabledInit() { - disableTime = Timer.getFPGATimestamp(); + // disableTime = Timer.getFPGATimestamp(); - if (DriverStation.isFMSAttached() == false) { - robotContainer.disableBrakes(); - } + // if (DriverStation.isFMSAttached() == false) { + // robotContainer.disableBrakes(); + // } // robotContainer.disableBrakes(); } @@ -207,9 +211,10 @@ public void teleopInit() { autonomousCommand.cancel(); } - hasBeenEnabled = true; - - robotContainer.enableBrakes(); + // hasBeenEnabled = true; + // if (DriverStation.isFMSAttached() == false) { + // robotContainer.enableBrakes(); + // } } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 264203e..76a8da2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,24 +15,25 @@ import static edu.wpi.first.units.Units.Meters; -import com.ctre.phoenix.led.CANdle; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -76,7 +77,6 @@ import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -107,12 +107,14 @@ public class RobotContainer { private final SendableChooser autoChooser; private Boolean m_FieldRelative = true; - private Translation2d m_PreviousChassisSpeeds = new Translation2d(); - private Double m_PreviousTime = 0.0; - private Double m_PreviousAngle = 0.0; - private static final LoggedNetworkNumber latency = new LoggedNetworkNumber("latency", 0); - private CANdle m_CANdle = new CANdle(1); + // private Translation2d m_PreviousChassisSpeeds = new Translation2d(); + // private Double m_PreviousTime = 0.0; + // private Double m_PreviousAngle = 0.0; + // private static final LoggedNetworkNumber latency = new LoggedNetworkNumber("latency", 0); + // private CANdle m_CANdle = new CANdle(1); public LinearFilter odometryAverageFilterAngle = LinearFilter.movingAverage(9); + private Alliance m_alliance = Alliance.Red; + // private Boolean m_intaking = false; // private final Command m_testAutoCommand = new TestAuto(drive); @@ -209,6 +211,7 @@ public RobotContainer() { NamedCommands.registerCommand("ElevatorDown", autoL1()); NamedCommands.registerCommand("1stPieceUp", autoL41stPiece()); NamedCommands.registerCommand("1stPieceUpFront", autoL4Piece1FrontFace()); + NamedCommands.registerCommand("1stPieceUpFrontCopy", autoL4Piece1FrontFaceCopy()); NamedCommands.registerCommand("ScoreNoDelay", autoScoreNoDelay()); NamedCommands.registerCommand("5PieceScore", autoScoreFor5Piece()); NamedCommands.registerCommand("HoldPiece", autoGamePieceHold()); @@ -216,6 +219,10 @@ public RobotContainer() { NamedCommands.registerCommand("autoL3DeAlgeafy", autoL3DeAlgeafy()); NamedCommands.registerCommand("ReefL3", autoL3()); NamedCommands.registerCommand("autoBallDrop", autoBallDrop()); + NamedCommands.registerCommand("Processor4Piece1", processorAutoPiece1()); + NamedCommands.registerCommand("Processor4Piece2", processorAutoPiece2()); + NamedCommands.registerCommand("Processor4Piece3", processorAutoPiece3()); + NamedCommands.registerCommand("Processor4Piece4", processorAutoPiece4()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -283,8 +290,8 @@ private void configureButtonBindings() { driveController.povRight().whileTrue(Limelight.limelightDrive(drive)); driveController - .leftBumper() - .and(driveController.a()) + .leftStick() + // .and(driveController.a()) .onTrue( Commands.sequence( humanCmd(), @@ -295,32 +302,32 @@ private void configureButtonBindings() { // driveController.leftBumper().and(driveController.a()).onFalse(climber.climbCmd(0)); driveController - .leftBumper() - .and(driveController.b()) + .rightStick() + // .and(driveController.b()) .onTrue( Commands.sequence(humanCmd(), intake.intakeDownCmd(), climber.climbCmd(5)) .withName("ClimberUp")); driveController - .leftBumper() - .and(driveController.b()) + .rightStick() + // .and(driveController.b()) .onFalse(climber.climbCmd(0).withName("ClimberStop")); driveController .rightBumper() - .and(feeder.hasCoral().negate()) + .and(feeder.beambreakTrigger().negate()) .onTrue(intakeUpAndOff().withName("IntakeUp")); driveController.rightTrigger().onTrue(intakeDownAndOn().withName("IntakeDown")); driveController .a() - .and(driveController.leftBumper().negate()) + .and(driveController.leftStick().negate()) .onTrue(driveSetFieldRelativeCommand(true).ignoringDisable(true).withName("FieldRelative")); driveController .b() - .and(driveController.leftBumper().negate()) + .and(driveController.leftStick().negate()) .onTrue( driveSetFieldRelativeCommand(false).ignoringDisable(true).withName("RobotRelative")); @@ -331,8 +338,11 @@ private void configureButtonBindings() { intake .setIntakeRollerVoltsCmd(0) .alongWith(feeder.setFeederRollerVoltsCmd(0)) + .alongWith(feeder.feederZeroCmd()) .withName("IntakeStop")); + driveController.leftBumper().onTrue(shooter.setShooterVelocityCmd(100)); + // Zero Gyro Heading driveController .x() @@ -370,7 +380,7 @@ private void configureButtonBindings() { // Zerointake driveController - .back() + .start() .onTrue(intake.zeroIntakePivot().ignoringDisable(true).withName("ZeroIntake")); // Drives Normally But Angles Robot To Closest Reef Face @@ -399,10 +409,23 @@ private void configureButtonBindings() { new WaitUntilCommand(slide::isElevatorSafeToMoveDown), elevator.setM_ElevatorHeightCmd(0), elevator.setM_ElevatorAlignCmd(false), - elevator.HomeCmd(), - new WaitUntilCommand(slide::isAlignedWithFeedersmallerTolerance), - slide.voltageOutCommand(0.0), - angulator.angulatorPosStateCmd(0)) + new ParallelCommandGroup( + elevator.HomeCmd(), + new SequentialCommandGroup( + new WaitUntilCommand(slide::isElevatorSafeToMoveDown), + new WaitUntilCommand( + () -> elevator.getElevatorPosition().magnitude() < 0.35), + angulator.angulatorPosStateCmd(0))), + + // elevator.HomeCmd() + // .alongWith( + // new WaitUntilCommand(slide::isAlignedWithFeedersmallerTolerance) + // .andThen( + // new WaitUntilCommand( + // () -> elevator.getElevatorPosition().magnitude() < + // 0.2))), + new WaitUntilCommand(slide::isAlignedWithFeederSmallerTolerance), + slide.voltageOutCommand(0.0)) .withName("ElevatorDown")); // Commands Elevator While in Ball Mode, Cancels Slide and Elevator Adjust, but Keeps Shooter @@ -422,7 +445,7 @@ private void configureButtonBindings() { elevator.setM_ElevatorHeightCmd(0), elevator.setM_ElevatorAlignCmd(false), elevator.HomeCmd(), - new WaitUntilCommand(slide::isAlignedWithFeedersmallerTolerance), + new WaitUntilCommand(slide::isAlignedWithFeederSmallerTolerance), slide.voltageOutCommand(0.0), angulator.angulatorPosStateCmd(0)) .withName("ElevatorDownBallMode")); @@ -456,6 +479,19 @@ private void configureButtonBindings() { new WaitUntilCommand(slide::isSlideCloseToTarget), angulator.angulatorPosStateCmd(7)) .withName("GroundAlgaePickUp")); + // operatorController + // .leftStick() + // .onTrue( + // Commands.sequence( + // humanCmd(), + // angulator.angulatorPosStateCmd(4), + // new WaitUntilCommand(angulator::isAngulatorSafeToMove), + // slide.setSlidePositionCmd(Constants.kCenterPos), + // new WaitUntilCommand(slide::isSlideCloseToTarget), + // elevator.setM_ElevatorAlignCmd(8), + + // ) + // ) // Commands Elevator to L2 Coral Height, Slide and Elevator Auto Adjust operatorController @@ -463,16 +499,19 @@ private void configureButtonBindings() { .and(operatorController.leftBumper().negate()) .and(operatorController.back().negate()) .and(operatorController.start().negate()) - .and(feeder.hasCoral().negate()) + .and(feeder.hasCoral().negate().debounce(0.01)) + .and(feeder.beambreakTrigger().negate().debounce(0.01)) .onTrue( Commands.sequence( humanCmd(), + elevator.setHeightAlign(false), slide.slideAlignCmd(true), angulator.angulatorPosStateCmd(4), new WaitUntilCommand(angulator::isAngulatorSafeToMove), elevator.setM_ElevatorHeightCmd(1), elevator.setM_ElevatorAlignCmd(true), - new WaitUntilCommand(elevator::isElevatorClose), + // new WaitUntilCommand(elevator::isElevatorClose), + new WaitUntilCommand(elevator::canAngulatorAngle), angulator.angulatorPosStateCmd(1)) .withName("ElevatorL2")); @@ -520,16 +559,19 @@ private void configureButtonBindings() { .and(operatorController.leftBumper().negate()) .and(operatorController.back().negate()) .and(operatorController.start().negate()) - .and(feeder.hasCoral().negate()) + .and(feeder.hasCoral().negate().debounce(0.01)) + .and(feeder.beambreakTrigger().negate().debounce(0.01)) .onTrue( Commands.sequence( humanCmd(), + elevator.setHeightAlign(false), slide.slideAlignCmd(true), angulator.angulatorPosStateCmd(4), new WaitUntilCommand(angulator::isAngulatorSafeToMove), elevator.setM_ElevatorHeightCmd(2), elevator.setM_ElevatorAlignCmd(true), - new WaitUntilCommand(elevator::isElevatorClose), + // new WaitUntilCommand(elevator::isElevatorClose), + new WaitUntilCommand(elevator::canAngulatorAngle), angulator.angulatorPosStateCmd(1)) .withName("ElevatorL3")); @@ -575,16 +617,23 @@ private void configureButtonBindings() { operatorController .y() .and(operatorController.leftBumper().negate()) - .and(feeder.hasCoral().negate()) + .and(feeder.hasCoral().negate().debounce(0.01)) + .and(feeder.beambreakTrigger().negate().debounce(0.01)) .onTrue( Commands.sequence( - humanCmd(), - slide.slideAlignCmd(true), - angulator.angulatorPosStateCmd(4), + humanCmd() + .alongWith( + elevator + .setHeightAlign(false) + .alongWith( + slide + .slideAlignCmd(true) + .alongWith(angulator.angulatorPosStateCmd(4)))), new WaitUntilCommand(angulator::isAngulatorSafeToMove), - elevator.setM_ElevatorHeightCmd(3), - elevator.setM_ElevatorAlignCmd(true), - new WaitUntilCommand(elevator::isElevatorClose), + elevator + .setM_ElevatorHeightCmd(3) + .alongWith(elevator.setM_ElevatorAlignCmd(true)), + new WaitUntilCommand(elevator::isElevatorCloseEnoughToAngle), angulator.angulatorPosStateCmd(2)) .withName("ElevatorL4")); @@ -601,7 +650,7 @@ private void configureButtonBindings() { slide.setSlidePositionCmd(Meters.of(0.0)), elevator.setM_ElevatorHeightCmd(6), elevator.setM_ElevatorAlignCmd(true), - shooter.setShooterVelocityCmd(-55), + // shooter.setShooterVelocityCmd(-55), new WaitUntilCommand(elevator::isElevatorCloseAlgae), angulator.angulatorPosStateCmd(6)) .withName("ElevatorBarge")); @@ -609,16 +658,38 @@ private void configureButtonBindings() { // Commands Elevator to High Trough Score Height, No Slide or Elevator Auto Adjust operatorController .back() - // .and(operatorController.x()) + .and(operatorController.leftBumper().negate()) .and(feeder.hasCoral().negate()) - .onTrue( - Commands.sequence( - humanCmd(), - elevator.setM_ElevatorHeightCmd(8), - elevator.setM_ElevatorAlignCmd(true), - angulator.angulatorPosStateCmd(8), - slide.slideAlignCmd(false)) - .withName("ElevatorTroughScoreHeight2")); + .and(feeder.beambreakTrigger().negate()) + .onTrue(troughModeCmd().withName("ElevatorTroughModeCmd")); + + operatorController.back().and(operatorController.leftBumper()).onTrue(troughMode1Cmd()); + + // operatorController + // .back() + // .and(elevatorTroughPosTrigger().negate()) + // .and(elevatorTrough2PosTrigger().negate()) + // .onTrue( + // Commands.sequence( + // humanCmd(), + // elevator.setM_ElevatorHeightCmd(7), + // elevator.setM_ElevatorAlignCmd(true), + // angulator.angulatorPosStateCmd(5), + // slide.slideAlignCmd(false)) + // .withName("ElevatorTroughScoreHeight")); + + // operatorController + // .back() + // .and(elevatorTroughPosTrigger().debounce(0.25)) + // .and(feeder.hasCoral().negate()) + // .onTrue( + // Commands.sequence( + // humanCmd(), + // elevator.setM_ElevatorHeightCmd(8), + // elevator.setM_ElevatorAlignCmd(true), + // angulator.angulatorPosStateCmd(8), + // slide.slideAlignCmd(false)) + // .withName("ElevatorTrough2ScoreHeight")); // Commands Elevator to Low Trough Score Height, No Slide or Elevator Auto Adjust // operatorController @@ -676,15 +747,40 @@ private void configureButtonBindings() { // Trough Score operatorController .rightBumper() - .and( - elevatorHomePosTrigger().or(elevatorTroughPosTrigger()).or(elevatorTrough2PosTrigger())) + .and(elevatorTroughPosTrigger()) .and(operatorController.leftBumper().negate()) .onTrue( - shooter - .setShooterVelocityCmd(Constants.kL1ShooterVelo) - .alongWith(shooter.setHasCoralCmd(false)) + Commands.sequence( + shooter.setShooterVelocityCmd(Constants.kL1ShooterVelo), + new WaitCommand(0.10), + shooter.setShooterVelocityCmd(Constants.kL1PostSlideShooterVelo), + slide + .setSlidePositionCmd(Meters.of(0.04)) + .alongWith(shooter.setHasCoralCmd(false))) .withName("TroughScore")); + operatorController + .rightBumper() + .and(elevatorTrough2PosTrigger()) + .and(operatorController.leftBumper().negate()) + .onTrue( + Commands.sequence( + shooter.setShooterVelocityCmd(Constants.kL1ShooterVelo), + new WaitCommand(0.10), + shooter.setShooterVelocityCmd(Constants.kL1PostSlideShooterVelo), + slide + .setSlidePositionCmd(Meters.of(0.04)) + .alongWith(shooter.setHasCoralCmd(false))) + .withName("Trough2Score")); + + operatorController + .rightBumper() + .and(elevatorHomePosTrigger()) + .onTrue( + shooter + .setShooterVelocityCmd(Constants.kStowShooterVelo) + .alongWith(shooter.setHasCoralCmd(false))); + // L2 and L3 Score operatorController .rightBumper() @@ -743,12 +839,24 @@ private void configureButtonBindings() { operatorController .leftBumper() - .onTrue(shooter.setShooterVelocityCmd(-70).withName("AlgaePickup")); + .onTrue( + shooter + .setShooterVelocityCmd(-100) + .withName("AlgaePickup") + .alongWith( + Commands.runOnce( + () -> operatorController.setRumble(RumbleType.kBothRumble, 0.3)))); // Score In Barge operatorController .leftBumper() - .onFalse(shooter.setShooterVelocityCmd(100).withName("BargeScore")); + .onFalse( + shooter + .setShooterVelocityCmd(0) + .withName("BargeScore") + .alongWith( + Commands.runOnce( + () -> operatorController.setRumble(RumbleType.kBothRumble, 0.0)))); // Stop Shooter When Right Bumper is Released operatorController @@ -849,37 +957,80 @@ private void configureTriggerBindings() { slide .isAlignedWithFeederTrigger() .and(elevator.isAlignedWithFeederTrigger()) - .and(feeder.hasCoral()) + .and(feeder.beambreakTrigger()) // .hasCoral()) .and(angulator.isAlignedWithFeederTrigger()) + .and(elevatorHomePosTrigger()) + .and(angulator.angulatorHomePosStateTrigger()) // added to make sure elevator setpoint AND + // position are at + // zero, when feeding past beambreak .onTrue( - Commands.sequence(feeder.setFeederRollerVoltsCmd(4), shooter.setShooterVelocityCmd(10)) + feeder + .setFeederVelocityCmd(40) + .alongWith(shooter.setShooterVelocityCmd(50)) .withName("startMovingCoralToShooter")); // shooter.setShooterVoltageCmd(1.5))); - // Turn feeder off and intake off + // Turn feeder off and intake off if something isn't aligned properly feeder - .hasCoral() + // .hasCoral() + .beambreakTrigger() .and( slide .isAlignedWithFeederTrigger() .negate() .or(elevator.isAlignedWithFeederTrigger().negate()) - .or(angulator.isAlignedWithFeederTrigger().negate())) - .onTrue(feeder.setFeederPosZero().withName("stopFeederIfAnythingIsntAligned")); + .or(angulator.isAlignedWithFeederTrigger().negate()) + .or(elevatorHomePosTrigger().negate()) + .or(angulator.angulatorHomePosStateTrigger().negate())) + .onTrue( + Commands.sequence( + // feeder.setFeederPosZero(), + feeder.setFeederVelocityCmd(-15), + new WaitUntilCommand(feeder.hasCoral().negate()), + feeder.setFeederRollerVoltsCmd(0)) + .withName("stopFeederIfAnythingIsntAligned")); feeder - .hasCoral() + // .hasCoral() + .beambreakTrigger() .onTrue(intake.setIntakeRollerVoltsCmd(0).withName("finishedIntakingGamePiece")); // Turn off feeder and shooter once coral moves from feeder to shooter feeder .hasCoral() + .and(feeder.beambreakTrigger()) + .and(operatorController.leftBumper().negate()) .onFalse( - Commands.sequence( - feeder.setFeederRollerVoltsCmd(0), - shooter.positionVoltageNoMovement(), // setShooterVoltageCmd(0) - shooter.setHasCoralCmd(true)) - .withName("finishedMovingCoralToShooter")); + // Commands.sequence( + feeder + .setFeederRollerVoltsCmd(0) + .alongWith( + feeder + .feederZeroCmd() + .alongWith(shooter.positionVoltageNoMovement()) + .alongWith(shooter.setHasCoralCmd(true))) // setShooterVoltageCmd(0) + // humanCmd(), + // elevator.setM_ElevatorHeightCmd(7), + // elevator.setM_ElevatorAlignCmd(true), + // angulator.angulatorPosStateCmd(5), + // slide.slideAlignCmd(false)) + // intaking(false)) + .withName("finishedMovingCoralToShooterTele")); + // feeder + // .hasCoral() + // .and(autoEnabled().negate()) + // .onFalse( + // Commands.sequence( + // feeder + // .setFeederRollerVoltsCmd(0) + // .alongWith( + // feeder + // .feederZeroCmd() + // .alongWith( + // shooter + // .positionVoltageNoMovement() + // .alongWith(shooter.setHasCoralCmd(true))))) + // .withName("finishedMovingCoralToShooterAuto")); // Align for Coral scoring // elevator @@ -897,6 +1048,15 @@ private void configureTriggerBindings() { // .slideAlignCmd(false) // .andThen(slide.setSlidePositionCmd(Constants.kAlgaeSlidePosition))); + coralBetweenCoralAndReefTrigger() + .and(robotNotMovingTrigger()) + .debounce(0.05) + .onTrue( + elevator + .setHeightAlign(true) + .alongWith(new WaitCommand(0.1)) + .withName("EnableElevatorHeightTracking")); + // // Feed position when down. elevator .isAngulatorSafeToMoveBackTrigger() @@ -916,25 +1076,25 @@ private void configureTriggerBindings() { // slide.setSlidePositionCmd(Constants.kSlideFeederPosition))); Trigger autoFireTriggerL4 = elevator - .isElevatorCloseTrigger() + .isElevatorReadyToScore() .and(elevatorL4PosTrigger()) .and(slide.isSlideReadyToScoreTrigger()) .and(angulator.readyToScoreTrigger()) .and(seesTarget()) .and(elevator.peggedTrigger.negate()) .and(shooter.isShooterVelocityLowEnoughToScore()) - .and(slide.angleTooLargeTrigger()) + .and(vision.angleTooLargeTrigger) .and(shooter.isShooterZAccelLowEnough()) - // .and(drive.isDriveAccelLowEnoughToScore().debounce(0.05)) // was 0.25 - .and(robotMovingTrigger()); // was 0.06 + .and(drive.isDriveAccelLowEnoughToScore()) // .debounce(0.05)) // was 0.25 + .and(robotNotMovingTrigger()); // was 0.06 Trigger autoFireTriggerL2and3 = elevator - .isElevatorCloseTrigger() + .isElevatorReadyToScore() .and(slide.isSlideReadyToScoreTrigger()) .and(angulator.readyToScoreTrigger()) .and(seesTarget()) - .and(robotMovingTrigger()) + .and(robotNotMovingTrigger()) .and(elevatorL2PosTrigger().or(elevatorL3PosTrigger())); operatorController @@ -956,6 +1116,17 @@ private void configureTriggerBindings() { .setShooterVelocityCmd(Constants.kL2andL3ShooterVelo) .andThen(shooter.setHasCoralCmd(false)) .andThen(new WaitCommand(0.1).withName("autoScoreL2and3"))); + + // operatorController + // .leftTrigger() + // .onTrue(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0.3))) + // .onFalse(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0))); + + // operatorController + // .rightTrigger() + // .onTrue(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0.3))) + // .onFalse(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0))); + } public double scaleAxis(double input) { @@ -982,18 +1153,19 @@ public double scaleAxis(double input) { } private final Command intakeDownAndOn() { - return intake.intakeOnCmd().andThen(feeder.setFeederRollerVoltsCmd(1.9)); + return intake.intakeOnCmd().andThen(feeder.setFeederVelocityCmd(40)); } private final Command intakeUpAndOff() { return intake .setIntakePivotPositionCmd(0) .alongWith(intake.setIntakeRollerVoltsCmd(0)) - .alongWith(feeder.setFeederRollerVoltsCmd(0)); + .alongWith(feeder.setFeederRollerVoltsCmd(0)) + .alongWith(feeder.feederZeroCmd()); } private final Command intakeBarfCmd() { - return intake.intakeBarfCommand().andThen(feeder.setFeederRollerVoltsCmd(-2)); + return intake.intakeBarfCommand().andThen(feeder.setFeederVelocityCmd(-18)); } private final Command driveSetFieldRelativeCommand(Boolean fieldRelative) { @@ -1003,6 +1175,11 @@ private final Command driveSetFieldRelativeCommand(Boolean fieldRelative) { }); } + private final Trigger coralBetweenCoralAndReefTrigger() { + return new Trigger( + () -> slide.coralToReefBranch() < Constants.kElevatorHeightMaxTrackingDistance.magnitude()); + } + private final Boolean elevatorHomePos() { return elevator.m_elevatorHeight == 0; } @@ -1069,6 +1246,41 @@ public final Trigger seesTarget() { () -> LimelightHelpers.getTV("limelight-fl") || LimelightHelpers.getTV("limelight-fr")); } + public Command troughModeCmd() { + return new ConditionalCommand(troughMode2Cmd(), troughMode1Cmd(), () -> elevatorTroughPos()); + } + + public final Command troughMode1Cmd() { + return Commands.sequence( + humanCmd(), + elevator.setM_ElevatorHeightCmd(7), + elevator.setM_ElevatorAlignCmd(true), + angulator.angulatorPosStateCmd(5), + slide.slideAlignCmd(false)); + } + + public final Command troughMode2Cmd() { + return Commands.sequence( + humanCmd(), + elevator.setM_ElevatorHeightCmd(8), + elevator.setM_ElevatorAlignCmd(true), + angulator.angulatorPosStateCmd(8), + slide.slideAlignCmd(false)); + } + + public final Trigger autoEnabled() { + return new Trigger(() -> DriverStation.isAutonomousEnabled() == true); + } + + // public final Command intaking(Boolean yes) { + // return Commands.runOnce(() -> m_intaking = yes); + // } + + // public final Trigger hasCoral() { + // return new Trigger( + // () -> feeder.beambreakTrigger().getAsBoolean() == true); // m_intaking == true && ); + // } + private final Command autoGamePieceHold() { return shooter.positionVoltageNoMovement(); } @@ -1100,6 +1312,15 @@ private final Command autoL4Piece1FrontFace() { slide.setSlidePositionCmd(Meters.of(0.31))); } + private final Command autoL4Piece1FrontFaceCopy() { + return Commands.sequence( + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.0))); + } + private final Command autoL4Piece2() { return Commands.sequence( new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), @@ -1155,6 +1376,45 @@ private final Command autoL1() { angulator.angulatorPosStateCmd(0)); } + private final Command processorAutoPiece1() { + return Commands.sequence( + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.27))); + } + + private final Command processorAutoPiece2() { + return Commands.sequence( + new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.03))); + } + + private final Command processorAutoPiece3() { + return Commands.sequence( + new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.21))); + } + + private final Command processorAutoPiece4() { + return Commands.sequence( + new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.25))); + } + private final Command autoScoreFor3Piece() { return new SequentialCommandGroup( new WaitUntilCommand( @@ -1172,12 +1432,12 @@ private final Command autoScoreFor3Piece() { private final Command autoScoreFor5Piece() { return new SequentialCommandGroup( - new WaitUntilCommand( - () -> - slide.isSlideReadyToScore() - && elevator.isElevatorClose() - && angulator.readyToScore()) - .withTimeout(0.1), + // new WaitUntilCommand( + // () -> + // slide.isSlideReadyToScore() + // && elevator.isElevatorClose() + // && angulator.readyToScore()) + // .withTimeout(0.1), shooter.setShooterVelocityCmd(65), // new WaitCommand(0.05), // was 0.25 // shooter.positionVoltageNoMovement(), @@ -1205,8 +1465,7 @@ private final Command autoIntakeDown() { private final Command autoL3DeAlgeafy() { return Commands.sequence( angulator.angulatorPosStateCmd(4), - slide.centerSlideCmd(true), - slide.slideAlignCmd(true), + slide.setSlidePositionCmd(Meters.of(0.06)), new WaitUntilCommand(angulator::isAngulatorSafeToMove), elevator.setM_ElevatorAlignCmd(true), elevator.setM_ElevatorHeightCmd(5), @@ -1232,132 +1491,134 @@ private final Command humanCmd() { return new InstantCommand(() -> {}, dummy); } - private Trigger robotMovingTrigger() { + private Trigger robotNotMovingTrigger() { return new Trigger( () -> (Math.abs(drive.getChassisSpeeds().vxMetersPerSecond) < 0.1) - && Math.abs(drive.getChassisSpeeds().vyMetersPerSecond) < 0.2 + && Math.abs(drive.getChassisSpeeds().vyMetersPerSecond) < 0.1 && Math.abs(drive.getChassisSpeeds().omegaRadiansPerSecond) < 0.1); } - public Pose2d getLatencyCompTarget(double latency) { - Pose2d currentPose = drive.getPose(); - - Rotation2d poseRotation = currentPose.getRotation(); - ChassisSpeeds chassisSpeeds = drive.getChassisSpeeds(); - if (RobotBase.isReal() == false) { - chassisSpeeds = new ChassisSpeeds(1, 0, 0); - } - Translation2d globalVelocity = - new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) - .rotateBy(poseRotation); + // public Pose2d getLatencyCompTarget(double latency) { + // Pose2d currentPose = drive.getPose(); + + // Rotation2d poseRotation = currentPose.getRotation(); + // ChassisSpeeds chassisSpeeds = drive.getChassisSpeeds(); + // if (RobotBase.isReal() == false) { + // chassisSpeeds = new ChassisSpeeds(1, 0, 0); + // } + // Translation2d globalVelocity = + // new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) + // .rotateBy(poseRotation); + + // double angularSpeed = chassisSpeeds.omegaRadiansPerSecond; + + // Integer reefFaceIdx = Align.getClosestReefFaceIndex(currentPose); + + // Pose2d reefFace = Align.getClosestReefFace(reefFaceIdx); + + // if (RobotBase.isReal() == false) { + // reefFace = new Pose2d(5, 6, Rotation2d.fromDegrees(60)); + // } + + // Rotation2d robot_angle_to_reef_face = + // poseRotation.rotateBy(reefFace.getRotation().unaryMinus()); + + // Pose2d robotRelativeToTarget = currentPose.relativeTo(reefFace); + // // Pose2d targetToRobot = + // // currentPose.transformBy( + // // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()).inverse()); + + // Logger.recordOutput("RobotContainer/currentPose", currentPose); // works + // Logger.recordOutput("RobotContainer/TargetToRobot", robotRelativeToTarget); + // Logger.recordOutput("RobotContainer/ClosestReefFace", reefFace); // works + // Logger.recordOutput("RobotContainer/ChassisSpeeds", chassisSpeeds); // works + // Logger.recordOutput("RobotContainer/globalVelocitys", globalVelocity); // works + // Logger.recordOutput("RobotContainer/angularSpeed", angularSpeed); // works + + // double currentTime = Timer.getFPGATimestamp(); + // // double dt = currentTime - m_PreviousTime; + // Logger.recordOutput("RobotContainer/currentTime", currentTime); + + // double dtX = latency; // latency.get(); + // double accelX = (0); + // // targetSpaceVelocity.getX() - m_PreviousChassisSpeeds.getX()); // *dt/; + // // if (accelX < 0) { + // // double stopTimeX = Math.abs(targetSpaceVelocity.getX() / accelX); + // // dtX = Math.min(latency, stopTimeX); + // // } + // double futureX = currentPose.getX() + (globalVelocity.getX() * dtX); + // // + (0.5 * accelX * Math.pow(dtX, 2)); + // futureX = Math.max(0, futureX); + + // double dtY = latency; // latency.get(); + // double accelY = (0); + // // targetSpaceVelocity.getY() - m_PreviousChassisSpeeds.getY()); // *dt; + // // if (accelY < 0) { + // // double stopTimeY = Math.abs(targetSpaceVelocity.getY() / accelY); + // // dtY = Math.min(latency, stopTimeY); + // // } + // double futureY = currentPose.getY() + (globalVelocity.getY() * dtY); + // // + (0.5 * accelY * Math.pow(dtY, 2)); + + // double dtOmega = latency; // latency.get(); + // double accelOmega = (0); + // // angularSpeed - m_PreviousAngle); // * dt; + // // if (accelOmega != 0) { + // // double stopTimeOmega = Math.abs(angularSpeed / accelOmega); + // // dtOmega = Math.min(latency, stopTimeOmega); + // // } + // double futureOmega = poseRotation.getRadians() + (angularSpeed * dtOmega); + // // + (0.5 * accelOmega * Math.pow(dtOmega, 2)); + // m_PreviousAngle = accelOmega; + + // m_PreviousChassisSpeeds = globalVelocity; + + // m_PreviousTime = Timer.getFPGATimestamp(); + + // // futureX = targetToRobot.getX(); + // // futureY = targetToRobot.getY(); + // // futureOmega = targetToRobot.getRotation().getRadians(); + // // Logger.recordOutput("RobotContainer/dt", dt); + // // Logger.recordOutput("RobotContainer/accelX", accelX); + // // Logger.recordOutput("RobotContainer/accelY", accelY); + // // Logger.recordOutput("RobotContainer/accelOmega", accelOmega); + + // Logger.recordOutput("RobotContainer/m_PreviousAngle", m_PreviousAngle); + // Logger.recordOutput("RobotContainer/previousTime", m_PreviousTime); + // Logger.recordOutput("RobotContainer/PreviousChassisSpeeds", m_PreviousChassisSpeeds); + + // Logger.recordOutput("RobotContainer/futureX", futureX); + // Logger.recordOutput("RobotContainer/futureY", futureY); + // Logger.recordOutput("RobotContainer/futureOmega", futureOmega); + + // Logger.recordOutput("RobotContainer/DtX", dtX); + // Logger.recordOutput("RobotContainer/DtY", dtY); + // Logger.recordOutput("RobotContainer/DtOmega", dtOmega); + // Logger.recordOutput("RobotContainer/robot_angle_to_reef_face", robot_angle_to_reef_face); + + // Logger.recordOutput( + // "RobotContainer/futurePose", + // new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega))); + + // Pose2d futurePose2d = + // new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega)); + + // Pose2d futurePose2dTargetSpace = futurePose2d.relativeTo(reefFace); + // // Logger.recordOutput("RobotContainer/pose", futurePose2d); // .transformBy( + // // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()))); + + // Logger.recordOutput("RobotContainer/TargetSpaceY", futurePose2dTargetSpace.getX()); + // Logger.recordOutput("RobotContainer/TargetSpaceX", futurePose2dTargetSpace.getY()); + // Logger.recordOutput("RobotContainer/TargetSpaceOmega", + // futurePose2dTargetSpace.getRotation()); + + // return new Pose2d( + // futurePose2dTargetSpace.getY(), + // -futurePose2dTargetSpace.getX(), + // futurePose2dTargetSpace.getRotation()); + // } - double angularSpeed = chassisSpeeds.omegaRadiansPerSecond; - - Integer reefFaceIdx = Align.getClosestReefFaceIndex(currentPose); - - Pose2d reefFace = Align.getClosestReefFace(reefFaceIdx); - - if (RobotBase.isReal() == false) { - reefFace = new Pose2d(5, 6, Rotation2d.fromDegrees(60)); - } - - Rotation2d robot_angle_to_reef_face = - poseRotation.rotateBy(reefFace.getRotation().unaryMinus()); - - Pose2d robotRelativeToTarget = currentPose.relativeTo(reefFace); - // Pose2d targetToRobot = - // currentPose.transformBy( - // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()).inverse()); - - Logger.recordOutput("RobotContainer/currentPose", currentPose); // works - Logger.recordOutput("RobotContainer/TargetToRobot", robotRelativeToTarget); - Logger.recordOutput("RobotContainer/ClosestReefFace", reefFace); // works - Logger.recordOutput("RobotContainer/ChassisSpeeds", chassisSpeeds); // works - Logger.recordOutput("RobotContainer/globalVelocitys", globalVelocity); // works - Logger.recordOutput("RobotContainer/angularSpeed", angularSpeed); // works - - double currentTime = Timer.getFPGATimestamp(); - // double dt = currentTime - m_PreviousTime; - Logger.recordOutput("RobotContainer/currentTime", currentTime); - - double dtX = latency; // latency.get(); - double accelX = (0); - // targetSpaceVelocity.getX() - m_PreviousChassisSpeeds.getX()); // *dt/; - // if (accelX < 0) { - // double stopTimeX = Math.abs(targetSpaceVelocity.getX() / accelX); - // dtX = Math.min(latency, stopTimeX); - // } - double futureX = currentPose.getX() + (globalVelocity.getX() * dtX); - // + (0.5 * accelX * Math.pow(dtX, 2)); - futureX = Math.max(0, futureX); - - double dtY = latency; // latency.get(); - double accelY = (0); - // targetSpaceVelocity.getY() - m_PreviousChassisSpeeds.getY()); // *dt; - // if (accelY < 0) { - // double stopTimeY = Math.abs(targetSpaceVelocity.getY() / accelY); - // dtY = Math.min(latency, stopTimeY); - // } - double futureY = currentPose.getY() + (globalVelocity.getY() * dtY); - // + (0.5 * accelY * Math.pow(dtY, 2)); - - double dtOmega = latency; // latency.get(); - double accelOmega = (0); - // angularSpeed - m_PreviousAngle); // * dt; - // if (accelOmega != 0) { - // double stopTimeOmega = Math.abs(angularSpeed / accelOmega); - // dtOmega = Math.min(latency, stopTimeOmega); - // } - double futureOmega = poseRotation.getRadians() + (angularSpeed * dtOmega); - // + (0.5 * accelOmega * Math.pow(dtOmega, 2)); - m_PreviousAngle = accelOmega; - - m_PreviousChassisSpeeds = globalVelocity; - - m_PreviousTime = Timer.getFPGATimestamp(); - - // futureX = targetToRobot.getX(); - // futureY = targetToRobot.getY(); - // futureOmega = targetToRobot.getRotation().getRadians(); - // Logger.recordOutput("RobotContainer/dt", dt); - // Logger.recordOutput("RobotContainer/accelX", accelX); - // Logger.recordOutput("RobotContainer/accelY", accelY); - // Logger.recordOutput("RobotContainer/accelOmega", accelOmega); - - Logger.recordOutput("RobotContainer/m_PreviousAngle", m_PreviousAngle); - Logger.recordOutput("RobotContainer/previousTime", m_PreviousTime); - Logger.recordOutput("RobotContainer/PreviousChassisSpeeds", m_PreviousChassisSpeeds); - - Logger.recordOutput("RobotContainer/futureX", futureX); - Logger.recordOutput("RobotContainer/futureY", futureY); - Logger.recordOutput("RobotContainer/futureOmega", futureOmega); - - Logger.recordOutput("RobotContainer/DtX", dtX); - Logger.recordOutput("RobotContainer/DtY", dtY); - Logger.recordOutput("RobotContainer/DtOmega", dtOmega); - Logger.recordOutput("RobotContainer/robot_angle_to_reef_face", robot_angle_to_reef_face); - - Logger.recordOutput( - "RobotContainer/futurePose", - new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega))); - - Pose2d futurePose2d = - new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega)); - - Pose2d futurePose2dTargetSpace = futurePose2d.relativeTo(reefFace); - // Logger.recordOutput("RobotContainer/pose", futurePose2d); // .transformBy( - // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()))); - - Logger.recordOutput("RobotContainer/TargetSpaceY", futurePose2dTargetSpace.getX()); - Logger.recordOutput("RobotContainer/TargetSpaceX", futurePose2dTargetSpace.getY()); - Logger.recordOutput("RobotContainer/TargetSpaceOmega", futurePose2dTargetSpace.getRotation()); - - return new Pose2d( - futurePose2dTargetSpace.getY(), - -futurePose2dTargetSpace.getX(), - futurePose2dTargetSpace.getRotation()); - } // /** // * Use this to pass the autonomous command to the main {@link Robot} class. // * @@ -1372,8 +1633,42 @@ public void periodic() { Logger.recordOutput("RobotContainer/Height1", elevatorL2Pos()); Logger.recordOutput("RobotContainer/Height2", elevatorL3Pos()); Logger.recordOutput("RobotContainer/Height3", elevatorL4Pos()); + Logger.recordOutput("RobotContainer/AngleToGreat", vision.angleTooLargeTrigger); + Logger.recordOutput("RobotContainer/SeesTarget", seesTarget()); + + // Logger.recordOutput("RobotContainer/intakeRunning", m_intaking); + // Logger.recordOutput("RobotContainer/hasCoral", hasCoral()); + + var alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance != m_alliance) { + m_alliance = alliance; + if (m_alliance == Alliance.Blue) { + int[] blueTagIds = {17, 18, 19, 20, 21, 22}; + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", blueTagIds); + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", blueTagIds); + } else { + int[] redTagIds = {6, 7, 8, 9, 10, 11}; + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", redTagIds); + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", redTagIds); + } + } + + if (slide.isSlideReadyToScore() && vision.angleTooLargeTrigger.getAsBoolean() + || (!operatorController.leftTrigger().getAsBoolean() + || !operatorController.rightTrigger().getAsBoolean())) { + driveController.setRumble(RumbleType.kBothRumble, 0.0); + } + + if ((!slide.isSlideReadyToScore() + || !vision.angleTooLargeTrigger.getAsBoolean() + || elevator.m_pegged == true) + && (operatorController.leftTrigger().getAsBoolean() + || operatorController.rightTrigger().getAsBoolean())) { + driveController.setRumble(RumbleType.kBothRumble, 0.3); + } // Boolean doRejectUpdate = false; + // LimelightHelpers.PoseEstimate mt1 = // LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-fl"); @@ -1438,17 +1733,19 @@ public void periodic() { double tipOfCoralToBranch = slide.coralToReefBranch(); - Pose2d llPose2dElevator = elevator.getPoseFromLL(); - Pose2d llPose2dSlide = slide.getPoseFromLL(); - double canRangeAngle = elevator.getCANrangeAngle(); + // Pose2d llPose2dElevator = elevator.getPoseFromLL(); + // Pose2d llPose2dSlide = slide.getPoseFromLL(); + // double canRangeAngle = elevator.getCANrangeAngle(); + + Pose2d blendedPose = vision.getBlendedRobotPoseInTarget(); // elevator.moveElevator(latencyCompensatedTargetElevator); - elevator.moveElevator(llPose2dSlide, tipOfCoralToBranch); + elevator.moveElevator(blendedPose, tipOfCoralToBranch); // Logger.recordOutput("RobotContainer/LatencyTargetElevator", // latencyCompensatedTargetElevator); // Logger.recordOutput("RobotContainer/LatencyTargetSlide", latencyCompensatedTargetSlide); slide.commonSlideMove( - llPose2dSlide, elevator.elevatorPos(), averaged_robot_angle_to_reef_face.getDegrees()); + blendedPose, elevator.elevatorPos(), averaged_robot_angle_to_reef_face.getDegrees()); } catch (Exception exception) { } @@ -1457,36 +1754,6 @@ public void periodic() { // slide.alignWithReefLatencyCompensated( // drive.getPose(), latencyCompensatedTargetSlide, elevator.elevatorPos()); angulator.setAngulatorPosState(); - // if ((elevator - // .isElevatorCloseTrigger() - // .and(slide.isSlideReadyToScoreTrigger()) - // .and(angulator.readyToScoreTrigger()) - // .and(seesTarget()) - // .and(shooter.isShooterVelocityLowEnoughToScore()) - // .and(elevator.peggedTrigger.negate()) - // .and(robotMovingTrigger()) - // .debounce(0.05)) - // .getAsBoolean()) { - // m_CANdle.setLEDs(255, 255, 255); - // } else if ((elevator - // .isElevatorCloseTrigger() - // .and(slide.isSlideReadyToScoreTrigger()) - // .and(angulator.readyToScoreTrigger()) - // .and(seesTarget()) - // .and(elevator.peggedTrigger.negate()) - // .and(shooter.isShooterVelocityLowEnoughToScore())) - // .and(robotMovingTrigger()) - // .getAsBoolean()) { - // m_CANdle.setLEDs(255, 165, 0); - // } else { - // m_CANdle.setLEDs(255, 0, 0); - // } - - // if (slide.isSlideReadyToScore() && elevator.isElevatorClose() && angulator.readyToScore()) { - // m_CANdle.setLEDs(255, 255, 255); - // } else { - // m_CANdle.setLEDs(255, 0, 0); - // } - Logger.recordOutput("RobotContainer/RobotMoving", robotMovingTrigger()); + Logger.recordOutput("RobotContainer/RobotMoving", robotNotMovingTrigger()); } } diff --git a/src/main/java/frc/robot/subsystems/angulator/Angulator.java b/src/main/java/frc/robot/subsystems/angulator/Angulator.java index 17954a3..d242108 100644 --- a/src/main/java/frc/robot/subsystems/angulator/Angulator.java +++ b/src/main/java/frc/robot/subsystems/angulator/Angulator.java @@ -29,6 +29,7 @@ public void periodic() { Logger.recordOutput("Angulator/APosState", m_AngulatorPosState); Logger.recordOutput("Angulator/AScoreReady", readyToScore()); Logger.recordOutput("Angulator/AFeederReady", isAlignedWithFeeder()); + Logger.recordOutput("Angulator/AHomeState", angulatorHomePosStateTrigger()); } public Command setAngulatorPositionCmd(Double angle) { @@ -122,4 +123,8 @@ public Command angulatorVoltageCmd(double volts, boolean overrideLimits) { public Command angulatorZeroCmd() { return m_AngulatorIO.angulatorZeroCmd(); } + + public Trigger angulatorHomePosStateTrigger() { + return new Trigger(() -> m_AngulatorPosState == 0); + } } diff --git a/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java index 67d7110..b5042a8 100644 --- a/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java @@ -34,7 +34,7 @@ import org.littletonrobotics.junction.Logger; public class AngulatorIOTalonFX implements AngulatorIO { - private final TalonFX m_angulatorTalonFX = new TalonFX(Constants.kAngulatorCANID); + private final TalonFX m_angulatorTalonFX = new TalonFX(Constants.kAngulatorCANID, "2481"); private final MotionMagicVoltage m_MotionMagicVoltage = new MotionMagicVoltage(0); private final VoltageOut m_VoltageOut = new VoltageOut(0); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 2832c61..2c570d3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -238,7 +238,7 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.01); + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.0142); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts); @@ -372,7 +372,7 @@ public double getMaxAngularSpeedRadPerSec() { public Trigger isDriveAccelLowEnoughToScore() { return new Trigger( - () -> Math.abs(gyroInputs.accelX) < 0.4 && Math.abs(gyroInputs.accelY) < 0.4); + () -> Math.abs(gyroInputs.accelX) < 0.3 && Math.abs(gyroInputs.accelY) < 0.3); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index a5ad496..194747b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,6 +35,8 @@ public class Elevator extends SubsystemBase { public ElevatorOverride m_branchOverride = ElevatorOverride.AUTO; public Boolean m_pegged = false; public final Trigger peggedTrigger = new Trigger(() -> m_pegged); + public Boolean m_setHeightAlign = true; + public double m_angleSetpoint = 0.0; public LinearFilter canRangeAverageFilterAngle = LinearFilter.movingAverage(9); @@ -58,14 +60,15 @@ public void periodic() { Logger.recordOutput("Elevator/RobotAngleToReefFace", m_robotAngleToReefFace); Logger.recordOutput("Elevator/EHeightOffset", m_heightDistanceOffset); Logger.recordOutput("Elevator/EAlgaeReady", isElevatorCloseAlgae()); - Logger.recordOutput("Elevator/EScoreReady", isElevatorClose()); + Logger.recordOutput("Elevator/EScoreReady", isElevatorReadyToScore()); Logger.recordOutput("RobotContainer/EBranchOverride", m_branchOverride); - Logger.recordOutput( - "Elevator/AngleFromCANrange", - canRangeAverageFilterAngle.calculate( - Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256)))); - Logger.recordOutput( - "Elevator/CANRangeDistance", -(inputs.CANrange1Dis + inputs.CANrange2Dis) / 2.0); + Logger.recordOutput("Elevator/elevatorHeight", m_elevatorHeight); + // Logger.recordOutput( + // "Elevator/AngleFromCANrange", + // canRangeAverageFilterAngle.calculate( + // Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256)))); + // Logger.recordOutput( + // "Elevator/CANRangeDistance", -(inputs.CANrange1Dis + inputs.CANrange2Dis) / 2.0); Logger.recordOutput("Elevator/EMaxOffset", m_pegged == false); } @@ -208,6 +211,10 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { m_heightDistanceOffset = m_heightDistanceOffset.times(Constants.kElevatorL23FudgeFactor); } + // if (m_setHeightAlign == false) { + // m_heightDistanceOffset = Meters.of(0.0); + // } + if (DriverStation.isAutonomousEnabled()) { m_heightDistanceOffset = Meters.of(0.0063); } @@ -216,25 +223,48 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { // Meters.of(m_heightOffsetFilter.calculate(m_heightDistanceOffset.magnitude())); if (m_elevatorHeight == 1) { + if (m_setHeightAlign == false) { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2); + m_elevatorSetpoint = Constants.kReefL2.plus(m_heightDistanceOffset); + } else { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2.plus(m_heightDistanceOffset)); + m_elevatorSetpoint = Constants.kReefL2.plus(m_heightDistanceOffset); + } m_pegged = false; - m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2.plus(m_heightDistanceOffset)); } if (m_elevatorHeight == 2) { + // if(DriverStation.isAutonomousEnabled()) { + // m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)) + // } + if (m_setHeightAlign == false) { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3); + m_elevatorSetpoint = Constants.kReefL3.plus(m_heightDistanceOffset); + } else { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)); + m_elevatorSetpoint = Constants.kReefL3.plus(m_heightDistanceOffset); + } m_pegged = false; - m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)); } if (m_elevatorHeight == 3) { - if ((Constants.kReefL4.plus(m_heightDistanceOffset)).magnitude() - > (Constants.kElevatorForwardSoftLimitThreshold)) { - m_pegged = true; - m_ElevatorIO.setElevatorPositionMeters( - Meters.of(Constants.kElevatorForwardSoftLimitThreshold)); - m_elevatorSetpoint = - Meters.of(Constants.kElevatorForwardSoftLimitThreshold).minus(m_heightDistanceOffset); - } else { - m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4.plus(m_heightDistanceOffset)); + if (DriverStation.isAutonomousEnabled()) { + m_ElevatorIO.setElevatorPositionMeters(Meters.of(1.6102)); // ReefL4 + 0.0127 + } else if (m_setHeightAlign == false) { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4); m_elevatorSetpoint = Constants.kReefL4; m_pegged = false; + } else { + if ((Constants.kReefL4.plus(m_heightDistanceOffset)).magnitude() + > (Constants.kElevatorForwardSoftLimitThreshold)) { + m_pegged = true; + m_ElevatorIO.setElevatorPositionMeters( + Meters.of(Constants.kElevatorForwardSoftLimitThreshold)); + m_elevatorSetpoint = Meters.of(Constants.kElevatorForwardSoftLimitThreshold); + // .minus(m_heightDistanceOffset); + } else { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4.plus(m_heightDistanceOffset)); + m_elevatorSetpoint = Constants.kReefL4.plus(m_heightDistanceOffset); + m_pegged = false; + } } // m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4.plus(m_heightDistanceOffset)); @@ -266,13 +296,16 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { public void setM_ElevatorHeight(int num) { m_elevatorHeight = num; if (m_elevatorHeight == 1) { - m_elevatorSetpoint = Constants.kReefL2; + m_angleSetpoint = Constants.kReefL2.magnitude(); + // m_elevatorSetpoint = Constants.kReefL2; } if (m_elevatorHeight == 2) { - m_elevatorSetpoint = Constants.kReefL3; + m_angleSetpoint = Constants.kReefL3.magnitude(); + // m_elevatorSetpoint = Constants.kReefL3; } if (m_elevatorHeight == 3) { - m_elevatorSetpoint = Constants.kReefL4; + m_angleSetpoint = Constants.kReefL4.magnitude(); + // m_elevatorSetpoint = Constants.kReefL4; } if (m_elevatorHeight == 4) { m_elevatorSetpoint = Constants.kDeAlgaeL2; @@ -291,6 +324,10 @@ public void setM_ElevatorHeight(int num) { } } + public Command setHeightAlign(boolean yes) { + return Commands.runOnce(() -> m_setHeightAlign = yes); + } + public Command setM_ElevatorHeightCmd(int num) { return Commands.runOnce(() -> setM_ElevatorHeight(num)); } @@ -346,11 +383,29 @@ public Boolean isAlignedWithFeeder() { public Boolean isElevatorClose() { return MathUtil.isNear( - m_elevatorSetpoint.plus(m_heightDistanceOffset).magnitude(), + m_elevatorSetpoint.magnitude(), inputs.masterPosition.magnitude(), Constants.kElevatorClosetoTarget); } + public Boolean canAngulatorAngle() { + return MathUtil.isNear( + m_angleSetpoint, inputs.masterPosition.magnitude(), Constants.kElevatorClosetoTarget); + } + + public Boolean isElevatorCloseEnoughToAngle() { + return MathUtil.isNear(m_elevatorSetpoint.magnitude(), inputs.masterPosition.magnitude(), 0.75); + } + + public Trigger isElevatorReadyToScore() { + return new Trigger( + () -> + MathUtil.isNear( + m_elevatorSetpoint.magnitude(), // plus(m_heightDistanceOffset).magnitude(), + inputs.masterPosition.magnitude(), + Constants.kElevatorClosetoTarget)); + } + public Boolean isElevatorCloseAlgae() { return MathUtil.isNear( m_elevatorSetpoint.magnitude(), @@ -382,10 +437,10 @@ public Distance getElevatorPosition() { return inputs.masterPosition; } - public double getCANrangeAngle() { - return canRangeAverageFilterAngle.calculate( - Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256))); - } + // public double getCANrangeAngle() { + // return canRangeAverageFilterAngle.calculate( + // Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256))); + // } public Trigger isElevatorLevel1Coral() { return new Trigger(() -> m_elevatorSetpoint == Constants.kReefL1); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index ec3640b..6b647ed 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -34,8 +34,8 @@ public static class ElevatorIOInputs { public double slaveAppliedVolts = 0.0; public double slaveCurrentAmps = 0.0; public double slavePosition = 0.0; - public double CANrange1Dis = 0.0; - public double CANrange2Dis = 0.0; + // public double CANrange1Dis = 0.0; + // public double CANrange2Dis = 0.0; } public default void updateInputs(ElevatorIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index 674e6b3..eb8a3f0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -17,17 +17,13 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANrangeConfiguration; -import com.ctre.phoenix6.configs.FovParamsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; -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 com.ctre.phoenix6.signals.UpdateModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -56,11 +52,11 @@ public class ElevatorIOTalonFX implements ElevatorIO { private final StatusSignal m_SlaveCurrent; private final StatusSignal m_SlavePosition; - private final CANrange m_CANrange = new CANrange(0, "2481"); - private final CANrange m_CaNrange2 = new CANrange(1, "2481"); + // private final CANrange m_CANrange = new CANrange(0, "2481"); + // private final CANrange m_CaNrange2 = new CANrange(1, "2481"); - private final StatusSignal m_CANrange1Dis; - private final StatusSignal m_CANrange2Dis; + // private final StatusSignal m_CANrange1Dis; + // private final StatusSignal m_CANrange2Dis; public ElevatorIOTalonFX() { @@ -99,19 +95,20 @@ public ElevatorIOTalonFX() { TalonFXConfiguration elevatorRightCfg = new TalonFXConfiguration(); elevatorRightCfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; elevatorRightCfg.CurrentLimits.StatorCurrentLimit = 80.0; + elevatorRightCfg.Feedback.SensorToMechanismRatio = 23.622; // elevatorRightCfg.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; m_elevatorRightTalonFX.getConfigurator().apply(elevatorRightCfg); - CANrangeConfiguration CANrangecfg = new CANrangeConfiguration(); + // CANrangeConfiguration CANrangecfg = new CANrangeConfiguration(); - CANrangecfg.ProximityParams.MinSignalStrengthForValidMeasurement = 2000; - CANrangecfg.ProximityParams.ProximityThreshold = 0.1; - CANrangecfg.withFovParams(new FovParamsConfigs().withFOVRangeX(6.75).withFOVRangeY(6.75)); + // CANrangecfg.ProximityParams.MinSignalStrengthForValidMeasurement = 2000; + // CANrangecfg.ProximityParams.ProximityThreshold = 0.1; + // CANrangecfg.withFovParams(new FovParamsConfigs().withFOVRangeX(6.75).withFOVRangeY(6.75)); - CANrangecfg.ToFParams.UpdateMode = UpdateModeValue.ShortRange100Hz; + // CANrangecfg.ToFParams.UpdateMode = UpdateModeValue.ShortRange100Hz; - m_CANrange.getConfigurator().apply(CANrangecfg); - m_CaNrange2.getConfigurator().apply(CANrangecfg); + // m_CANrange.getConfigurator().apply(CANrangecfg); + // m_CaNrange2.getConfigurator().apply(CANrangecfg); m_MasterVelocity = m_elevatorLeftTalonFX.getVelocity(); m_MasterAppliedVolts = m_elevatorLeftTalonFX.getMotorVoltage(); @@ -123,11 +120,13 @@ public ElevatorIOTalonFX() { m_SlaveCurrent = m_elevatorRightTalonFX.getStatorCurrent(); m_SlavePosition = m_elevatorRightTalonFX.getRotorPosition(); - m_CANrange1Dis = m_CANrange.getDistance(); - m_CANrange2Dis = m_CaNrange2.getDistance(); + // m_CANrange1Dis = m_CANrange.getDistance(); + // m_CANrange2Dis = m_CaNrange2.getDistance(); m_elevatorRightTalonFX.setControl(new Follower(m_elevatorLeftTalonFX.getDeviceID(), true)); - // m_elevatorLeftTalonFX.setPosition(0); + + m_elevatorLeftTalonFX.setPosition(0); + m_elevatorRightTalonFX.setPosition(0); } @Override @@ -145,12 +144,9 @@ public void updateInputs(ElevatorIOInputs inputs) { var elevatorSlaveStatus = BaseStatusSignal.refreshAll( - m_SlaveVelocity, - m_SlaveAppliedVolts, - m_SlaveCurrent, - m_SlavePosition, - m_CANrange1Dis, - m_CANrange2Dis); + m_SlaveVelocity, m_SlaveAppliedVolts, m_SlaveCurrent, m_SlavePosition); + // m_CANrange1Dis, + // m_CANrange2Dis); inputs.slaveConnected = elevatorSlaveStatus.isOK(); inputs.slaveVelocityRotPerSec = m_SlaveVelocity.getValueAsDouble(); @@ -158,21 +154,22 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.slaveCurrentAmps = m_SlaveCurrent.getValueAsDouble(); inputs.slavePosition = m_SlavePosition.getValueAsDouble(); - inputs.CANrange1Dis = m_CANrange1Dis.getValueAsDouble(); - inputs.CANrange2Dis = m_CANrange2Dis.getValueAsDouble(); + // inputs.CANrange1Dis = m_CANrange1Dis.getValueAsDouble(); + // inputs.CANrange2Dis = m_CANrange2Dis.getValueAsDouble(); } @Override public Command setElevatorHomeCmd() { return new FunctionalCommand( () -> { - m_elevatorLeftTalonFX.setControl(m_MotionMagicVoltage.withPosition(0)); + m_elevatorLeftTalonFX.setControl(m_MotionMagicVoltage.withPosition(0.04)); }, () -> {}, interrupted -> { - m_elevatorLeftTalonFX.setControl(m_Voltage.withOutput(0)); + // m_elevatorLeftTalonFX.setControl(m_Voltage.withOutput(0)); + m_elevatorLeftTalonFX.setControl(m_MotionMagicVoltage.withPosition(0.0)); }, - () -> (m_MasterPosition.getValueAsDouble()) < 0.01); + () -> (m_MasterPosition.getValueAsDouble()) < 0.04); } @Override diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 07db402..b562c7d 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -9,6 +9,9 @@ public class Feeder extends SubsystemBase { private FeederIO m_FeederIO; + // private final DigitalInput m_beambreak = new DigitalInput(0); + // private final DigitalInput m_beambreak2 = new DigitalInput(2); + private final FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); public Feeder(FeederIO feederIO) { @@ -25,8 +28,20 @@ public Command setFeederRollerVoltsCmd(double roller) { return m_FeederIO.setFeederRollerVoltsCmd(roller); } + public Command setFeederVelocityCmd(double velo) { + return m_FeederIO.setFeederVelocityCmd(velo); + } + + public Command feederZeroCmd() { + return m_FeederIO.feederZeroCmd(); + } + public Trigger hasCoral() { - return m_FeederIO.hasCoral(); + return new Trigger(() -> inputs.hasCoral); // m_FeederIO.hasCoral(); + } + + public Trigger beambreakTrigger() { + return new Trigger(() -> inputs.beambreakTrigger); // m_FeederIO.beambreakTrigger(); } public Command setFeederPosZero() { diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index f856d81..57af99f 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.junction.AutoLog; public interface FeederIO { @@ -27,6 +26,7 @@ public static class FeederIOInputs { public double currentAmps = 0.0; public boolean hasCoral = false; public double position = 0.0; + public boolean beambreakTrigger = false; } public default void updateInputs(FeederIOInputs inputs) {} @@ -35,11 +35,23 @@ public default Command setFeederRollerVoltsCmd(double roller) { return new InstantCommand(); } - public default Trigger hasCoral() { - return new Trigger(() -> true); - } + // public default Trigger hasCoral() { + // return new Trigger(() -> true); + // } + + // public default Trigger beambreakTrigger() { + // return new Trigger(() -> true); + // } public default Command setFeederPosZero() { return new InstantCommand(); } + + public default Command setFeederVelocityCmd(double velo) { + return new InstantCommand(); + } + + public default Command feederZeroCmd() { + return new InstantCommand(); + } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index 76d48e9..1934a97 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -29,16 +30,17 @@ import edu.wpi.first.wpilibj.DigitalInput; 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.Constants; public class FeederIOTalonFX implements FeederIO { - private final TalonFX m_feederTalonFX = new TalonFX(Constants.kFeederCANID); + private final TalonFX m_feederTalonFX = new TalonFX(Constants.kFeederCANID, "2481"); private final VoltageOut m_VoltageOut = new VoltageOut(0); private final DigitalInput m_beambreak = new DigitalInput(0); - private final Trigger m_beambreakTrigger = new Trigger(m_beambreak::get); + private final DigitalInput m_beambreak2 = new DigitalInput(2); + // private final Trigger m_beambreakTrigger = new Trigger(m_beambreak::get); // TODO check if right private final PositionVoltage m_PositionVoltage = new PositionVoltage(0); + private final VelocityVoltage m_VelocityVoltage = new VelocityVoltage(0); private final StatusSignal m_Velocity; private final StatusSignal m_AppliedVolts; @@ -50,12 +52,19 @@ public FeederIOTalonFX() { feederCfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; feederCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; feederCfg.CurrentLimits.StatorCurrentLimit = 50.0; - feederCfg.Slot0.kP = Constants.kFeederkP; - feederCfg.Slot0.kI = Constants.kFeederkI; - feederCfg.Slot0.kD = Constants.kFeederkD; - feederCfg.Slot0.kV = Constants.kFeederkV; - feederCfg.Slot0.kA = Constants.kFeederkA; - feederCfg.Slot0.kS = Constants.kFeederkS; + feederCfg.Slot0.kP = Constants.kVeloFeederkP; + feederCfg.Slot0.kI = Constants.kVeloFeederkI; + feederCfg.Slot0.kD = Constants.kVeloFeederkD; + feederCfg.Slot0.kV = Constants.kVeloFeederkV; + feederCfg.Slot0.kA = Constants.kVeloFeederkA; + feederCfg.Slot0.kS = Constants.kVeloFeederkS; + + feederCfg.Slot1.kP = Constants.kPosFeederkP; + feederCfg.Slot1.kI = Constants.kPosFeederkI; + feederCfg.Slot1.kD = Constants.kPosFeederkD; + feederCfg.Slot1.kV = Constants.kPosFeederkV; + feederCfg.Slot1.kA = Constants.kPosFeederkA; + feederCfg.Slot1.kS = Constants.kPosFeederkS; m_feederTalonFX.getConfigurator().apply(feederCfg); @@ -77,12 +86,18 @@ public void updateInputs(FeederIOInputs inputs) { inputs.currentAmps = m_Current.getValueAsDouble(); inputs.position = m_Position.getValueAsDouble(); inputs.hasCoral = !m_beambreak.get(); + inputs.beambreakTrigger = !m_beambreak2.get(); } - @Override - public Trigger hasCoral() { - return new Trigger(() -> !m_beambreak.get()); - } + // @Override + // public Trigger hasCoral() { + // return new Trigger(() -> !m_beambreak.get()); + // } + + // @Override + // public Trigger beambreakTrigger() { + // return new Trigger(() -> !m_beambreak2.get()); + // } @Override public Command setFeederRollerVoltsCmd(double roller) { @@ -97,7 +112,20 @@ public Command setFeederPosZero() { return Commands.runOnce( () -> { m_feederTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 0.25)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() - 0.0).withSlot(1)); }); } + + @Override + public Command setFeederVelocityCmd(double velo) { + return Commands.runOnce( + () -> { + m_feederTalonFX.setControl(m_VelocityVoltage.withVelocity(velo).withSlot(0)); + }); + } + + @Override + public Command feederZeroCmd() { + return Commands.runOnce(() -> m_feederTalonFX.setPosition(0)); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 67cd995..7567cbb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -66,12 +66,12 @@ public Trigger isShooterVelocityLowEnoughToScore() { // Left and right rock public Trigger isShooterXVelocityLowEnough() { - return new Trigger(() -> Math.abs(inputs.shooterXVelocity) < 20.0); + return new Trigger(() -> Math.abs(inputs.shooterXVelocity) < 30.0); // 20 } // Forward and backwards rock; angulating motion public Trigger isShooterYVelocityLowEnough() { - return new Trigger(() -> Math.abs(inputs.shooterYVelocity) < 75.0); + return new Trigger(() -> Math.abs(inputs.shooterYVelocity) < 100.0); // 75 } public Trigger isShooterZAccelLowEnough() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 064616c..0022610 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -34,7 +34,7 @@ import frc.robot.Constants; public class ShooterIOTalonFX implements ShooterIO { - private final TalonFX m_shooterTalonFX = new TalonFX(Constants.kShooterCANID); + private final TalonFX m_shooterTalonFX = new TalonFX(Constants.kShooterCANID, "2481"); private final VelocityVoltage m_VelocityVoltage = new VelocityVoltage(0); private final VoltageOut m_VoltageOut = new VoltageOut(0); private final PositionVoltage m_PositionVoltage = new PositionVoltage(0); @@ -43,7 +43,7 @@ public class ShooterIOTalonFX implements ShooterIO { private final StatusSignal m_Current; private final StatusSignal m_Position; - private final Pigeon2 pigeon = new Pigeon2(Constants.kShooterPigeon2); + private final Pigeon2 pigeon = new Pigeon2(Constants.kShooterPigeon2, "2481"); private final StatusSignal shooterXVelocity; private final StatusSignal shooterYVelocity; private final StatusSignal shooterZVelocity; @@ -89,7 +89,13 @@ public ShooterIOTalonFX() { shooterAccelY = pigeon.getAccelerationY(); shooterAccelZ = pigeon.getAccelerationZ(); BaseStatusSignal.setUpdateFrequencyForAll( - 30, shooterXVelocity, shooterYVelocity, shooterZVelocity); + 70, + shooterXVelocity, + shooterYVelocity, + shooterZVelocity, + shooterAccelX, + shooterAccelY, + shooterAccelZ); } @Override @@ -127,7 +133,7 @@ public Command positionVoltageNoMovement() { return Commands.runOnce( () -> { m_shooterTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 0.5).withSlot(0)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 3.0).withSlot(0)); }); } diff --git a/src/main/java/frc/robot/subsystems/slide/Slide.java b/src/main/java/frc/robot/subsystems/slide/Slide.java index 6e61ef0..767dc1f 100644 --- a/src/main/java/frc/robot/subsystems/slide/Slide.java +++ b/src/main/java/frc/robot/subsystems/slide/Slide.java @@ -68,7 +68,6 @@ public void periodic() { Logger.recordOutput("Slide/SDisScoreReady", m_slideCloseEnoughToScore); Logger.recordOutput("Slide/SClose", isSlideCloseToTarget()); Logger.recordOutput("Slide/SlideAlign", m_slideAlign); - Logger.recordOutput("Slide/angleToGreatToScore", angleTooLargeTrigger()); } public Command slideVoltageCmd(double volts, boolean overrideLimits) { @@ -167,10 +166,6 @@ else if (fl_visible) { return llPose2d; } - public Trigger angleTooLargeTrigger() { - return new Trigger(() -> Math.abs(m_averagedRobotAngleToReefFace) < 12.5); - } - public void commonSlideMove(Pose2d pose, Double elevatorHeight, double theta) { Pose2d bumperPose2d = robotCenterOfBumper(pose, pose.getRotation()); Logger.recordOutput("Slide/BumperPose", bumperPose2d); @@ -260,7 +255,7 @@ public void commonSlideMove(Pose2d pose, Double elevatorHeight, double theta) { m_SlideIO.setSlideAlignPosition(m_slidePos); // if distance to the tip of reef branch is less than 16 inches do nothing - } else if (slideLegA > .406) { + } else if (Math.abs(m_coralToReefBranch) > .35) { // checks to make sure setpoint is greater than a tenth of an inch different from last // setpoint befor emovement @@ -365,7 +360,7 @@ public Boolean isAlignedWithFeeder() { Constants.kSlideFeederPositionTolerance); } - public Boolean isAlignedWithFeedersmallerTolerance() { + public Boolean isAlignedWithFeederSmallerTolerance() { return MathUtil.isNear( Constants.kSlideFeederPosition.magnitude(), inputs.position.magnitude(), diff --git a/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java b/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java index de4ac2a..fe3903f 100644 --- a/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java @@ -23,7 +23,7 @@ import org.littletonrobotics.junction.Logger; public class SlideIOTalonFX implements SlideIO { - private final TalonFX m_slideTalonFX = new TalonFX(Constants.kSlideCANID); + private final TalonFX m_slideTalonFX = new TalonFX(Constants.kSlideCANID, "2481"); private final MotionMagicVoltage m_MotionMagicVoltage = new MotionMagicVoltage(0); private final VoltageOut m_VoltageOut = new VoltageOut(0); private final StatusSignal m_Velocity; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 7e06d58..6be2fab 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -13,18 +13,24 @@ package frc.robot.subsystems.vision; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.filter.LinearFilter; 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.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; import java.util.List; @@ -36,6 +42,14 @@ public class Vision extends SubsystemBase { private final VisionIOInputsAutoLogged[] inputs; private final Alert[] disconnectedAlerts; + private final LinearFilter slidingAverageFilterX = LinearFilter.movingAverage(9); + private final LinearFilter slidingAverageFilterY = LinearFilter.movingAverage(9); + private final LinearFilter slidingAverageFilterAngle = LinearFilter.movingAverage(9); + + public double m_averagedRobotAngleToReefFace = 0.0; + public final Trigger angleTooLargeTrigger = + new Trigger(() -> Math.abs(m_averagedRobotAngleToReefFace) < 12.5); + public Vision(VisionConsumer consumer, VisionIO... io) { this.consumer = consumer; this.io = io; @@ -64,6 +78,56 @@ public Rotation2d getTargetX(int cameraIndex) { return inputs[cameraIndex].latestTargetObservation.tx(); } + /** + * Returns the best target, which can be used for simple servoing with vision. + * + * @param cameraIndex The index of the camera to use. + */ + public Pose2d getRobotPoseInTarget(int cameraIndex) { + return inputs[cameraIndex].robotPoseTarget; + } + + public Pose2d getBlendedRobotPoseInTarget() { + Pose2d leftCameraPose = getRobotPoseInTarget(0); + Pose2d rightCameraPose = getRobotPoseInTarget(1); + + var leftVisible = (leftCameraPose.getX() != 0.0) && (leftCameraPose.getY() != 0.0); + var rightVisible = (rightCameraPose.getX() != 0.0) && (rightCameraPose.getY() != 0.0); + + Translation2d translation = new Translation2d(); + Rotation2d rotation = new Rotation2d(); + + if (leftVisible && rightVisible) { + translation = leftCameraPose.getTranslation().plus(rightCameraPose.getTranslation()).div(2.0); + rotation = leftCameraPose.getRotation().plus(rightCameraPose.getRotation()).div(2.0); + } else if (leftVisible) { + translation = leftCameraPose.getTranslation(); + rotation = leftCameraPose.getRotation(); + } else if (rightVisible) { + translation = rightCameraPose.getTranslation(); + rotation = rightCameraPose.getRotation(); + } + + Distance averaged_robot_to_tag_x = + Meters.of(slidingAverageFilterX.calculate(translation.getX())); + Distance averaged_robot_to_tag_y = + Meters.of(slidingAverageFilterY.calculate(translation.getY())); + Rotation2d averaged_robot_angle_to_reef_face = + new Rotation2d(Radians.of(slidingAverageFilterAngle.calculate(rotation.getRadians()))); + + m_averagedRobotAngleToReefFace = averaged_robot_angle_to_reef_face.getDegrees(); + + Logger.recordOutput("Vision/averagedAngle", averaged_robot_angle_to_reef_face.getDegrees()); + + Pose2d llPose2d = + new Pose2d( + averaged_robot_to_tag_x, averaged_robot_to_tag_y, averaged_robot_angle_to_reef_face); + + Logger.recordOutput("Vision/averagedPose", llPose2d); + + return llPose2d; + } + @Override public void periodic() { for (int i = 0; i < io.length; i++) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 3183358..a9de77c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; @@ -25,6 +26,7 @@ public static class VisionIOInputs { new TargetObservation(new Rotation2d(), new Rotation2d()); public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; + public Pose2d robotPoseTarget = new Pose2d(); } /** Represents the angle to a simple target, not used for pose estimation. */ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index d6e2a77..ccbfbe2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.vision; +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; @@ -38,6 +39,7 @@ public class VisionIOLimelight implements VisionIO { private final DoubleSubscriber tySubscriber; private final DoubleArraySubscriber megatag1Subscriber; private final DoubleArraySubscriber megatag2Subscriber; + private final DoubleArraySubscriber robotPoseTargetSubscriber; /** * Creates a new VisionIOLimelight. @@ -55,6 +57,8 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + robotPoseTargetSubscriber = + table.getDoubleArrayTopic("botpose_targetspace").subscribe(new double[] {}); } @Override @@ -140,6 +144,15 @@ public void updateInputs(VisionIOInputs inputs) { for (int id : tagIds) { inputs.tagIds[i++] = id; } + + for (var robotPoseTarget : robotPoseTargetSubscriber.readQueue()) { + if (robotPoseTarget.value.length == 0) continue; + inputs.robotPoseTarget = + new Pose2d( + robotPoseTarget.value[0], + robotPoseTarget.value[2], + Rotation2d.fromDegrees(robotPoseTarget.value[4])); + } } /** Parses the 3D pose from a Limelight botpose array. */