diff --git a/resources/shuffleboard/gpeakshuffleboard.json b/resources/limelight-pipelines/practice-bot/gpeakshuffleboard.json similarity index 100% rename from resources/shuffleboard/gpeakshuffleboard.json rename to resources/limelight-pipelines/practice-bot/gpeakshuffleboard.json diff --git a/resources/limelight-pipelines/practice-bot/pipeline0PracticeField.vpr b/resources/limelight-pipelines/practice-bot/pipeline0PracticeField.vpr deleted file mode 100644 index cdb3cc18..00000000 --- a/resources/limelight-pipelines/practice-bot/pipeline0PracticeField.vpr +++ /dev/null @@ -1 +0,0 @@ -{"area_max":100.0,"area_min":0.001,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":1975.0,"botfloorsnap":0,"botlength":0.7112,"bottype":"swerve","botwidth":0.7112,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"cpu","clip_labels":"face,hand,computer,keyboard","contour_grouping":0,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":10.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"desc":"Pipeline_Name","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"cpu","dilation_steps":0,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":0,"exposure":185.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"","fiducial_locfilters":"","fiducial_qualitythreshold":2.0,"fiducial_resdiv":2,"fiducial_size":165.1,"fiducial_skip3d":0,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dtargposebotspace","flicker":0,"force_convex":1,"hue_max":85,"hue_min":55,"image_flip":0,"image_source":0,"img_to_show":0,"intersection_filter":0,"invert_hue":0,"lcgain":8.0,"margin_tv":0.2,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":1,"pipeline_led_power":100,"pipeline_res":0,"pipeline_type":"pipe_fiducial","python_snapscript_name":"","quality_focus":0.3,"red_balance":1200.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.07599,"rspitch":-25.0,"rsroll":0.0,"rss":0.07376,"rsu":0.4203,"rsyaw":0.0,"sat_max":255,"sat_min":70,"send_corners":0,"send_json":0,"tsf":-0.15,"tss":0.163,"tsu":0.0,"tv_conf":0.5,"val_max":255,"val_min":70,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0,"zsclassifier_conf":0.1} \ No newline at end of file diff --git a/resources/limelight-pipelines/practice-bot/pipeline1PracticeField.vpr b/resources/limelight-pipelines/practice-bot/pipeline1PracticeField.vpr deleted file mode 100644 index 020f7ee5..00000000 --- a/resources/limelight-pipelines/practice-bot/pipeline1PracticeField.vpr +++ /dev/null @@ -1 +0,0 @@ -{"area_max":100.0,"area_min":0.001,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":1975.0,"botfloorsnap":0,"botlength":0.7112,"bottype":"swerve","botwidth":0.7112,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"cpu","clip_labels":"face,hand,computer,keyboard","contour_grouping":0,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":10.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"desc":"Pipeline_Name","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"cpu","dilation_steps":0,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":0,"exposure":185.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"","fiducial_locfilters":"","fiducial_qualitythreshold":2.0,"fiducial_resdiv":2,"fiducial_size":165.1,"fiducial_skip3d":0,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dtargposebotspace","flicker":0,"force_convex":1,"hue_max":85,"hue_min":55,"image_flip":0,"image_source":0,"img_to_show":0,"intersection_filter":0,"invert_hue":0,"lcgain":8.0,"margin_tv":0.2,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":1,"pipeline_led_power":100,"pipeline_res":0,"pipeline_type":"pipe_fiducial","python_snapscript_name":"","quality_focus":0.3,"red_balance":1200.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.07599,"rspitch":-25.0,"rsroll":0.0,"rss":0.07376,"rsu":0.4203,"rsyaw":0.0,"sat_max":255,"sat_min":70,"send_corners":0,"send_json":0,"tsf":-0.15,"tss":-0.164,"tsu":0.0,"tv_conf":0.5,"val_max":255,"val_min":70,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0,"zsclassifier_conf":0.1} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd3..880ba549 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,6 +90,13 @@ } ], "robotJoysticks": [ + { + "useGamepad": true + }, + {}, + {}, + {}, + {}, { "guid": "78696e70757401000000000000000000", "useGamepad": true diff --git a/src/main/deploy/pathplanner/autos/red left 3.5p.auto b/src/main/deploy/pathplanner/autos/red left 3.5p.auto deleted file mode 100644 index 8c273af0..00000000 --- a/src/main/deploy/pathplanner/autos/red left 3.5p.auto +++ /dev/null @@ -1,182 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "red left 1" - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "shoot" - } - }, - { - "type": "named", - "data": { - "name": "shoot" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "red left 2" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.75 - } - }, - { - "type": "path", - "data": { - "pathName": "red left 3" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "shoot" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "red left 4" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.75 - } - }, - { - "type": "path", - "data": { - "pathName": "red left 5" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "shoot" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "red left 6" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.75 - } - }, - { - "type": "path", - "data": { - "pathName": "red left 7" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "shoot" - } - }, - { - "type": "named", - "data": { - "name": "Elevator L1" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/blue left 3.5p.auto b/src/main/deploy/pathplanner/autos/red left 4p.auto similarity index 81% rename from src/main/deploy/pathplanner/autos/blue left 3.5p.auto rename to src/main/deploy/pathplanner/autos/red left 4p.auto index 01048212..dc85c26e 100644 --- a/src/main/deploy/pathplanner/autos/blue left 3.5p.auto +++ b/src/main/deploy/pathplanner/autos/red left 4p.auto @@ -11,7 +11,7 @@ { "type": "path", "data": { - "pathName": "blue left 1" + "pathName": "red left 1" } }, { @@ -24,22 +24,9 @@ } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.75 - } - } - ] + "name": "shoot" } }, { @@ -53,19 +40,19 @@ { "type": "path", "data": { - "pathName": "blue left 2" + "pathName": "red left 2" } }, { "type": "wait", "data": { - "waitTime": 0.75 + "waitTime": 0.5 } }, { "type": "path", "data": { - "pathName": "blue left 3" + "pathName": "red left 3" } } ] @@ -93,7 +80,7 @@ { "type": "wait", "data": { - "waitTime": 0.75 + "waitTime": 0.25 } } ] @@ -110,19 +97,19 @@ { "type": "path", "data": { - "pathName": "blue left 4" + "pathName": "red left 4" } }, { "type": "wait", "data": { - "waitTime": 0.75 + "waitTime": 0.5 } }, { "type": "path", "data": { - "pathName": "blue left 5" + "pathName": "red left 5" } } ] @@ -150,7 +137,7 @@ { "type": "wait", "data": { - "waitTime": 0.75 + "waitTime": 0.25 } } ] @@ -167,13 +154,19 @@ { "type": "path", "data": { - "pathName": "blue left 6" + "pathName": "red left 6" } }, { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.5 + } + }, + { + "type": "path", + "data": { + "pathName": "red left 7" } } ] @@ -187,6 +180,31 @@ } ] } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.25 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Elevator L1" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/blue left 1.path b/src/main/deploy/pathplanner/paths/blue left 1.path deleted file mode 100644 index 1b59b6c7..00000000 --- a/src/main/deploy/pathplanner/paths/blue left 1.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.025929054054054, - "y": 7.463851351351351 - }, - "prevControl": null, - "nextControl": { - "x": 6.291349513482974, - "y": 6.616380873736169 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.069, - "y": 5.2700000000000005 - }, - "prevControl": { - "x": 6.691157285877081, - "y": 7.12451097767749 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "elevator", - "waypointRelativePos": 0.73, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Elevator L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -119.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0.0, - "rotation": -113.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue left 2.path b/src/main/deploy/pathplanner/paths/blue left 2.path deleted file mode 100644 index c6218e62..00000000 --- a/src/main/deploy/pathplanner/paths/blue left 2.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.069, - "y": 5.27 - }, - "prevControl": null, - "nextControl": { - "x": 4.4963175675675675, - "y": 6.198932432432431 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.289, - "y": 7.193 - }, - "prevControl": { - "x": 2.8602364864864867, - "y": 6.224587837837837 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "fast", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "lower elevator", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Elevator L1" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -54.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -119.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue left 3.path b/src/main/deploy/pathplanner/paths/blue left 3.path deleted file mode 100644 index ec8d27b3..00000000 --- a/src/main/deploy/pathplanner/paths/blue left 3.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.289, - "y": 7.193 - }, - "prevControl": null, - "nextControl": { - "x": 2.159564176637463, - "y": 6.375376061331924 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.756, - "y": 5.162 - }, - "prevControl": { - "x": 3.2018278023033546, - "y": 5.668605220132636 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "fas", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "elevator", - "waypointRelativePos": 0.57, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Elevator L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -54.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue left 4.path b/src/main/deploy/pathplanner/paths/blue left 4.path deleted file mode 100644 index e8384634..00000000 --- a/src/main/deploy/pathplanner/paths/blue left 4.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.756, - "y": 5.162 - }, - "prevControl": null, - "nextControl": { - "x": 2.5318928809624266, - "y": 6.296336106304676 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.289, - "y": 7.193 - }, - "prevControl": { - "x": 2.049756725928367, - "y": 6.483881389360883 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "fast", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "elevator down", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Elevator L1" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -54.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue left 5.path b/src/main/deploy/pathplanner/paths/blue left 5.path deleted file mode 100644 index 57122f47..00000000 --- a/src/main/deploy/pathplanner/paths/blue left 5.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.289, - "y": 7.193 - }, - "prevControl": null, - "nextControl": { - "x": 2.5435531846299573, - "y": 6.195349261656525 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.997, - "y": 5.265000000000001 - }, - "prevControl": { - "x": 2.357249882476706, - "y": 6.575458387015469 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "fast", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "elevator", - "waypointRelativePos": 0.75, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Elevator L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -54.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue left 6.path b/src/main/deploy/pathplanner/paths/blue left 6.path deleted file mode 100644 index ff720433..00000000 --- a/src/main/deploy/pathplanner/paths/blue left 6.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.997, - "y": 5.265 - }, - "prevControl": null, - "nextControl": { - "x": 2.8726172484224484, - "y": 6.114468827133139 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.289, - "y": 7.193 - }, - "prevControl": { - "x": 2.458004266422662, - "y": 6.311000132765435 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "fast", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "lower elevator", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Elevator L1" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -54.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -59.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue right 1.path b/src/main/deploy/pathplanner/paths/blue right 1.path index 59ac2afe..c6d87d89 100644 --- a/src/main/deploy/pathplanner/paths/blue right 1.path +++ b/src/main/deploy/pathplanner/paths/blue right 1.path @@ -3,32 +3,37 @@ "waypoints": [ { "anchor": { - "x": 7.0600000000000005, - "y": 0.616 + "x": 7.156500000000001, + "y": 0.83 }, "prevControl": null, "nextControl": { - "x": 6.017989864864864, - "y": 1.7818412162162156 + "x": 4.571251780643654, + "y": 3.01304312334466 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.076, - "y": 2.784 + "x": 4.946, + "y": 2.7436505681818177 }, "prevControl": { - "x": 5.890591216216215, - "y": 1.940304054054054 + "x": 5.174692033673075, + "y": 2.6426557478469475 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.11423550087873358, + "rotationDegrees": 119.99999999999999 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ @@ -60,7 +65,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 110.0 + "rotation": 90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/blue right 2.path b/src/main/deploy/pathplanner/paths/blue right 2.path index 82e7472a..b8083b61 100644 --- a/src/main/deploy/pathplanner/paths/blue right 2.path +++ b/src/main/deploy/pathplanner/paths/blue right 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.076, - "y": 2.784 + "x": 4.946, + "y": 2.744 }, "prevControl": null, "nextControl": { - "x": 3.9835000000041516, - "y": 1.5457499999946689 + "x": 3.8535000000041517, + "y": 1.5057499999946693 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.452, - "y": 0.729 + "x": 1.472, + "y": 0.7489999999946702 }, "prevControl": { - "x": 2.6902500000000003, - "y": 1.3432499999999994 + "x": 2.7102500000000003, + "y": 1.3632499999946697 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/blue right 3.path b/src/main/deploy/pathplanner/paths/blue right 3.path index 709773c8..be7b51f8 100644 --- a/src/main/deploy/pathplanner/paths/blue right 3.path +++ b/src/main/deploy/pathplanner/paths/blue right 3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.452, - "y": 0.729 + "x": 1.472, + "y": 0.749 }, "prevControl": null, "nextControl": { - "x": 1.8632997663865138, - "y": 1.0718522523859908 + "x": 1.8885159493241042, + "y": 1.048685409689395 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.943, - "y": 2.76 + "x": 3.869, + "y": 2.823423295454545 }, "prevControl": { - "x": 3.0081014805958786, - "y": 1.9785538877547855 + "x": 2.918996707037624, + "y": 2.0298962762724995 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/blue right 4.path b/src/main/deploy/pathplanner/paths/blue right 4.path index b10e0d9d..9f871b53 100644 --- a/src/main/deploy/pathplanner/paths/blue right 4.path +++ b/src/main/deploy/pathplanner/paths/blue right 4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.943, - "y": 2.76 + "x": 3.869, + "y": 2.823 }, "prevControl": null, "nextControl": { - "x": 2.825000850624595, - "y": 1.8516839616141945 + "x": 2.751000850624595, + "y": 1.9146839616141946 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.452, - "y": 0.729 + "x": 1.541, + "y": 0.739 }, "prevControl": { - "x": 2.059959023543265, - "y": 1.2364023683583318 + "x": 2.148959023543265, + "y": 1.2464023683583318 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/blue right 5.path b/src/main/deploy/pathplanner/paths/blue right 5.path index 79f6c084..2d28ef51 100644 --- a/src/main/deploy/pathplanner/paths/blue right 5.path +++ b/src/main/deploy/pathplanner/paths/blue right 5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.452, - "y": 0.729 + "x": 1.541, + "y": 0.7392499999999995 }, "prevControl": null, "nextControl": { - "x": 2.3777500000000003, - "y": 1.0702500000000001 + "x": 2.46675, + "y": 1.0804999999999996 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.9031960227272724 }, "prevControl": { - "x": 6.433023648648649, - "y": 0.7146114864864854 + "x": 4.843534090909092, + "y": 1.0078181818181815 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/red left 1.path b/src/main/deploy/pathplanner/paths/red left 1.path index 77588392..25babad9 100644 --- a/src/main/deploy/pathplanner/paths/red left 1.path +++ b/src/main/deploy/pathplanner/paths/red left 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 12.47, - "y": 2.7899999999999996 + "x": 12.510304054054053, + "y": 2.7304898648648646 }, "prevControl": { - "x": 12.290382642035121, - "y": 2.6146757155505367 + "x": 12.330391548676307, + "y": 2.5548774873850046 }, "nextControl": null, "isLocked": false, @@ -35,8 +35,8 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, + "maxVelocity": 3.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -48,7 +48,7 @@ "eventMarkers": [ { "name": "elevator", - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.6, "endWaypointRelativePos": null, "command": { "type": "named", diff --git a/src/main/deploy/pathplanner/paths/red left 2.path b/src/main/deploy/pathplanner/paths/red left 2.path index 749ad343..2b8ff627 100644 --- a/src/main/deploy/pathplanner/paths/red left 2.path +++ b/src/main/deploy/pathplanner/paths/red left 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 12.47, - "y": 2.79 + "x": 12.51, + "y": 2.76 }, "prevControl": null, "nextControl": { - "x": 13.02297297296791, - "y": 2.1373479729738745 + "x": 13.06297297296791, + "y": 2.107347972973874 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 16.285, - "y": 0.8924831081090099 + "x": 16.225, + "y": 1.012 }, "prevControl": { - "x": 15.768486012782757, - "y": 1.312564366622182 + "x": 15.708486012782759, + "y": 1.4320812585131724 }, "nextControl": null, "isLocked": false, @@ -35,8 +35,8 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, + "maxVelocity": 3.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/red left 3.path b/src/main/deploy/pathplanner/paths/red left 3.path index 854ad10a..c3dc2e11 100644 --- a/src/main/deploy/pathplanner/paths/red left 3.path +++ b/src/main/deploy/pathplanner/paths/red left 3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 16.285, - "y": 0.892 + "x": 16.225, + "y": 1.012 }, "prevControl": null, "nextControl": { - "x": 14.7324554387651, - "y": 2.2334984633908594 + "x": 14.672455438765102, + "y": 2.3534984633908596 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.864, - "y": 2.958 + "x": 13.87, + "y": 2.952 }, "prevControl": { - "x": 15.022062184670208, - "y": 1.9322947943683024 + "x": 15.028062184670206, + "y": 1.9262947943683022 }, "nextControl": null, "isLocked": false, @@ -31,12 +31,25 @@ "rotationTargets": [], "constraintZones": [ { - "name": "Constraints Zone", + "name": "slow down ", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.25, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "go fast", "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, + "maxVelocity": 3.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/red left 4.path b/src/main/deploy/pathplanner/paths/red left 4.path index cbc0ef9b..365818dd 100644 --- a/src/main/deploy/pathplanner/paths/red left 4.path +++ b/src/main/deploy/pathplanner/paths/red left 4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 13.864, - "y": 2.958 + "x": 13.85, + "y": 2.972 }, "prevControl": null, "nextControl": { - "x": 15.429776673536038, - "y": 1.6249437142740153 + "x": 15.415776673536037, + "y": 1.638943714274015 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 16.285, - "y": 0.892 + "x": 16.225, + "y": 0.992 }, "prevControl": { - "x": 15.649557348439215, - "y": 1.4030860795130118 + "x": 15.589557348439216, + "y": 1.5030860795130119 }, "nextControl": null, "isLocked": false, @@ -35,8 +35,8 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, + "maxVelocity": 3.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/red left 5.path b/src/main/deploy/pathplanner/paths/red left 5.path index b4eeb49b..19f8ed8c 100644 --- a/src/main/deploy/pathplanner/paths/red left 5.path +++ b/src/main/deploy/pathplanner/paths/red left 5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 16.285, - "y": 0.892 + "x": 16.225, + "y": 0.992 }, "prevControl": null, "nextControl": { - "x": 16.678998901310894, - "y": 0.6067192093795181 + "x": 16.618998901310896, + "y": 0.706719209379518 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.554, - "y": 2.7300000000000004 + "x": 13.544, + "y": 2.74 }, "prevControl": { - "x": 15.00533286816942, - "y": 1.6942674710619114 + "x": 14.99533286816942, + "y": 1.7042674710619112 }, "nextControl": null, "isLocked": false, @@ -31,12 +31,25 @@ "rotationTargets": [], "constraintZones": [ { - "name": "Constraints Zone", + "name": "slow", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.55, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "fast", "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, + "maxVelocity": 3.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/red left 6.path b/src/main/deploy/pathplanner/paths/red left 6.path index a8f9dba4..d3d6069d 100644 --- a/src/main/deploy/pathplanner/paths/red left 6.path +++ b/src/main/deploy/pathplanner/paths/red left 6.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 13.554, - "y": 2.73 + "x": 13.558, + "y": 2.76 }, "prevControl": null, "nextControl": { - "x": 14.245926747092218, - "y": 2.1501854693994074 + "x": 14.249926747092218, + "y": 2.180185469399407 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 16.008, - "y": 0.724 + "y": 0.824 }, "prevControl": { "x": 16.481828265745367, - "y": 0.33151309969315107 + "y": 0.43151309969315105 }, "nextControl": null, "isLocked": false, @@ -35,8 +35,8 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, + "maxVelocity": 3.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/red left 7.path b/src/main/deploy/pathplanner/paths/red left 7.path index 872f791a..77b9d0b7 100644 --- a/src/main/deploy/pathplanner/paths/red left 7.path +++ b/src/main/deploy/pathplanner/paths/red left 7.path @@ -4,12 +4,12 @@ { "anchor": { "x": 16.008, - "y": 0.724 + "y": 0.824 }, "prevControl": null, "nextControl": { "x": 15.25598878320848, - "y": 2.2834416838650053 + "y": 2.3834416838650054 }, "isLocked": false, "linkedName": null @@ -17,11 +17,11 @@ { "anchor": { "x": 14.417, - "y": 3.71 + "y": 3.67 }, "prevControl": { "x": 15.276714253357527, - "y": 3.0067078114286847 + "y": 2.9667078114286842 }, "nextControl": null, "isLocked": false, @@ -31,12 +31,25 @@ "rotationTargets": [], "constraintZones": [ { - "name": "Constraints Zone", + "name": "slow", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.25, + "constraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "fast", "minWaypointRelativePos": 0, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, + "maxVelocity": 3.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/red right 1.path b/src/main/deploy/pathplanner/paths/red right 1.path index 00075805..3bf5b3ad 100644 --- a/src/main/deploy/pathplanner/paths/red right 1.path +++ b/src/main/deploy/pathplanner/paths/red right 1.path @@ -3,47 +3,38 @@ "waypoints": [ { "anchor": { - "x": 10.514, - "y": 7.444087837837838 + "x": 10.403, + "y": 7.244 }, "prevControl": null, "nextControl": { - "x": 11.275247883097851, - "y": 6.656885112040727 + "x": 11.141841405850913, + "y": 6.542995822052234 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 12.658, - "y": 5.279 + "x": 12.583, + "y": 5.259 }, "prevControl": { - "x": 11.907614045584399, - "y": 6.039452252763509 + "x": 11.854553394298936, + "y": 6.0507703947950375 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], - "constraintZones": [ + "rotationTargets": [ { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "waypointRelativePos": 0.19527559055118116, + "rotationDegrees": -59.99999999999999 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -62,7 +53,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -67.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/red right 2.path b/src/main/deploy/pathplanner/paths/red right 2.path index 8d4ecdb6..729622ce 100644 --- a/src/main/deploy/pathplanner/paths/red right 2.path +++ b/src/main/deploy/pathplanner/paths/red right 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 12.658, - "y": 5.279 + "x": 12.583, + "y": 5.259 }, "prevControl": null, "nextControl": { - "x": 13.272250000003844, - "y": 5.961499999999401 + "x": 13.197250000003844, + "y": 5.941499999999402 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 16.283, - "y": 7.145 + "x": 16.263, + "y": 7.1255 }, "prevControl": { - "x": 15.772044400857094, - "y": 6.728771205226123 + "x": 15.752044400857095, + "y": 6.709271205226123 }, "nextControl": null, "isLocked": false, @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { diff --git a/src/main/deploy/pathplanner/paths/red right 3.path b/src/main/deploy/pathplanner/paths/red right 3.path index 2ce4e571..5a754041 100644 --- a/src/main/deploy/pathplanner/paths/red right 3.path +++ b/src/main/deploy/pathplanner/paths/red right 3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 16.283, - "y": 7.145 + "x": 16.263, + "y": 7.125 }, "prevControl": null, "nextControl": { - "x": 16.871555236728764, - "y": 7.564426999650049 + "x": 16.851555236728764, + "y": 7.5444269996500495 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.675, - "y": 5.194999999999999 + "x": 13.7, + "y": 5.185 }, "prevControl": { - "x": 13.459342275196981, - "y": 5.068541525658479 + "x": 13.48434227519698, + "y": 5.058541525658479 }, "nextControl": null, "isLocked": false, @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { diff --git a/src/main/deploy/pathplanner/paths/red right 4.path b/src/main/deploy/pathplanner/paths/red right 4.path index 53cdea8e..f5afbd4d 100644 --- a/src/main/deploy/pathplanner/paths/red right 4.path +++ b/src/main/deploy/pathplanner/paths/red right 4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 13.675, - "y": 5.195 + "x": 13.7, + "y": 5.185 }, "prevControl": null, "nextControl": { - "x": 14.263555236728763, - "y": 5.61442699965005 + "x": 14.288555236728762, + "y": 5.604426999650049 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 16.283, - "y": 7.145 + "x": 16.263, + "y": 7.125 }, "prevControl": { - "x": 16.06734227519698, - "y": 7.018541525658479 + "x": 16.047342275196982, + "y": 6.99854152565848 }, "nextControl": null, "isLocked": false, @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { diff --git a/src/main/deploy/pathplanner/paths/red right 5.path b/src/main/deploy/pathplanner/paths/red right 5.path index c1fdead0..4d2f508f 100644 --- a/src/main/deploy/pathplanner/paths/red right 5.path +++ b/src/main/deploy/pathplanner/paths/red right 5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 16.283, - "y": 7.145 + "x": 16.263, + "y": 7.125 }, "prevControl": null, "nextControl": { - "x": 13.71875, - "y": 6.2875000000000005 + "x": 13.69875, + "y": 6.267500000000001 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 12.36, - "y": 5.165 + "x": 12.5, + "y": 5.2 }, "prevControl": { - "x": 11.931, - "y": 6.169250000000001 + "x": 12.071, + "y": 6.204250000000001 }, "nextControl": null, "isLocked": false, @@ -29,21 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.25, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { diff --git a/src/main/java/frc/robot/CommandFactory.java b/src/main/java/frc/robot/CommandFactory.java index 93af8dd5..87057737 100644 --- a/src/main/java/frc/robot/CommandFactory.java +++ b/src/main/java/frc/robot/CommandFactory.java @@ -3,11 +3,11 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -18,11 +18,12 @@ import frc.robot.Constants.*; import frc.robot.Constants.SetPointConstants.ElevatorHeights; import frc.robot.commands.AlignWithLimelight; +import frc.robot.commands.*; import frc.robot.commands.SetCoralIntake; -import frc.robot.commands.SmartIntake; import frc.robot.commands.SnapDrivebaseToAngle; import frc.robot.generated.WoodBotDriveTrain; import frc.robot.subsystems.AlgaeArm.AlgaeArm; +import frc.robot.subsystems.AlgaeArm.AlgaeArm; import frc.robot.subsystems.AlgaeRoller.AlgaeRoller; import frc.robot.subsystems.AlgaeShooter.AlgaeShooter; import frc.robot.subsystems.AlgaeTilt.AlgaeTilt; @@ -35,6 +36,8 @@ import frc.robot.subsystems.Vision.Vision; import frc.robot.utils.CommandLogger; import java.util.Map; +import java.util.Map; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.Logger; import org.opencv.calib3d.StereoBM; @@ -44,6 +47,7 @@ public class CommandFactory { private final Elevator elevator; private final Vision vision; private final ClimberWinch climberWinch; + private final ClimberWheel climberWheel; private final AlgaeShooter algaeShooter; private final AlgaeArm algaeArm; private final AlgaeRoller algaeRoller; @@ -51,27 +55,28 @@ public class CommandFactory { private final CommandXboxController driverCont; private final AlgaeTilt algaeTilt; private final Servo servo; - - private final Timer climbTimer; + private final PathOnTheFly pathOnTheFly; // ↓ constructor ↓ // public CommandFactory( - CoralShooter coralShooter, - Elevator elevator, - Vision vision, - ClimberWinch climberWinch, - AlgaeShooter algaeShooter, - AlgaeArm algaeArm, - CommandSwerveDrivetrain driveTrain, - CommandXboxController driverCont, - AlgaeTilt algaeTilt, - AlgaeRoller algaeRoller, - Servo servo - ) { + CoralShooter coralShooter, + Elevator elevator, + Vision vision, + ClimberWinch climberWinch, + ClimberWheel climberWheel, + AlgaeShooter algaeShooter, + AlgaeArm algaeArm, + CommandSwerveDrivetrain driveTrain, + CommandXboxController driverCont, + AlgaeTilt algaeTilt, + AlgaeRoller algaeRoller, + Servo servo, + PathOnTheFly pathOnTheFly) { this.coralShooter = coralShooter; this.elevator = elevator; this.vision = vision; this.climberWinch = climberWinch; + this.climberWheel = climberWheel; this.algaeShooter = algaeShooter; this.algaeArm = algaeArm; this.drivetrain = driveTrain; @@ -79,17 +84,12 @@ public CommandFactory( this.algaeTilt = algaeTilt; this.algaeRoller = algaeRoller; this.servo = servo; - this.climbTimer = new Timer(); + this.pathOnTheFly = pathOnTheFly; } public Command rumbleDriverController(CommandXboxController controller) { - return CommandLogger.logCommand( - Commands.runEnd( - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.15), - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0) - ), - "rumbling" - ); + return Commands.runEnd(() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.15), + () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)); } /* @@ -97,9 +97,8 @@ public Command rumbleDriverController(CommandXboxController controller) { */ public Command setElevatorHeight(double height) { return CommandLogger.logCommand( - elevator.isAtHeight(height).deadlineFor(elevator.setElevatorHeight(height)), - "SetElevatorHeight" - ); + elevator.isAtHeight(height).deadlineFor(elevator.setElevatorHeight(height)), + "SetElevatorHeight"); } public Command setElevatorLevelFour() { @@ -141,18 +140,12 @@ public Command setAlgaeTiltPosition(double position) { * @param pipeline 0 is right, 1 is left * @return */ - public Command alignWithLimelight( - double goalTY, - double goalTX, - int pipeline, - CommandXboxController driverCont - ) { - return CommandLogger - .logCommand( + public Command alignWithLimelight(double goalTY, double goalTX, int pipeline, CommandXboxController driverCont) { + return CommandLogger.logCommand( // vision.waitUntilTargetTxTy(goalTX, + // goalTY).alongWith(drivetrain.waitUntilDrivetrainAtHeadingSetpoint()) new AlignWithLimelight(vision, drivetrain, goalTY, goalTX, pipeline, driverCont), - "AlignWithLimelightBase" - ) - .andThen(this.rumbleDriverController(driverCont).withTimeout(0.1)); + "AlignWithLimelightBase").andThen(this.rumbleDriverController(driverCont).withTimeout(0.1)); // no more + // timeout } /** @@ -171,25 +164,22 @@ public Command alignWithLimelightAutomated(boolean isLeft) { int pipeline = isLeft ? 1 : 0; return Commands - .waitUntil( - () -> { - boolean onTX = drivetrain.strafeController.atSetpoint(); - boolean onTY = drivetrain.forwardController.atSetpoint(); - boolean onHeading = drivetrain.isAtRotationSetpoint(); - - String cmdTag = "AlignWithLimelightAutomated: "; - Logger.recordOutput(cmdTag + "onTX", onTX); - Logger.recordOutput(cmdTag + "onTY", onTY); - Logger.recordOutput(cmdTag + "onHeading", onHeading); - return ( - onTX && - onTY && - onHeading && - vision.isTargetInView(Constants.PracticeBotConstants.CORAL_LIMELIGHT_NAME) - ); - } - ) - .deadlineFor(alignWithLimelight(goalTY, goalTX, pipeline, driverCont).repeatedly()); + .waitUntil( + () -> { + boolean onTX = drivetrain.strafeController.atSetpoint(); + boolean onTY = drivetrain.forwardController.atSetpoint(); + boolean onHeading = drivetrain.isAtRotationSetpoint(); + + String cmdTag = "AlignWithLimelightAutomated: "; + Logger.recordOutput(cmdTag + "onTX", onTX); + Logger.recordOutput(cmdTag + "onTY", onTY); + Logger.recordOutput(cmdTag + "onHeading", onHeading); + return (onTX && + onTY && + onHeading && + vision.isTargetInView(Constants.PracticeBotConstants.CORAL_LIMELIGHT_NAME)); + }) + .deadlineFor(alignWithLimelight(goalTY, goalTX, pipeline, driverCont).repeatedly()); } /** @@ -202,19 +192,16 @@ public Command alignWithLimelightAutomated(boolean isLeft) { */ public Command scoringRoutine(int level, boolean isLeft) { return alignWithLimelightAutomated(isLeft) - .andThen( - new SelectCommand( - Map.ofEntries( - Map.entry(1, setElevatorLevelOne()), - Map.entry(2, setElevatorLevelTwo()), - Map.entry(3, setElevatorLevelThree()), - Map.entry(4, setElevatorLevelFour()) - ), - () -> level - ) - .raceWith(drivetrain.xOutCmd()) - ) - .andThen(coralShooter.basicShootCmd().raceWith(drivetrain.xOutCmd())); + .andThen( + new SelectCommand( + Map.ofEntries( + Map.entry(1, setElevatorLevelOne()), + Map.entry(2, setElevatorLevelTwo()), + Map.entry(3, setElevatorLevelThree()), + Map.entry(4, setElevatorLevelFour())), + () -> level) + .raceWith(drivetrain.xOutCmd())) + .andThen(coralShooter.basicShootCmd().raceWith(drivetrain.xOutCmd())); } public Command scoreLevelOne() { @@ -225,114 +212,57 @@ public Command scoringRoutineTeleop(int level, boolean isLeft) { return scoringRoutine(level, isLeft).andThen(setElevatorHeightZeroAndZero()); } - public Command hasCoral(Elevator elevator, CoralShooter coralShooter) { - return Commands.either( - new SequentialCommandGroup( - elevator.setElevatorHeight(ElevatorHeights.AUTO_LEVEL_FOUR), - coralShooter.basicShootCmd() - ), - Commands.none(), - () -> coralShooter.getIntakeSensor() || coralShooter.getOuttakeSensor() - ); - } - public Command alignToReefWoodbotLeft(int pipeline) { return new SequentialCommandGroup( - new SnapDrivebaseToAngle(vision, drivetrain, pipeline), - new AlignWithLimelight( - vision, - drivetrain, - -12.64, - -11.16, - 0, - new CommandXboxController(0) - ) - ); + new SnapDrivebaseToAngle(vision, drivetrain, pipeline), + new AlignWithLimelight(vision, drivetrain, -12.64, -11.16, 0, new CommandXboxController(0))); } private boolean climberDeployed = false; public Command homeAlgaeTilt() { - return Commands.either( - algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.07 : 7.2), // used to be 10, 4 works - // for some reason 3/15 - algaeTilt.setPositionCmd(0.907), - () -> !climberDeployed - ); + return Commands.either(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.07 : 10.0), + algaeTilt.setPositionCmd(0.907), () -> !climberDeployed); } public Command groundPickupAlgaeTilt() { return algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.3 : 35.0); } - public Command driverIntakeAlgae() { - return algaeRoller - .setDutyCycleCmd(-0.1) - .alongWith(algaeShooter.setDutyCycleCmd(-1.0)) - .alongWith(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.32 : 23.5)); - } - - public Command driverProcessAlgae() { - return algaeTilt - .setPositionCmd(Constants.isCompBot() ? 0.253 : 21) - .alongWith(algaeShooter.setDutyCycleCmd(0.6)) - .alongWith(algaeRoller.setDutyCycleCmd(0.8)); + public Command climberSetupAlgaeTilt() { + return algaeTilt.setPositionCmd(0.25); } - public Command operatorIntakeAlgae() { - return algaeRoller.setDutyCycleCmd(-0.1).alongWith(algaeShooter.setDutyCycleCmd(-1.0)); + public Command intakeAlgaeFromGround() { + return algaeRoller.setDutyCycleCmd(-0.1).alongWith( + algaeShooter.setDutyCycleCmd(-1.0)); } - public Command operatorOutakeAlgae() { + public Command outtakeAlgaeFromGround() { return algaeShooter.setDutyCycleCmd(0.9); } - public Command processOrShoot() { - if (algaeTilt.getPositionRelative() >= 19.0 || algaeTilt.getPositionAbsolute() >= 0.2) { - return operatorOutakeAlgae(); - } else { - return shootAlgae(); - } - } - public Command shootAlgae() { return Commands - .waitUntil(() -> algaeShooter.getVelocity() > 5750) - .andThen(algaeRoller.setDutyCycleCmd(1.0)) - .alongWith(algaeShooter.setVelocityCmd(6250)) - .alongWith(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.03 : 3.0)); - } - - public Command processAndScore() { - return algaeTilt - .setPositionCmd(Constants.isCompBot() ? 0.253 : 30) - .alongWith(this.shootAlgae()); - } - - public Command spinUpAlgaeShooter() { - return algaeShooter.setVelocityCmd(6250.0); + .waitUntil(() -> algaeShooter.getVelocity() > 5500) + .andThen(algaeRoller.setDutyCycleCmd(1.0)) + .alongWith(algaeShooter.setVelocityCmd(6000)); } /** * This command assumes the elevator is already above the algae - * + * * @return */ public Command intakeAlgaeFromReef() { - return algaeArm - .setAlgaeArmAngleCmd(110.0) - .alongWith(coralShooter.pullAlgae()) - .alongWith(algaeShooter.setDutyCycleCmd(-0.8)) - .alongWith(algaeTilt.setPositionCmd(0.0)) - .alongWith( - Commands - .waitUntil(() -> coralShooter.getVelocity() < -6000.0) - .andThen( - elevator.setElevatorHeight( - SetPointConstants.ElevatorHeights.TELE_LEVEL_THREE - 3.0 - ) - ) - ); + + return algaeArm.setAlgaeArmAngleCmd(110.0).alongWith(coralShooter.pullAlgae()) + .alongWith(algaeShooter.setDutyCycleCmd(-0.8)) + .alongWith(algaeTilt.setPositionCmd(0.0)).alongWith( + Commands.waitUntil(() -> coralShooter.getVelocity() < -6000.0) + .andThen(elevator + .setElevatorHeight(SetPointConstants.ElevatorHeights.TELE_LEVEL_THREE - 3.0))); + } public Command removeAlgaeL2() { @@ -351,12 +281,13 @@ public Command extendAlgaeArm() { return this.setAlgaeArmAngle(110.0); } - private Command removeAlgae(int level) { // NOT BEING USED + private Command removeAlgae(int level) { //NOT BEING USED + double height; if (level == 2) { - height = ElevatorHeights.TELE_LEVEL_THREE - 6.0; // - 3.0 rotations from L4 + height = SetPointConstants.ElevatorHeights.TELE_LEVEL_THREE - 6.0; // - 3.0 rotations from L4 GPEAK } else { - height = ElevatorHeights.TELE_LEVEL_FOUR - 6.5; // - 3.0 rotations from L3 + height = SetPointConstants.ElevatorHeights.TELE_LEVEL_FOUR - 6.5; // - 3.0 rotations from L3 GPEAK } algaeArm.setAlgaeArmAngleCmd(60.0); @@ -366,55 +297,52 @@ private Command removeAlgae(int level) { // NOT BEING USED } else { return coralShooter.pullAlgae(); } - } - public Command climberSetupAlgaeTilt() { - return algaeTilt.setPositionCmd(0.25); + // return Commands.run(() -> elevator.setElevatorHeight(height), elevator) + // .until(() -> Math.abs(elevator.getHeight() - height) < 0.5) + // .andThen(coralShooter.pullAlgae().alongWith(algaeArm.setAlgaeArmAngleCmd(110.0))); + } public Command deployClimb() { - return Commands - .waitUntil(() -> (climbTimer.get() > 3.5)) - .deadlineFor( - servo - .runWithTimeout(3.5, 0) - .alongWith(new InstantCommand(() -> climbTimer.reset())) - .alongWith(new InstantCommand(() -> climbTimer.start())) - .alongWith(algaeTilt.setPositionCmd(0.256)) - .andThen( - new InstantCommand(() -> System.out.println("TIMEOUT IS DONE HERERERE")) - ) // 20 - // for - // practice - .andThen(new InstantCommand(() -> this.climberDeployed = true)) - ); + return servo.runWithTimeout(3.5, 0).deadlineFor(algaeTilt.setPositionCmd(0.256)) + .andThen(new InstantCommand(() -> this.climberDeployed = true)); } double climberWinchSetPoint = -44.33; public Command initiateClimb() { - return Commands - .waitUntil(() -> climberWinch.getPosition() < climberWinchSetPoint + 1.0) - .deadlineFor(climberWinch.setDutyCycleCmd(-0.3)) - .alongWith(algaeTilt.setPositionCmd(0.907)); // -5 for comp bot + return Commands.waitUntil(() -> climberWinch.getPosition() < climberWinchSetPoint) + .deadlineFor(climberWinch.setDutyCycleCmd(-0.3)).alongWith(algaeTilt.setPositionCmd(0.907)); } public Command depolyAndInitiateClimb() { - return deployClimb().andThen(initiateClimb()); + return deployClimb().andThen(Commands.waitSeconds(1.0).andThen(initiateClimb())); } public Command climb() { - return climberWinch.setDutyCycleCmd(-0.8); + return climberWinch.setDutyCycleCmd(-0.60) + .alongWith(algaeTilt.setPositionCmd(0.907)); } public Command climbAutomated() { - return Commands - .waitUntil(() -> climberWinch.getPosition() < -160.0) - .deadlineFor(climb()) - .alongWith(algaeTilt.setPositionCmd(0.907)); + return Commands.waitUntil(() -> climberWinch.getPosition() < -145.5) + .deadlineFor(climb()); } public void resetClimberDeployed() { climberDeployed = false; } + + public Command pathFindToReefLeft() { + return AlignToReefFieldRelative.moveToReef(drivetrain, () -> this.drivetrain.getPose(), false); + } + + public Command pathFindToReefRight() { + return AlignToReefFieldRelative.moveToReef(drivetrain, () -> this.drivetrain.getPose(), true); + } + + public Command pathFindToProcessor() { + return PathOnTheFly.pathfindToProcessor(drivetrain); + } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a873968b..a6c8d78b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,9 +4,16 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.signals.ConnectedMotorValue; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.util.function.BooleanConsumer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.generated.OldCompBot; + /** * The Constants class provides a convenient place for teams to hold robot-wide * numerical or boolean @@ -22,19 +29,19 @@ public final class Constants { public final class SetPointConstants{ - public static final double RIGHT_GOAL_TY = isCompBot() ? 14.5 : 11.75; //TUNED FOR GPEAK + public static final double RIGHT_GOAL_TY = isCompBot() ? 14.0 : 11.75; //TUNED FOR GPEAK public static final double RIGHT_GOAL_TX = 0.0; public static final double LEFT_GOAL_TY = RIGHT_GOAL_TY; public static final double LEFT_GOAL_TX = 0.0; public class ElevatorHeights { - public static final double TELE_LEVEL_FOUR = isCompBot() ? 29.5 - 0.3 : 29.5; //added 0.2 3/20 - public static final double TELE_LEVEL_THREE = isCompBot() ? 16.1 : 16.0; - public static final double TELE_LEVEL_TWO = isCompBot() ? 7.4 : 7.0; + public static final double TELE_LEVEL_FOUR = isCompBot() ? 29.3 : 29.5; + public static final double TELE_LEVEL_THREE = isCompBot() ? 15.9 : 16.0; + public static final double TELE_LEVEL_TWO = isCompBot() ? 7.2 : 7.0; public static final double TELE_LEVEL_ONE = 0.0; - public static final double AUTO_LEVEL_FOUR = isCompBot() ? 29.0 + 0.2 : 29.5; + public static final double AUTO_LEVEL_FOUR = isCompBot() ? 29.0 : 29.5; public static final double AUTO_LEVEL_THREE = 0.0; public static final double AUTO_LEVEL_TWO = 0.0; public static final double AUTO_LEVEL_ONE = 0.0; @@ -117,9 +124,6 @@ public static final class WoodbotConstants { public static final class PracticeBotConstants { public static final String CANBUS_NAME = "Default Name"; - - public static final int SERVO_PORT = 0; // is the actual port :) - public static final int BACK_ELEVATOR_ID = 14; public static final int FRONT_ELEVATOR_ID = 15; @@ -137,12 +141,6 @@ public static final class PracticeBotConstants { public static final int INTAKE_SENSOR_ID = 20; public static final int OUTTAKE_SENSOR_ID = 21; - public static final double RIGHT_GOAL_TY = 12.0; //praccy bot 3/15 - public static final double RIGHT_GOAL_TX = 0.0; - - public static final double LEFT_GOAL_TY = RIGHT_GOAL_TY; - public static final double LEFT_GOAL_TX = 0; - public static final String CORAL_LIMELIGHT_NAME = "limelight-coral"; public static final String ALGAE_LIMELIGHT_NAME = "limelight-algae"; @@ -169,13 +167,6 @@ public static final class CompBotConstants { // Currently just a copy of practic public static final int ALGAE_ROLLER = 23; public static final int ALGAE_TILT = 24; - - public static final double RIGHT_GOAL_TY = 13.75;//practice fiedl: 15.5 | gpeak: 14.0 | auburn: 13.75 - public static final double RIGHT_GOAL_TX = 0.0; - - public static final double LEFT_GOAL_TY = RIGHT_GOAL_TY; - public static final double LEFT_GOAL_TX = 0.0; - public static final String CORAL_LIMELIGHT_NAME = "limelight-coral"; public static final String ALGAE_LIMELIGHT_NAME = "limelight-algae"; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 667979f6..a6524a1f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,12 +9,9 @@ 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.InstantCommand; -import frc.robot.Constants.CompBotConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Elevator.Elevator; import frc.robot.subsystems.Elevator.ElevatorIOSim; -import frc.robot.subsystems.Vision.Vision; import frc.robot.utils.RobotUtils; import org.littletonrobotics.junction.LogFileUtil; @@ -34,11 +31,11 @@ public class Robot extends LoggedRobot { private Command m_autonomousCommand; private CommandSwerveDrivetrain drivetrain; - private Vision vision; private final RobotContainer m_robotContainer; - + public void robotInit() {} + /** * This function is run when the robot is first started up and should be used * for any @@ -83,7 +80,6 @@ public Robot() { // and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - m_robotContainer.onInit(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 02b73c41..9ab0d2b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,4 +1,3 @@ - // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. @@ -17,8 +16,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.simulation.ElevatorSim; @@ -35,24 +34,22 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; //import frc.robot.Constants.PracticeBotConstants.ElevatorHeights; import frc.robot.Constants.*; +import frc.robot.commands.AlignToReefFieldRelative; import frc.robot.commands.AlignWithLimelight; -import frc.robot.commands.BargeAlign; -import frc.robot.commands.HasCoral; -import frc.robot.commands.RemoveAlgae; +import frc.robot.commands.PathOnTheFly; import frc.robot.commands.SetCoralIntake; import frc.robot.commands.SmartIntake; +import frc.robot.commands.RemoveAlgae; import frc.robot.commands.SnapDrivebaseToAngle; import frc.robot.generated.CompBotDriveTrain; import frc.robot.generated.OldCompBot; import frc.robot.generated.PracticeBotDriveTrain; import frc.robot.generated.WoodBotDriveTrain; -import frc.robot.subsystems.AlgaeArm.AlgaeArm; -import frc.robot.subsystems.AlgaeArm.AlgaeArmIOCB; -import frc.robot.subsystems.AlgaeArm.AlgaeArmIOPB; -import frc.robot.subsystems.AlgaeArm.AlgaeArmIOSim; -import frc.robot.subsystems.AlgaeRoller.AlgaeRoller; -import frc.robot.subsystems.AlgaeRoller.AlgaeRollerIOCB; -import frc.robot.subsystems.AlgaeRoller.AlgaeRollerIOPB; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.ClimberWinch.ClimberWinch; +import frc.robot.subsystems.ClimberWinch.ClimberWinchIOCB; +import frc.robot.subsystems.ClimberWinch.ClimberWinchIOPB; +import frc.robot.subsystems.ClimberWinch.ClimberWinchIOSim; import frc.robot.subsystems.AlgaeShooter.AlgaeShooter; import frc.robot.subsystems.AlgaeShooter.AlgaeShooterIOCB; import frc.robot.subsystems.AlgaeShooter.AlgaeShooterIOPB; @@ -64,30 +61,32 @@ import frc.robot.subsystems.ClimberWheel.ClimberWheelIOCB; import frc.robot.subsystems.ClimberWheel.ClimberWheelIOPB; import frc.robot.subsystems.ClimberWheel.ClimberWheelIOSim; -import frc.robot.subsystems.ClimberWinch.ClimberWinch; -import frc.robot.subsystems.ClimberWinch.ClimberWinchIOCB; -import frc.robot.subsystems.ClimberWinch.ClimberWinchIOPB; -import frc.robot.subsystems.ClimberWinch.ClimberWinchIOSim; -import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Elevator.Elevator; +import frc.robot.subsystems.Elevator.ElevatorIOSim; import frc.robot.subsystems.CoralShooter.CoralShooter; import frc.robot.subsystems.CoralShooter.CoralShooterIOCB; import frc.robot.subsystems.CoralShooter.CoralShooterIOPB; import frc.robot.subsystems.CoralShooter.CoralShooterIOSim; import frc.robot.subsystems.CoralShooter.CoralShooterIOWB; -import frc.robot.subsystems.Elevator.Elevator; import frc.robot.subsystems.Elevator.ElevatorIO; import frc.robot.subsystems.Elevator.ElevatorIOCB; import frc.robot.subsystems.Elevator.ElevatorIOPB; import frc.robot.subsystems.Elevator.ElevatorIOSim; import frc.robot.subsystems.Elevator.ElevatorIOSim; -import frc.robot.subsystems.Elevator.ElevatorIOSim; import frc.robot.subsystems.Elevator.ElevatorIOWB; import frc.robot.subsystems.Servo.Servo; import frc.robot.subsystems.Servo.ServoIOCB; -import frc.robot.subsystems.Servo.ServoIOPB; import frc.robot.subsystems.Vision.Vision; import frc.robot.subsystems.Vision.VisionIO; import frc.robot.subsystems.Vision.VisionIOLimelight; +import frc.robot.subsystems.AlgaeArm.AlgaeArm; +import frc.robot.subsystems.AlgaeArm.AlgaeArmIOCB; +import frc.robot.subsystems.AlgaeArm.AlgaeArmIOPB; +import frc.robot.subsystems.AlgaeArm.AlgaeArmIOSim; +import frc.robot.subsystems.AlgaeRoller.AlgaeRoller; +import frc.robot.subsystems.AlgaeRoller.AlgaeRollerIOCB; +import frc.robot.subsystems.AlgaeRoller.AlgaeRollerIOPB; + import java.lang.ModuleLayer.Controller; import java.util.HashMap; import java.util.Map; @@ -120,6 +119,7 @@ public class RobotContainer { private AlgaeRoller algaeRoller; private AlgaeTilt algaeTilt; private Servo servo; + private PathOnTheFly pathOnTheFly; private ShuffleboardTab diagnosticTab; @@ -164,12 +164,11 @@ public class RobotContainer { private Command consumeVisionMeasurements; private boolean isAlgaeMode = false; - private BargeAlign bargeAlign; - private Command hasCoral; public RobotContainer() { switch (Constants.getRobotType()) { case WOODBOT: + driveTrain = WoodBotDriveTrain.createDrivetrain(); logger = new Telemetry(WoodBotDriveTrain.kSpeedAt12Volts.in(MetersPerSecond)); vision = new Vision( @@ -212,6 +211,7 @@ public RobotContainer() { algaeRoller = new AlgaeRoller(new AlgaeRollerIOPB()); algaeShooter = new AlgaeShooter(new AlgaeShooterIOPB()); algaeTilt = new AlgaeTilt(new AlgaeTiltIOPB()); + climberWheel = new ClimberWheel(new ClimberWheelIOPB()); climberWinch = new ClimberWinch(new ClimberWinchIOPB()); vision = new Vision( @@ -232,7 +232,6 @@ public RobotContainer() { // ) )); // practice bot stuff - servo = new Servo(new ServoIOPB()); break; case SIM: driveTrain = WoodBotDriveTrain.createDrivetrain(); @@ -244,14 +243,16 @@ public RobotContainer() { new VisionIOLimelight( Constants.PracticeBotConstants.CORAL_LIMELIGHT_NAME, () -> driveTrain.getAngle(), - () -> driveTrain.getAngularRate())), - Map.entry( - Constants.PracticeBotConstants.ALGAE_LIMELIGHT_NAME, - new VisionIOLimelight( - Constants.PracticeBotConstants.ALGAE_LIMELIGHT_NAME, - () -> driveTrain.getAngle(), - () -> driveTrain.getAngularRate(), - false)))); + () -> driveTrain.getAngularRate())) + // Map.entry( + // Constants.PracticeBotConstants.ALGAE_LIMELIGHT_NAME, + // new VisionIOLimelight( + // Constants.PracticeBotConstants.ALGAE_LIMELIGHT_NAME, + // () -> driveTrain.getAngle(), + // () -> driveTrain.getAngularRate() + // ) + // ) + )); elevator = new Elevator(new ElevatorIOSim()); algaeArm = new AlgaeArm(new AlgaeArmIOSim(() -> elevator.getHeight())); coralShooter = new CoralShooter(new CoralShooterIOSim(() -> elevator.getHeight())); @@ -278,15 +279,15 @@ public RobotContainer() { new VisionIOLimelight( Constants.CompBotConstants.CORAL_LIMELIGHT_NAME, () -> driveTrain.getAngle(), - () -> driveTrain.getAngularRate(), - true)), + () -> driveTrain.getAngularRate(), true)), Map.entry( Constants.PracticeBotConstants.ALGAE_LIMELIGHT_NAME, new VisionIOLimelight( Constants.PracticeBotConstants.ALGAE_LIMELIGHT_NAME, () -> driveTrain.getAngle(), - () -> driveTrain.getAngularRate(), - false)))); + () -> driveTrain.getAngularRate(), false) + + ))); servo = new Servo(new ServoIOCB()); // competition bot stuff break; @@ -297,15 +298,17 @@ public RobotContainer() { elevator, vision, climberWinch, + climberWheel, algaeShooter, algaeArm, driveTrain, driverCont, algaeTilt, algaeRoller, - servo); + servo, + pathOnTheFly); - initializeCommands(); + // initializeCommands(); field = new Field2d(); SmartDashboard.putData("Field", field); @@ -326,8 +329,9 @@ public RobotContainer() { diagnosticTab.addString("Serial Address", HALUtil::getSerialNumber); diagnosticTab.addBoolean("Sim", Constants::isSim); - configureBindings(); - // configureTestController(); + // configureBindings(); + + configureTestController(); } public void initializeCommands() { @@ -349,28 +353,25 @@ public void initializeCommands() { zeroElevatorEncoder = elevator.zeroElevatorCmd(); levelOneAndZero = new SequentialCommandGroup(levelOne, zeroElevatorEncoder); - NamedCommands.registerCommand( + registerPathplannerCommand( "raise to l4", commandFactory.setElevatorHeight(33.0).raceWith(elevator.isAtHeight(33.0))); - NamedCommands.registerCommand( + registerPathplannerCommand( "zero", commandFactory.setElevatorHeight(0.0).raceWith(elevator.isAtHeight(0.0))); } if (Objects.nonNull(driveTrain)) { rightAlign = commandFactory.alignWithLimelight( - Constants.CompBotConstants.RIGHT_GOAL_TY, - Constants.CompBotConstants.RIGHT_GOAL_TX, - 0, - driverCont); + Constants.SetPointConstants.RIGHT_GOAL_TY, + Constants.SetPointConstants.RIGHT_GOAL_TX, + 0, driverCont); // Periodically adds the vision measurement to drivetrain for pose estimation leftAlign = commandFactory.alignWithLimelight( - Constants.CompBotConstants.LEFT_GOAL_TY, - Constants.CompBotConstants.LEFT_GOAL_TX, - 1, - driverCont); - + Constants.SetPointConstants.LEFT_GOAL_TY, + Constants.SetPointConstants.LEFT_GOAL_TX, + 1, driverCont); } registerPathplannerCommand("Elevator L4", autoLevelFour); @@ -398,7 +399,8 @@ public void initializeCommands() { scoreLevel3RightTeleop = commandFactory.scoringRoutineTeleop(3, false); - removeAlgae = new RemoveAlgae(algaeArm, algaeShooter, algaeTilt, coralShooter, elevator, vision); + removeAlgae = new RemoveAlgae(3, algaeArm, algaeShooter, algaeTilt, coralShooter, elevator, driveTrain); + } registerPathplannerCommand("Score Coral L4 Left", scoreCoralL4Left); @@ -422,12 +424,11 @@ public void initializeCommands() { intake = coralShooter.basicIntakeCmd(); smartIntake = SmartIntake.newCommand(coralShooter); - hasCoral = commandFactory.hasCoral(elevator, coralShooter); - NamedCommands.registerCommand("shoot", coralShooter.basicShootCmd()); - NamedCommands.registerCommand("intake", smartIntake); - NamedCommands.registerCommand("hasCoral", hasCoral); + registerPathplannerCommand("shoot", coralShooter.basicShootCmd()); + registerPathplannerCommand("intake", smartIntake); } + // registerPathplannerCommand("Intake Coral", intake); } @@ -452,6 +453,7 @@ private void registerPathplannerCommand(String commandName, Command command) { } private void configureBindings() { + vision.setDefaultCommand(consumeVisionMeasurements.ignoringDisable(true)); algaeTilt.setDefaultCommand(commandFactory.homeAlgaeTilt()); @@ -460,109 +462,87 @@ private void configureBindings() { // elevator.setDefaultCommand(elevator.setDutyCycleCommand(() -> // testCont.getLeftY() * 0.1)); - // algaeRoller.setDefaultCommand(algaeRoller.setDutyCycleCmd(() -> - // testCont.getLeftY())); - // testCont.a().whileTrue(algaeRoller.setDutyCycleCmd(0.5)); - // testCont.a().whileTrue(algaeArm.zeroPositionAndZeroArm()); // driverCont.rightStick().toggleOnTrue(isAlgaeMode); // testCont.leftTrigger(0.25).whileTrue(smartIntake); // testCont.rightTrigger(.25).whileTrue(coralShooter.basicShootCmd()); - // testCont.pov(0).whileTrue(commandFactory.climb()); - - operatorCont.leftStick().toggleOnTrue(algaeTilt.setDutyCycleCmd(() -> operatorCont.getLeftY() * 0.1)); - operatorCont.rightStick().toggleOnTrue(elevator.setDutyCycleCommand(() -> operatorCont.getRightY() * -0.1)); - operatorCont.leftBumper().whileTrue(algaeRoller.setDutyCycleCmd(-0.1)); operatorCont.rightBumper().whileTrue(algaeRoller.setDutyCycleCmd(1.0)); operatorCont.y().whileTrue(algaeTilt.setPositionCmd(0.001)); // 0.001 used to be 0 - operatorCont.x().whileTrue(algaeTilt.setPositionCmd(0.03)); // 0.065 used to be 3 + operatorCont.x().whileTrue(algaeTilt.setPositionCmd(0.03)); // .065 used to be 3 operatorCont.b().whileTrue(algaeTilt.setPositionCmd(0.253)); // 0.244 used to be 30 operatorCont.a().whileTrue(algaeTilt.setPositionCmd(0.32)); // 0.361 used to be 35 - operatorCont.pov(90).whileTrue(commandFactory.operatorOutakeAlgae()); - operatorCont.pov(270).whileTrue(commandFactory.operatorIntakeAlgae()); + operatorCont.pov(90).whileTrue(commandFactory.outtakeAlgaeFromGround()); + operatorCont.pov(270).whileTrue(commandFactory.intakeAlgaeFromGround()); operatorCont.pov(180).whileTrue(commandFactory.shootAlgae()); operatorCont.pov(0).whileTrue(commandFactory.climb()); operatorCont.leftTrigger(0.25).whileTrue(coralShooter.setDutyCycleCmd(0.3)); - operatorCont.rightTrigger(0.25).whileTrue(commandFactory.spinUpAlgaeShooter()); - + // operatorCont.rightTrigger(0.25).whileTrue(coralShooter.setDutyCycleCmd(-0.4)); // if (Math.abs(operatorCont.getLeftY()) > 0.05) { // algaeArm.setDutyCycleCmd(operatorCont.getLeftY()); // } - // testCont.a().whileTrue(bargeAlign); - // testCont.leftTrigger().whileTrue(commandFactory.driverIntakeAlgae()); - // driveTrain.setDefaultCommand(driveTrain.fieldOrientedDrive(testCont)); - // testCont.pov(0).onTrue(new InstantCommand(() -> driveTrain.zero(), - // driveTrain)); driveTrain.setDefaultCommand(driveTrain.fieldOrientedDrive(driverCont)); // driverCont.rightStick().whileTrue(driveTrain.robotCentricDrive(driverCont)); - driverCont.leftStick().whileTrue(removeAlgae); + driverCont.pov(180).whileTrue(removeAlgae); driverCont.pov(0).onTrue(new InstantCommand(() -> driveTrain.zero(), driveTrain)); driverCont.start().onTrue(commandFactory.depolyAndInitiateClimb()); - driverCont.back().onTrue(commandFactory.climbAutomated()); + driverCont.back().whileTrue(commandFactory.climbAutomated()); // driverCont.pov(180).onTrue(commandFactory.setAlgaeArmAngle(0.0)); - driverCont - .rightStick() - .onTrue( - new InstantCommand(() -> toggleIsAlgaeMode()) - .andThen( - Commands.either( - Commands.none(), - commandFactory.homeAlgaeTilt(), - () -> isAlgaeMode)) - .alongWith( - Commands.either( - Commands.none(), - elevator.setElevatorHeight(0.0), - () -> !isAlgaeMode)) - .alongWith( - Commands.either( - new InstantCommand( - () -> vision - .turnOnLights(CompBotConstants.ALGAE_LIMELIGHT_NAME)), - new InstantCommand( - () -> vision - .turnOffLights(CompBotConstants.ALGAE_LIMELIGHT_NAME)), - () -> isAlgaeMode))); - - driverCont.leftTrigger(0.25).and(() -> !isAlgaeMode).whileTrue(smartIntake); - driverCont.leftTrigger(0.25).and(() -> isAlgaeMode).whileTrue(commandFactory.driverIntakeAlgae()); - - driverCont.rightTrigger(0.25).and(() -> !isAlgaeMode).whileTrue(coralShooter.basicShootCmd()); - driverCont.rightTrigger(0.25).and(() -> isAlgaeMode).whileTrue(commandFactory.shootAlgae()); - - driverCont.a().and(() -> !isAlgaeMode).onTrue(levelOneAndZero); - driverCont.a().and(() -> isAlgaeMode).onTrue(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.32 : 35.0)); - - driverCont.x().and(() -> !isAlgaeMode).onTrue(levelTwo); - driverCont.x().and(() -> isAlgaeMode).onTrue(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.03 : 3.0)); - - driverCont.b().and(() -> !isAlgaeMode).onTrue(levelThree); - driverCont.b().and(() -> isAlgaeMode).onTrue(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.253 : 30)); - - driverCont.y().and(() -> !isAlgaeMode).onTrue(levelFour); - driverCont.y().and(() -> isAlgaeMode).onTrue(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.001 : 0.0)); - - driverCont.leftBumper().and(() -> !isAlgaeMode).whileTrue(leftAlign); - driverCont.leftBumper().and(() -> isAlgaeMode).whileTrue(algaeRoller - .setDutyCycleCmd(-0.1) - .alongWith(driveTrain.fieldOrientedDrive(driverCont))); - - driverCont.rightBumper().and(() -> !isAlgaeMode).whileTrue(rightAlign); - driverCont.rightBumper().and(() -> isAlgaeMode).whileTrue(commandFactory.driverProcessAlgae()); + driverCont.leftTrigger(0.25).whileTrue(Commands.either( + commandFactory.intakeAlgaeFromGround(), + smartIntake, + () -> isAlgaeMode)); + + driverCont.rightTrigger(0.25).whileTrue(Commands.either( + commandFactory.shootAlgae(), + coralShooter.basicShootCmd(), + () -> isAlgaeMode)); + + driverCont.rightStick().onTrue(new InstantCommand(() -> toggleIsAlgaeMode()) + .andThen(Commands.either(Commands.none(), commandFactory.homeAlgaeTilt(), () -> isAlgaeMode))); + + driverCont.a() + .onTrue(Commands.either( + algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.32 : 35.0), + levelOneAndZero, + () -> isAlgaeMode)); + driverCont.x() + .onTrue(Commands.either( + algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.03 : 3.0), + levelTwo, + () -> isAlgaeMode)); + + driverCont.b().onTrue(Commands.either( + algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.253 : 30), + levelThree, + () -> isAlgaeMode)); + + driverCont.y().onTrue(Commands.either( + algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.001 : 0.0), + levelFour, + () -> isAlgaeMode)); + + driverCont.leftBumper().whileTrue(Commands.either( + algaeRoller.setDutyCycleCmd(-0.1), + leftAlign, + () -> isAlgaeMode)); + driverCont.rightBumper().whileTrue(Commands.either( + algaeRoller.setDutyCycleCmd(1.0), + rightAlign, + () -> isAlgaeMode)); // if (Objects.nonNull(coralShooter)) { // driverCont.leftBumper().whileTrue(leftAlign); @@ -572,7 +552,7 @@ private void configureBindings() { // testCont.leftBumper().onTrue(Commands.runOnce(SignalLogger::start)); // testCont.rightBumper().onTrue(Commands.runOnce(SignalLogger::stop)); /* - * + * * Joystick Y = quasistatic forward * Joystick A = qu * asistatic reverse @@ -588,8 +568,6 @@ private void configureBindings() { // testCont.a().whileTrue(driveTrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); // testCont.b().whileTrue(driveTrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); // testCont.x().whileTrue(driveTrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - - // testCont.a().whileTrue(commandFactory.newDeploy()); } private void toggleIsAlgaeMode() { @@ -597,17 +575,17 @@ private void toggleIsAlgaeMode() { } private void configureTestController() { + driveTrain.setDefaultCommand(driveTrain.fieldOrientedDrive(testCont)); + + testCont.rightBumper().whileTrue(AlignToReefFieldRelative.moveToReef(driveTrain, driveTrain::getPose, true)); + testCont.leftBumper().whileTrue(AlignToReefFieldRelative.moveToReef(driveTrain, driveTrain::getPose, false)); // elevator.setDefaultCommand( // elevator.setDutyCycleCommand(() -> // MathUtil.applyDeadband(testCont.getLeftY(), 0.1))); // algaeTilt.setDefaultCommand(commandFactory.homeAlgaeTilt()); // algaeTilt.setDefaultCommand(commandFactory.homeAlgaeTilt()); - // servo.setDefaultCommand(servo.setSpeedCmd(() -> testCont.getLeftY())); - // driveTrain.setDefaultCommand(driveTrain.fieldOrientedDrive(driverCont)); - - // testCont.a().whileTrue(servo.setSpeedCmd(0)); - + // servo.setDefaultCommand(servo.setServoSpeedCmd(() -> testCont.getLeftY())); // testCont.a().whileTrue(servo.setPositionCmd(0)); // testCont.b().whileTrue(servo.setPositionCmd(1.0)); // testCont.x().whileTrue(servo.setPositionCmd(-1.0)); @@ -640,7 +618,7 @@ public void onDisable() { coralShooter.stop(); if (Objects.nonNull(algaeArm)) algaeArm.stop(); - if (Objects.nonNull(algaeArm)) + if (Objects.nonNull(algaeRoller)) algaeRoller.stop(); if (Objects.nonNull(algaeShooter)) algaeShooter.stop(); @@ -652,18 +630,9 @@ public void onDisable() { climberWheel.stop(); if (Objects.nonNull(servo)) servo.stop(); - if (Objects.nonNull(vision)) - vision.setPipeline(CompBotConstants.CORAL_LIMELIGHT_NAME, 0); - vision.turnOffLights(CompBotConstants.ALGAE_LIMELIGHT_NAME); } public Command getAutonomousCommand() { return autoChooser.getSelected(); } - - public void onInit() { - if (Objects.nonNull(vision)) { - vision.turnOffLights(CompBotConstants.CORAL_LIMELIGHT_NAME); - } - } } diff --git a/src/main/java/frc/robot/commands/AlignToReefFieldRelative.java b/src/main/java/frc/robot/commands/AlignToReefFieldRelative.java new file mode 100644 index 00000000..c9fea46f --- /dev/null +++ b/src/main/java/frc/robot/commands/AlignToReefFieldRelative.java @@ -0,0 +1,301 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import java.util.Collections; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; +import java.util.Set; +import java.util.function.Supplier; +import java.util.stream.Collectors; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +/** Add your docs here. */ +public class AlignToReefFieldRelative { + private static class ReefPositions { + private static final Map LEFT_TAG_ID_TO_POSITION_OFFSET = Map.ofEntries( + Map.entry( + 6, + new Translation2d(0, 0)), + Map.entry( + 7, + new Translation2d(0, 0)), + Map.entry( + 8, + new Translation2d(0, 0)), + Map.entry( + 9, + new Translation2d(0, 0)), + Map.entry( + 10, + new Translation2d(0, 0)), + Map.entry( + 11, + new Translation2d(0, 0)), + Map.entry( + 17, + new Translation2d(0, 0)), + Map.entry( + 18, + new Translation2d(0, 0)), + Map.entry( + 19, + new Translation2d(0, 0)), + Map.entry( + 20, + new Translation2d(0, 0)), + Map.entry( + 21, + new Translation2d(0, 0)), + Map.entry( + 22, + new Translation2d(0, 0))); + // RIGHT STARTS HERE + private static final Map RIGHT_TAG_ID_TO_POSITION = Map.ofEntries( + Map.entry( + 6, + new Translation2d(0, 0)), + Map.entry( + 7, + new Translation2d(0, 0)), + Map.entry( + 8, + new Translation2d(0, 0)), + Map.entry( + 9, + new Translation2d(0, 0)), + Map.entry( + 10, + new Translation2d(0, 0)), + Map.entry( + 11, + new Translation2d(0, 0)), + Map.entry( + 17, + new Translation2d(0, 0)), + Map.entry( + 18, + new Translation2d(0, 0)), + Map.entry( + 19, + new Translation2d(0, 0)), + Map.entry( + 20, + new Translation2d(0, 0)), + Map.entry( + 21, + new Translation2d(0, 0)), + Map.entry( + 22, + new Translation2d(0, 0))); + + private static final Set REEF_TAG_IDS_RED = Set.of(6, 7, 8, 9, 10, 11); + private static final Set REEF_TAG_IDS_BLUE = Set.of(17, 18, 19, 20, 21, 22); + private static final Set REEF_TAG_IDS = Set.of(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22); + + private static final List REEF_TAGS = Constants.FIELD_LAYOUT.getTags().stream() + .filter(tag -> REEF_TAG_IDS.contains(tag.ID)) + .collect(Collectors.collectingAndThen(Collectors.toList(), Collections::unmodifiableList)); + private static final List REEF_TAGS_RED = Constants.FIELD_LAYOUT.getTags().stream() + .filter(tag -> REEF_TAG_IDS_RED.contains(tag.ID)) + .collect(Collectors.collectingAndThen(Collectors.toList(), Collections::unmodifiableList)); + private static final List REEF_TAGS_BLUE = Constants.FIELD_LAYOUT.getTags().stream() + .filter(tag -> REEF_TAG_IDS_BLUE.contains(tag.ID)) + .collect(Collectors.collectingAndThen(Collectors.toList(), Collections::unmodifiableList)); + + private static final Map LEFT_TAG_ID_TO_POSITION_DYNAMIC = REEF_TAGS.stream() + .map(tag -> createTagScoringPositionEntry(tag, false)) + .collect(Collectors.toMap( + Map.Entry::getKey, + Map.Entry::getValue)); + + private static final Map RIGHT_TAG_ID_TO_POSITION_DYNAMIC = REEF_TAGS.stream() + .map(tag -> createTagScoringPositionEntry(tag, true)) + .collect(Collectors.toMap( + Map.Entry::getKey, + Map.Entry::getValue)); + + private static Entry createTagScoringPositionEntry(AprilTag tag, boolean right) { + return Map.entry(tag.ID, transformTagPoseToScoringPose(tag, right)); + } + + private static Pose2d transformTagPoseToScoringPose(AprilTag tag, boolean right) { + Pose2d tagPose = tag.pose.toPose2d(); + Rotation2d tagRotation = tagPose.getRotation(); + + if (right) { + Translation2d offset = RIGHT_TAG_ID_TO_POSITION.get(tag.ID); + // x is forwards-backwards, y is right-left relative to the tag position + Translation2d tagTranslation = new Translation2d(0.45, 0.14).plus(offset); + tagTranslation.rotateBy(tagRotation); + return tagPose.plus(new Transform2d(tagTranslation, Rotation2d.k180deg)); + } else { + Translation2d offset = LEFT_TAG_ID_TO_POSITION_OFFSET.get(tag.ID); + // x is forwards-backwards, y is right-left relative to the tag position + Translation2d tagTranslation = new Translation2d(0.45, -0.14).plus(offset); + tagTranslation.rotateBy(tagRotation); + return tagPose.plus(new Transform2d(tagTranslation, Rotation2d.k180deg)); + } + } + } + + /** + * Initializes constants used for reef alignment + */ + private static void initializeConstants(){ + List tags = ReefPositions.REEF_TAGS; + tags = ReefPositions.REEF_TAGS_RED; + tags = ReefPositions.REEF_TAGS_BLUE; + + Map tagIDPositionMapRight = ReefPositions.RIGHT_TAG_ID_TO_POSITION_DYNAMIC; + Map tagIDPositionMapLeft = ReefPositions.LEFT_TAG_ID_TO_POSITION_DYNAMIC; + + Set reefTagIDs = ReefPositions.REEF_TAG_IDS; + } + + private static final String LOGGING_PREFIX = "AlignToReefFieldRelative: "; + + + /** + * WORK IN PROGRESS + * Moves to the closest reef scoring position on the robot's alliance side + * + * @param drivetrain + * @param currentBotPose supplier for the robot's current position + * @param right if true, move to the right pole of the reef. If false, + * move to the left pole + * @return Command to path find to the reef + */ + public static Command moveToReefOptimized(CommandSwerveDrivetrain drivetrain, Supplier currentBotPose, boolean right) { + initializeConstants(); + // Create commands + return findNearestTagFirst(currentBotPose.get()).andThen( + Commands.either( + // Red Alliance + moveToReefRightOrLeft(drivetrain, currentBotPose, right, true), + // Blue Alliance + moveToReefRightOrLeft(drivetrain, currentBotPose, right, false), + () -> isRedAlliance() + ) + ); + } + + private static boolean isRedAlliance() { + return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; + } + + private static int nearestTag = 6; + + /** + * Sets nearest tag based on odometry in a command + */ + private static Command findNearestTagFirst(Pose2d currentBotPose) { + return new InstantCommand(() -> nearestTag = getNearestReefTagID(currentBotPose, isRedAlliance())); + } + + /** + * Moves to the closest reef scoring position on the robot's alliance side + * + * @param drivetrain + * @param currentBotPose supplier for the robot's current position + * @param right if true, move to the right pole of the reef. If false, + * move to the left pole + * @return Command to path find to the reef + */ + public static Command moveToReef(CommandSwerveDrivetrain drivetrain, Supplier currentBotPose, boolean right) { + initializeConstants(); + // Create commands + return Commands.either( + // Red Alliance + moveToReefRightOrLeft(drivetrain, currentBotPose, right, true), + // Blue Alliance + moveToReefRightOrLeft(drivetrain, currentBotPose, right, false), + () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red + ); + } + + private static Command moveToReefRightOrLeft(CommandSwerveDrivetrain drivetrain, Supplier currentBotPose, boolean right, boolean isRed) { + Map tagIDPositionMapRight = logPositions(ReefPositions.RIGHT_TAG_ID_TO_POSITION_DYNAMIC, "RIGHT_TAG_ID_TO_POSITION_DYNAMIC"); + Map tagIDPositionMapLeft = logPositions(ReefPositions.LEFT_TAG_ID_TO_POSITION_DYNAMIC, "LEFT_TAG_ID_TO_POSITION_DYNAMIC"); + return Commands.either( + moveToReefHelper(drivetrain, currentBotPose, tagIDPositionMapRight, isRed), + moveToReefHelper(drivetrain, currentBotPose, tagIDPositionMapLeft, isRed), + () -> right); + } + + private static Map logPositions(Map tagIDPositionMap, String name) { + for (int key : tagIDPositionMap.keySet()) { + Logger.recordOutput(LOGGING_PREFIX + "Reef " + name + ": " + key, tagIDPositionMap.get(key)); + } + return tagIDPositionMap; + } + /** + * Helper method to fill out the select commands for the left and right sides of + * the reef + * + * @param drivetrain + * @param currentBotPose + * @param tagIDPositionMap + * @return + */ + private static Command moveToReefHelper(CommandSwerveDrivetrain drivetrain, Supplier currentBotPose, + Map tagIDPositionMap, boolean isRed) { + return Commands.select(buildMoveToReefCommandMap(drivetrain, tagIDPositionMap), + () -> getNearestReefTagID(currentBotPose.get(), isRed)); + } + + private static Map buildMoveToReefCommandMap(CommandSwerveDrivetrain drivetrain, Map tagIDPositionMap) { + return ReefPositions.REEF_TAG_IDS.stream() + .map(tagId -> buildMoveToReefCommandEntry(drivetrain, tagId, tagIDPositionMap)) + .collect(Collectors.toMap( + Map.Entry::getKey, + Map.Entry::getValue)); + } + + private static Entry buildMoveToReefCommandEntry(CommandSwerveDrivetrain drivetrain, int tagID, + Map tagIDPositionMap) { + Pose2d endPose = tagIDPositionMap.get(tagID); + return Map.entry(tagID, DriveToPose.getCommand(drivetrain, endPose)); + } + + private static int getNearestReefTagID(Pose2d currentBotPose, boolean isRed) { + List tags = ReefPositions.REEF_TAGS; + // If red alliance + if (isRed) { + tags = ReefPositions.REEF_TAGS_RED; + // If blue alliance + } else { + tags = ReefPositions.REEF_TAGS_BLUE; + } + return getNearestTagID(currentBotPose, tags); + } + + private static int getNearestTagID(Pose2d currentBotPose, List tags) { + AprilTag closestTag = tags.get(0); + double minDist = Double.MAX_VALUE; + for (AprilTag tag : tags) { + double distance = currentBotPose.getTranslation().getDistance(tag.pose.toPose2d().getTranslation()); + if (distance < minDist) { + closestTag = tag; + minDist = distance; + } + } + return closestTag.ID; + }} diff --git a/src/main/java/frc/robot/commands/AlignWithLimelight.java b/src/main/java/frc/robot/commands/AlignWithLimelight.java index 3b3a5d23..fcb3926f 100644 --- a/src/main/java/frc/robot/commands/AlignWithLimelight.java +++ b/src/main/java/frc/robot/commands/AlignWithLimelight.java @@ -35,34 +35,38 @@ public class AlignWithLimelight extends Command { private SlewRateLimiter forwardsAccelerationLimit = new SlewRateLimiter(0.75); private SlewRateLimiter leftAccelerationLimit = new SlewRateLimiter(0.75); private int pipeline; - + private CommandXboxController driverCont; private final String LIMELIGHT_NAME = Constants.PracticeBotConstants.CORAL_LIMELIGHT_NAME; private final String CMD_NAME = "AlignWithLimelight: "; private static final Map tagIDToAngle = Map.ofEntries( - Map.entry(21, 180.0), - Map.entry(7, 180.0), - Map.entry(22, 120.0), - Map.entry(6, 120.0), - Map.entry(17, 60.0), - Map.entry(11, 60.0), - Map.entry(18, 0.0), - Map.entry(10, 0.0), - Map.entry(19, -60.0), - Map.entry(9, -60.0), - Map.entry(20, -120.0), - Map.entry(8, -120.0)); + Map.entry(21, 180.0), + Map.entry(7, 180.0), + Map.entry(22, 120.0), + Map.entry(6, 120.0), + Map.entry(17, 60.0), + Map.entry(11, 60.0), + Map.entry(18, 0.0), + Map.entry(10, 0.0), + Map.entry(19, -60.0), + Map.entry(9, -60.0), + Map.entry(20, -120.0), + Map.entry(8, -120.0) + ); + + /** Creates a new AlignWithLimelight. */ public AlignWithLimelight( - Vision vision, - CommandSwerveDrivetrain driveTrain, - double goalTY, - double goalTX, - int pipeline, - CommandXboxController driverCont) { + Vision vision, + CommandSwerveDrivetrain driveTrain, + double goalTY, + double goalTX, + int pipeline, + CommandXboxController driverCont + ) { this.vision = vision; this.driveTrain = driveTrain; this.goalTY = goalTY; @@ -103,36 +107,40 @@ public void initialize() { Logger.recordOutput(CMD_NAME + "GoalTx", goalTX); Logger.recordOutput(CMD_NAME + "GoalTy", goalTY); - Logger.recordOutput(CMD_NAME + "endEarly", endEarly); LimelightHelpers.setPriorityTagID("limelight", priorityID); - - if(endEarly) return; - driveRobot(); } public static Translation2d rotateTranslation( - Translation2d translationToRotate, - Rotation2d rotation) { + Translation2d translationToRotate, + Rotation2d rotation + ) { return translationToRotate.rotateBy(rotation); } - private void driveRobot() { + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (endEarly) return; + double velX = -driveTrain.forwardController.calculate( - vision.getTYRaw(LIMELIGHT_NAME), - goalTY, - driveTrain.getState().Timestamp); + vision.getTYRaw(LIMELIGHT_NAME), + goalTY, + driveTrain.getState().Timestamp + ); double velY = driveTrain.strafeController.calculate( - vision.getTXRaw(LIMELIGHT_NAME), - goalTX, - driveTrain.getState().Timestamp); + vision.getTXRaw(LIMELIGHT_NAME), + goalTX, + driveTrain.getState().Timestamp + ); Logger.recordOutput(CMD_NAME + "PID OutputX", velX); Logger.recordOutput(CMD_NAME + "PID OutputY", velY); Translation2d PIDSpeed = new Translation2d( - forwardsAccelerationLimit.calculate(velX), - leftAccelerationLimit.calculate(velY)); + forwardsAccelerationLimit.calculate(velX), + leftAccelerationLimit.calculate(velY) + ); Translation2d rotatedPIDSpeeds = rotateTranslation(PIDSpeed, angleToFaceRotation2d); @@ -144,46 +152,36 @@ private void driveRobot() { if (vision.getTV(LIMELIGHT_NAME) == 1) { driveTrain.driveFieldCentricFacingAngle( - rotatedVelocityX, // forward & backward motion - rotatedVelocityY, - angleToFaceRotation2d.getDegrees() // side to side motion + rotatedVelocityX, // forward & backward motion + rotatedVelocityY, + angleToFaceRotation2d.getDegrees() // side to side motion ); } else { driveTrain.driveFieldCentricFacingAngle(0.0, 0.0, angleToFaceRotation2d.getDegrees()); } } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (endEarly) return; - driveRobot(); - } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + + // LimelightHelpers.SetFiducialIDFiltersOverride("limelight", new int[] { 6, 7, 8 }); LimelightHelpers.setPriorityTagID("limelight", -1); - driveTrain.robotCentricDrive(0.0, 0.0, 0.0); + driveTrain.xOut(); } - + // Returns true when the command should end. @Override public boolean isFinished() { boolean onTX = driveTrain.strafeController.atSetpoint(); boolean onTY = driveTrain.forwardController.atSetpoint(); boolean onHeading = driveTrain.isAtRotationSetpoint(); - boolean onCorrectPipeline = (vision.getPipeline(LIMELIGHT_NAME) == pipeline); - boolean vel0 = ((Math.abs(driveTrain.getXRate()) <= 0.1) && (Math.abs(driveTrain.getYRate()) <= 0.1)); Logger.recordOutput(CMD_NAME + "onTX", onTX); Logger.recordOutput(CMD_NAME + "onTY", onTY); Logger.recordOutput(CMD_NAME + "setPointTX", goalTX); Logger.recordOutput(CMD_NAME + "setPointTY", goalTY); Logger.recordOutput(CMD_NAME + "onHeading", onHeading); - Logger.recordOutput(CMD_NAME + "using correct pipeline", onCorrectPipeline); - Logger.recordOutput(CMD_NAME + "vel is 0", vel0); - - return endEarly || (vel0 && onCorrectPipeline && onTX && onTY && onHeading && vision.isTargetInView(LIMELIGHT_NAME)); + return endEarly || (onTX && onTY && onHeading && vision.isTargetInView(LIMELIGHT_NAME)); } } diff --git a/src/main/java/frc/robot/commands/BargeAlign.java b/src/main/java/frc/robot/commands/BargeAlign.java deleted file mode 100644 index 0410e8fd..00000000 --- a/src/main/java/frc/robot/commands/BargeAlign.java +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants; -import frc.robot.Constants.*; -import frc.robot.subsystems.AlgaeRoller.AlgaeRoller; -import frc.robot.subsystems.AlgaeShooter.AlgaeShooter; -import frc.robot.subsystems.AlgaeTilt.AlgaeTilt; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Vision.Vision; -import java.util.Map; -import java.util.Optional; -import org.littletonrobotics.junction.Logger; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class BargeAlign extends Command { - private final CommandSwerveDrivetrain driveTrain; - private final CommandXboxController cont; - private final AlgaeShooter algaeShooter; - private final AlgaeTilt algaeTilt; - private final AlgaeRoller algaeRoller; - private final Vision vision; - private boolean endEarly = false; - - private double angle = 0.0; - private double goalTY = -1.0; - private int id; - - private final String CMD_NAME = "Barge Align: "; - - private static final Map tagIDToAngle = Map.ofEntries( //blue pov - Map.entry(15, -90.0), - Map.entry(14, -90.0), - Map.entry(4, 90.0), - Map.entry(5, 90.0) - ); - - /** Creates a new BargeAlign. */ - public BargeAlign( - CommandSwerveDrivetrain driveTrain, - Vision vision, - AlgaeShooter algaeShooter, - AlgaeTilt algaeTilt, - AlgaeRoller algaeRoller, - CommandXboxController cont - ) { - this.driveTrain = driveTrain; - this.vision = vision; - this.algaeShooter = algaeShooter; - this.cont = cont; - this.algaeTilt = algaeTilt; - this.algaeRoller = algaeRoller; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(driveTrain, algaeShooter, algaeTilt, algaeRoller); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - System.out.println("Running"); - id = vision.getAprilTagID(Constants.isCompBot() ? CompBotConstants.ALGAE_LIMELIGHT_NAME - : PracticeBotConstants.ALGAE_LIMELIGHT_NAME); - - endEarly = true; - if (vision.getTV(Constants.isCompBot() ? CompBotConstants.ALGAE_LIMELIGHT_NAME - : PracticeBotConstants.ALGAE_LIMELIGHT_NAME) == 1 && tagIDToAngle.containsKey(id)) { - endEarly = false; - angle = tagIDToAngle.get(id); - } - if (endEarly) return; - - Logger.recordOutput(CMD_NAME + "angle", angle); - state = AlgaeShooterStates.SET_POSE_AND_ROT; - } - private enum AlgaeShooterStates{ - SET_POSE_AND_ROT, - SHOOT - } - - private AlgaeShooterStates state = AlgaeShooterStates.SET_POSE_AND_ROT; - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(driveTrain.forwardController.atSetpoint() && driveTrain.headingController.atSetpoint() && algaeShooter.getVelocity() > 5850){ - state = AlgaeShooterStates.SHOOT; - } - Logger.recordOutput(CMD_NAME + "State " , state); - - switch(state){ - case SET_POSE_AND_ROT: - setPose(); - break; - case SHOOT: - setPose(); - algaeRoller.setDutyCycle(1.0); - break; - } - // System.out.println("Executing"); - // return Commands.waitUntil(() -> algaeShooter.getVelocity() > 5750) - // .andThen(algaeRoller.setDutyCycleCmd(1.0)) - // .alongWith(algaeShooter.setVelocityCmd(6250)) - // .alongWith(algaeTilt.setPositionCmd(Constants.isCompBot() ? 0.03 : 3.0)); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - algaeRoller.setDutyCycle(0.0); - algaeShooter.setDutyCycle(0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - // //shooter is at vel angle is good ty is good - // boolean onTY = driveTrain.forwardController.atSetpoint(); - // boolean atAngle = driveTrain.headingController.atSetpoint(); - // boolean atVelocity = algaeShooter.getVelocity() > 5750; - - // Logger.recordOutput(CMD_NAME + "onTY", onTY); - // Logger.recordOutput(CMD_NAME + "atAngle", atAngle); - // Logger.recordOutput(CMD_NAME + "atVelocity", atVelocity); - - // return onTY && atAngle && atVelocity; - return endEarly; - } - private void setPose(){ - algaeShooter.setVelocity(6250.0); - algaeTilt.setPosition(Constants.isCompBot() ? 0.03 : 3.0); - - double velX = driveTrain.forwardController.calculate( - vision.getTYRaw(PracticeBotConstants.ALGAE_LIMELIGHT_NAME), - goalTY, - driveTrain.getState().Timestamp - ); - - velX = velX * -Math.signum(angle); - - Logger.recordOutput(CMD_NAME + "velX", velX); - - double velY = Math.pow(MathUtil.applyDeadband(-cont.getLeftX(), 0.1), 3) * driveTrain.getMaxSpeed(); - - Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent() && alliance.get() == Alliance.Red) { - velY = -velY; - } - Logger.recordOutput(CMD_NAME + "vely", velY); - - - driveTrain.driveFieldCentricFacingAngleBluePerspective(velX, velY, angle); - } -} diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java new file mode 100644 index 00000000..3a48a61d --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -0,0 +1,109 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.utils.CommandLogger; +import frc.robot.utils.RobotUtils; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class DriveToPose extends Command { + private final CommandSwerveDrivetrain drivetrain; + private final Pose2d setpointPose; + + /** Creates a new FaceAngle. */ + private DriveToPose(CommandSwerveDrivetrain drivetrain, Pose2d setpointPose) { + this.setpointPose = setpointPose; + this.drivetrain = drivetrain; + addRequirements(drivetrain); + + Logger.recordOutput(LOGGING_PREFIX + "isAtRotationSetpoint", false); + Logger.recordOutput(LOGGING_PREFIX + "isAtPoseXSetPoint", false); + Logger.recordOutput(LOGGING_PREFIX + "isAtPoseYSetPoint", false); + + Logger.recordOutput(LOGGING_PREFIX + "headingSetPoint", 0.0); + Logger.recordOutput(LOGGING_PREFIX + "poseXSetpoint", 0.0); + Logger.recordOutput(LOGGING_PREFIX + "poseYSetpoint", 0.0); + + Logger.recordOutput(LOGGING_PREFIX + "headingPositionError", 0.0); + Logger.recordOutput(LOGGING_PREFIX + "poseXPositionError", 0.0); + Logger.recordOutput(LOGGING_PREFIX + "poseYPositionError", 0.0); + + Logger.recordOutput(LOGGING_PREFIX + "headingVelocityError", 0.0); + Logger.recordOutput(LOGGING_PREFIX + "poseXVelocityError", 0.0); + Logger.recordOutput(LOGGING_PREFIX + "poseYVelocityError", 0.0); + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drivetrain.driveToPose(setpointPose); + final double headingSetpoint = setpointPose.getRotation().getDegrees(); + final double poseXSetpoint = setpointPose.getX(); + final double poseYSetpoint = setpointPose.getY(); + + Logger.recordOutput(LOGGING_PREFIX + "positionSetpoint", headingSetpoint); + Logger.recordOutput(LOGGING_PREFIX + "headingSetpoint", headingSetpoint); + Logger.recordOutput(LOGGING_PREFIX + "poseXSetpoint", poseXSetpoint); + Logger.recordOutput(LOGGING_PREFIX + "poseYSetpoint", poseYSetpoint); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drivetrain.driveToPose(setpointPose); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + private final String LOGGING_PREFIX = "DriveToPose: "; + + // Returns true when the command should end. + @Override + public boolean isFinished() { + final boolean isAtHeadingSetpoint = drivetrain.isAtRotationSetpoint(); + final boolean isAtPoseXSetPoint = drivetrain.isAtPoseXSetpoint(); + final boolean isAtPoseYSetPoint = drivetrain.isAtPoseYSetpoint(); + + final double headingPositionError = Math.toDegrees(drivetrain.getHeadingControllerPositionError()); + final double poseXPositionError = drivetrain.getPoseXControllerPositionError(); + final double poseYPositionError = drivetrain.getPoseYControllerPositionError(); + + final double headingVelocityError = Math.toDegrees(drivetrain.getHeadingControllerVelocityError()); + final double poseXVelocityError = drivetrain.getPoseXControllerVelocityError(); + final double poseYVelocityError = drivetrain.getPoseYControllerVelocityError(); + + Logger.recordOutput(LOGGING_PREFIX + "isAtRotationSetpoint", isAtHeadingSetpoint); + Logger.recordOutput(LOGGING_PREFIX + "isAtPoseXSetPoint", isAtPoseXSetPoint); + Logger.recordOutput(LOGGING_PREFIX + "isAtPoseYSetPoint", isAtPoseYSetPoint); + + Logger.recordOutput(LOGGING_PREFIX + "headingPositionError", headingPositionError); + Logger.recordOutput(LOGGING_PREFIX + "poseXPositionError", poseXPositionError); + Logger.recordOutput(LOGGING_PREFIX + "poseYPositionError", poseYPositionError); + + Logger.recordOutput(LOGGING_PREFIX + "headingVelocityError", headingVelocityError); + Logger.recordOutput(LOGGING_PREFIX + "poseXVelocityError", poseXVelocityError); + Logger.recordOutput(LOGGING_PREFIX + "poseYVelocityError", poseYVelocityError); + + return isAtHeadingSetpoint && isAtPoseXSetPoint && isAtPoseYSetPoint; + } + + public static Command getCommand(CommandSwerveDrivetrain drivetrain, Pose2d setpointPose) { + return CommandLogger.logCommand( + new DriveToPose(drivetrain, setpointPose), + "DriveToPose"); + } +} diff --git a/src/main/java/frc/robot/commands/HasCoral.java b/src/main/java/frc/robot/commands/HasCoral.java deleted file mode 100644 index 2f5083d1..00000000 --- a/src/main/java/frc/robot/commands/HasCoral.java +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.*; -import frc.robot.Constants.SetPointConstants.ElevatorHeights; -import frc.robot.subsystems.CoralShooter.CoralShooter; -import frc.robot.subsystems.Elevator.Elevator; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class HasCoral extends Command { - private final CoralShooter coralShooter; - private final Elevator elevator; - private final String CMD_NAME = "HasCoral"; - - private boolean isFinished; - - /** Creates a new HasCoral. */ - public HasCoral(CoralShooter coralShooter, Elevator elevator) { - this.coralShooter = coralShooter; - this.elevator = elevator; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(coralShooter, elevator); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - isFinished = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - if(coralShooter.getIntakeSensor() || coralShooter.getOuttakeSensor()) { - - new SequentialCommandGroup( - elevator.setElevatorHeight(ElevatorHeights.AUTO_LEVEL_FOUR), - coralShooter.basicShootCmd() - ); - - isFinished = true; - - } else { - isFinished = true; - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - Logger.recordOutput(CMD_NAME, isFinished); - return isFinished; - } -} diff --git a/src/main/java/frc/robot/commands/PathOnTheFly.java b/src/main/java/frc/robot/commands/PathOnTheFly.java new file mode 100644 index 00000000..3355ecb1 --- /dev/null +++ b/src/main/java/frc/robot/commands/PathOnTheFly.java @@ -0,0 +1,65 @@ +package frc.robot.commands; + +import java.util.Collections; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; +import java.util.Set; +import java.util.function.Supplier; +import java.util.stream.Collectors; + +import org.littletonrobotics.junction.Logger; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.utils.CommandLogger; + +public class PathOnTheFly { + private static final String LOGGING_PREFIX = "PathOnTheFly: "; + + public static Command pathfindToProcessor(CommandSwerveDrivetrain drivetrain){ + final int processorTagIDBlue = 16; + final int processorTagIDRed = 3; + + final Pose2d processorPoseBlue = Constants.FIELD_LAYOUT.getTagPose(processorTagIDBlue).get().toPose2d().plus(new Transform2d(0.5, 0.0,Rotation2d.kCCW_90deg)); + final Pose2d processorPoseRed = Constants.FIELD_LAYOUT.getTagPose(processorTagIDRed).get().toPose2d().plus(new Transform2d(0.5, 0.0,Rotation2d.kCCW_90deg)); + + Logger.recordOutput(LOGGING_PREFIX + "Processor: Blue", processorPoseBlue); + Logger.recordOutput(LOGGING_PREFIX + "Processor: Red", processorPoseRed); + + return Commands.either( + pathfindToPose(drivetrain, processorPoseRed), + pathfindToPose(drivetrain, processorPoseBlue), + () -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red + ); + } + + /** + * Move to the given position using Path Planner's pathfinding functionality + * + * @param drivetrain + * @param endPose + * @return + */ + public static Command pathfindToPose(CommandSwerveDrivetrain drivetrain, Pose2d endPose) { + // test constraint + PathConstraints constraints = new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI); + return CommandLogger.logCommand(AutoBuilder.pathfindToPose(endPose, constraints), "PathFind") + .andThen(DriveToPose.getCommand(drivetrain, endPose)); + } +} diff --git a/src/main/java/frc/robot/commands/RemoveAlgae.java b/src/main/java/frc/robot/commands/RemoveAlgae.java index bad40bbf..fb53a6a4 100644 --- a/src/main/java/frc/robot/commands/RemoveAlgae.java +++ b/src/main/java/frc/robot/commands/RemoveAlgae.java @@ -4,25 +4,19 @@ package frc.robot.commands; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Constants; -import frc.robot.Constants.*; -import frc.robot.Constants.SetPointConstants.ElevatorHeights; +import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.AlgaeArm.AlgaeArm; import frc.robot.subsystems.AlgaeShooter.AlgaeShooter; import frc.robot.subsystems.AlgaeTilt.AlgaeTilt; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.CoralShooter.CoralShooter; import frc.robot.subsystems.Elevator.Elevator; -import frc.robot.subsystems.Vision.Vision; - -import java.util.HashMap; -import java.util.Map; - -import org.littletonrobotics.junction.Logger; +import frc.robot.subsystems.CoralShooter.CoralShooter; +import frc.robot.Constants.*; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class RemoveAlgae extends Command { @@ -31,81 +25,56 @@ public class RemoveAlgae extends Command { private AlgaeTilt algaeTilt; private CoralShooter coralShooter; private Elevator elevator; - private Vision vision; - - private double height = SetPointConstants.ElevatorHeights.TELE_LEVEL_FOUR; - - private int id; - - private static final Map removeHeight = Map.ofEntries( - Map.entry(6, "low"), - Map.entry(8, "low"), - Map.entry(10, "low"), - Map.entry(17, "low"), - Map.entry(21, "low"), - Map.entry(19, "low"), - Map.entry(7, "high"), - Map.entry(9, "high"), - Map.entry(11, "high"), - Map.entry(18, "high"), - Map.entry(20, "high"), - Map.entry(22, "high") - ); + + private CommandSwerveDrivetrain drivetrain; + + private int level; + + private double height; /** Creates a new RemoveAlgae. */ - public RemoveAlgae( - AlgaeArm algaeArm, - AlgaeShooter algaeShooter, - AlgaeTilt algaeTilt, - CoralShooter coralShooter, - Elevator elevator, - Vision vision - ) { + public RemoveAlgae(int level, AlgaeArm algaeArm, AlgaeShooter algaeShooter, AlgaeTilt algaeTilt, + CoralShooter coralShooter, + Elevator elevator, CommandSwerveDrivetrain drivetrain) { this.algaeArm = algaeArm; this.algaeShooter = algaeShooter; this.algaeTilt = algaeTilt; this.coralShooter = coralShooter; this.elevator = elevator; - this.vision = vision; + this.level = level; + this.drivetrain = drivetrain; // Use addRequirements() here to declare subsystem dependencies. - addRequirements(algaeArm, algaeShooter, algaeTilt, elevator); + addRequirements(algaeArm, algaeShooter, algaeTilt, elevator, drivetrain); } // Called when the command is initially scheduled. @Override public void initialize() { - id = vision.getAprilTagID(Constants.CompBotConstants.CORAL_LIMELIGHT_NAME); height = elevator.getHeight(); - - if (removeHeight.get(id) == "high") { - height = ElevatorHeights.TELE_LEVEL_THREE; - } else if (removeHeight.get(id) == "low") { - height = ElevatorHeights.TELE_LEVEL_TWO; - } else if(height >= 11.0) { - height = ElevatorHeights.TELE_LEVEL_THREE; - } else if (height < 11.0) { - height = ElevatorHeights.TELE_LEVEL_TWO; - } } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + coralShooter.setDutyCycle(-1.0); algaeTilt.setPosition(0.0); algaeShooter.setDutyCycle(-0.8); algaeArm.setPosition(100.0); elevator.setElevatorPostion(height + 1.0); + // if (coralShooter.getVelocity() < -6000.0) { // } } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { coralShooter.stop(); algaeShooter.stop(); + } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/SmartIntake.java b/src/main/java/frc/robot/commands/SmartIntake.java index 3e0f12ba..77efc0fd 100644 --- a/src/main/java/frc/robot/commands/SmartIntake.java +++ b/src/main/java/frc/robot/commands/SmartIntake.java @@ -53,7 +53,7 @@ public void execute() { stallTimer.stop(); coralShooter.setDutyCycle(0.2); unJammedTimer.start(); - if(unJammedTimer.hasElapsed(0.05)){ + if(unJammedTimer.hasElapsed(0.025)){ unJammedTimer.reset(); unJammedTimer.stop(); updateStates(); @@ -61,14 +61,14 @@ public void execute() { break; case EMPTY: stallTimer.start(); - coralShooter.setDutyCycle(-0.6); + coralShooter.setDutyCycle(-0.65); timer.reset(); timer.stop(); updateStates(); break; case JUST_INTAKE: stallTimer.start(); - coralShooter.setDutyCycle(-0.1); + coralShooter.setDutyCycle(-0.15); timer.reset(); timer.stop(); updateStates(); @@ -82,10 +82,10 @@ public void execute() { break; case FULL: default: - coralShooter.stop(); stallTimer.start(); + coralShooter.stop(); timer.start(); - if (timer.get() > 0.05) { + if (timer.get() > 0.1) { if (coralShooter.getOuttakeSensor() && coralShooter.getIntakeSensor()) { timer.stop(); coralShooter.stop(); diff --git a/src/main/java/frc/robot/generated/CompBotDriveTrain.java b/src/main/java/frc/robot/generated/CompBotDriveTrain.java index 2f1d697e..3cf55ac4 100644 --- a/src/main/java/frc/robot/generated/CompBotDriveTrain.java +++ b/src/main/java/frc/robot/generated/CompBotDriveTrain.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import com.ctre.phoenix6.swerve.utility.PhoenixPIDController; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; @@ -21,18 +22,18 @@ public class CompBotDriveTrain { public static final double maxAngularRate = RotationsPerSecond.of(2.0).in(RadiansPerSecond); // 3/4 of a rotation - // per - // second max angular - // velocity - public static final double headingKP = 2.0; + // per + // second max angular + // velocity + public static final double headingKP = 1.75; public static final double headingKI = 0.0; public static final double headingKD = 0.0; public static final double headingKIZone = 0.0; - public static final double stafeKP = 0.011; - public static final double stafeKI = 0.0;//0.00015; - public static final double stafeKD = 0.00001;//0.003; - public static final double strafeIRMax = 0.05; //DOES NOTHING WE ARENT DOING INTEGRATOR RANGE :((())) + public static final double stafeKP = 0.01; + public static final double stafeKI = 0.0;// 0.00015; + public static final double stafeKD = 0.0;// 0.003; + public static final double strafeIRMax = 0.05; // DOES NOTHING WE ARENT DOING INTEGRATOR RANGE :((())) public static final double strafeIRMin = -0.05; public static final double forwardKP = 0.04; @@ -42,17 +43,18 @@ public class CompBotDriveTrain { public static final double forwardIRMin = -0.05; // Both sets of gains need to be tuned to your individual robot. - // The steer motor uses any SwerveModule.SteerRequestType control request with the + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.18).withKV(2.66).withKA(0.018) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.18).withKV(2.66).withKA(0.018) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.12).withKI(0).withKD(0) - .withKS(0.2).withKV(0.124).withKA(0.006); + .withKP(0.12).withKI(0).withKD(0) + .withKS(0.2).withKV(0.124).withKA(0.006); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -74,17 +76,19 @@ public class CompBotDriveTrain { // This needs to be tuned to your individual robot private static final Current kSlipCurrent = Amps.of(120.0); - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; @@ -122,8 +126,7 @@ public class CompBotDriveTrain { .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) .withCouplingGearRatio(kCoupleRatio) @@ -145,7 +148,6 @@ public class CompBotDriveTrain { .withSteerFrictionVoltage(kSteerFrictionVoltage) .withDriveFrictionVoltage(kDriveFrictionVoltage); - // Front Left private static final int kFrontLeftDriveMotorId = 12; private static final int kFrontLeftSteerMotorId = 10; @@ -190,69 +192,83 @@ public class CompBotDriveTrain { private static final Distance kBackRightXPos = Inches.of(-11.375); private static final Distance kBackRightYPos = Inches.of(-11.375); - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); + public static final SwerveModuleConstants FrontLeft = ConstantCreator + .createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants FrontRight = ConstantCreator + .createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants BackLeft = ConstantCreator + .createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants BackRight = ConstantCreator + .createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + private static PhoenixPIDController poseXController = new PhoenixPIDController(0.5, 0.0, 0.0); // TODO: Find actual value + private static PhoenixPIDController poseYController = new PhoenixPIDController(0.5, 0.0, 0.0); // TODO: Find actual value + + private static double poseXControllerPositionTolerance = 0.05; + private static double poseYControllerPositionTolerance = 0.05; + + private static double poseXControllerVelocityTolerance = 0.01; + private static double poseYControllerVelocityTolerance = 0.01; /** * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ public static CommandSwerveDrivetrain createDrivetrain() { + poseXController.setTolerance(poseXControllerPositionTolerance, poseXControllerVelocityTolerance); + poseYController.setTolerance(poseYControllerPositionTolerance, poseYControllerVelocityTolerance); return new CommandSwerveDrivetrain( - headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, + headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, + forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, + poseXController, poseYController, kSpeedAt12Volts.in(MetersPerSecond), maxAngularRate, DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected + * device types. */ public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module */ public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules); } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through * getters in the classes. * * @param drivetrainConstants Drivetrain-wide constants for the swerve drive @@ -262,47 +278,51 @@ public TunerSwerveDrivetrain( * @param modules Constants for each specific module */ public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules); } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on + * unspecified or set to 0 Hz, this is 250 Hz + * on * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters + * @param odometryStandardDeviation The standard deviation for odometry + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters + * @param visionStandardDeviation The standard deviation for vision + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters * and radians * @param modules Constants for each specific module */ public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules); } } } diff --git a/src/main/java/frc/robot/generated/OldCompBot.java b/src/main/java/frc/robot/generated/OldCompBot.java index d0706961..3d9deba0 100644 --- a/src/main/java/frc/robot/generated/OldCompBot.java +++ b/src/main/java/frc/robot/generated/OldCompBot.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import com.ctre.phoenix6.swerve.utility.PhoenixPIDController; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; @@ -214,16 +215,24 @@ public class OldCompBot { kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted); + private static PhoenixPIDController poseXController = new PhoenixPIDController(0.5, 0.0, 0.0); + private static PhoenixPIDController poseYController = new PhoenixPIDController(0.5, 0.0, 0.0); + /** * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ public static CommandSwerveDrivetrain createDrivetrain() { + poseXController.setTolerance(0.02, 0.02); + poseYController.setTolerance(0.02, 0.02); return new CommandSwerveDrivetrain( - headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, - kSpeedAt12Volts.in(MetersPerSecond), - maxAngularRate, DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - } + headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, + forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, + poseXController, poseYController, + kSpeedAt12Volts.in(MetersPerSecond), + maxAngularRate, DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); +} + /** * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected * device types. diff --git a/src/main/java/frc/robot/generated/PracticeBotDriveTrain.java b/src/main/java/frc/robot/generated/PracticeBotDriveTrain.java index c757b17e..95190256 100644 --- a/src/main/java/frc/robot/generated/PracticeBotDriveTrain.java +++ b/src/main/java/frc/robot/generated/PracticeBotDriveTrain.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import com.ctre.phoenix6.swerve.utility.PhoenixPIDController; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; @@ -218,16 +219,34 @@ public class PracticeBotDriveTrain { kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted ); + private static final double POSE_CONTROLLER_KP = 1; + private static final double POSE_CONTROLLER_KI = 0.0006; + private static final double POSE_CONTROLLER_KD = 0.0; + private static final double POSE_CONTROLLER_IZONE = 0.08; + + private static final double POSE_CONTROLLER_POSITION_TOLERANCE = 0.03; + private static final double POSE_CONTROLLER_VELOCITY_TOLERANCE = 0.02; + + private static final PhoenixPIDController POSE_X_CONTROLLER = new PhoenixPIDController(POSE_CONTROLLER_KP, POSE_CONTROLLER_KI, POSE_CONTROLLER_KD); + private static final PhoenixPIDController POSE_Y_CONTROLLER = new PhoenixPIDController(POSE_CONTROLLER_KP, POSE_CONTROLLER_KI, POSE_CONTROLLER_KD); + /** * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ public static CommandSwerveDrivetrain createDrivetrain() { + POSE_X_CONTROLLER.setTolerance(POSE_CONTROLLER_POSITION_TOLERANCE, POSE_CONTROLLER_VELOCITY_TOLERANCE); + POSE_X_CONTROLLER.setIZone(POSE_CONTROLLER_IZONE); + POSE_Y_CONTROLLER.setTolerance(POSE_CONTROLLER_POSITION_TOLERANCE, POSE_CONTROLLER_VELOCITY_TOLERANCE); + POSE_Y_CONTROLLER.setIZone(POSE_CONTROLLER_IZONE); return new CommandSwerveDrivetrain( - headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, - kSpeedAt12Volts.in(MetersPerSecond), - maxAngularRate, DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - } + headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, + forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, + POSE_X_CONTROLLER, POSE_Y_CONTROLLER, + kSpeedAt12Volts.in(MetersPerSecond), + maxAngularRate, DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); +} + /** diff --git a/src/main/java/frc/robot/generated/WoodBotDriveTrain.java b/src/main/java/frc/robot/generated/WoodBotDriveTrain.java index 94167f38..ba843e0f 100644 --- a/src/main/java/frc/robot/generated/WoodBotDriveTrain.java +++ b/src/main/java/frc/robot/generated/WoodBotDriveTrain.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import com.ctre.phoenix6.swerve.utility.PhoenixPIDController; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; @@ -220,16 +221,24 @@ public class WoodBotDriveTrain { ); + private static PhoenixPIDController poseXController = new PhoenixPIDController(0.5, 0.0, 0.0); + private static PhoenixPIDController poseYController = new PhoenixPIDController(0.5, 0.0, 0.0); + /** * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ public static CommandSwerveDrivetrain createDrivetrain() { + poseXController.setTolerance(0.02, 0.02); + poseYController.setTolerance(0.02, 0.02); return new CommandSwerveDrivetrain( - headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, + headingKP, headingKI, headingKD, headingKIZone, stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin, + forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin, + poseXController, poseYController, kSpeedAt12Volts.in(MetersPerSecond), maxAngularRate, DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - } +} + /** * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected diff --git a/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTilt.java b/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTilt.java index 832a7780..af69054e 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTilt.java +++ b/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTilt.java @@ -33,14 +33,6 @@ public void setEncoder(double value) { public void setPosition(double position) { io.setPosition(position); } - - public double getPositionRelative() { - return inputs.armPositionRelative; - } - - public double getPositionAbsolute() { - return inputs.armPositionAbsolute; - } public void stop() { io.setDutyCycle(0.0); diff --git a/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOCB.java b/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOCB.java index 01a563bb..5e5e7f96 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOCB.java +++ b/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOCB.java @@ -35,7 +35,7 @@ public class AlgaeTiltIOCB implements AlgaeTiltIO { private final double forwardLimit = 38.0; private final double reverseLimit = -10.0; - private final double ZERO_OFFSET = 0.5551491; //0.7218491 + 0.833; // TODO: find the zero offset + private final double ZERO_OFFSET = 0.2145; // TODO: find the zero offset private final double positionConversionFactor = 1.0; private final SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOPB.java b/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOPB.java index 6cfffedd..3183fa53 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOPB.java +++ b/src/main/java/frc/robot/subsystems/AlgaeTilt/AlgaeTiltIOPB.java @@ -28,8 +28,8 @@ public class AlgaeTiltIOPB implements AlgaeTiltIO { private final double kI = 0.0; private final double kD = 0.0; - private final double forwardLimit = 27.0; - private final double reverseLimit = -5.0; //used to be 10 3/15 + private final double forwardLimit = 38.0; + private final double reverseLimit = -10.0; private final double positionConversionFactor = 1.0; private final SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); @@ -37,7 +37,7 @@ public class AlgaeTiltIOPB implements AlgaeTiltIO { /** Creates a new AlgaeIntakeIOPB. */ public AlgaeTiltIOPB() { sparkMaxConfig.idleMode(IdleMode.kBrake); - sparkMaxConfig.inverted(true); //USED TO BE FALSE 3/15 + sparkMaxConfig.inverted(false); SoftLimitConfig softLimitConfig = new SoftLimitConfig(); softLimitConfig.forwardSoftLimit(forwardLimit); diff --git a/src/main/java/frc/robot/subsystems/ClimberWheel/ClimberWheelIOSim.java b/src/main/java/frc/robot/subsystems/ClimberWheel/ClimberWheelIOSim.java index d8cfc837..58f38b58 100644 --- a/src/main/java/frc/robot/subsystems/ClimberWheel/ClimberWheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/ClimberWheel/ClimberWheelIOSim.java @@ -22,7 +22,7 @@ public class ClimberWheelIOSim implements ClimberWheelIO { private final DCMotor gearbox = DCMotor.getNEO(1); - private final Encoder encoder = new Encoder(10, 11); + private final Encoder encoder = new Encoder(12, 11); private final PWMSparkMax wheelMotor = new PWMSparkMax(5); diff --git a/src/main/java/frc/robot/subsystems/ClimberWinch/ClimberWinchIOSim.java b/src/main/java/frc/robot/subsystems/ClimberWinch/ClimberWinchIOSim.java index 2c1eb907..1b87e3a7 100644 --- a/src/main/java/frc/robot/subsystems/ClimberWinch/ClimberWinchIOSim.java +++ b/src/main/java/frc/robot/subsystems/ClimberWinch/ClimberWinchIOSim.java @@ -26,9 +26,9 @@ public class ClimberWinchIOSim implements ClimberWinchIO { private DCMotor gearbox = DCMotor.getNEO(1); - private Encoder winchEncoder = new Encoder(8, 9); + private Encoder winchEncoder = new Encoder(9, 10); - private final PWMSparkMax winchMotor = new PWMSparkMax(5); + private final PWMSparkMax winchMotor = new PWMSparkMax(7); private final LinearSystem plant = LinearSystemId.createFlywheelSystem( gearbox, 0.00113951385, 1.0); // TODO: find actual MOI diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index a99e6669..7d976759 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -18,6 +18,10 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; @@ -40,13 +44,18 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.RobotContainer; +import frc.robot.Constants.PracticeBotConstants; import frc.robot.generated.OldCompBot; import frc.robot.generated.OldCompBot.TunerSwerveDrivetrain; import frc.robot.subsystems.Vision.Vision; import frc.robot.subsystems.Vision.VisionMeasurement; +import frc.robot.utils.LimelightHelpers; + +import java.util.ArrayList; import frc.robot.utils.CommandLogger; import java.util.List; +import java.util.Map; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -63,6 +72,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su public PhoenixPIDController headingController; public PhoenixPIDController strafeController; public PhoenixPIDController forwardController; + public PhoenixPIDController poseXController; + public PhoenixPIDController poseYController; /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; @@ -83,7 +94,8 @@ public final Command fieldOrientedDrive( SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() // creates a fieldcentric drive .withDeadband(maxSpeed * 0.01) .withRotationalDeadband(maxAngularRate * 0.01); - // .withDriveRequestType(DriveRequestType.Velocity); // Use closed-loop control for drive motors + // .withDriveRequestType(DriveRequestType.Velocity); // Use closed-loop control + // for drive motors return CommandLogger.logCommand(this.applyRequest( () -> drive @@ -104,7 +116,6 @@ public final Command fieldOrientedDrive( ), "DrivetrainFieldOriented"); } - public void xOut() { SwerveRequest xOutReq = new SwerveRequest.SwerveDriveBrake(); this.setControl(xOutReq); @@ -123,17 +134,22 @@ public void addHeadingController(double kP, double kI, double kD, double kIZone) headingController = new PhoenixPIDController(kP, kI, kD); headingController.enableContinuousInput(-Math.PI, Math.PI); - headingController.setTolerance(Math.toRadians(1)); + headingController.setTolerance(Math.toRadians(2)); } public void addStrafeController(double kP, double kI, double kD, double irMax, double irMin) { strafeController = new PhoenixPIDController(kP, kI, kD); - strafeController.setTolerance(1.0); + // strafeController.setIntegratorRange(-irMin, irMax); + strafeController.setIZone(2.5); + strafeController.setTolerance(1.0, 0.1); } public void addForwardContrller(double kP, double kI, double kD, double irMax, double irMin) { forwardController = new PhoenixPIDController(kP, kI, kD); - forwardController.setTolerance(0.75); + + // forwardController.setIntegratorRange(-irMin, irMax); + forwardController.setIZone(3.0); + forwardController.setTolerance(0.75, 0.5); } public void driveFieldCentricFacingAngle(double x, double y, double desiredAngle) { @@ -148,17 +164,71 @@ public void driveFieldCentricFacingAngle(double x, double y, double desiredAngle request.withDriveRequestType(DriveRequestType.Velocity); } - public void driveFieldCentricFacingAngleBluePerspective(double x, double y, double desiredAngle) { + public double getHeadingControllerSetpoint(){ + return headingController.getSetpoint(); + } + + public double getHeadingControllerPositionError(){ + return headingController.getPositionError(); + } + + public double getHeadingControllerVelocityError(){ + return headingController.getVelocityError(); + } + + public double getPoseXSetpoint(){ + return poseXController.getSetpoint(); + } + + public boolean isAtPoseXSetpoint(){ + return poseXController.atSetpoint(); + } + + public double getPoseXControllerPositionError(){ + return poseXController.getPositionError(); + } + + public double getPoseXControllerVelocityError(){ + return poseXController.getVelocityError(); + } + + public double getPoseYSetpoint(){ + return poseYController.getSetpoint(); + } + + public boolean isAtPoseYSetpoint(){ + return poseYController.atSetpoint(); + } + + public double getPoseYControllerPositionError(){ + return poseYController.getPositionError(); + } + + public double getPoseYControllerVelocityError(){ + return poseYController.getVelocityError(); + } + + /** + * Field centric facing angle command without flipping based on operator perspective + * + * @param setpointPose the position on the field to move to + */ + public void driveToPose(Pose2d setpointPose) { + Pose2d currentPose = getPose(); + double timestamp = getStateCopy().Timestamp; + double x = poseXController.calculate(currentPose.getX(), setpointPose.getX(), timestamp); + double y = poseYController.calculate(currentPose.getY(), setpointPose.getY(), timestamp); + FieldCentricFacingAngle request = new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(x * maxSpeed) .withVelocityY(y * maxSpeed) - .withTargetDirection(Rotation2d.fromDegrees(desiredAngle)); + .withTargetDirection(setpointPose.getRotation()); request.HeadingController = headingController; request.withDeadband(0.1); request.withRotationalDeadband(0.04); request.ForwardPerspective = ForwardPerspectiveValue.BlueAlliance; - this.setControl(request); request.withDriveRequestType(DriveRequestType.Velocity); + this.setControl(request); } // public Command backUpBot(double meters) { @@ -262,10 +332,6 @@ public final Command robotCentricDrive( private double maxSpeed; private double maxAngularRate; - public double getMaxSpeed(){ - return maxSpeed; - } - /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

@@ -293,6 +359,8 @@ public CommandSwerveDrivetrain( double forwardKD, double forwardIRMax, double forwardIRMin, + PhoenixPIDController poseXController, + PhoenixPIDController poseYController, double maxSpeed, double maxAngularRate, SwerveDrivetrainConstants drivetrainConstants, @@ -304,6 +372,8 @@ public CommandSwerveDrivetrain( addHeadingController(headingKP, headingKI, headingKD, headingKIZone); addStrafeController(stafeKP, stafeKI, stafeKD, strafeIRMax, strafeIRMin); addForwardContrller(forwardKP, forwardKI, forwardKD, forwardIRMax, forwardIRMin); + this.poseXController = poseXController; + this.poseYController = poseYController; this.maxSpeed = maxSpeed; this.maxAngularRate = maxAngularRate; @@ -465,14 +535,6 @@ public double getAngularRate() { return Math.toDegrees(this.getStateCopy().Speeds.omegaRadiansPerSecond); } - public double getXRate() { - return this.getStateCopy().Speeds.vxMetersPerSecond; - } - - public double getYRate() { - return this.getStateCopy().Speeds.vyMetersPerSecond; - } - public void addVisionMeasurements(List measurements) { for (VisionMeasurement measurement : measurements) { this.addVisionMeasurement( @@ -485,10 +547,10 @@ public void addVisionMeasurements(List measurements) { @Override public void periodic() { Logger.recordOutput("Swerve: Current Pose", this.getPose()); - // Logger.recordOutput("Swerve: Rotation", this.getRotation2d()); - // Logger.recordOutput("Swerve: Angle", this.getAngle()); + Logger.recordOutput("Swerve: Rotation", this.getRotation2d()); + Logger.recordOutput("Swerve: Angle", this.getAngle()); // Logger.recordOutput("swerve: pithc", this.isFlat()); - // Logger.recordOutput("Rotation2d", this.getPigeon2().getRotation2d()); + Logger.recordOutput("Rotation2d", this.getPigeon2().getRotation2d()); Logger.recordOutput( "Swerve: Heading Controller: Setpoint", headingController.getSetpoint()); @@ -501,9 +563,8 @@ public void periodic() { Logger.recordOutput( "Swerve: Heading Controller: PositionTolerance", headingController.getPositionTolerance()); - // Logger.recordOutput("Swerve: CurrentState", this.getStateCopy().ModuleStates); - // Logger.recordOutput("Swerve: TargetState", this.getStateCopy().ModuleTargets); - + Logger.recordOutput("Swerve: CurrentState", this.getStateCopy().ModuleStates); + Logger.recordOutput("Swerve: TargetState", this.getStateCopy().ModuleTargets); /* * Periodically try to apply the operator perspective. * If we haven't applied the operator perspective before, then we should apply diff --git a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntake.java b/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntake.java deleted file mode 100644 index b7e8544a..00000000 --- a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntake.java +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.CoralIntake; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.CoralIntake.CoralIntakeIO.CoralIntakeIOInputs; - -public class CoralIntake extends SubsystemBase { - - private CoralIntakeIO io; - private CoralIntakeIOInputs inputs; - /** Creates a new CoralIntake. */ - public CoralIntake(CoralIntakeIO io) { - this.io = io; - } - - public void setDutyCycle(double dutyCycle) { - io.setDutyCycle(dutyCycle); - } - - public void stop() { - io.stop(); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - } -} diff --git a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIO.java b/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIO.java deleted file mode 100644 index 515887a5..00000000 --- a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIO.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.CoralIntake; - -import org.littletonrobotics.junction.AutoLog; - -/** Add your docs here. */ -public interface CoralIntakeIO { - - @AutoLog - public static class CoralIntakeIOInputs{ - public double statorCurrent = 0.0; - public double supplyCurrent = 0.0; - public double voltage = 0.0; - public double velocity = 0.0; - public double position = 0.0; - public double temperature = 0.0; - } - - public default void updateInputs(CoralIntakeIOInputs inputs) {} - - /** - * sets the speed ot a number between -1 and 1 - * @param dutyCycle - */ - public void setDutyCycle(double dutyCycle); - - - public void stop(); -} diff --git a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIOCB.java b/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIOCB.java deleted file mode 100644 index 43e132f4..00000000 --- a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIOCB.java +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.CoralIntake; - -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; - -import frc.robot.Constants; - -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; - -import com.revrobotics.spark.SparkMax; - -/** Add your docs here. */ -public class CoralIntakeIOCB implements CoralIntakeIO { - - private final SparkMax motor = new SparkMax(Constants.CompBotConstants.CORAL_SHOOTER_ID, MotorType.kBrushless); - private final RelativeEncoder encoder = motor.getEncoder(); - private final SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); - - private final double KP = 0.0; - private final double KI = 0.0; - private final double KD = 0.0; - private final double KF = 0.0; - - CoralIntakeIOCB(){ - sparkMaxConfig.idleMode(IdleMode.kBrake); - sparkMaxConfig.inverted(false); - sparkMaxConfig.smartCurrentLimit(20, 5); - - motor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - @Override - public void updateInputs(CoralIntakeIOInputs inputs) { - inputs.position = encoder.getPosition(); - inputs.velocity = encoder.getVelocity(); - - inputs.statorCurrent = motor.getOutputCurrent(); - inputs.temperature = motor.getMotorTemperature(); - inputs.voltage = motor.getAppliedOutput() * motor.getBusVoltage(); - } - - public void setDutyCycle(double dutyCycle){ - motor.set(dutyCycle); - } - - public void stop() { - motor.set(0.0); - } -} diff --git a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIOPB.java b/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIOPB.java deleted file mode 100644 index b5b9a3d4..00000000 --- a/src/main/java/frc/robot/subsystems/CoralIntake/CoralIntakeIOPB.java +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.CoralIntake; - -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; - -import frc.robot.Constants; - -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; - -import com.revrobotics.spark.SparkMax; - -/** Add your docs here. */ -public class CoralIntakeIOPB implements CoralIntakeIO { - - private final SparkMax motor = new SparkMax(Constants.CompBotConstants.CORAL_SHOOTER_ID, MotorType.kBrushless); - private final RelativeEncoder encoder = motor.getEncoder(); - private final SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); - - private final double KP = 0.0; - private final double KI = 0.0; - private final double KD = 0.0; - private final double KF = 0.0; - - CoralIntakeIOPB(){ - sparkMaxConfig.idleMode(IdleMode.kBrake); - sparkMaxConfig.inverted(false); - sparkMaxConfig.smartCurrentLimit(20, 5); - - motor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - @Override - public void updateInputs(CoralIntakeIOInputs inputs) { - inputs.position = encoder.getPosition(); - inputs.velocity = encoder.getVelocity(); - - inputs.statorCurrent = motor.getOutputCurrent(); - inputs.temperature = motor.getMotorTemperature(); - inputs.voltage = motor.getAppliedOutput() * motor.getBusVoltage(); - } - - public void setDutyCycle(double dutyCycle){ - motor.set(dutyCycle); - } - - public void stop() { - motor.set(0.0); - } -} diff --git a/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooter.java b/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooter.java index c3f5ebd8..13a9a610 100644 --- a/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooter.java +++ b/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooter.java @@ -45,7 +45,6 @@ public boolean getIntakeSensor() { return inputs.intakeSensor; } - public void stop() { io.stop(); } @@ -84,7 +83,7 @@ public Command pullAlgae() { public Command basicShootCmd() { String cmdName = "ShootCoral"; - return CommandLogger.logCommand(waitUntilEmpty().raceWith(setDutyCycleCmd(-0.35)), cmdName); + return CommandLogger.logCommand(waitUntilEmpty().raceWith(setDutyCycleCmd(-0.40)), cmdName); } public Command basicIntakeCmd() { diff --git a/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOCB.java b/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOCB.java index df7f85a6..e508639e 100644 --- a/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOCB.java +++ b/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOCB.java @@ -43,11 +43,11 @@ public void setDutyCycle(double dutyCycle) { } private boolean isInOuttakeSensor() { - return outtakeSensor.getProximity() < 0.1; + return outtakeSensor.getProximity() < 0.2; } private boolean isInIntakeSensor() { - return intakeSensor.getProximity() < 0.1; + return intakeSensor.getProximity() < 0.2; } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOPB.java b/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOPB.java index c4d960ee..5eef4de0 100644 --- a/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOPB.java +++ b/src/main/java/frc/robot/subsystems/CoralShooter/CoralShooterIOPB.java @@ -41,11 +41,11 @@ public void setDutyCycle(double dutyCycle) { } private boolean isInOuttakeSensor() { - return outtakeSensor.getProximity() < 0.2; //tuned for praccy bot 3/15 + return outtakeSensor.getProximity() < 0.1; } private boolean isInIntakeSensor() { - return intakeSensor.getProximity() < 0.2; + return intakeSensor.getProximity() < 0.06; } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/Servo/Servo.java b/src/main/java/frc/robot/subsystems/Servo/Servo.java index 89375594..2adc6f4d 100644 --- a/src/main/java/frc/robot/subsystems/Servo/Servo.java +++ b/src/main/java/frc/robot/subsystems/Servo/Servo.java @@ -37,6 +37,7 @@ public Command runWithTimeout(double timeout, double speed) { return Commands.waitSeconds(timeout).deadlineFor(this.setSpeedCmd(speed)); } + public Command setSpeedCmd(DoubleSupplier speed) { System.out.println("speed"); diff --git a/src/main/java/frc/robot/subsystems/Servo/ServoIOPB.java b/src/main/java/frc/robot/subsystems/Servo/ServoIOPB.java deleted file mode 100644 index bfe9dfbe..00000000 --- a/src/main/java/frc/robot/subsystems/Servo/ServoIOPB.java +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.Servo; - -import edu.wpi.first.wpilibj.Servo; -import frc.robot.Constants; - -public class ServoIOPB implements ServoIO { - private final Servo servo = new Servo(Constants.PracticeBotConstants.SERVO_PORT); - - /** Creates a new ServoIOCB. */ - public ServoIOPB() {} - - public void setServoSpeed(double speed) { - servo.setPosition(speed); - } - - public void updateInputs(ServoIOInputs inputs) { - inputs.setOutput = servo.getPosition(); - } -} diff --git a/src/main/java/frc/robot/subsystems/Vision/Vision.java b/src/main/java/frc/robot/subsystems/Vision/Vision.java index ca3323ff..1cd7b495 100644 --- a/src/main/java/frc/robot/subsystems/Vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision/Vision.java @@ -23,7 +23,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.hal.HALUtil; import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -37,175 +36,150 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Constants.CompBotConstants; import frc.robot.utils.CommandLogger; public class Vision extends SubsystemBase { - private NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); - private VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged(); - // private final VisionIO[] ios; - private final Map ios; - - private final Map visionInputs; - private Timer snapshotTimer = new Timer(); - List acceptedMeasurements = Collections.emptyList(); - - private final String VISION_LOGGING_PREFIX = "Vision: "; - - private static final InterpolatingMatrixTreeMap MEASUREMENT_STD_DEV_DISTANCE_MAP = new InterpolatingMatrixTreeMap<>(); - - static { - MEASUREMENT_STD_DEV_DISTANCE_MAP.put(1.0, VecBuilder.fill(1.5, 1.5, 999999.0)); // n1 and n2 are for x and y, n3 - // is for angle - MEASUREMENT_STD_DEV_DISTANCE_MAP.put(8.0, VecBuilder.fill(10.0, 10.0, 999999.0)); - } - - private static final Matrix stdDevMatrix = VecBuilder.fill(3.0, 3.0, 999999.0); - - /** Creates a new Vision. */ - public Vision(Map visionIos) { - this.ios = visionIos; - // Creates the same number of inputs as vision IO layers - visionInputs = new HashMap<>(); - for (String key : visionIos.keySet()) { - visionInputs.put(key, new VisionIOInputsAutoLogged()); - } - } - - public void turnOnLights(String name) { - ios.get(name).setLEDMode(3); - } - - public void turnOffLights(String name) { - ios.get(name).setLEDMode(1); - } - - public void blinkLights(String name) { - ios.get(name).setLEDMode(2); - } - - public int getAprilTagID(String name) { - return ios.get(name).getAprilTagID(); - } - - public double getTXRaw(String name) { - // TODO: replace with more robust code - return ios.get(name).getTXRaw(); - } - - public double getTYRaw(String name) { - // TODO: replace with more robust code - return ios.get(name).getTYRaw(); - } - - public double getTV(String name) { - // TODO: replace with more robust code - return ios.get(name).getTV(); - } - - public double getPipeline(String name) { - // TODO: replace with more robust code - return ios.get(name).getPipeline(); - } - - public void setPipeline(String name, int pipeline) { - // TODO: replace with more robust code - if (ios.get(name).getPipeline() != pipeline) { - ios.get(name).setPipeline(pipeline); - } - } - - public void takeSnapshot(String name) { - // TODO: replace with more robust code - ios.get(name).takeSnapshot(); - Logger.recordOutput(VISION_LOGGING_PREFIX + "snapshot", true); - snapshotTimer.stop(); - snapshotTimer.reset(); - snapshotTimer.start(); - } - - public void resetSnapshot(String name) { - // TODO: replace with more robust code - ios.get(name).resetSnapshot(); - Logger.recordOutput(VISION_LOGGING_PREFIX + "snapshot", false); - snapshotTimer.stop(); - } - - public boolean isOnTargetTX(String name, double goal) { - if (Math.abs(getTXRaw(name) - goal) < 1.0) { - return true; - } - return false; - } - - public boolean isOnTargetTY(String name, double goal) { - if (Math.abs(getTYRaw(name) - goal) < 1.0) { - return true; - } - return false; - } - - public boolean isTargetInView(String name) { - // TODO: replace with more robust code - return getTV(name) == 1; - } - - public Command waitUntilTargetTxTy(String name, double goalTX, double goalTY) { - return Commands - .waitUntil(() -> isTargetInView(name) && isOnTargetTX(name, goalTX) && isOnTargetTY(name, goalTY)); - } - - @Override - public void periodic() { - - if(DriverStation.isDisabled()) { - turnOffLights(CompBotConstants.ALGAE_LIMELIGHT_NAME); - } - - long periodicStartTime = HALUtil.getFPGATime(); - - for (String key : ios.keySet()) { - VisionIO io = ios.get(key); - VisionIOInputsAutoLogged input = visionInputs.get(key); - - io.updateInputs(input); - Logger.processInputs("Limelight: " + key, input); - } - - List acceptedMeasurements = new ArrayList<>(); - - for (String key : visionInputs.keySet()) { - VisionIOInputsAutoLogged input = visionInputs.get(key); - // skip input if not updated - if (!input.poseUpdated) - continue; - - Pose2d pose = input.estimatedPose; - double timestamp = input.timestampSeconds; - - // Skip measurements that are not with in the field boundary - if (pose.getX() < 0.0 || pose.getX() > Constants.FIELD_LAYOUT.getFieldLength() || - pose.getY() < 0.0 || pose.getY() > Constants.FIELD_LAYOUT.getFieldWidth()) - continue; - - // get standard deviation based on distance to nearest tag - OptionalDouble closestTagDistance = Arrays.stream(input.distancesToTargets).min(); - - Matrix cprStdDevs = MEASUREMENT_STD_DEV_DISTANCE_MAP - .get(closestTagDistance.orElse(Double.MAX_VALUE)); - - acceptedMeasurements.add(new VisionMeasurement(timestamp, pose, cprStdDevs)); - } - this.acceptedMeasurements = acceptedMeasurements; - long periodicLoopTime = HALUtil.getFPGATime() - periodicStartTime; - Logger.recordOutput(VISION_LOGGING_PREFIX + "periodic loop time", (periodicLoopTime / 1000)); - } - - /** - * @return Command that consumes vision measurements - */ - public Command consumeVisionMeasurements(Consumer> visionMeasurementConsumer) { - return CommandLogger.logCommand(run(() -> visionMeasurementConsumer.accept(acceptedMeasurements)), - "Consume Vision Measurements"); - } + private NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + private VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged(); + //private final VisionIO[] ios; + private final Map ios; + + private final Map visionInputs; + private Timer snapshotTimer = new Timer(); + List acceptedMeasurements = Collections.emptyList(); + + + private final String VISION_LOGGING_PREFIX = "Vision: "; + + private static final InterpolatingMatrixTreeMap MEASUREMENT_STD_DEV_DISTANCE_MAP = new InterpolatingMatrixTreeMap<>(); + + static { + MEASUREMENT_STD_DEV_DISTANCE_MAP.put(1.0, VecBuilder.fill(1.5, 1.5, 999999.0)); //n1 and n2 are for x and y, n3 is for angle + MEASUREMENT_STD_DEV_DISTANCE_MAP.put(8.0, VecBuilder.fill(10.0, 10.0, 999999.0)); + } + + private static final Matrix stdDevMatrix = VecBuilder.fill(3.0, 3.0, 999999.0); + + /** Creates a new Vision. */ + public Vision( Map visionIos) { + this.ios = visionIos; + // Creates the same number of inputs as vision IO layers + visionInputs = new HashMap<>(); + for(String key: visionIos.keySet()){ + visionInputs.put(key, new VisionIOInputsAutoLogged()); + } + } + + public int getAprilTagID(String name) { + return ios.get(name).getAprilTagID(); + } + + public double getTXRaw(String name) { + // TODO: replace with more robust code + return ios.get(name).getTXRaw(); + } + + + public double getTYRaw(String name) { + // TODO: replace with more robust code + return ios.get(name).getTYRaw(); + } + + public double getTV(String name) { + // TODO: replace with more robust code + return ios.get(name).getTV(); + } + + public double getPipeline(String name) { + // TODO: replace with more robust code + return ios.get(name).getPipeline(); + } + + public void setPipeline(String name, int pipeline) { + // TODO: replace with more robust code + if (ios.get(name).getPipeline() != pipeline) { + ios.get(name).setPipeline(pipeline); + } + } + + public void takeSnapshot(String name) { + // TODO: replace with more robust code + ios.get(name).takeSnapshot(); + Logger.recordOutput(VISION_LOGGING_PREFIX + "snapshot", true); + snapshotTimer.stop(); + snapshotTimer.reset(); + snapshotTimer.start(); + } + + public void resetSnapshot(String name) { + // TODO: replace with more robust code + ios.get(name).resetSnapshot(); + Logger.recordOutput(VISION_LOGGING_PREFIX + "snapshot", false); + snapshotTimer.stop(); + } + + public boolean isOnTargetTX(String name, double goal) { + if (Math.abs(getTXRaw(name) - goal) < 1.0) { + return true; + } + return false; + } + + public boolean isOnTargetTY(String name, double goal) { + if (Math.abs(getTYRaw(name) - goal) < 1.0) { + return true; + } + return false; + } + + public boolean isTargetInView(String name) { + // TODO: replace with more robust code + return getTV(name) == 1; + } + + public Command waitUntilTargetTxTy(String name, double goalTX, double goalTY) { + return Commands.waitUntil(() -> isTargetInView(name) && isOnTargetTX(name, goalTX) && isOnTargetTY(name, goalTY)); + } + @Override + public void periodic() { + for (String key : ios.keySet()) { + VisionIO io = ios.get(key); + VisionIOInputsAutoLogged input = visionInputs.get(key); + + io.updateInputs(input); + Logger.processInputs("Limelight: " + key, input); + } + + List acceptedMeasurements = new ArrayList<>(); + + for (String key: visionInputs.keySet()) { + VisionIOInputsAutoLogged input = visionInputs.get(key); + // skip input if not updated + if (!input.poseUpdated) + continue; + + Pose2d pose = input.estimatedPose; + double timestamp = input.timestampSeconds; + + // Skip measurements that are not with in the field boundary + if (pose.getX() < 0.0 || pose.getX() > Constants.FIELD_LAYOUT.getFieldLength() || + pose.getY() < 0.0 || pose.getY() > Constants.FIELD_LAYOUT.getFieldWidth()) + continue; + + // get standard deviation based on distance to nearest tag + OptionalDouble closestTagDistance = Arrays.stream(input.distancesToTargets).min(); + + Matrix cprStdDevs = MEASUREMENT_STD_DEV_DISTANCE_MAP.get(closestTagDistance.orElse(Double.MAX_VALUE)); + + acceptedMeasurements.add(new VisionMeasurement(timestamp, pose, cprStdDevs)); + } + this.acceptedMeasurements = acceptedMeasurements; + } + + /** + * @return Command that consumes vision measurements + */ + public Command consumeVisionMeasurements(Consumer> visionMeasurementConsumer) { + return CommandLogger.logCommand(run(() -> visionMeasurementConsumer.accept(acceptedMeasurements)), "Consume Vision Measurements"); + } } diff --git a/src/main/java/frc/robot/subsystems/Vision/VisionIO.java b/src/main/java/frc/robot/subsystems/Vision/VisionIO.java index 3af9be6b..a7e8453a 100644 --- a/src/main/java/frc/robot/subsystems/Vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/Vision/VisionIO.java @@ -32,8 +32,6 @@ public static class VisionIOInputs { public void updateInputs(VisionIOInputs inputs); - public void setLEDMode(int mode); - public int getAprilTagID(); public double getTXRaw(); diff --git a/src/main/java/frc/robot/subsystems/Vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/Vision/VisionIOLimelight.java index 6fed348c..f6878f58 100644 --- a/src/main/java/frc/robot/subsystems/Vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/Vision/VisionIOLimelight.java @@ -56,19 +56,12 @@ public VisionIOLimelight(String name, DoubleSupplier gyroAngleSupplier, DoubleSu this.gryoAngleRateSupplier = gryoAngleRateSupplier; } - public void setLEDMode(int mode) { - table.getEntry("ledMode").setNumber(mode); - } - - public void updateInputs(VisionIOInputs inputs) { // Get the pose estimate from limelight helpers - Optional newPoseEstimate; + Optional newPoseEstimate = getMegatag1PoseEst(); // If enabled, get megatag 2 pose if (DriverStation.isEnabled()) { newPoseEstimate = getMegatag2PoseEst(); - } else { - newPoseEstimate = getMegatag1PoseEst(); } // Assume that the pose hasn't been updated diff --git a/src/main/java/frc/robot/utils/RobotUtils.java b/src/main/java/frc/robot/utils/RobotUtils.java index a2e71a3b..787adb09 100644 --- a/src/main/java/frc/robot/utils/RobotUtils.java +++ b/src/main/java/frc/robot/utils/RobotUtils.java @@ -2,6 +2,8 @@ import java.io.File; +import edu.wpi.first.math.geometry.Rotation2d; + public class RobotUtils { public static boolean isUsbWriteable() { File usb = new File("/U"); @@ -18,4 +20,14 @@ public static boolean isUsbWriteable() { } return false; } + + public static Rotation2d flipForRedAlliancePerspective(Rotation2d rotation2d) { + double angle = rotation2d.getDegrees(); + if (angle <= 0.0) { + angle = angle + 180.0; + } else { + angle = angle - 180.0; + } + return Rotation2d.fromDegrees(angle); + } }