diff --git a/Shuffleboard/shuffleboard.json b/Shuffleboard/shuffleboard.json index 4dd33cb..3917dd8 100644 --- a/Shuffleboard/shuffleboard.json +++ b/Shuffleboard/shuffleboard.json @@ -519,6 +519,6 @@ "x": -8.0, "y": -8.0, "width": 1382.0, - "height": 784.0 + "height": 736.0 } } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto index 70cc0e8..7d3b3c8 100644 --- a/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto +++ b/src/main/deploy/pathplanner/autos/Mid C-B-3-4.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3186789318946823, - "y": 5.567111941824834 + "x": 1.27, + "y": 5.53 }, "rotation": 0 }, @@ -14,31 +14,43 @@ { "type": "path", "data": { - "pathName": "MID_C" + "pathName": "Mid C-B-A-3 SUB - Note C" } }, { "type": "path", "data": { - "pathName": "C_B" + "pathName": "Mid C-B-A-3 SUB - Score C" } }, { "type": "path", "data": { - "pathName": "B_3" + "pathName": "Mid C-B-A-3 SUB - Note B" } }, { "type": "path", "data": { - "pathName": "3_MID" + "pathName": "Mid C-B-A-3 SUB - Score B" } }, { "type": "path", "data": { - "pathName": "MID_4" + "pathName": "Mid C-B-A-3 SUB - Note 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Score 3 Yeet" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Note 3" } } ] diff --git a/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto new file mode 100644 index 0000000..01843d7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid C-B-A-1 SUB.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.27, + "y": 5.53 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Note C" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Score C" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Note B" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Score B" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Note A" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-3 SUB - Score A" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-1 SUB - Note 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid C-B-A-1 SUB - Score 1 Normal" + } + } + ] + } + }, + "folder": "Mid Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto b/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto index 47db1a2..50d246a 100644 --- a/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto +++ b/src/main/deploy/pathplanner/autos/Top A-1-2-3.auto @@ -17,6 +17,12 @@ "pathName": "TOP_A" } }, + { + "type": "path", + "data": { + "pathName": "A_TOP" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/1_A.path b/src/main/deploy/pathplanner/paths/1_A.path index c876a19..3fc6f38 100644 --- a/src/main/deploy/pathplanner/paths/1_A.path +++ b/src/main/deploy/pathplanner/paths/1_A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.328810723241988, - "y": 7.37782702384235 + "x": 8.284942222836145, + "y": 7.4309552554704394 }, "prevControl": null, "nextControl": { - "x": 6.908320747662531, - "y": 7.268558564182392 + "x": 6.864452247256688, + "y": 7.321686795810481 }, "isLocked": false, "linkedName": "TOP_A_1_2_3-1" @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 35.44787634349656, + "rotation": 29.24882633654691, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/2_A.path b/src/main/deploy/pathplanner/paths/2_A.path index 72ba0c2..c415f01 100644 --- a/src/main/deploy/pathplanner/paths/2_A.path +++ b/src/main/deploy/pathplanner/paths/2_A.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.25, - "y": 5.8 + "x": 8.29962790515921, + "y": 5.6539876943796825 }, "prevControl": null, "nextControl": { - "x": 6.724426116469921, - "y": 6.534646792546081 + "x": 6.774054021629132, + "y": 6.388634486925763 }, "isLocked": false, "linkedName": "TOP_A_1_2_3-2" }, { "anchor": { - "x": 2.93935385724081, - "y": 6.990384785778517 + "x": 2.9246681749177457, + "y": 6.67 }, "prevControl": { - "x": 5.330836567521922, - "y": 6.862550215827963 + "x": 5.316150885198857, + "y": 6.5421654300494465 }, "nextControl": null, "isLocked": false, - "linkedName": "TOP_A_1_2_3-A" + "linkedName": "TOP_A_1_2_3-A late" } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 30.210305589469193, + "rotation": 26.142109373851337, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/3_MID.path b/src/main/deploy/pathplanner/paths/3_MID.path index 795c7c8..9a7b2f6 100644 --- a/src/main/deploy/pathplanner/paths/3_MID.path +++ b/src/main/deploy/pathplanner/paths/3_MID.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.666769963235812, - "y": 4.46444742621149 + "x": 8.26051834784571, + "y": 4.323757544312242 }, "prevControl": null, "nextControl": { - "x": 6.765567110948872, - "y": 3.8697121749832677 + "x": 6.556334631055977, + "y": 3.7005132136005674 }, "isLocked": false, "linkedName": "Mid C-B-3 - Note 3" }, { "anchor": { - "x": 2.263812470379861, - "y": 5.771473152964195 + "x": 1.8917403433857882, + "y": 5.950035719763017 }, "prevControl": { - "x": 3.8058091143015926, - "y": 5.624616329733555 + "x": 3.3816838214933846, + "y": 5.326791389051343 }, "nextControl": null, "isLocked": false, - "linkedName": "Mid C-B-3 - C Turn" + "linkedName": "Mid C-B-3 - Mid" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/A_1.path b/src/main/deploy/pathplanner/paths/A_1.path index 637fca8..676ef88 100644 --- a/src/main/deploy/pathplanner/paths/A_1.path +++ b/src/main/deploy/pathplanner/paths/A_1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.93935385724081, - "y": 6.990384785778517 + "x": 0.9127296966579629, + "y": 6.623242727701913 }, "prevControl": null, "nextControl": { - "x": 4.161140321454306, - "y": 7.216123744966993 + "x": 2.1345161608714593, + "y": 6.848981686890389 }, "isLocked": false, - "linkedName": "TOP_A_1_2_3-A" + "linkedName": "TOP_A_1_2_3-TOP" }, { "anchor": { - "x": 8.328810723241988, - "y": 7.37782702384235 + "x": 8.284942222836145, + "y": 7.4309552554704394 }, "prevControl": { - "x": 5.130961637508229, - "y": 7.329136060505501 + "x": 5.087093137102387, + "y": 7.38226429213359 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Top A-1-2-3", "previewStartingState": { - "rotation": 32.0, + "rotation": 59.85861444792467, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A_2.path b/src/main/deploy/pathplanner/paths/A_2.path index 7c54ac3..14af31c 100644 --- a/src/main/deploy/pathplanner/paths/A_2.path +++ b/src/main/deploy/pathplanner/paths/A_2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.25, - "y": 5.8 + "x": 8.29962790515921, + "y": 5.6539876943796825 }, "prevControl": { - "x": 5.546180666975807, - "y": 7.196524381186367 + "x": 5.5958085721350175, + "y": 7.05051207556605 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A_3.path b/src/main/deploy/pathplanner/paths/A_3.path index 17a4cec..3834839 100644 --- a/src/main/deploy/pathplanner/paths/A_3.path +++ b/src/main/deploy/pathplanner/paths/A_3.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.93935385724081, - "y": 6.990384785778517 + "x": 2.9246681749177457, + "y": 6.67 }, "prevControl": null, "nextControl": { - "x": 3.810565385428992, - "y": 7.138218203628034 + "x": 3.7958797031059275, + "y": 6.817833417849517 }, "isLocked": false, - "linkedName": "TOP_A_1_2_3-A" + "linkedName": "TOP_A_1_2_3-A late" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/A_TOP.path b/src/main/deploy/pathplanner/paths/A_TOP.path new file mode 100644 index 0000000..03a0bea --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A_TOP.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.93935385724081, + "y": 6.990384785778517 + }, + "prevControl": null, + "nextControl": { + "x": 2.2491267880622807, + "y": 6.902270691847475 + }, + "isLocked": false, + "linkedName": "TOP_A_1_2_3-A" + }, + { + "anchor": { + "x": 0.9127296966579629, + "y": 6.623242727701913 + }, + "prevControl": { + "x": 1.4854713072629475, + "y": 6.667299774678448 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TOP_A_1_2_3-TOP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 57.15314553496158, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.85861444792467, + "rotateFast": false + }, + "reversed": false, + "folder": "Top A-1-2-3", + "previewStartingState": { + "rotation": 34.85445556626899, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B_3.path b/src/main/deploy/pathplanner/paths/B_3.path index 83a8c6a..d7514f7 100644 --- a/src/main/deploy/pathplanner/paths/B_3.path +++ b/src/main/deploy/pathplanner/paths/B_3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.666769963235812, - "y": 4.46444742621149 + "x": 8.26051834784571, + "y": 4.323757544312242 }, "prevControl": { - "x": 6.317060791545553, - "y": 4.46444742621149 + "x": 5.910809176155452, + "y": 4.323757544312242 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B_MID.path b/src/main/deploy/pathplanner/paths/B_MID.path new file mode 100644 index 0000000..e745690 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B_MID.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9980965865330664, + "y": 5.888958611548708 + }, + "prevControl": null, + "nextControl": { + "x": 1.989122270059487, + "y": 5.6968427104114 + }, + "isLocked": false, + "linkedName": "Mid C-B-3 - Note B" + }, + { + "anchor": { + "x": 1.8917403433857882, + "y": 5.950035719763017 + }, + "prevControl": { + "x": 2.425546604054034, + "y": 6.051790251731009 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid C-B-3 - Mid" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Mid C-B-3-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path index 2e5a2a5..8ccb1ff 100644 --- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path +++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6456402107795283, - "y": 3.539249439858451 + "x": 1.9113560946263224, + "y": 4.640675614088259 }, "prevControl": null, "nextControl": { - "x": 4.26106526631658, - "y": 1.2482829974604495 + "x": 3.526781150163374, + "y": 2.349709171690258 }, "isLocked": false, "linkedName": "Bottom 4-5 Start" }, { "anchor": { - "x": 8.784255421820326, - "y": 3.039936240874271 + "x": 8.62271291626662, + "y": 3.451135345920066 }, "prevControl": { - "x": 6.507974661745389, - "y": 1.8063389257368865 + "x": 6.346432156191683, + "y": 2.2175380307826815 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path index 184e9ea..7e5e4de 100644 --- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path +++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Note 5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.461170410712915, - "y": 0.7489697984762701 + "x": 8.417113363743722, + "y": 0.6755413868609491 }, "prevControl": { - "x": 6.5114625112971165, - "y": 0.8076076300376479 + "x": 6.610774438006836, + "y": 0.631484339891757 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path index b7d478a..765a53f 100644 --- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path +++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 4.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 8.784255421820326, - "y": 3.039936240874271 + "x": 8.62271291626662, + "y": 3.451135345920066 }, "prevControl": null, "nextControl": { - "x": 6.537346026391517, - "y": 1.967881431290591 + "x": 7.345058554160041, + "y": 3.0252505585512077 }, "isLocked": false, "linkedName": "Bottom 4-5 Note 4 Pickup" }, { "anchor": { - "x": 1.7057565421034249, - "y": 5.184045860041631 + "x": 4.231693901670451, + "y": 3.039936240874271 + }, + "prevControl": { + "x": 6.3376567059933215, + "y": 2.2180970977238825 + }, + "nextControl": { + "x": 3.0274679511791946, + "y": 3.5098780752123226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.131641329472284, + "y": 6.094558164071605 }, "prevControl": { - "x": 3.526781150163374, - "y": 3.5392494398584513 + "x": 2.5575261168411423, + "y": 4.816903801965028 }, "nextControl": null, "isLocked": false, @@ -30,7 +46,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2, + "waypointRelativePos": 0.4, "rotationDegrees": -27.663471922084646, "rotateFast": true } diff --git a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path index 500e7fe..0f733e9 100644 --- a/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path +++ b/src/main/deploy/pathplanner/paths/Bottom 5-4-C To Shoot Note 5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.461170410712915, - "y": 0.7489697984762701 + "x": 8.417113363743722, + "y": 0.6755413868609491 }, "prevControl": null, "nextControl": { - "x": 6.115657148257819, - "y": 0.8955643773797127 + "x": 6.071600101288626, + "y": 0.8221359657643917 }, "isLocked": false, "linkedName": "Bottom 4-5 Note 5 Pickup" }, { "anchor": { - "x": 2.6456402107795283, - "y": 3.539249439858451 + "x": 1.9113560946263224, + "y": 4.640675614088259 }, "prevControl": { - "x": 3.776437749655465, - "y": 1.8357102903830143 + "x": 3.0421536335022594, + "y": 2.9371364646128226 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_MID.path b/src/main/deploy/pathplanner/paths/C_MID.path new file mode 100644 index 0000000..560eac0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C_MID.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6162688461334, + "y": 4.376333332273105 + }, + "prevControl": null, + "nextControl": { + "x": 2.125456967402666, + "y": 4.9275254896891765 + }, + "isLocked": false, + "linkedName": "Mid C-B-3 - Note C" + }, + { + "anchor": { + "x": 1.8917403433857882, + "y": 5.950035719763017 + }, + "prevControl": { + "x": 2.552143108729842, + "y": 5.535666040360404 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid C-B-3 - Mid" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Mid C-B-3-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MID_3.path b/src/main/deploy/pathplanner/paths/MID_3.path new file mode 100644 index 0000000..6878739 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MID_3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8917403433857882, + "y": 5.950035719763017 + }, + "prevControl": null, + "nextControl": { + "x": 3.255087316817576, + "y": 5.365744159720822 + }, + "isLocked": false, + "linkedName": "Mid C-B-3 - Mid" + }, + { + "anchor": { + "x": 8.26051834784571, + "y": 4.323757544312242 + }, + "prevControl": { + "x": 7.199055347102391, + "y": 3.5252257455879086 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid C-B-3 - Note 3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Mid C-B-3-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MID_B.path b/src/main/deploy/pathplanner/paths/MID_B.path new file mode 100644 index 0000000..e42de3a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MID_B.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8917403433857882, + "y": 5.950035719763017 + }, + "prevControl": null, + "nextControl": { + "x": 2.6415811787732713, + "y": 5.988988490432497 + }, + "isLocked": false, + "linkedName": "Mid C-B-3 - Mid" + }, + { + "anchor": { + "x": 2.9980965865330664, + "y": 5.888958611548708 + }, + "prevControl": { + "x": 2.4565555180932432, + "y": 5.75527186641562 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid C-B-3 - Note B" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Mid C-B-3-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MID_C.path b/src/main/deploy/pathplanner/paths/MID_C.path index b92c2da..08585f8 100644 --- a/src/main/deploy/pathplanner/paths/MID_C.path +++ b/src/main/deploy/pathplanner/paths/MID_C.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.309243119380694, - "y": 5.565873600441298 + "x": 1.2198050493372643, + "y": 5.52155524239874 }, "prevControl": null, "nextControl": { - "x": 1.6763851774572966, - "y": 5.095931766103247 + "x": 1.586947107413868, + "y": 5.051613408060689 }, "isLocked": false, - "linkedName": "Mid C-B-3 - Mid" + "linkedName": null }, { "anchor": { @@ -20,8 +20,8 @@ "y": 4.376333332273105 }, "prevControl": { - "x": 2.043527235533899, - "y": 4.8169038019650285 + "x": 1.9112167287205282, + "y": 4.245852002973283 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path index 26ffe63..bcd1cea 100644 --- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path +++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Note 3.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 4.628186374775031, - "y": 4.2978770123018135 + "x": 4.540093230454797, + "y": 4.670046978734387 }, "prevControl": { - "x": 4.291274013623357, - "y": 4.548401588542802 + "x": 4.203180869303123, + "y": 4.920571554975376 }, "nextControl": { - "x": 5.0849090000605095, - "y": 3.958262752474149 + "x": 4.996815855740276, + "y": 4.330432718906723 }, "isLocked": false, "linkedName": "Mid C-B-A-1 SUB - mid point" diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path index caec868..c8d998a 100644 --- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path +++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3 Yeet.path @@ -16,28 +16,28 @@ }, { "anchor": { - "x": 4.628186374775031, - "y": 4.2978770123018135 + "x": 4.540093230454797, + "y": 4.670046978734387 }, "prevControl": { - "x": 4.991504045099071, - "y": 4.017957375681643 + "x": 4.903410900778837, + "y": 4.390127342114217 }, "nextControl": { - "x": 4.27690278809944, - "y": 4.5685249382481645 + "x": 4.188809643779206, + "y": 4.940694904680738 }, "isLocked": false, "linkedName": "Mid C-B-A-1 SUB - mid point" }, { "anchor": { - "x": 3.7264519607498547, - "y": 4.883418839590889 + "x": 1.4120428956421427, + "y": 5.551187918118234 }, "prevControl": { - "x": 4.323704624584712, - "y": 4.742888801041511 + "x": 2.009295559477004, + "y": 5.4106578795688565 }, "nextControl": null, "isLocked": false, @@ -55,7 +55,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -5.648247373735291, + "rotation": 1.5452967179226196, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path index d6453d3..4843d30 100644 --- a/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path +++ b/src/main/deploy/pathplanner/paths/Mid C-B-A-3 SUB - Score 3.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 4.628186374775031, - "y": 4.2978770123018135 + "x": 4.540093230454797, + "y": 4.670046978734387 }, "prevControl": { - "x": 4.991504045099071, - "y": 4.017957375681643 + "x": 4.903410900778837, + "y": 4.390127342114217 }, "nextControl": { - "x": 4.27690278809944, - "y": 4.5685249382481645 + "x": 4.188809643779206, + "y": 4.940694904680738 }, "isLocked": false, "linkedName": "Mid C-B-A-1 SUB - mid point" diff --git a/src/main/deploy/pathplanner/paths/TA123_T-A.path b/src/main/deploy/pathplanner/paths/TA123_T-A.path new file mode 100644 index 0000000..7597604 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TA123_T-A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7426336086361384, + "y": 6.641447399146282 + }, + "prevControl": null, + "nextControl": { + "x": 1.6775001047036502, + "y": 6.359039811792554 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8947741881248894, + "y": 6.9920223351715975 + }, + "prevControl": { + "x": 1.9501694993900078, + "y": 6.485636316468361 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 30.529705899934207, + "rotateFast": false + }, + "reversed": false, + "folder": "Top A-1-2-3", + "previewStartingState": { + "rotation": 59.47029410006579, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TOP_A.path b/src/main/deploy/pathplanner/paths/TOP_A.path index 9869642..9661e2a 100644 --- a/src/main/deploy/pathplanner/paths/TOP_A.path +++ b/src/main/deploy/pathplanner/paths/TOP_A.path @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 22.742478873939014, + "rotation": 30.963756532073536, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/Autonomous/AutoCmds.java b/src/main/java/frc/robot/Autonomous/AutoCmds.java new file mode 100644 index 0000000..a4bafa9 --- /dev/null +++ b/src/main/java/frc/robot/Autonomous/AutoCmds.java @@ -0,0 +1,13 @@ +package frc.robot.Autonomous; + +public enum AutoCmds { + FOLLOW_WITH_POSE_AIM, + FOLLOW_WITH_SHOOT_AIM, + FOLLOW_WITH_FULL_AIM, + FOLLOW_WITH_PASSTHROUGH, + FOLLOW, + SHOOT, + AIM, + POSE_AIM, + SHOOT_AIM +} diff --git a/src/main/java/frc/robot/Autonomous/Bottom_5_4.java b/src/main/java/frc/robot/Autonomous/Bottom_5_4.java index fa003e6..7c55561 100644 --- a/src/main/java/frc/robot/Autonomous/Bottom_5_4.java +++ b/src/main/java/frc/robot/Autonomous/Bottom_5_4.java @@ -129,9 +129,9 @@ public void execute() _drivetrain.drive(new ChassisSpeeds()); nextStep(); } + break; // drives to 3 default: - _jukebox.setState(JukeboxEnum.IDLE); _drivetrain.drive(new ChassisSpeeds()); break; } @@ -153,6 +153,7 @@ public void initialize(){ _intake.setWantedState(IntakeState.INTAKE); _jukebox.setState(JukeboxEnum.IDLE); _loopCounter = 0; + _jukebox.setIdleSpeedMax(); } private void nextStep() { _count++; diff --git a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java index 913ee8a..05cd60a 100644 --- a/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java +++ b/src/main/java/frc/robot/Autonomous/Mid_C_B_3_4.java @@ -28,7 +28,7 @@ public void execute(){ switch (_count) { case 0: // Initial Position - Prep for Speaker Shot - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER); nextStep(); break; case 1: @@ -56,21 +56,33 @@ public void execute(){ if (_pathFollower.isPathFinished()){ // At Note C turn position (1) _drivetrain.drive(new ChassisSpeeds()); + + _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); + nextStep(); } break; case 4: - // Once the note is detected, we can set prep speaker. + _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); + if (_jukebox.hasNote()) { - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER); + } + + if (_pathFollower.isPathFinished()){ + // At Note C turn position (1) + _drivetrain.drive(new ChassisSpeeds()); + nextStep(); } + break; case 5: // Once ready for the shot, set score. if (_jukebox.isReadyToScore()) { _jukebox.setState(JukeboxEnum.SCORE); + nextStep(); } break; @@ -86,83 +98,108 @@ public void execute(){ break; case 7: - // Follow Path to Note B (2) + // Follow Path to Note C turn position (1) _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); if (_pathFollower.isPathFinished()){ - // At Note B (2) + // At Note C turn position (1) _drivetrain.drive(new ChassisSpeeds()); + + _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); + nextStep(); } break; case 8: - // Once the note is detected, we can set prep speaker. + _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); + if (_jukebox.hasNote()) { - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER); + } + + if (_pathFollower.isPathFinished()){ + // At Note C turn position (1) + _drivetrain.drive(new ChassisSpeeds()); + nextStep(); } break; - case 9: - // Once ready for the shot, set score. - if (_jukebox.isReadyToScore()) { - _jukebox.setState(JukeboxEnum.SCORE); - nextStep(); - } + case 9: + // Once ready for the shot, set score. + if (_jukebox.isReadyToScore()) { + _jukebox.setState(JukeboxEnum.SCORE); + + nextStep(); + } break; case 10: // After the note has left the robot, transition to idle and start the next path. if (!_jukebox.hasNote()) { _jukebox.setState(JukeboxEnum.IDLE); - // Start Path to Note 3 (3) + // Start Path to Note B (2) _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); nextStep(); } + break; - // Drives to note 3 (3) case 11: + // Follow Path to Note C turn position (1) _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); + if (_pathFollower.isPathFinished()){ - _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); + // At Note C turn position (1) _drivetrain.drive(new ChassisSpeeds()); + + _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); + nextStep(); } + break; - // Drives back to Mid case 12: _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); + if (_jukebox.hasNote()) { - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER); } + if (_pathFollower.isPathFinished()){ + // At Note C turn position (1) _drivetrain.drive(new ChassisSpeeds()); + nextStep(); } + break; - // Scores at Mid case 13: + // Once ready for the shot, set score. if (_jukebox.isReadyToScore()) { _jukebox.setState(JukeboxEnum.SCORE); + nextStep(); } break; case 14: + // After the note has left the robot, transition to idle and start the next path. if (!_jukebox.hasNote()) { _jukebox.setState(JukeboxEnum.IDLE); - _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); + + // Start Path to Note B (2) nextStep(); + _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); } break; - // Drives to Note 4 case 15: _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); if (_pathFollower.isPathFinished()){ - _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); + // At Note C turn position (1) _drivetrain.drive(new ChassisSpeeds()); nextStep(); } + break; // DONE default: diff --git a/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java b/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java index f7e9a67..d557186 100644 --- a/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java +++ b/src/main/java/frc/robot/Autonomous/Mid_C_B_A_1.java @@ -238,7 +238,6 @@ public void initialize(){ // Set initial subsystem states. We should never need to change the Intake state as it is autonomous. _intake.setWantedState(IntakeState.INTAKE); _jukebox.setState(JukeboxEnum.IDLE); - _jukebox.setIdleSpeedMax(); } private void nextStep() { diff --git a/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java b/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java index 54c6500..cf34d64 100644 --- a/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java +++ b/src/main/java/frc/robot/Autonomous/TOP_A_1_2_3.java @@ -35,7 +35,7 @@ public void execute() { switch (_step) { case 0: // Initial Position - Prep for Speaker Shot - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER); nextStep(); break; case 1: @@ -62,6 +62,7 @@ public void execute() { if (_pathFollower.isPathFinished()){ // At Note A (1) + _pathFollower.startNextPath(new ChassisSpeeds(), _drivetrain.getPose()); _drivetrain.drive(new ChassisSpeeds()); nextStep(); } @@ -69,10 +70,18 @@ public void execute() { break; case 4: // Once the note is detected, we can set prep speaker. + _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); + if (_jukebox.hasNote()) { - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER); + } + + if (_pathFollower.isPathFinished()){ + // At Note A (2) + _drivetrain.drive(new ChassisSpeeds()); nextStep(); } + break; case 5: // Once ready for the shot, set score. @@ -116,7 +125,7 @@ public void execute() { _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); if (_jukebox.hasNote()) { - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_PODIUM); } if (_pathFollower.isPathFinished()){ @@ -167,7 +176,7 @@ public void execute() { _drivetrain.drive(_pathFollower.getPathTarget(_drivetrain.getPose())); if (_jukebox.hasNote()) { - _jukebox.setState(JukeboxEnum.PREP_SPEAKER); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_PODIUM); } if (_pathFollower.isPathFinished()){ diff --git a/src/main/java/frc/robot/Constants/Constants.java b/src/main/java/frc/robot/Constants/Constants.java index 213f3a9..a0b5f32 100644 --- a/src/main/java/frc/robot/Constants/Constants.java +++ b/src/main/java/frc/robot/Constants/Constants.java @@ -4,8 +4,29 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import frc.robot.Utilities.AllianceSpecific; public final class Constants { + public static final double kFieldLength = 16.54; + public static final double kFieldWidth = 8.21; + + public static final Translation2d kBlueSpeaker = new Translation2d( + 0, + 5.5 + ); + + public static final Translation2d kRedSpeaker = AllianceSpecific + .mirrorTranslation(kBlueSpeaker); + + public static final Translation2d kBlueZone = new Translation2d( + 1.3, + 7.0 + ); + + public static final Translation2d kRedZone = AllianceSpecific + .mirrorTranslation(kBlueZone); + + // public static final double k // Controller USB ports public static final int kDriverControllerUsbSlot = 0; diff --git a/src/main/java/frc/robot/Enums/JukeboxEnum.java b/src/main/java/frc/robot/Enums/JukeboxEnum.java index c750f2d..a33c963 100644 --- a/src/main/java/frc/robot/Enums/JukeboxEnum.java +++ b/src/main/java/frc/robot/Enums/JukeboxEnum.java @@ -11,6 +11,9 @@ public enum JukeboxEnum /**Spins up the shooter for a speaker shot. */ PREP_SPEAKER, + /* Spins up shooter and pivots for feed shot (sucessor to prep_launch) */ + PREP_FEED, + /**Spins up the shooter for a speaker shot from the podium (Doesn't use auto aim) */ PREP_SPEAKER_PODIUM, diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java new file mode 100644 index 0000000..4d5c6d3 --- /dev/null +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -0,0 +1,949 @@ +//LimelightHelpers v1.4.0 (March 21, 2024) + +package frc.robot; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public String error; + + public LimelightResults() { + targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 682801a..9a1e28f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,15 +4,21 @@ package frc.robot; +import java.util.Optional; + +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Autonomous.AutonomousMode; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.Constants; import frc.robot.Enums.*; import frc.robot.Subsystems.*; +import frc.robot.Utilities.AllianceSpecific; import frc.robot.Utilities.AutoUtil; import frc.robot.Utilities.LEDController; import frc.robot.Utilities.OneDimensionalLookup; @@ -23,6 +29,7 @@ * project. */ public class Robot extends TimedRobot { + private static Translation2d kSpeaker; /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -64,6 +71,14 @@ public void robotPeriodic() { _jukebox.logTelemetry(); _intake.logTelemetry(); _ledController.handleLights(); + + Optional alliance = DriverStation.getAlliance(); + + if (alliance.isPresent()) { + kSpeaker = alliance.get() == Alliance.Blue ? + Constants.kBlueSpeaker : + Constants.kRedSpeaker; + } } @Override @@ -98,7 +113,7 @@ public void teleopPeriodic() { // _ledController.handleBottomLights(); Add this code when we get the bottom lights setup on the robot teleopDrive(); teleopJukebox(); - _drivetrain.updateOdometry(); + _drivetrain.updateOdometryWithVision(); teleopIntake(); teleopClimb(); @@ -126,11 +141,39 @@ private void teleopDrive() { _drivetrain.reset(); } + double speakerDistance = _drivetrain.getDistanceToTarget( + AllianceSpecific.getSpeaker() + ); + + double speakerAngle = _drivetrain.getAngleToTarget( + AllianceSpecific.getSpeaker() + ); + + // amp + // rotation = _drivetrain.getRotationDifference(90) / 105; + + // feed + // rotation = _drivetrain.getAngleToTarget(AllianceSpecific.getZone()) / 105 + + SmartDashboard.putNumber("angle to speaker", speakerAngle); + SmartDashboard.putNumber("distance to speaker", speakerDistance); + if (Math.abs(_driverController.getLeftTriggerAxis()) > 0.25) { - rotation = -(_visionSystem.getTargetAngle() / 105) ; + + rotation = speakerAngle / 105; //target angle range is -27 to 27 degrees - } + } else if (_driverController.getLeftBumper()) { + var feedAngle = _drivetrain.getAngleToTarget( + AllianceSpecific.getZone() + ); + + SmartDashboard.putNumber("angle to zone", feedAngle); + rotation = feedAngle / 105; + } + // else if (_driverController.getLeftBumper()) { + // rotation = _drivetrain.getRotationDifference(90); + // } // if (_driverController.getBackButton() && _driverController.getStartButton()) // { @@ -148,7 +191,7 @@ public void teleopJukebox() { _jukebox.setState(JukeboxEnum.PREP_AMP); //_jukebox.setState(JukeboxEnum.PREP_SPEAKER_LINE); } else if (_operatorController.getXButton()) { - _jukebox.setState(JukeboxEnum.PREP_SPEAKER_LINE); + _jukebox.setState(JukeboxEnum.PREP_SPEAKER_SUBWOOFER); //_jukebox.setState(JukeboxEnum.PREP_AMP); } else if (_operatorController.getYButton()) { _jukebox.setState(JukeboxEnum.PREP_SPEAKER_PODIUM); @@ -159,12 +202,12 @@ public void teleopJukebox() { } } if (_driverController.getXButtonPressed()) { - // _jukebox.setState(JukeboxEnum.JUKEBOX_TEST); - if (_jukebox.getDisableAutoSpinup()) { - _jukebox.enableAutoSpinup(); - } else { - _jukebox.disableAutoSpinup(); - } + _jukebox.setState(JukeboxEnum.JUKEBOX_TEST); + // if (_jukebox.getDisableAutoSpinup()) { + // _jukebox.enableAutoSpinup(); + // } else { + // _jukebox.disableAutoSpinup(); + // } } if (_score) { diff --git a/src/main/java/frc/robot/Subsystems/Drivetrain.java b/src/main/java/frc/robot/Subsystems/Drivetrain.java index 3c255ff..3ac1077 100644 --- a/src/main/java/frc/robot/Subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/Subsystems/Drivetrain.java @@ -6,13 +6,16 @@ import com.swervedrivespecialties.swervelib.SdsModuleConfigurations; import com.swervedrivespecialties.swervelib.SwerveModule; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -21,7 +24,11 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.LimelightHelpers; import frc.robot.Constants.Constants; +import frc.robot.LimelightHelpers.PoseEstimate; +import frc.robot.LimelightHelpers.RawFiducial; +import frc.robot.Utilities.AllianceSpecific; public class Drivetrain extends Subsystem{ private static Drivetrain _instance; @@ -44,6 +51,32 @@ public class Drivetrain extends Subsystem{ private ChassisSpeeds _chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); private final Field2d m_field = new Field2d(); + private final Field2d m_vPEField = new Field2d(); + + private final String kLimelightName = ""; + + // in meters + private final double kNearTagAreaMin = 0.8; + private final double kFarTagAreaMin = 0.1; + + private final double kMidTrustPoseDiffMax = 0.5; + private final double kLowTrustPoseDiffMax = 0.3; + + // std = standard deviation + // in meters + private final double kHighTrustXYStds = 0.5; + private final double kMidTrustXYStds = 1.0; + private final double kLowTrustXYStds = 2.0; + + // rotation deviation + // in degrees + private final double kHighTrustDegStds = 6; + private final double kMidTrustDegStds = 12; + private final double kLowTrustDegStds = 30; + + // TODO: get these values + private final Translation2d kSpeakerTargetBlue = new Translation2d(0.5, 0.5); + private final Translation2d kSpeakerTargetRed = new Translation2d(0.5, 0.5); private Drivetrain() { @@ -108,7 +141,9 @@ private Drivetrain() }, new Pose2d() ); - SmartDashboard.putData("Field",m_field); + + SmartDashboard.putData("Field", m_field); + } public static Drivetrain getInstance() @@ -131,6 +166,8 @@ public void logTelemetry() m_field.setRobotPose(pose); + // m_vPEField.setRobotPose(_visionPoseEstimator.getEstimatedPosition()); + SmartDashboard.putNumber("drivetrainGyroAngle", getGyroRotation().getDegrees()); SmartDashboard.putNumber("drivetrainGyroAngleWithOffset", @@ -154,8 +191,21 @@ public void logTelemetry() SmartDashboard.putNumber("drivetrainOdometryZ", pose.getRotation().getDegrees()); } + + public void updateOdometry() { + _drivePoseEstimator.updateWithTime( + Timer.getFPGATimestamp(), + getGyroRotation(), + new SwerveModulePosition[] { + _frontLeftModule.getPosition(), + _frontRightModule.getPosition(), + _backLeftModule.getPosition(), + _backRightModule.getPosition() + } + ); + } - public void updateOdometry() + public void updateOdometryWithVision() { _drivePoseEstimator.updateWithTime( @@ -168,6 +218,73 @@ public void updateOdometry() _backRightModule.getPosition() } ); + + PoseEstimate poseEstimate = LimelightHelpers + .getBotPoseEstimate_wpiBlue(kLimelightName); + + if (poseEstimate.tagCount == 0) return; + + if (poseEstimate.tagCount == 1 && + poseEstimate.rawFiducials[0].ambiguity > 0.5) { + + return; + } + + double poseDifference = _drivePoseEstimator + .getEstimatedPosition() + .getTranslation() + .getDistance(poseEstimate.pose.getTranslation()); + + double bestTagArea = 0; + for (int i = 0; i < poseEstimate.rawFiducials.length; i++) { + RawFiducial rawFiducial = poseEstimate.rawFiducials[i]; + + if (rawFiducial.ta > bestTagArea) { + bestTagArea = rawFiducial.ta; + } + } + + double xyStds; + double degStds; + + // note that besides the tag count check, there are no equals checks + // (e.g. <=) just < and >, for var name consistency we use = + if (poseEstimate.tagCount >= 2) { + xyStds = kHighTrustXYStds; + degStds = kHighTrustDegStds; + + } else if (bestTagArea >= kNearTagAreaMin && + poseDifference <= kMidTrustPoseDiffMax) { + + xyStds = kMidTrustXYStds; + degStds = kMidTrustDegStds; + + } else if (bestTagArea >= kFarTagAreaMin && + poseDifference <= kLowTrustPoseDiffMax) { + + xyStds = kLowTrustXYStds; + degStds = kLowTrustDegStds; + } else { + return; + } + + _drivePoseEstimator.setVisionMeasurementStdDevs( + // VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)) + VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)) + ); + + // Pose2d pose = new Pose2d( + // poseEstimate.pose.getX(), + // poseEstimate.pose.getY(), + // getGyroRotation() + // ); + + SmartDashboard.putNumber("latency", poseEstimate.latency); + + _drivePoseEstimator.addVisionMeasurement( + poseEstimate.pose, + Timer.getFPGATimestamp() - (poseEstimate.latency / 1000) + ); } // public void resetOdometry() @@ -240,10 +357,11 @@ public SwerveModulePosition[] getModulePositions() */ public void fieldOrientedDrive(double translationX, double translationY, - double rotationZ) - { + double rotationZ) { + var alliance = DriverStation.getAlliance(); var invert = alliance.isPresent() && alliance.get() == Alliance.Red ? -1 : 1; + _chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( invert * translationX * Constants.MAX_VELOCITY_METERS_PER_SECOND, invert * translationY * Constants.MAX_VELOCITY_METERS_PER_SECOND, @@ -281,6 +399,10 @@ public void drive(ChassisSpeeds chassisSpeeds) _chassisSpeeds = chassisSpeeds; } + // public void getTargetAngle() { + // Pose2d targetPose + // } + /** * Method that resets the pigeon current direction the robot is facing will be the front */ @@ -298,4 +420,26 @@ public void setStartingPose(Pose2d startingPose) { public Pose2d getPose(){ return _drivePoseEstimator.getEstimatedPosition(); } + + public double getAngleToTarget(Translation2d target) { + return target + .minus(getPose().getTranslation()) + .getAngle() + .plus(AllianceSpecific.getAllianceAngleOffset()) + .minus(getGyroRotationWithOffset()) + .getDegrees(); + } + + public double getDistanceToTarget(Translation2d target) { + return getPose() + .getTranslation() + .getDistance(target); + } + + public double getRotationDifference(double angle) { + return Rotation2d + .fromDegrees(angle) + .minus(getGyroRotationWithOffset()) + .getDegrees(); + } } diff --git a/src/main/java/frc/robot/Subsystems/Jukebox.java b/src/main/java/frc/robot/Subsystems/Jukebox.java index 26b8885..be3fe56 100644 --- a/src/main/java/frc/robot/Subsystems/Jukebox.java +++ b/src/main/java/frc/robot/Subsystems/Jukebox.java @@ -8,11 +8,16 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -77,13 +82,13 @@ public class Jukebox extends Subsystem{ private final double kPhotoEyeDebounceTime = 0.04; // Set Points - private final double kTrapElevatorPosition = 60; - private final double kTrapShooterAngle = 14; + private final double kTrapElevatorPosition = 79; + private final double kTrapShooterAngle = 11; private final double kExtendClimbElevatorPosition = 83; // change this private final double kExtendClimbShooterAngle = 4; private final double kAmpElevatorPosition = 60; private final double kFeedShooterAngle = 7; - private final double kPodiumSpeakerShotAngle = 5.9; + private final double kPodiumSpeakerShotAngle = 6; private final double kPodiumSpeakerShotSpeed = 38; private final double kLineSpeakerShotAngle = 5.2; private final double kLineSpeakerShotSpeed = 35; @@ -135,11 +140,18 @@ public class Jukebox extends Subsystem{ //private final double[] kShooterSpeeds = {35, 36, 38, 41, 44}; // Old - private final double[] kDistanceIDs = {1.77, 2, 2.5, 3, 3.5, 4}; + private final double[] kDistanceIDs = {1.5, 2, 2.5, 3, 3.5, 4}; // private final double[] kShooterAngles = {5.25, 5.75, 5.85, 6.2, 6.375}; - private final double[] kShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35}; + // private final double[] kShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35}; + private final double[] kShooterAngles = {3, 4, 5, 5.39, 5.73, 6.2}; private final double[] kShooterSpeeds = {67, 67, 67, 67, 67, 67}; + private final double[] kFeedDistanceIDs = {1.77, 2, 2.5, 3, 3.5, 4}; + // private final double[] kShooterAngles = {5.25, 5.75, 5.85, 6.2, 6.375}; + // private final double[] kFeedShooterAngles = {4.5, 5.1, 5.55, 5.85, 6.2, 6.35}; + private final double[] kFeedShooterAngles = {4.5, 5.15, 5.60, 5.90, 6.25, 6.40}; // might work + private final double kFeedShooterSpeed = 67; + private double _manualElevatorSpeed = 0; private double _manualFeederSpeed = 0; private double _manualShooterAngleSpeed = 0; @@ -155,6 +167,8 @@ public class Jukebox extends Subsystem{ private int _loopCounter = 0; + private static Translation2d kSpeaker; + public Jukebox() { // Config Elevator @@ -243,7 +257,7 @@ private void configShooter() { kShooterFeedForwardKv ); - _shootReadyDebouncer = new Debouncer(.04, DebounceType.kRising); + _shootReadyDebouncer = new Debouncer(.25, DebounceType.kBoth); _manualShooterSpeed = 0.0; _shooterSetpoint = 0.0; @@ -567,11 +581,15 @@ private void prepTrap() { private void prepSpeaker() { setElevatorPosition(0); - _targetDistance = _visionSystem.getDistance(); + // _targetDistance = _visionSystem.getDistance(); + + _targetDistance = Drivetrain.getInstance().getDistanceToTarget( + AllianceSpecific.getSpeaker() + ); feeder(); - if (_targetDistance != 0.0) { + if (_targetDistance > 1.75) { double desiredShooterAngle = OneDimensionalLookup.interpLinear( kDistanceIDs, kShooterAngles, @@ -582,6 +600,7 @@ private void prepSpeaker() { kShooterSpeeds, _targetDistance ); + setShooterAngle(desiredShooterAngle); setShooterSpeed(desiredShooterSpeed); @@ -591,18 +610,45 @@ private void prepSpeaker() { } } + private void prepFeed() { + setElevatorPosition(0); + + // _targetDistance = _visionSystem.getDistance(); + + _targetDistance = Drivetrain.getInstance().getDistanceToTarget( + AllianceSpecific.getZone() + ); + + feeder(); + + if (_targetDistance != 0.0) { + double desiredShooterAngle = OneDimensionalLookup.interpLinear( + kFeedDistanceIDs, + kFeedShooterAngles, + _targetDistance + ); + + setShooterAngle(desiredShooterAngle); + setShooterSpeed(kFeedShooterSpeed); + + } else { + setShooterAngle(Constants.KMinShooterAngle); + setShooterSpeed(Constants.kMaxShooterSpeed); + } + } + /** Preps the speaker for a shot from the podium (Doesn't use auto aim) */ private void prepSpeakerPodium() { feeder(); setShooterAngle(kPodiumSpeakerShotAngle); - setShooterSpeed(kPodiumSpeakerShotSpeed); + setShooterSpeed(67); setElevatorPosition(0); } private void prepSpeakerLine() { feeder(); setShooterAngle(kLineSpeakerShotAngle); - setShooterSpeed(kLineSpeakerShotSpeed); + setShooterSpeed(67); setElevatorPosition(0); } @@ -778,6 +824,9 @@ public void handleCurrentState() case PREP_SPEAKER: prepSpeaker(); break; + case PREP_FEED: + prepFeed(); + break; case PREP_SPEAKER_PODIUM: prepSpeakerPodium(); break; diff --git a/src/main/java/frc/robot/utilities/AllianceSpecific.java b/src/main/java/frc/robot/utilities/AllianceSpecific.java new file mode 100644 index 0000000..b7ccbb5 --- /dev/null +++ b/src/main/java/frc/robot/utilities/AllianceSpecific.java @@ -0,0 +1,40 @@ +package frc.robot.Utilities; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Constants.Constants; + +public class AllianceSpecific { + private static boolean isRed() { + return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; + } + + public static Translation2d getSpeaker() { + return isRed() ? Constants.kRedSpeaker : Constants.kBlueSpeaker; + } + + public static Translation2d getZone() { + return isRed() ? Constants.kRedZone : Constants.kBlueZone; + } + + public static Rotation2d getAllianceAngleOffset() { + //return Rotation2d.fromDegrees(isRed() ? 180 : 0); + return Rotation2d.fromDegrees(180); + } + + public static Translation2d mirrorTranslation( + Translation2d blueTranslation + ) { + return new Translation2d( + blueTranslation.getX() + + Constants.kFieldLength - + (2 * blueTranslation.getX()), + + blueTranslation.getY() + ); + } +} diff --git a/src/main/java/frc/robot/utilities/LEDController.java b/src/main/java/frc/robot/utilities/LEDController.java index 5b7f1d2..fa58cc9 100644 --- a/src/main/java/frc/robot/utilities/LEDController.java +++ b/src/main/java/frc/robot/utilities/LEDController.java @@ -58,7 +58,7 @@ public class LEDController { // animation for SCORE --color is Green SCORE_LIGHTS = new ColorFlowAnimation(0, 255, 0, 0, 0.5, jukeboxNumLeds, Direction.Forward); // animation for PREP_SPEAKER --color is Turquoise - PREP_SPEAKER_LIGHTS = new FireAnimation(.5, .5, _jukebox.getShooterLeds(jukeboxNumLeds), .25, .1); + PREP_SPEAKER_LIGHTS = new FireAnimation(.5, .5, jukeboxNumLeds, .75, .1); // animation for PREP_AMP --color is Indigo PREP_AMP_LIGHTS = new RainbowAnimation(.5, .5, jukeboxNumLeds); // animation for PREP_TRAP --color is Sunset Orange @@ -123,21 +123,36 @@ public void handleLights() } break; case SCORE: + upperCandle.clearAnimation(0); upperCandle.setLEDs(0, 255, 255, 0, 0, jukeboxNumLeds); break; case PREP_LAUNCH: + upperCandle.clearAnimation(0); upperCandle.setLEDs(170, 51, 106, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds)); break; + case PREP_SPEAKER_SUBWOOFER: + upperCandle.clearAnimation(0); + upperCandle.setLEDs(0, 0, 255, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds)); + break; case PREP_SPEAKER_PODIUM: + upperCandle.clearAnimation(0); + upperCandle.setLEDs(255, 0, 255, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds)); + break; case PREP_SPEAKER_LINE: case PREP_SPEAKER: // upperCandle.clearAnimation(0); // upperCandle.animate(PREP_SPEAKER_LIGHTS); - upperCandle.setLEDs(255, 0, 0, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds)); + if (_jukebox.getIsReadyToScore()) { + upperCandle.animate(PREP_SPEAKER_LIGHTS); + } else { + upperCandle.clearAnimation(0); + upperCandle.setLEDs(255, 0, 0, 0, 0, _jukebox.getShooterLeds(jukeboxNumLeds)); + } break; case PREP_AMP: // upperCandle.clearAnimation(0); // upperCandle.animate(PREP_AMP_LIGHTS); + upperCandle.clearAnimation(0); upperCandle.setLEDs(255, 153, 51, 0, 0, jukeboxNumLeds); break; case PREP_TRAP: