diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 00000000..0967ef42 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/.gitignore b/.gitignore index f20c8041..1cc6504d 100644 --- a/.gitignore +++ b/.gitignore @@ -186,3 +186,5 @@ compile_commands.json # Generated BuildConstants.java + +.factorypath diff --git a/.vscode/settings.json b/.vscode/settings.json index 57b1c91f..26bbbfd3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.compile.nullAnalysis.mode": "automatic" + "java.compile.nullAnalysis.mode": "automatic", + "java.debug.settings.onBuildFailureProceed": true } diff --git a/notes/bringUpSFR.md b/notes/bringUpSFR.md new file mode 100644 index 00000000..36b7eb9a --- /dev/null +++ b/notes/bringUpSFR.md @@ -0,0 +1,12 @@ +# Checklist for bringing up elevator changes + other stuff before SFR + +- [ ] Sysid elevator +- [ ] Tune elevator +- [ ] Test new cameras +- [ ] Algae cam tuning +- [ ] Make new climb work +- [ ] Test dot product selection +- [ ] Test IK extension +- [ ] Test intermediate pose autoaims +- [ ] Test smoother autoaims if ready +- [ ] Test adjusted extend with clearance diff --git a/notes/gyroPostmortem.md b/notes/gyroPostmortem.md new file mode 100644 index 00000000..37972e1c --- /dev/null +++ b/notes/gyroPostmortem.md @@ -0,0 +1,67 @@ +# OCR Gyro Issue Postmortem + +This writeup pertains to the gyro issue which cost us quals 66 and 77 and OCR. +Match videos for those quals are available on TBA and logs were sent in a zip file on the **#software** channel on slack. + +## Symptoms + +The issue was first noticed in the week (ish) before OCR as a ```Missing Gyro Data``` alert. +This alert would appear at startup and never clear. +Data from the gyro would not appear in the ```Async Odo``` inputs. +The gyro inputs would have an `isConnected` value of `false` and the position and velocity inputs would not change. +During debugging we logged the `StatusCode` of the `yaw` status signal in `GyroIOPigeon2`. +This would return a value of `CanMessageStale` when the issue occurred, and `Ok` otherwise. +Once we saw a value of `RxTimeout`, but this did not reproduce and is likely another manifestation of the same issue. + +## Debugging + +Our initial theory for the cause of the issue was a bug in the pose estimation stack. +Because of the rewrite this offseason, we weren't 100% confident in it. +Thorough inspection did not yield any issues and logging of the `StatusCode` of the `yaw` signal indicated an issue elsewhere. + +At this point we had not noticed that the issue only appeared on startup, and did not investigate it as such. + +The next major theory was that there was a hardware issue somewhere on the robot. +A CAN bus break seemed unlikely because the Pigeon was the first device on the bus, so we should see the entire bus go down. +Similar reasoning was applied to a CANivore issue. + +This lead us to believe there was an internal hardware issue with the Pigeon. +This made sense for an intermittent issue, and given that we did not have a clear smoking gun in our code or wiring made sense to us. +However, swapping the Pigeon would be somewhat difficult and require equipment and spares we did not have stocked at the Loom. +Therefore, we did not conduct the swap. +Without any further evidence or realizing that the issue was only at startup, we did not come up with any other theories before OCR. + +The issue reappeared in quals 66 at OCR, and the prevailing "internal hardware issue" theory was applied. +After the Pigeon was swapped, the robot was powered on once and the issue did not reappear. +The issue appeared on the field the next match. + +Around this time at OCR, it was realized that the issue was exclusively on startup. +Similarly, it was found that the Pigeon was the first CAN device initialized. +This lead to the theory that there was a race condition involving the CANivore initialization, possible related to our use of the ```"*"``` wildcard identifier for CANivores. + +Restarting the RoboRIO appeared to clear the issue, leading further credibility to a startup issue. +For the remainder of OCR, a status indicator was added to the dashboard and the drive team was instructed to restart the robot whenever the issue reappeared. + +After OCR we reproduced the issue on an elevator TalonFX by initializing that device first. +This appears to confirm the race condition theory. +A fix explicitly blocking on CANivore initialization before any devices are initialized appears to work at time of writing after several reboots to attempt to reproduce the issue. + +## Process failures + +Several process failures resulted in this issue costing us quals 66 and 77. +In roughly chronological order, they are: + + 1. Not resolving the issue after it appeared. + This issue was clearly problematic as soon as it appeared. + It should not have been ignored and the assumed fix should have been checked. + 2. Not rigorously testing the "fix" after applying it. + We only powered on once after swapping the pigeon. + The issue was known to be intermittent, therefore we should have tested it many times to ensure it would not reappear. + 3. Not checking for the failure on the field after "fixing" it. + It was easy to add a widget to elastic to display the gyro's status. + We even did so for elims. + We did not do so immediately after making the fix, which resulted in the issue going unnoticed during quals 77. + 4. Not recording the failure on slack, in the failures sheet, or otherwise properly communicating it before OCR. + If the failure had been properly recorded and reported, it would have had a greater chance of being given attention and solved before OCR. + The sheet has been largely unused by the team, which contributed to it not being filled out for this issue. + Building momentum behind it will help prevent similar failures in the future. diff --git a/src/main/deploy/choreo/EtoPRO.traj b/src/main/deploy/choreo/EtoPRO.traj index 2591965f..54447587 100644 --- a/src/main/deploy/choreo/EtoPRO.traj +++ b/src/main/deploy/choreo/EtoPRO.traj @@ -3,7 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.009561061859131, "y":2.7982044219970703, "heading":2.0928880900706415, "intervals":53, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.009561061859131, "y":2.7982044219970703, "heading":2.0928880900706415, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.943572521209717, "y":2.3063879013061523, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.0615134239196777, "y":1.1718214750289917, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":1.6333351135253906, "y":0.6246773600578308, "heading":0.9420001549844138, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +15,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"E.x", "val":5.009561061859131}, "y":{"exp":"E.y", "val":2.7982044219970703}, "heading":{"exp":"2.0928880900706415 rad", "val":2.0928880900706415}, "intervals":53, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"E.x", "val":5.009561061859131}, "y":{"exp":"E.y", "val":2.7982044219970703}, "heading":{"exp":"2.0928880900706415 rad", "val":2.0928880900706415}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.943572521209717 m", "val":4.943572521209717}, "y":{"exp":"2.3063879013061523 m", "val":2.3063879013061523}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.0615134239196777 m", "val":2.0615134239196777}, "y":{"exp":"1.1718214750289917 m", "val":1.1718214750289917}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"PRO.x", "val":1.6333351135253906}, "y":{"exp":"PRO.y", "val":0.6246773600578308}, "heading":{"exp":"0.9420001549844138 rad", "val":0.9420001549844138}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,62 +30,97 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.9484], + "waypoints":[0.0,0.53233,1.68627,2.27513], "samples":[ - {"t":0.0, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.55988, "ay":-2.29199, "alpha":-1.3843, "fx":[-62.51967,-62.1241,-54.84488,-53.3006], "fy":[-30.65475,-31.48719,-42.92983,-44.80661]}, - {"t":0.03676, "x":5.00716, "y":2.79666, "heading":2.09289, "vx":-0.13087, "vy":-0.08426, "omega":-0.05089, "ax":-3.55998, "ay":-2.29202, "alpha":-1.37635, "fx":[-62.49458,-62.10553,-54.8624,-53.33312], "fy":[-30.69796,-31.51744,-42.9028,-44.76253]}, - {"t":0.07352, "x":4.99994, "y":2.79201, "heading":2.09102, "vx":-0.26174, "vy":-0.16852, "omega":-0.10149, "ax":-3.56009, "ay":-2.29206, "alpha":-1.36771, "fx":[-62.46071,-62.09146,-54.89029,-53.36007], "fy":[-30.75821,-31.53821,-42.86208,-44.72462]}, - {"t":0.11029, "x":4.98791, "y":2.78427, "heading":2.08729, "vx":-0.39262, "vy":-0.25278, "omega":-0.15177, "ax":-3.5602, "ay":-2.2921, "alpha":-1.3583, "fx":[-62.41786,-62.0815,-54.92855,-53.38208], "fy":[-30.83572,-31.55023,-42.80758,-44.69211]}, - {"t":0.14705, "x":4.97107, "y":2.77342, "heading":2.08171, "vx":-0.5235, "vy":-0.33704, "omega":-0.2017, "ax":-3.56032, "ay":-2.29214, "alpha":-1.348, "fx":[-62.36572,-62.07518,-54.97717,-53.39995], "fy":[-30.93079,-31.55433,-42.73915,-44.66397]}, - {"t":0.18381, "x":4.94942, "y":2.75948, "heading":2.07429, "vx":-0.65439, "vy":-0.42131, "omega":-0.25126, "ax":-3.56046, "ay":-2.29218, "alpha":-1.3367, "fx":[-62.30392,-62.07192,-55.03618,-53.41467], "fy":[-31.04387,-31.55158,-42.65659,-44.63898]}, - {"t":0.22057, "x":4.92296, "y":2.74245, "heading":2.06506, "vx":-0.78528, "vy":-0.50557, "omega":-0.3004, "ax":-3.5606, "ay":-2.29222, "alpha":-1.32424, "fx":[-62.23199,-62.07099,-55.10562,-53.42743], "fy":[-31.17548,-31.54323,-42.55962,-44.61563]}, - {"t":0.25734, "x":4.89168, "y":2.72231, "heading":2.05401, "vx":-0.91617, "vy":-0.58984, "omega":-0.34908, "ax":-3.56075, "ay":-2.29227, "alpha":-1.31043, "fx":[-62.14936,-62.07156,-55.18553,-53.43963], "fy":[-31.32627,-31.5308,-42.44792,-44.59211]}, - {"t":0.2941, "x":4.8556, "y":2.69908, "heading":2.04118, "vx":-1.04707, "vy":-0.67411, "omega":-0.39725, "ax":-3.56092, "ay":-2.29232, "alpha":-1.29507, "fx":[-62.05536,-62.07258,-55.27601,-53.45296], "fy":[-31.49698,-31.51613,-42.32107,-44.56626]}, - {"t":0.33086, "x":4.8147, "y":2.67275, "heading":2.02657, "vx":-1.17798, "vy":-0.75838, "omega":-0.44486, "ax":-3.5611, "ay":-2.29238, "alpha":-1.27786, "fx":[-61.94922,-62.07278,-55.37712,-53.46944], "fy":[-31.68847,-31.50147,-42.17859,-44.5355]}, - {"t":0.36762, "x":4.76899, "y":2.64332, "heading":2.01022, "vx":-1.3089, "vy":-0.84265, "omega":-0.49184, "ax":-3.56129, "ay":-2.29244, "alpha":-1.25849, "fx":[-61.83002,-62.07061,-55.48897,-53.49144], "fy":[-31.90166,-31.48957,-42.0199,-44.49673]}, - {"t":0.40439, "x":4.71846, "y":2.61079, "heading":1.99214, "vx":-1.43982, "vy":-0.92693, "omega":-0.53811, "ax":-3.56149, "ay":-2.2925, "alpha":-1.23653, "fx":[-61.69671,-62.06414,-55.61169,-53.52187], "fy":[-32.13759,-31.48385,-41.84427,-44.44617]}, - {"t":0.44115, "x":4.66312, "y":2.57517, "heading":1.97236, "vx":-1.57075, "vy":-1.01121, "omega":-0.58356, "ax":-3.56171, "ay":-2.29256, "alpha":-1.21143, "fx":[-61.54809,-62.05092,-55.74542,-53.56423], "fy":[-32.3974,-31.4886,-41.65083,-44.37915]}, - {"t":0.47791, "x":4.60297, "y":2.53644, "heading":1.9509, "vx":-1.70168, "vy":-1.09549, "omega":-0.6281, "ax":-3.56194, "ay":-2.29262, "alpha":-1.1825, "fx":[-61.38271,-62.02781,-55.89034,-53.62287], "fy":[-32.68233,-31.50933,-41.43848,-44.28986]}, - {"t":0.51467, "x":4.53801, "y":2.49462, "heading":1.92781, "vx":-1.83263, "vy":-1.17977, "omega":-0.67157, "ax":-3.56218, "ay":-2.29268, "alpha":-1.14879, "fx":[-61.19887,-61.99067,-56.0467,-53.70328], "fy":[-32.9938,-31.55322,-41.20581,-44.17077]}, - {"t":0.55143, "x":4.46823, "y":2.4497, "heading":1.90313, "vx":-1.96358, "vy":-1.26405, "omega":-0.7138, "ax":-3.56243, "ay":-2.29272, "alpha":-1.10902, "fx":[-60.99446,-61.93387,-56.21486,-53.81256], "fy":[-33.33344,-31.62988,-40.95092,-44.012]}, - {"t":0.5882, "x":4.39364, "y":2.40168, "heading":1.87688, "vx":-2.09454, "vy":-1.34834, "omega":-0.75457, "ax":-3.56268, "ay":-2.29273, "alpha":-1.06135, "fx":[-60.76685,-61.84953,-56.39532,-53.96015], "fy":[-33.70323,-31.75252,-40.67118,-43.80003]}, - {"t":0.62496, "x":4.31423, "y":2.35057, "heading":1.84914, "vx":-2.22552, "vy":-1.43262, "omega":-0.79359, "ax":-3.5629, "ay":-2.29268, "alpha":-1.00311, "fx":[-60.51252,-61.72617,-56.5889,-54.15912], "fy":[-34.1057,-31.94004,-40.36272,-43.51565]}, - {"t":0.66172, "x":4.23001, "y":2.29635, "heading":1.81997, "vx":-2.3565, "vy":-1.51691, "omega":-0.83047, "ax":-3.56308, "ay":-2.29254, "alpha":-0.93022, "fx":[-60.22661,-61.54621,-56.79696,-54.42824], "fy":[-34.54437,-32.2205,-40.01959,-43.13027]}, - {"t":0.69848, "x":4.14097, "y":2.23904, "heading":1.78944, "vx":-2.48748, "vy":-1.60119, "omega":-0.86466, "ax":-3.56312, "ay":-2.29221, "alpha":-0.83611, "fx":[-59.90188,-61.2811,-57.02186,-54.79592], "fy":[-35.02448,-32.63794,-39.63198,-42.59888]}, - {"t":0.73525, "x":4.04711, "y":2.17862, "heading":1.75765, "vx":-2.61847, "vy":-1.68545, "omega":-0.8954, "ax":-3.56285, "ay":-2.29153, "alpha":-0.70954, "fx":[-59.52683,-60.88096,-57.26798,-55.30779], "fy":[-35.5546,-33.26609,-39.18245,-41.8455]}, - {"t":0.77201, "x":3.94845, "y":2.11511, "heading":1.72474, "vx":-2.74945, "vy":-1.7697, "omega":-0.92149, "ax":-3.56185, "ay":-2.29011, "alpha":-0.52958, "fx":[-59.08145,-60.24956,-57.54412,-56.04252], "fy":[-36.1499,-34.23915,-38.63677,-40.73011]}, - {"t":0.80877, "x":3.84496, "y":2.04851, "heading":1.69086, "vx":-2.88039, "vy":-1.85389, "omega":-0.94095, "ax":-3.55883, "ay":-2.28698, "alpha":-0.25256, "fx":[-58.52705,-59.1744,-57.87016,-57.14848], "fy":[-36.84013,-35.83082,-37.91763,-38.96218]}, - {"t":0.84553, "x":3.73667, "y":1.97881, "heading":1.65627, "vx":-3.01122, "vy":-1.93796, "omega":-0.95024, "ax":-3.54927, "ay":-2.27891, "alpha":0.22967, "fx":[-57.77748,-57.08046,-58.29988,-58.93735], "fy":[-37.69137,-38.69614,-36.80834,-35.82734]}, - {"t":0.8823, "x":3.62357, "y":1.90603, "heading":1.62134, "vx":-3.1417, "vy":-2.02174, "omega":-0.9418, "ax":-3.51004, "ay":-2.24994, "alpha":1.27109, "fx":[-56.59562,-51.77734,-59.03393,-62.12317], "fy":[-38.87925,-44.81872,-34.39937,-29.03171]}, - {"t":0.91906, "x":3.5057, "y":1.83018, "heading":1.58671, "vx":-3.27074, "vy":-2.10445, "omega":-0.89507, "ax":-3.20276, "ay":-1.93507, "alpha":4.97689, "fx":[-54.04087,-27.39594,-60.98347,-67.01606], "fy":[-41.0086,-60.99343,-17.31238,-7.22454]}, - {"t":0.95582, "x":3.3833, "y":1.75151, "heading":1.55381, "vx":-3.38848, "vy":-2.17559, "omega":-0.71211, "ax":0.083, "ay":0.04406, "alpha":-0.15301, "fx":[1.79038,0.93722,0.92325,1.77663], "fy":[1.13073,1.14577,0.30983,0.29518]}, - {"t":0.99258, "x":3.25879, "y":1.67156, "heading":1.52763, "vx":-3.38543, "vy":-2.17397, "omega":-0.71773, "ax":3.1585, "ay":1.88415, "alpha":-5.33491, "fx":[54.78476,24.87696,59.59757,67.28228], "fy":[40.06571,62.15542,17.47368,3.51402]}, - {"t":1.02934, "x":3.13646, "y":1.59291, "heading":1.50124, "vx":-3.26932, "vy":-2.1047, "omega":-0.91385, "ax":3.50318, "ay":2.24627, "alpha":-1.45936, "fx":[57.09302,50.72237,58.43933,62.8268], "fy":[38.16372,46.0207,35.28795,27.41656]}, - {"t":1.06611, "x":3.01864, "y":1.51706, "heading":1.46765, "vx":-3.14053, "vy":-2.02213, "omega":-0.9675, "ax":3.54824, "ay":2.27856, "alpha":-0.32746, "fx":[57.91591,56.61718,58.14607,59.3484], "fy":[37.48383,39.3664,37.02297,35.12714]}, - {"t":1.10287, "x":2.90559, "y":1.44426, "heading":1.43208, "vx":-3.01009, "vy":-1.93836, "omega":-0.97954, "ax":3.55885, "ay":2.2872, "alpha":0.20113, "fx":[58.23343,59.01458,58.14463,57.32912], "fy":[37.30191,36.07985,37.48575,38.69825]}, - {"t":1.13963, "x":2.79734, "y":1.37455, "heading":1.39607, "vx":-2.87926, "vy":-1.85428, "omega":-0.97215, "ax":3.56198, "ay":2.29042, "alpha":0.50509, "fx":[58.32245,60.30281,58.24151,56.05929], "fy":[37.35803,34.12537,37.5743,40.71842]}, - {"t":1.17639, "x":2.69389, "y":1.30793, "heading":1.36033, "vx":-2.74831, "vy":-1.77008, "omega":-0.95358, "ax":3.56284, "ay":2.29183, "alpha":0.70225, "fx":[58.2902,61.10732,58.38062,55.20471], "fy":[37.54153,32.82393,37.50464,41.99819]}, - {"t":1.21316, "x":2.59527, "y":1.2444, "heading":1.32528, "vx":-2.61733, "vy":-1.68582, "omega":-0.92776, "ax":3.5629, "ay":2.29251, "alpha":0.84061, "fx":[58.18758,61.65786,58.53914,54.60225], "fy":[37.79676,31.8926,37.35625,42.86709]}, - {"t":1.24992, "x":2.50146, "y":1.18398, "heading":1.29117, "vx":-2.48635, "vy":-1.60155, "omega":-0.89686, "ax":3.56265, "ay":2.29286, "alpha":0.94332, "fx":[58.04233,62.05779,58.70578,54.16434], "fy":[38.09225,31.19339,37.16585,43.48378]}, - {"t":1.28668, "x":2.41246, "y":1.12665, "heading":1.2582, "vx":-2.35538, "vy":-1.51725, "omega":-0.86218, "ax":3.56227, "ay":2.29303, "alpha":1.02284, "fx":[57.87117,62.36041,58.87422,53.83978], "fy":[38.40865,30.65099,36.95304,43.93424]}, - {"t":1.32344, "x":2.32828, "y":1.07242, "heading":1.2265, "vx":-2.22442, "vy":-1.43296, "omega":-0.82458, "ax":3.56186, "ay":2.29312, "alpha":1.08647, "fx":[57.68496,62.59605,59.04065,53.59655], "fy":[38.73333,30.22056,36.72944,44.26937]}, - {"t":1.36021, "x":2.24891, "y":1.02129, "heading":1.19619, "vx":-2.09348, "vy":-1.34866, "omega":-0.78464, "ax":3.56143, "ay":2.29316, "alpha":1.13874, "fx":[57.49119,62.78334,59.20261,53.41341], "fy":[39.05768,29.87355,36.50244,44.52141]}, - {"t":1.39697, "x":2.17435, "y":0.97326, "heading":1.16734, "vx":-1.96256, "vy":-1.26436, "omega":-0.74278, "ax":3.56102, "ay":2.29316, "alpha":1.18258, "fx":[57.2952,62.93444,59.35844,53.27558], "fy":[39.3757,29.59067,36.27704,44.71195]}, - {"t":1.43373, "x":2.10461, "y":0.92833, "heading":1.14004, "vx":-1.83164, "vy":-1.18005, "omega":-0.6993, "ax":3.56063, "ay":2.29315, "alpha":1.21999, "fx":[57.10097,63.0577,59.50704,53.17241], "fy":[39.6831,29.35828,36.05675,44.85611]}, - {"t":1.47049, "x":2.03968, "y":0.8865, "heading":1.11433, "vx":-1.70075, "vy":-1.09575, "omega":-0.65445, "ax":3.56026, "ay":2.29311, "alpha":1.25234, "fx":[56.9115,63.15911,59.64763,53.0959], "fy":[39.97681,29.16628,35.84414,44.9649]}, - {"t":1.50725, "x":1.97957, "y":0.84777, "heading":1.09027, "vx":-1.56986, "vy":-1.01145, "omega":-0.60841, "ax":3.55992, "ay":2.29307, "alpha":1.28063, "fx":[56.72909,63.24313,59.77967,53.03991], "fy":[40.25467,29.00695,35.64113,45.04659]}, - {"t":1.54402, "x":1.92426, "y":0.81213, "heading":1.0679, "vx":-1.43899, "vy":-0.92715, "omega":-0.56133, "ax":3.55961, "ay":2.29302, "alpha":1.30559, "fx":[56.55552,63.3132,59.90279,52.99963], "fy":[40.51513,28.87416,35.44919,45.10757]}, - {"t":1.58078, "x":1.87376, "y":0.7796, "heading":1.04727, "vx":-1.30813, "vy":-0.84286, "omega":-0.51334, "ax":3.55931, "ay":2.29296, "alpha":1.32775, "fx":[56.39214,63.37202,60.01675,52.97117], "fy":[40.75713,28.76296,35.26946,45.15288]}, - {"t":1.61754, "x":1.82808, "y":0.75016, "heading":1.0284, "vx":-1.17728, "vy":-0.75856, "omega":-0.46453, "ax":3.55905, "ay":2.29291, "alpha":1.34752, "fx":[56.24003,63.42179,60.12139,52.95139], "fy":[40.97998,28.66924,35.10282,45.18655]}, - {"t":1.6543, "x":1.7872, "y":0.72383, "heading":1.01132, "vx":-1.04645, "vy":-0.67427, "omega":-0.41499, "ax":3.5588, "ay":2.29285, "alpha":1.36524, "fx":[56.09999,63.4643,60.2166,52.93769], "fy":[41.18325,28.58955,34.94997,45.21191]}, - {"t":1.69107, "x":1.75114, "y":0.70059, "heading":0.99606, "vx":-0.91562, "vy":-0.58998, "omega":-0.3648, "ax":3.55858, "ay":2.29279, "alpha":1.38115, "fx":[55.97266,63.50108,60.30234,52.9279], "fy":[41.3667,28.52095,34.8115,45.23168]}, - {"t":1.72783, "x":1.71988, "y":0.68045, "heading":0.98265, "vx":-0.78479, "vy":-0.50569, "omega":-0.31402, "ax":3.55838, "ay":2.29273, "alpha":1.39546, "fx":[55.85852,63.53337,60.37859,52.92022], "fy":[41.53023,28.46091,34.68784,45.24814]}, - {"t":1.76459, "x":1.69344, "y":0.66341, "heading":0.97111, "vx":-0.65398, "vy":-0.4214, "omega":-0.26272, "ax":3.55819, "ay":2.29268, "alpha":1.40835, "fx":[55.75791,63.56228,60.44533,52.91311], "fy":[41.67382,28.40726,34.57936,45.26318]}, - {"t":1.80135, "x":1.6718, "y":0.64946, "heading":0.96145, "vx":-0.52317, "vy":-0.33712, "omega":-0.21095, "ax":3.55802, "ay":2.29263, "alpha":1.41995, "fx":[55.67114,63.58869,60.50256,52.9053], "fy":[41.7975,28.35816,34.48638,45.27841]}, - {"t":1.83811, "x":1.65497, "y":0.63862, "heading":0.9537, "vx":-0.39237, "vy":-0.25284, "omega":-0.15875, "ax":3.55787, "ay":2.29259, "alpha":1.43039, "fx":[55.59841,63.61339,60.5503,52.8957], "fy":[41.90133,28.31202,34.40915,45.29516]}, - {"t":1.87488, "x":1.64295, "y":0.63087, "heading":0.94786, "vx":-0.26158, "vy":-0.16856, "omega":-0.10617, "ax":3.55774, "ay":2.29255, "alpha":1.43975, "fx":[55.53993,63.63701,60.58855,52.88342], "fy":[41.98537,28.26751,34.3479,45.31456]}, - {"t":1.91164, "x":1.63574, "y":0.62623, "heading":0.94396, "vx":-0.13079, "vy":-0.08428, "omega":-0.05324, "ax":3.55762, "ay":2.29252, "alpha":1.44813, "fx":[55.49584,63.66008,60.6173,52.86771], "fy":[42.04964,28.22354,34.30281,45.33752]}, - {"t":1.9484, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.65612, "ay":-3.91125, "alpha":-0.86939, "fx":[31.16649,21.94068,23.56828,31.62211], "fy":[-62.22006,-66.04259,-65.49563,-62.00774]}, + {"t":0.0242, "x":5.01005, "y":2.79706, "heading":2.09289, "vx":0.04007, "vy":-0.09464, "omega":-0.02104, "ax":1.52058, "ay":-3.96416, "alpha":-0.91579, "fx":[29.06948,19.23233,21.29963,29.83268], "fy":[-63.21522,-66.87153,-66.25998,-62.87884]}, + {"t":0.04839, "x":5.01146, "y":2.79361, "heading":2.09238, "vx":0.07687, "vy":-0.19056, "omega":-0.0432, "ax":1.36078, "ay":-4.01977, "alpha":-0.9681, "fx":[26.55725,16.04729,18.6588,27.72123], "fy":[-64.29763,-67.69533,-67.04244,-63.82725]}, + {"t":0.07259, "x":5.01372, "y":2.78782, "heading":2.09133, "vx":0.10979, "vy":-0.28783, "omega":-0.06662, "ax":1.17065, "ay":-4.07683, "alpha":-1.027, "fx":[23.50964,12.27534,15.56327,25.2034], "fy":[-65.45803,-68.46734,-67.81775,-64.8507]}, + {"t":0.09679, "x":5.01672, "y":2.77966, "heading":2.08972, "vx":0.13812, "vy":-0.38647, "omega":-0.09147, "ax":0.94241, "ay":-4.1328, "alpha":-1.09297, "fx":[19.76436,7.78342,11.91142,22.16733], "fy":[-66.66678,-69.10823,-68.54214,-65.93652]}, + {"t":0.12098, "x":5.02034, "y":2.7691, "heading":2.08751, "vx":0.16092, "vy":-0.48648, "omega":-0.11792, "ax":0.66628, "ay":-4.183, "alpha":-1.16595, "fx":[15.10397,2.41953,7.58134,18.46466], "fy":[-67.8541,-69.48753,-69.14248,-67.05197]}, + {"t":0.14518, "x":5.02443, "y":2.75611, "heading":2.08466, "vx":0.17704, "vy":-0.58769, "omega":-0.14613, "ax":0.33048, "ay":-4.21914, "alpha":-1.24484, "fx":[9.24595,-3.97027,2.43431,13.90088], "fy":[-68.87358,-69.39917,-69.50025,-68.12653]}, + {"t":0.16938, "x":5.02881, "y":2.74065, "heading":2.08112, "vx":0.18504, "vy":-0.68978, "omega":-0.17625, "ax":-0.07781, "ay":-4.22722, "alpha":-1.32687, "fx":[1.85232,-11.49924,-3.67087,8.22958], "fy":[-69.43987,-68.53673,-69.43058,-69.02099]}, + {"t":0.19358, "x":5.03326, "y":2.72272, "heading":2.07685, "vx":0.18316, "vy":-0.79207, "omega":-0.20836, "ax":-0.56941, "ay":-4.18498, "alpha":-1.40715, "fx":[-7.39962,-20.15727,-10.84241,1.16432], "fy":[-69.03968,-66.48869,-68.66056,-69.47695]}, + {"t":0.21777, "x":5.03753, "y":2.70233, "heading":2.07181, "vx":0.16938, "vy":-0.89333, "omega":-0.24241, "ax":-1.14614, "ay":-4.06059, "alpha":-1.47922, "fx":[-18.61003,-29.69128,-19.08709,-7.56054], "fy":[-66.86415,-62.79605,-66.8227,-69.04878]}, + {"t":0.24197, "x":5.04129, "y":2.67953, "heading":2.06595, "vx":0.14165, "vy":-0.99159, "omega":-0.2782, "ax":-1.78944, "ay":-3.81704, "alpha":-1.53541, "fx":[-31.27169,-39.50828,-28.20875,-18.02711], "fy":[-61.93464,-57.11857,-63.49684,-67.0553]}, + {"t":0.26617, "x":5.04419, "y":2.65442, "heading":2.05922, "vx":0.09835, "vy":-1.08395, "omega":-0.31535, "ax":-2.45058, "ay":-3.42786, "alpha":-1.56352, "fx":[-43.95876,-48.73161,-37.71595,-29.84305], "fy":[-53.66415,-49.47898,-58.34047,-62.67241]}, + {"t":0.29036, "x":5.04585, "y":2.62718, "heading":2.05159, "vx":0.03905, "vy":-1.16689, "omega":-0.35318, "ax":-3.05776, "ay":-2.90038, "alpha":-1.54357, "fx":[-54.73624,-56.49014,-46.84065,-41.8871], "fy":[-42.62486,-40.40318,-51.29717,-55.33764]}, + {"t":0.31456, "x":5.0459, "y":2.5981, "heading":2.04304, "vx":-0.03494, "vy":-1.23707, "omega":-0.39053, "ax":-3.54624, "ay":-2.28438, "alpha":-1.46286, "fx":[-62.3277,-62.27879,-54.75419,-52.53659], "fy":[-30.51155,-30.76488,-42.7521,-45.3528]}, + {"t":0.33876, "x":5.04402, "y":2.5675, "heading":2.03359, "vx":-0.12075, "vy":-1.29235, "omega":-0.42593, "ax":-3.88817, "ay":-1.6508, "alpha":-1.33553, "fx":[-66.75454,-66.09391,-60.89035,-60.51795], "fy":[-19.06354,-21.42241,-33.45659,-34.0075]}, + {"t":0.36295, "x":5.03996, "y":2.53574, "heading":2.02328, "vx":-0.21483, "vy":-1.33229, "omega":-0.45825, "ax":-4.09553, "ay":-1.05918, "alpha":-1.19077, "fx":[-68.83779,-68.28043,-65.12745,-65.5706], "fy":[-9.22441,-12.94537,-24.23326,-22.85912]}, + {"t":0.38715, "x":5.03356, "y":2.5032, "heading":2.0122, "vx":-0.31393, "vy":-1.35792, "omega":-0.48706, "ax":-4.20114, "ay":-0.54114, "alpha":-1.0504, "fx":[-69.47114,-69.29098,-67.7104,-68.24973], "fy":[-1.17559,-5.57203,-15.69489,-12.94423]}, + {"t":0.41135, "x":5.02474, "y":2.47018, "heading":2.00041, "vx":-0.41558, "vy":-1.37101, "omega":-0.51248, "ax":-4.23922, "ay":-0.10391, "alpha":-0.92384, "fx":[-69.3047,-69.52811,-69.0425,-69.33758], "fy":[5.28348,0.69166,-8.15062,-4.61957]}, + {"t":0.43555, "x":5.01344, "y":2.43698, "heading":1.98801, "vx":-0.51816, "vy":-1.37353, "omega":-0.53483, "ax":-4.23635, "ay":0.2589, "alpha":-0.81314, "fx":[-68.73633,-69.29138,-69.51768,-69.47959], "fy":[10.45611,5.95642,-1.66684,2.18458]}, + {"t":0.45974, "x":4.99966, "y":2.40382, "heading":1.97507, "vx":-0.62066, "vy":-1.36726, "omega":-0.55451, "ax":-4.21036, "ay":0.55859, "alpha":-0.7174, "fx":[-67.98979,-68.78317,-69.44674,-69.10585], "fy":[14.62599,10.37113,3.82845,7.70222]}, + {"t":0.48394, "x":4.98341, "y":2.3709, "heading":1.96165, "vx":-0.72254, "vy":-1.35375, "omega":-0.57186, "ax":-4.17237, "ay":0.80678, "alpha":-0.63483, "fx":[-67.18518,-68.13266,-69.04849,-68.47458], "fy":[18.02292,14.08249,8.46372,12.18823]}, + {"t":0.50814, "x":4.96471, "y":2.33838, "heading":1.94781, "vx":-0.8235, "vy":-1.33423, "omega":-0.58723, "ax":-4.12898, "ay":1.01362, "alpha":-0.56353, "fx":[-66.38456,-67.41937,-68.46724,-67.73248], "fy":[20.823,17.21909,12.37701,15.86425]}, + {"t":0.53233, "x":4.94357, "y":2.30639, "heading":1.9336, "vx":-0.92341, "vy":-1.3097, "omega":-0.60086, "ax":-4.10754, "ay":1.10382, "alpha":-0.52938, "fx":[-66.01855,-67.06317,-68.14912,-67.37092], "fy":[22.01835,18.62499,14.11737,17.42084]}, + {"t":0.55981, "x":4.91665, "y":2.27082, "heading":1.9171, "vx":-1.03626, "vy":-1.27937, "omega":-0.61541, "ax":-4.10759, "ay":1.10244, "alpha":-0.52769, "fx":[-66.02637,-67.04708,-68.14309,-67.38872], "fy":[21.97738,18.66385,14.12096,17.32916]}, + {"t":0.58728, "x":4.88663, "y":2.23609, "heading":1.90019, "vx":-1.14912, "vy":-1.24908, "omega":-0.6299, "ax":-4.10765, "ay":1.10094, "alpha":-0.52587, "fx":[-66.03509,-67.03079,-68.13653,-67.40666], "fy":[21.93202,18.70159,14.1248,17.23442]}, + {"t":0.61476, "x":4.85351, "y":2.20218, "heading":1.88288, "vx":-1.26197, "vy":-1.21883, "omega":-0.64435, "ax":-4.10771, "ay":1.09928, "alpha":-0.52391, "fx":[-66.04479,-67.01434,-68.12939,-67.42469], "fy":[21.88177,18.7378,14.12876,17.13642]}, + {"t":0.64223, "x":4.81729, "y":2.16911, "heading":1.86518, "vx":-1.37483, "vy":-1.18863, "omega":-0.65875, "ax":-4.10778, "ay":1.09746, "alpha":-0.52179, "fx":[-66.05556,-66.99779,-68.12163,-67.44276], "fy":[21.82604,18.77202,14.13265,17.03497]}, + {"t":0.66971, "x":4.77796, "y":2.13687, "heading":1.84708, "vx":-1.48769, "vy":-1.15848, "omega":-0.67308, "ax":-4.10786, "ay":1.09545, "alpha":-0.51949, "fx":[-66.06751,-66.98121,-68.11321,-67.46082], "fy":[21.76413,18.80367,14.13627,16.92977]}, + {"t":0.69718, "x":4.73554, "y":2.10545, "heading":1.82859, "vx":-1.60055, "vy":-1.12838, "omega":-0.68735, "ax":-4.10794, "ay":1.0932, "alpha":-0.51698, "fx":[-66.08077,-66.96466,-68.10405,-67.4788], "fy":[21.69521,18.83205,14.13931,16.8205]}, + {"t":0.72466, "x":4.69001, "y":2.07486, "heading":1.8097, "vx":-1.71342, "vy":-1.09835, "omega":-0.70156, "ax":-4.10804, "ay":1.09069, "alpha":-0.51422, "fx":[-66.09547,-66.94824,-68.09409,-67.49663], "fy":[21.61826,18.85631,14.1414,16.70671]}, + {"t":0.75213, "x":4.64139, "y":2.0451, "heading":1.79043, "vx":-1.82628, "vy":-1.06838, "omega":-0.71569, "ax":-4.10814, "ay":1.08785, "alpha":-0.51118, "fx":[-66.11179,-66.93207,-68.08323,-67.51423], "fy":[21.53205,18.87541,14.14203,16.58785]}, + {"t":0.7796, "x":4.58966, "y":2.01616, "heading":1.77076, "vx":-1.93915, "vy":-1.03849, "omega":-0.72973, "ax":-4.10826, "ay":1.08463, "alpha":-0.50778, "fx":[-66.12993,-66.91626,-68.07139,-67.53149], "fy":[21.43503,18.88802,14.14056,16.46318]}, + {"t":0.80708, "x":4.53483, "y":1.98803, "heading":1.75071, "vx":-2.05203, "vy":-1.00869, "omega":-0.74368, "ax":-4.1084, "ay":1.08094, "alpha":-0.50397, "fx":[-66.15013,-66.90101,-68.05843,-67.54828], "fy":[21.32526,18.89243,14.13612,16.33175]}, + {"t":0.83455, "x":4.4769, "y":1.96073, "heading":1.73028, "vx":-2.1649, "vy":-0.97899, "omega":-0.75753, "ax":-4.10855, "ay":1.07668, "alpha":-0.49965, "fx":[-66.1727,-66.88652,-68.04419,-67.56444], "fy":[21.20021,18.88646,14.12754,16.19228]}, + {"t":0.86203, "x":4.41587, "y":1.93424, "heading":1.70947, "vx":-2.27778, "vy":-0.94941, "omega":-0.77126, "ax":-4.10873, "ay":1.07168, "alpha":-0.49469, "fx":[-66.19803,-66.87308,-68.02847,-67.57978], "fy":[21.05655,18.86717,14.11325,16.04303]}, + {"t":0.8895, "x":4.35174, "y":1.90856, "heading":1.68828, "vx":-2.39067, "vy":-0.91997, "omega":-0.78485, "ax":-4.10893, "ay":1.06577, "alpha":-0.48894, "fx":[-66.22663,-66.86106,-68.01099,-67.59402], "fy":[20.88981,18.83061,14.09105,15.88159]}, + {"t":0.91698, "x":4.28451, "y":1.88368, "heading":1.66672, "vx":-2.50356, "vy":-0.89069, "omega":-0.79828, "ax":-4.10917, "ay":1.05865, "alpha":-0.48216, "fx":[-66.25916,-66.85093,-67.99136,-67.6068], "fy":[20.69375,18.77125,14.05782,15.70454]}, + {"t":0.94445, "x":4.21417, "y":1.85961, "heading":1.64478, "vx":-2.61646, "vy":-0.8616, "omega":-0.81153, "ax":-4.10945, "ay":1.04992, "alpha":-0.47402, "fx":[-66.29655,-66.84339,-67.96905,-67.61759], "fy":[20.45944,18.68113,14.009,15.50688]}, + {"t":0.97193, "x":4.14074, "y":1.83634, "heading":1.62249, "vx":-2.72936, "vy":-0.83276, "omega":-0.82455, "ax":-4.10978, "ay":1.03897, "alpha":-0.46403, "fx":[-66.34006,-66.83936,-67.94324,-67.62564], "fy":[20.17359,18.54835,13.93766,15.28105]}, + {"t":0.9994, "x":4.0642, "y":1.81385, "heading":1.59983, "vx":-2.84228, "vy":-0.80421, "omega":-0.8373, "ax":-4.11017, "ay":1.02485, "alpha":-0.45144, "fx":[-66.39154,-66.84021,-67.91263,-67.62973], "fy":[19.81546,18.35423,13.83285,15.01506]}, + {"t":1.02688, "x":3.98455, "y":1.79214, "heading":1.57683, "vx":-2.9552, "vy":-0.77605, "omega":-0.8497, "ax":-4.11064, "ay":1.00598, "alpha":-0.43501, "fx":[-66.45373,-66.84796,-67.87505,-67.6278], "fy":[19.35079,18.0677,13.67612,14.68889]}, + {"t":1.05435, "x":3.90181, "y":1.7712, "heading":1.55348, "vx":-3.06814, "vy":-0.74841, "omega":-0.86166, "ax":-4.11117, "ay":0.9795, "alpha":-0.41254, "fx":[-66.5308,-66.86567,-67.82642,-67.61605], "fy":[18.71846,17.63291,13.43407,14.26652]}, + {"t":1.08183, "x":3.81596, "y":1.75101, "heading":1.52981, "vx":-3.18109, "vy":-0.7215, "omega":-0.87299, "ax":-4.11167, "ay":0.93974, "alpha":-0.37971, "fx":[-66.62907,-66.89786,-67.75823,-67.58648], "fy":[17.79781,16.93824,13.03956,13.67628]}, + {"t":1.1093, "x":3.72701, "y":1.73154, "heading":1.50582, "vx":-3.29406, "vy":-0.69568, "omega":-0.88342, "ax":-4.1117, "ay":0.87358, "alpha":-0.32662, "fx":[-66.75643,-66.94951,-67.64893,-67.51885], "fy":[16.31274,15.72357,12.33552,12.75351]}, + {"t":1.13677, "x":3.63496, "y":1.71275, "heading":1.48155, "vx":-3.40703, "vy":-0.67168, "omega":-0.8924, "ax":-4.10872, "ay":0.74243, "alpha":-0.22442, "fx":[-66.90339,-67.00675,-67.4247,-67.34407], "fy":[13.46423,13.21189,10.85235,11.02096]}, + {"t":1.16425, "x":3.5398, "y":1.69458, "heading":1.45703, "vx":-3.51991, "vy":-0.65128, "omega":-0.89856, "ax":-4.07694, "ay":0.36498, "alpha":0.06402, "fx":[-66.69456,-66.66775,-66.60576,-66.63303], "fy":[5.63708,5.61403,6.29964,6.31584]}, + {"t":1.19172, "x":3.44155, "y":1.67682, "heading":1.43235, "vx":-3.63193, "vy":-0.64126, "omega":-0.8968, "ax":-1.30091, "ay":-3.03289, "alpha":3.08144, "fx":[-30.21336,-9.0759,-8.69005,-37.09005], "fy":[-50.48659,-57.27971,-50.80142,-39.75998]}, + {"t":1.2192, "x":3.34127, "y":1.65806, "heading":1.40771, "vx":-3.66767, "vy":-0.72458, "omega":-0.81214, "ax":3.63379, "ay":-1.82968, "alpha":1.09353, "fx":[55.65317,58.29853,62.57206,61.09873], "fy":[-36.61462,-33.15367,-23.81288,-26.06585]}, + {"t":1.24667, "x":3.24188, "y":1.63746, "heading":1.38539, "vx":-3.56783, "vy":-0.77485, "omega":-0.7821, "ax":3.8843, "ay":-1.50229, "alpha":0.81014, "fx":[61.41642,62.49072,65.3054,64.79103], "fy":[-29.54518,-27.65547,-20.03047,-21.00724]}, + {"t":1.27415, "x":3.14532, "y":1.61561, "heading":1.36391, "vx":-3.46111, "vy":-0.81613, "omega":-0.75984, "ax":3.96153, "ay":-1.38559, "alpha":0.71763, "fx":[63.14846,63.81548,66.18201,65.90817], "fy":[-26.99194,-25.67458,-18.70449,-19.23627]}, + {"t":1.30162, "x":3.05172, "y":1.59266, "heading":1.34303, "vx":-3.35227, "vy":-0.8542, "omega":-0.74012, "ax":3.99884, "ay":-1.32576, "alpha":0.67296, "fx":[63.97996,64.45196,66.60915,66.45257], "fy":[-25.66671,-24.68031,-18.0426,-18.30531]}, + {"t":1.3291, "x":2.96113, "y":1.56869, "heading":1.3227, "vx":-3.2424, "vy":-0.89062, "omega":-0.72163, "ax":4.0208, "ay":-1.28931, "alpha":0.64704, "fx":[64.4718,64.82067,66.85905,66.77842], "fy":[-24.84348,-24.09591,-17.65647,-17.7155]}, + {"t":1.35657, "x":2.87356, "y":1.54373, "heading":1.30287, "vx":-3.13193, "vy":-0.92605, "omega":-0.70386, "ax":4.03528, "ay":-1.26474, "alpha":0.63029, "fx":[64.79982,65.05815,67.02096,66.99752], "fy":[-24.27347,-23.71974,-17.41176,-17.29927]}, + {"t":1.38405, "x":2.78904, "y":1.51781, "heading":1.28353, "vx":-3.02107, "vy":-0.96079, "omega":-0.68654, "ax":4.04554, "ay":-1.24702, "alpha":0.61865, "fx":[65.0365,65.22186,67.1328,67.1563], "fy":[-23.84886,-23.46319,-17.24936,-16.98429]}, + {"t":1.41152, "x":2.70756, "y":1.49095, "heading":1.26467, "vx":-2.90992, "vy":-0.99506, "omega":-0.66954, "ax":4.0532, "ay":-1.23363, "alpha":0.61014, "fx":[65.21703,65.34011,67.21343,67.27755], "fy":[-23.51544,-23.28129,-17.13912,-16.73411]}, + {"t":1.439, "x":2.62914, "y":1.46314, "heading":1.24627, "vx":-2.79856, "vy":-1.02895, "omega":-0.65278, "ax":4.05913, "ay":-1.22314, "alpha":0.60367, "fx":[65.36059,65.4285,67.27331,67.37375], "fy":[-23.24304,-23.1488,-17.06394,-16.52831]}, + {"t":1.46647, "x":2.55379, "y":1.43441, "heading":1.22834, "vx":-2.68703, "vy":-1.06255, "omega":-0.63619, "ax":4.06386, "ay":-1.2147, "alpha":0.59861, "fx":[65.47846,65.49628,67.31871,67.45234], "fy":[-23.01356,-23.0505,-17.01342,-16.35453]}, + {"t":1.49394, "x":2.48149, "y":1.40476, "heading":1.21086, "vx":-2.57538, "vy":-1.09593, "omega":-0.61975, "ax":4.06773, "ay":-1.20775, "alpha":0.59456, "fx":[65.57773,65.54931,67.35361,67.51802], "fy":[-22.8155,-22.97663,-16.98081,-16.20481]}, + {"t":1.52142, "x":2.41227, "y":1.37419, "heading":1.19383, "vx":-2.46362, "vy":-1.12911, "omega":-0.60341, "ax":4.07095, "ay":-1.20193, "alpha":0.59124, "fx":[65.66308,65.59146,67.38068,67.57392], "fy":[-22.64122,-22.9207,-16.96154,-16.07378]}, + {"t":1.54889, "x":2.34612, "y":1.34272, "heading":1.17725, "vx":-2.35177, "vy":-1.16213, "omega":-0.58717, "ax":4.07367, "ay":-1.19699, "alpha":0.58849, "fx":[65.7377,65.6254,67.40177,67.6222], "fy":[-22.48543,-22.8782,-16.9524,-15.9577]}, + {"t":1.57637, "x":2.28305, "y":1.31034, "heading":1.16112, "vx":-2.23985, "vy":-1.19502, "omega":-0.571, "ax":4.076, "ay":-1.19273, "alpha":0.58617, "fx":[65.80385,65.65302,67.4182,67.6644], "fy":[-22.34441,-22.84593,-16.95108,-15.85383]}, + {"t":1.60384, "x":2.22304, "y":1.27705, "heading":1.14543, "vx":-2.12786, "vy":-1.22779, "omega":-0.55489, "ax":4.07802, "ay":-1.18902, "alpha":0.5842, "fx":[65.86318,65.67571,67.43096,67.70167], "fy":[-22.21542,-22.82152,-16.95581,-15.76016]}, + {"t":1.63132, "x":2.16612, "y":1.24287, "heading":1.13019, "vx":-2.01582, "vy":-1.26046, "omega":-0.53884, "ax":4.07979, "ay":-1.18577, "alpha":0.5825, "fx":[65.91689,65.69449,67.44078,67.73486], "fy":[-22.09642,-22.80322,-16.9653,-15.67512]}, + {"t":1.65879, "x":2.11228, "y":1.20779, "heading":1.11538, "vx":-1.90373, "vy":-1.29304, "omega":-0.52284, "ax":4.08135, "ay":-1.18288, "alpha":0.58103, "fx":[65.96592,65.71015,67.44822,67.76462], "fy":[-21.98587,-22.78971,-16.9785,-15.59753]}, + {"t":1.68627, "x":2.06151, "y":1.17182, "heading":1.10102, "vx":-1.7916, "vy":-1.32554, "omega":-0.50688, "ax":4.11965, "ay":-1.03538, "alpha":0.6048, "fx":[66.7173,66.38494,67.97559,68.31569], "fy":[-19.51334,-20.68018,-14.64401,-12.86847]}, + {"t":1.7108, "x":2.01879, "y":1.13899, "heading":1.08858, "vx":-1.69052, "vy":-1.35094, "omega":-0.49204, "ax":4.18442, "ay":-0.72309, "alpha":0.65768, "fx":[68.01965,67.61359,68.8516,69.14434], "fy":[-14.28287,-16.1878,-9.68848,-7.12541]}, + {"t":1.73534, "x":1.97858, "y":1.10562, "heading":1.07651, "vx":-1.58785, "vy":-1.36868, "omega":-0.4759, "ax":4.22759, "ay":-0.38514, "alpha":0.71025, "fx":[68.97338,68.5957,69.38466,69.49813], "fy":[-8.49349,-11.28476,-4.42376,-0.98315]}, + {"t":1.75987, "x":1.94089, "y":1.07192, "heading":1.06483, "vx":-1.48412, "vy":-1.37813, "omega":-0.45847, "ax":4.24371, "ay":-0.02554, "alpha":0.76108, "fx":[69.45211,69.25245,69.51377,69.28775], "fy":[-2.19591,-5.99866,1.08113,5.443]}, + {"t":1.78441, "x":1.90575, "y":1.0381, "heading":1.05358, "vx":-1.38, "vy":-1.37876, "omega":-0.4398, "ax":4.22815, "ay":0.34947, "alpha":0.80868, "fx":[69.33472,69.5057,69.19303,68.45546], "fy":[4.50447,-0.38548,6.73459,11.99934]}, + {"t":1.80895, "x":1.87317, "y":1.00438, "heading":1.04279, "vx":-1.27626, "vy":-1.37018, "omega":-0.41996, "ax":4.17791, "ay":0.73176, "alpha":0.85173, "fx":[68.52771,69.28756,68.39918,66.98918], "fy":[11.44615,5.46842,12.42819,18.50882]}, + {"t":1.83348, "x":1.84311, "y":0.97098, "heading":1.03249, "vx":-1.17375, "vy":-1.35223, "omega":-0.39906, "ax":4.09228, "ay":1.11195, "alpha":0.88913, "fx":[66.98822,68.55121,67.13651,64.92827], "fy":[18.42402,11.44924,18.04618,24.79349]}, + {"t":1.85802, "x":1.81554, "y":0.93814, "heading":1.0227, "vx":-1.07334, "vy":-1.32495, "omega":-0.37724, "ax":3.97318, "ay":1.48052, "alpha":0.92003, "fx":[64.7394,67.28042,65.43751,62.3583], "fy":[25.21585,17.42436,23.47678,30.69797]}, + {"t":1.88255, "x":1.7904, "y":0.90607, "heading":1.01344, "vx":-0.97585, "vy":-1.28862, "omega":-0.35467, "ax":3.82492, "ay":1.82899, "alpha":0.94383, "fx":[61.87055,65.49479,63.35905,59.39637], "fy":[31.61593,23.25594,28.62304,36.1073]}, + {"t":1.90709, "x":1.76761, "y":0.87501, "heading":1.00474, "vx":-0.88201, "vy":-1.24374, "omega":-0.33151, "ax":3.65358, "ay":2.15081, "alpha":0.96022, "fx":[58.52101,63.24868,60.9749,56.17196], "fy":[37.46498,28.81626,33.41074,40.95468]}, + {"t":1.93163, "x":1.74707, "y":0.84514, "heading":0.9966, "vx":-0.79236, "vy":-1.19097, "omega":-0.30795, "ax":3.46606, "ay":2.44186, "alpha":0.96925, "fx":[54.8538,60.62384,58.36667,52.80939], "fy":[42.66648,34.00128,37.792,45.21911]}, + {"t":1.95616, "x":1.72867, "y":0.81665, "heading":0.98905, "vx":-0.70732, "vy":-1.13106, "omega":-0.28417, "ax":3.26915, "ay":2.70042, "alpha":0.97139, "fx":[51.02891,57.71791,55.6152,49.41564], "fy":[47.18702,38.73904,41.74478,48.91614]}, + {"t":1.9807, "x":1.7123, "y":0.78971, "heading":0.98208, "vx":-0.62711, "vy":-1.0648, "omega":-0.26034, "ax":3.06893, "ay":2.92681, "alpha":0.96748, "fx":[47.18394,54.63188,52.79406,46.07451], "fy":[51.04443,42.99172,45.26919,52.08604]}, + {"t":2.00523, "x":1.69784, "y":0.76447, "heading":0.97569, "vx":-0.55181, "vy":-0.99299, "omega":-0.2366, "ax":2.8703, "ay":3.12281, "alpha":0.95862, "fx":[43.42464,51.45974,49.96543,42.84613], "fy":[54.29082,46.75227,48.38219,54.78269]}, + {"t":2.02977, "x":1.68516, "y":0.74104, "heading":0.96988, "vx":-0.48138, "vy":-0.91637, "omega":-0.21308, "ax":2.67698, "ay":3.29107, "alpha":0.94597, "fx":[39.82402,48.28155,47.17866,39.76962], "fy":[56.99623,50.03772,51.112,57.06493]}, + {"t":2.05431, "x":1.67416, "y":0.71955, "heading":0.96466, "vx":-0.4157, "vy":-0.83562, "omega":-0.18987, "ax":2.49149, "ay":3.43467, "alpha":0.93064, "fx":[36.42657,45.16044,44.47043,36.86718], "fy":[59.23617,52.88139,53.49318,58.99062]}, + {"t":2.07884, "x":1.66471, "y":0.70008, "heading":0.96, "vx":-0.35457, "vy":-0.75135, "omega":-0.16703, "ax":2.31543, "ay":3.55676, "alpha":0.91359, "fx":[33.25456,42.14254,41.86621,34.1484], "fy":[61.08351,55.32574,55.56276,60.6133]}, + {"t":2.10338, "x":1.6567, "y":0.68272, "heading":0.9559, "vx":-0.29776, "vy":-0.66408, "omega":-0.14462, "ax":2.14963, "ay":3.66035, "alpha":0.89558, "fx":[30.31436,39.25879,39.38212,31.61399], "fy":[62.60412,57.41676,57.3575,61.98056]}, + {"t":2.12791, "x":1.65005, "y":0.66753, "heading":0.95235, "vx":-0.24502, "vy":-0.57427, "omega":-0.12264, "ax":1.99435, "ay":3.74816, "alpha":0.87721, "fx":[27.60181,36.52768,37.02692,29.25882], "fy":[63.85516,59.19999,58.91217,63.13357]}, + {"t":2.15245, "x":1.64463, "y":0.65456, "heading":0.94934, "vx":-0.19608, "vy":-0.4823, "omega":-0.10112, "ax":1.84949, "ay":3.8226, "alpha":0.85891, "fx":[25.10627,33.95806,34.80382,27.07418], "fy":[64.88489,60.71816,60.25858,64.10734]}, + {"t":2.17699, "x":1.64038, "y":0.64388, "heading":0.94686, "vx":-0.1507, "vy":-0.38851, "omega":-0.08005, "ax":1.71468, "ay":3.88577, "alpha":0.84097, "fx":[22.81357,31.55179,32.71208,25.04937], "fy":[65.73337,62.00982,61.42517,64.9313]}, + {"t":2.20152, "x":1.6372, "y":0.63552, "heading":0.9449, "vx":-0.10863, "vy":-0.29317, "omega":-0.05941, "ax":1.5894, "ay":3.93944, "alpha":0.8236, "fx":[20.70802,29.30588,30.7482,23.17285], "fy":[66.43347,63.10897,62.43693,65.62997]}, + {"t":2.22606, "x":1.63501, "y":0.62951, "heading":0.94344, "vx":-0.06964, "vy":-0.19651, "omega":-0.03921, "ax":1.47307, "ay":3.98512, "alpha":0.80691, "fx":[18.7736,27.21414,28.9069,21.43295], "fy":[67.012,64.045,63.31561,66.22374]}, + {"t":2.25059, "x":1.63375, "y":0.62589, "heading":0.94248, "vx":-0.03349, "vy":-0.09873, "omega":-0.01941, "ax":1.36504, "ay":4.02407, "alpha":0.79098, "fx":[16.99479,25.2685,27.18184,19.8183], "fy":[67.49075,64.84306,64.07995,66.72949]}, + {"t":2.27513, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 328c5f8e..16a1499a 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -128,7 +128,8 @@ public void runPath( steps .get(startPos + "to" + endPos) .getFinalPose() - .get()))), + .get())) + .withTimeout(2.0)), steps.get(endPos + "to" + nextPos).cmd())); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8d877ab3..ec5b3a54 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,8 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.elevator.ElevatorSubsystem.ELEVATOR_ANGLE; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -25,6 +27,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; @@ -70,11 +73,16 @@ import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AlgaeIntakeTargets; import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.CageTargets; import frc.robot.utils.autoaim.CoralTargets; +import java.util.Arrays; +import java.util.Comparator; import java.util.HashMap; +import java.util.List; import java.util.Optional; import java.util.Set; import java.util.function.BiConsumer; +import java.util.stream.Collectors; import java.util.stream.Stream; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.GyroSimulation; @@ -178,6 +186,10 @@ public static enum AlgaeScoreTarget { @AutoLogOutput private boolean haveAutosGenerated = false; + private static CANBus canivore = new CANBus("*"); + + private static CANBusStatus canivoreStatus = canivore.getStatus(); + private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); @@ -366,7 +378,11 @@ public static enum AlgaeScoreTarget { .or(() -> Autos.autoScore), driver.rightTrigger().or(() -> Autos.autoPreScore), driver.leftTrigger(), - driver.x().and(driver.pov(-1).negate()).debounce(0.5), + driver + .x() + .and(driver.pov(-1).negate()) + .debounce(0.5) + .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)), driver.rightTrigger(), driver .y() @@ -452,6 +468,7 @@ public Robot() { // be added. swerve.startOdoThread(); SignalLogger.setPath("/media/sda1/"); + Logger.recordOutput("Canivore Status", canivoreStatus.Status); if (ROBOT_TYPE == RobotType.SIM) { SimulatedArena.getInstance().addDriveTrainSimulation(swerveDriveSimulation.orElse(null)); @@ -470,6 +487,18 @@ public Robot() { autos = new Autos(swerve, manipulator); autoChooser.addDefaultOption("None", autos.getNoneAuto()); + SmartDashboard.putData( + "Run Elevator Sysid", + elevator + .runSysid() + .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); + + SmartDashboard.putData( + "Step Elevator Current", + elevator + .setCurrent(60.0) + .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); + // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); @@ -533,7 +562,6 @@ public Robot() { } }) .ignoringDisable(true)); - elevator.setDefaultCommand( Commands.sequence( elevator.runCurrentZeroing().onlyIf(() -> !elevator.hasZeroed), @@ -549,6 +577,8 @@ public Robot() { funnel.setDefaultCommand(funnel.setVoltage(0.0)); + climber.setDefaultCommand(climber.setPosition(0.0)); + leds.setDefaultCommand( Commands.either( leds.setBlinkingCmd( @@ -593,14 +623,21 @@ public Robot() { AutoAim.translateToPose( swerve, () -> { - var twist = swerve.getVelocityFieldRelative().toTwist2d(0.3); - return CoralTargets.getHandedClosestTarget( - swerve - .getPose() - .plus( - new Transform2d( - twist.dx, twist.dy, Rotation2d.fromRadians(twist.dtheta))), - driver.leftBumper().getAsBoolean()); + List vectorTs = + Arrays.stream(CoralTargets.values()) + .map( + target -> + CoralTargets.getRobotTargetLocation( + target.location)) // Correct method call + .collect(Collectors.toList()); + + Pose2d maxPose = + vectorTs.stream() + .max( + Comparator.comparingDouble( + pose -> dotProd(pose) / pose.getTranslation().getNorm())) + .orElseThrow(); + return maxPose; }), Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); @@ -615,17 +652,23 @@ public Robot() { .whileTrue( Commands.parallel( Commands.sequence( + Commands.runOnce( + () -> { + algaeIntakeTarget = + AlgaeIntakeTargets.getClosestTarget(swerve.getPose()).height; + }), AutoAim.translateToPose( swerve, () -> AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTarget(swerve.getPose()))) + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()))) .until( () -> AutoAim.isInTolerance( swerve.getPose(), AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTarget(swerve.getPose())), + AlgaeIntakeTargets.getClosestTargetPose( + swerve.getPose())), swerve.getVelocityFieldRelative(), Units.inchesToMeters(1.0), Units.degreesToRadians(1.0)) @@ -636,10 +679,106 @@ public Robot() { && shoulder.isNearAngle( ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS)), AutoAim.approachAlgae( - swerve, () -> AlgaeIntakeTargets.getClosestTarget(swerve.getPose()), 0.75)), + swerve, + () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), + 0.75)), Commands.waitUntil(() -> AutoAim.isInToleranceAlgaeIntake(swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + driver + .rightBumper() + .or(driver.leftBumper()) + .and( + () -> + superstructure.getState() == SuperState.READY_ALGAE + || superstructure.getState() == SuperState.PRE_PROCESSOR) + .and(() -> algaeScoreTarget == AlgaeScoreTarget.PROCESSOR) + .whileTrue( + Commands.parallel( + AutoAim.translateToPose( + swerve, + () -> + swerve + .getPose() + .nearest( + List.of(AutoAim.BLUE_PROCESSOR_POS, AutoAim.RED_PROCESSOR_POS)) + // Moves the target pose inside the field, with the bumpers aligned + // with + // the wall + .transformBy( + new Transform2d( + -(ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) + - 0.5, + 0.0, + Rotation2d.kZero))) + .until( + () -> + AutoAim.isInTolerance( + swerve.getPose(), + swerve + .getPose() + .nearest( + List.of( + AutoAim.BLUE_PROCESSOR_POS, AutoAim.RED_PROCESSOR_POS)) + .transformBy( + new Transform2d( + -(ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) + - 0.5, + 0.0, + Rotation2d.kZero)))) + .andThen( + AutoAim.translateToPose( + swerve, + () -> + swerve + .getPose() + .nearest( + List.of( + AutoAim.BLUE_PROCESSOR_POS, AutoAim.RED_PROCESSOR_POS)) + // Moves the target pose inside the field, with the bumpers + // aligned with + // the wall + .transformBy( + new Transform2d( + -(ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0.0, + Rotation2d.kZero)))), + Commands.waitUntil( + () -> + AutoAim.isInTolerance( + swerve + .getPose() + .nearest( + List.of( + AutoAim.BLUE_PROCESSOR_POS, AutoAim.RED_PROCESSOR_POS)) + // Moves the target pose inside the field, with the bumpers + // aligned with the wall + .transformBy( + new Transform2d( + -(ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0.0, + Rotation2d.kZero)), + swerve.getPose())) + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + + driver + .rightBumper() + .or(driver.leftBumper()) + .and( + () -> + superstructure.getState() == SuperState.PRE_CLIMB + || superstructure.getState() == SuperState.CLIMB) + .whileTrue( + Commands.parallel( + AutoAim.translateToPose( + swerve, () -> CageTargets.getOffsetClosestTarget(swerve.getPose())), + Commands.waitUntil( + () -> + AutoAim.isInTolerance( + CageTargets.getOffsetClosestTarget(swerve.getPose()), + swerve.getPose())) + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + driver .rightBumper() .or(driver.leftBumper()) @@ -647,10 +786,10 @@ public Robot() { () -> superstructure.getState() == SuperState.READY_ALGAE || superstructure.getState() == SuperState.PRE_NET) + .and(() -> algaeScoreTarget == AlgaeScoreTarget.NET) .whileTrue( Commands.parallel( AutoAim.translateToXCoord( - // TODO: PUT ACUAL NET POSE swerve, () -> Math.abs(swerve.getPose().getX() - AutoAim.BLUE_NET_X) @@ -670,7 +809,7 @@ public Robot() { final var diff = swerve .getPose() - .minus(AlgaeIntakeTargets.getClosestTarget(swerve.getPose())); + .minus(AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose())); return MathUtil.isNear(0.0, diff.getX(), Units.inchesToMeters(1.0)) && MathUtil.isNear(0.0, diff.getY(), Units.inchesToMeters(1.0)) && MathUtil.isNear(0.0, diff.getRotation().getDegrees(), 2.0); @@ -680,12 +819,11 @@ public Robot() { driver .povUp() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setSecondBeambreak(true)).ignoringDisable(true)); + .onTrue(Commands.runOnce(() -> manipulator.setFirstBeambreak(true)).ignoringDisable(true)); driver .povDown() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue( - Commands.runOnce(() -> manipulator.setSecondBeambreak(false)).ignoringDisable(true)); + .onTrue(Commands.runOnce(() -> manipulator.setFirstBeambreak(false)).ignoringDisable(true)); driver .povRight() .and(() -> ROBOT_TYPE == RobotType.SIM) @@ -772,6 +910,12 @@ public Robot() { .map((target) -> CoralTargets.getRobotTargetLocation(target.location)) .toArray(Pose2d[]::new)); // Log locations of all autoaim targets + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "AutoAim/Targets/Cage", + Stream.of(CageTargets.values()) + .map((target) -> target.getLocation()) + .toArray(Pose2d[]::new)); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput( "AutoAim/Targets/Algae", @@ -953,6 +1097,25 @@ public static void setCurrentTarget(ReefTarget target) { currentTarget = target; } + public double dotProd(Pose2d targetPose) { + var twist = swerve.getVelocityFieldRelative().toTwist2d(0.3); + + // VR - robot, VT - target, VN - robot in 0.3 sec + Translation2d vectorR = new Translation2d(swerve.getPose().getX(), swerve.getPose().getY()); + + Translation2d vectorN = new Translation2d(vectorR.getX() + twist.dx, vectorR.getY() + twist.dy); + + Translation2d vectorT = new Translation2d(targetPose.getX(), targetPose.getY()); + + Translation2d vectorRT = vectorT.minus(vectorR); + Translation2d vectorNT = vectorT.minus(vectorN); + + double dotProd = vectorRT.getX() * vectorNT.getX() + vectorRT.getY() * vectorNT.getY(); + + // Arrays.stream(values()).map(()-> ((dotProd)/Math.pow(2, vectorT))) + return dotProd; + } + public ReefTarget getCurrentTarget() { return currentTarget; } diff --git a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java index 883491e5..4d8e7527 100644 --- a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java @@ -9,10 +9,10 @@ import org.littletonrobotics.junction.Logger; public class FunnelSubsystem extends RollerSubsystem { - public static final Rotation2d FIRST_LATCH_CLOSED_POSITION = Rotation2d.fromDegrees(0.0); - public static final Rotation2d FIRST_LATCH_OPEN_POSITION = Rotation2d.fromDegrees(180.0); - public static final Rotation2d SECOND_LATCH_CLOSED_POSITION = Rotation2d.fromDegrees(0.0); - public static final Rotation2d SECOND_LATCH_OPEN_POSITION = Rotation2d.fromDegrees(180.0); + public static final Rotation2d FIRST_LATCH_CLOSED_POSITION = Rotation2d.fromDegrees(125.0); + public static final Rotation2d FIRST_LATCH_OPEN_POSITION = Rotation2d.fromDegrees(45.0); + public static final Rotation2d SECOND_LATCH_CLOSED_POSITION = Rotation2d.fromDegrees(45.0); + public static final Rotation2d SECOND_LATCH_OPEN_POSITION = Rotation2d.fromDegrees(125.0); // this could be in its own subsystem but it doesnt really matter tbh private final ServoIO firstLatchIO, secondLatchIO; private final ServoIOInputsAutoLogged firstLatchInputs = new ServoIOInputsAutoLogged(), diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 59dbe2a1..354ddb05 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -68,7 +68,7 @@ public void periodic() { public Command index() { return Commands.sequence( - setVelocity(6.0).until(() -> firstBBInputs.get).unless(() -> firstBBInputs.get), + setVelocity(9.0).until(() -> firstBBInputs.get).unless(() -> firstBBInputs.get), setVelocity(3.0).until(() -> secondBBInputs.get).unless(() -> secondBBInputs.get), // TODO tune timeout // Commands.runOnce(() -> io.resetEncoder(0.0)), diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d5285989..0acabfa1 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -174,9 +174,11 @@ public void periodic() { private void configureStateTransitionCommands() { stateTriggers .get(SuperState.IDLE) - .whileTrue(elevator.setExtension(ElevatorSubsystem.HP_EXTENSION_METERS)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_HP_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_HP_POS)) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS)) .whileTrue(manipulator.index()) .whileTrue( funnel.setVoltage( @@ -194,7 +196,7 @@ private void configureStateTransitionCommands() { .min(Double::compare) .get() < 1.0) - ? 12.0 + ? 4.0 : 0.0))) .and(manipulator::getFirstBeambreak) .onTrue(this.forceState(SuperState.READY_CORAL)); @@ -252,8 +254,7 @@ private void configureStateTransitionCommands() { .get(SuperState.HOME) .whileTrue( Commands.parallel( - elevator.runCurrentZeroing(), wrist.currentZero(() -> shoulder.getInputs())) - .andThen(Commands.waitUntil(homeReq.negate()), this.forceState(SuperState.IDLE))) + elevator.runCurrentZeroing(), wrist.currentZero(() -> shoulder.getInputs()))) .and(() -> elevator.hasZeroed && wrist.hasZeroed && !homeReq.getAsBoolean()) .onTrue(this.forceState(prevState)); @@ -424,18 +425,20 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.SCORE_CORAL) .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) + .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) // .debounce(0.15) - .whileTrue( - this.extendWithClearance( - 0.0, ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS)) - .and(() -> elevator.isNearExtension(0)) - .onTrue(this.forceState(SuperState.IDLE)); + .onTrue(forceState(SuperState.IDLE)); - antiJamReq - .and(stateTriggers.get(SuperState.CLIMB).negate()) - .and(stateTriggers.get(SuperState.PRE_CLIMB).negate()) - .onTrue(forceState(SuperState.ANTI_JAM)) - .onFalse(forceState(SuperState.IDLE)); + stateTriggers + .get(SuperState.SCORE_CORAL) + .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) + .and(intakeAlgaeReq) + .and(() -> intakeTargetOnReef()) + .onTrue( + forceState( + algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH + ? SuperState.INTAKE_ALGAE_HIGH + : SuperState.INTAKE_ALGAE_LOW)); // ANTI_JAM logic stateTriggers @@ -447,7 +450,7 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.CHECK_ALGAE) .and(() -> stateTimer.hasElapsed(1.0)) - .and(() -> manipulator.getStatorCurrentAmps() <= 20.0) + .and(() -> manipulator.getStatorCurrentAmps() <= 20.0 && Robot.ROBOT_TYPE != RobotType.SIM) .onTrue(this.forceState(SuperState.IDLE)); // change intake target @@ -545,10 +548,10 @@ private void configureStateTransitionCommands() { ? WristSubsystem.WRIST_RETRACTED_POS : WristSubsystem.WRIST_INTAKE_ALGAE_REEF_RETRACT_POS)) .and(() -> stateTimer.hasElapsed(1.0)) - .and(() -> manipulator.getStatorCurrentAmps() > 20.0) + .and(() -> manipulator.getStatorCurrentAmps() > 20.0 || Robot.ROBOT_TYPE == RobotType.SIM) .and( () -> - AlgaeIntakeTargets.getClosestTarget(pose.get()) + AlgaeIntakeTargets.getClosestTargetPose(pose.get()) .getTranslation() .minus(pose.get().getTranslation()) .getNorm() @@ -585,7 +588,7 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.READY_ALGAE) - .and(() -> manipulator.getStatorCurrentAmps() < 20.0) + .and(() -> manipulator.getStatorCurrentAmps() < 20.0 && Robot.ROBOT_TYPE != RobotType.SIM) .onTrue(forceState(SuperState.CHECK_ALGAE)); // SPIT_ALGAE -> PRE_CLIMB stateTriggers @@ -627,13 +630,9 @@ private void configureStateTransitionCommands() { .onTrue(Commands.runOnce(() -> stateTimer.reset())) .whileTrue(manipulator.setVoltage(13.0)) .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) + .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) .and(() -> stateTimer.hasElapsed(1)) - .whileTrue( - this.extendWithClearance( - 0.0, ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS)) - .and(() -> elevator.isNearExtension(0)) .onTrue(forceState(SuperState.IDLE)); stateTriggers @@ -655,7 +654,7 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.CLIMB) .whileTrue( - climber.setPosition(0.0).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + climber.setPosition(1.3).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // May need more checks to see if canceling is safe stateTriggers @@ -722,6 +721,11 @@ public boolean stateIsAlgaeAlike() { || this.state == SuperState.SCORE_ALGAE_PROCESSOR; } + public boolean intakeTargetOnReef() { + return this.algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH + || this.algaeIntakeTarget.get() == AlgaeIntakeTarget.LOW; + } + private Command forceState(SuperState nextState) { return Commands.runOnce( () -> { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 0aa25225..da795491 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -1,13 +1,12 @@ package frc.robot.subsystems.climber; -import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface ClimberIO { @AutoLog class ClimberIOInputs { public double angularVelocityRPS = 0.0; - public Rotation2d position = new Rotation2d(); + public double position = 0.0; public double tempDegreesC = 0.0; public double supplyCurrentAmps = 0.0; public double appliedVoltage = 0.0; @@ -19,4 +18,6 @@ class ClimberIOInputs { public void setVoltage(final double volts); public void setPosition(final double position); + + public void resetEncoder(final double position); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index f357eb64..560b8e7c 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -3,12 +3,11 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -27,23 +26,24 @@ public class ClimberIOReal implements ClimberIO { private final StatusSignal position = motor.getPosition(); private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final MotionMagicTorqueCurrentFOC motionMagic = new MotionMagicTorqueCurrentFOC(0.0); + private final PositionVoltage motionMagic = new PositionVoltage(0.0); public ClimberIOReal() { final var config = new TalonFXConfiguration(); - config.Slot0.kP = 0.0; + config.Slot0.kP = 100.0; - config.MotionMagic.MotionMagicAcceleration = 5.0; - config.MotionMagic.MotionMagicCruiseVelocity = 0.75; + config.MotionMagic.MotionMagicCruiseVelocity = (6000 / 60) / ClimberSubsystem.CLIMB_GEAR_RATIO; + config.MotionMagic.MotionMagicAcceleration = + (6000 / 60) / (ClimberSubsystem.CLIMB_GEAR_RATIO * 0.01); config.Feedback.SensorToMechanismRatio = ClimberSubsystem.CLIMB_GEAR_RATIO; - config.CurrentLimits.StatorCurrentLimit = 10.0; + config.CurrentLimits.StatorCurrentLimit = 40.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; motor.getConfigurator().apply(config); BaseStatusSignal.setUpdateFrequencyForAll( @@ -62,7 +62,7 @@ public void updateInputs(ClimberIOInputsAutoLogged inputs) { BaseStatusSignal.refreshAll( angularVelocityRPS, temp, supplyCurrentAmps, statorCurrentAmps, position, appliedVoltage); - inputs.position = Rotation2d.fromRotations(position.getValueAsDouble()); + inputs.position = position.getValueAsDouble(); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); inputs.supplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); @@ -77,6 +77,11 @@ public void setVoltage(double volts) { @Override public void setPosition(final double position) { - // motor.setControl(motionMagic.withPosition(position)); + motor.setControl(motionMagic.withPosition(position)); + } + + @Override + public void resetEncoder(double position) { + motor.setPosition(position); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index e54d4802..5cb15e9a 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -37,7 +36,7 @@ public void updateInputs(final ClimberIOInputsAutoLogged inputs) { inputs.angularVelocityRPS = RadiansPerSecond.of(armSim.getVelocityRadPerSec()).in(RotationsPerSecond); - inputs.position = Rotation2d.fromRadians(armSim.getAngleRads()); + inputs.position = armSim.getAngleRads(); // TODO fix inputs.statorCurrentAmps = armSim.getCurrentDrawAmps(); inputs.supplyCurrentAmps = 0.0; inputs.tempDegreesC = 0.0; @@ -61,4 +60,9 @@ public void setPosition(final double position) { / ClimberSubsystem.CLIMBER_ARM_LENGTH_METERS)) + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); } + + @Override + public void resetEncoder(double position) { + armSim.setState(position, 0); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index f970ee6c..5ff570e5 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -5,7 +5,9 @@ package frc.robot.subsystems.climber; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -13,14 +15,18 @@ public class ClimberSubsystem extends SubsystemBase { // TODO update public static final double CLIMBER_ARM_LENGTH_METERS = Units.inchesToMeters(11.0); public static final double CLIMBER_DRUM_RADIUS_METERS = Units.inchesToMeters(1.0); - public static final double CLIMB_GEAR_RATIO = 64.0; - public static final double CLIMB_EXTENDED_POSITION = 10.0; + public static final double CLIMB_GEAR_RATIO = 125.0; + public static final double CLIMB_EXTENDED_POSITION = 3.5; private final ClimberIO io; private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); public ClimberSubsystem(ClimberIO io) { this.io = io; + + SmartDashboard.putData( + "rezero Climber", Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true)); + SmartDashboard.putData("Reset Climber (MANUAL STOP)", resetClimber()); } @Override @@ -32,4 +38,12 @@ public void periodic() { public Command setPosition(double position) { return this.run(() -> io.setPosition(position)); } + + public Command resetClimber() { + return this.run(() -> io.setVoltage(-8.0)); + } + + public Command zeroClimber() { + return Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true); + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 84681259..f82c23b5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -23,6 +23,8 @@ public static class ElevatorIOInputs { public void setVoltage(final double voltage); + public void setCurrent(final double amps); + public default void stop() { setVoltage(0); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 0090b8d2..4fd147ab 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -8,13 +8,13 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -27,8 +27,9 @@ public class ElevatorIOReal implements ElevatorIO { private final TalonFX follower = new TalonFX(17, "*"); private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final MotionMagicVoltage positionVoltage = - new MotionMagicVoltage(0.0).withEnableFOC(true); + private final TorqueCurrentFOC torque = new TorqueCurrentFOC(0.0); + private final MotionMagicExpoVoltage positionTorque = + new MotionMagicExpoVoltage(0.0).withEnableFOC(true); // misusing type system here - these correspond to linear meters, NOT rotations private final StatusSignal position = motor.getPosition(); @@ -45,32 +46,49 @@ public ElevatorIOReal() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + // Carriage position meters in direction of elevator + config.Feedback.SensorToMechanismRatio = + ElevatorSubsystem.GEAR_RATIO / (2 * Math.PI * ElevatorSubsystem.DRUM_RADIUS_METERS); + + // Carriage mass is 12lbs + // Manipulator is 5lbs + // First stage is ~4lbs + // 16lbs counterspringing from stage 1 to carriage + // (ie 16lbs of force pulling carriage up) config.Slot0.GravityType = GravityTypeValue.Elevator_Static; - config.Slot0.kG = 0.4; - config.Slot0.kS = 0.15; - config.Slot0.kV = 4.2; - config.Slot0.kA = 0.0; - config.Slot0.kP = 100.0; - config.Slot0.kD = 10.0; + config.Slot0.kG = 0.43832; + // (483.0 / 9.37) + // * config.Feedback.SensorToMechanismRatio + // * Units.lbsToKilograms(12 + 5 + (4.0 / 2.0) - 16); + config.Slot0.kS = 1.1062; + config.Slot0.kV = 1.9542; + // converts accel -> force, force -> motor torque, motor torque -> amperage + config.Slot0.kA = 0.26245; + // (483.0 / 9.37)$ + // * config.Feedback.SensorToMechanismRatio + // * Units.lbsToKilograms(12 + 5 + (4.0 / 2.0)); + config.Slot0.kP = 69.925; + config.Slot0.kD = 5.5908; config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.1; - // TODO increase once validated config.CurrentLimits.StatorCurrentLimit = 80.0; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = 40.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; + // Fuck it we ball + config.CurrentLimits.StatorCurrentLimitEnable = false; + config.CurrentLimits.SupplyCurrentLimit = 70.0; + config.CurrentLimits.SupplyCurrentLimitEnable = false; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.0; config.MotionMagic.MotionMagicAcceleration = 8.0; // Estimated from slightly less than motor free speed - config.MotionMagic.MotionMagicCruiseVelocity = 4.0; + config.MotionMagic.MotionMagicCruiseVelocity = + (5500.0 / 60.0) / config.Feedback.SensorToMechanismRatio; - // Carriage position meters in direction of elevator - config.Feedback.SensorToMechanismRatio = - ElevatorSubsystem.GEAR_RATIO / (2 * Math.PI * ElevatorSubsystem.DRUM_RADIUS_METERS); + config.MotionMagic.MotionMagicExpo_kV = 1.9542; + config.MotionMagic.MotionMagicExpo_kA = 0.26245; motor.getConfigurator().apply(config); - motor.setPosition(Units.inchesToMeters(0.0)); // Assume we boot nearly 0ed follower.getConfigurator().apply(config); follower.setControl(new Follower(motor.getDeviceID(), true)); @@ -93,7 +111,7 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { @Override public void setTarget(final double meters) { - motor.setControl(positionVoltage.withPosition(meters)); + motor.setControl(positionTorque.withPosition(meters)); } @Override @@ -101,6 +119,11 @@ public void setVoltage(final double voltage) { motor.setControl(voltageOut.withOutput(voltage)); } + @Override + public void setCurrent(final double amps) { + motor.setControl(torque.withOutput(amps)); + } + @Override public void resetEncoder(final double position) { motor.setPosition(position); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index cd65953f..b8632a12 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -62,6 +62,15 @@ public void setVoltage(final double voltage) { physicsSim.setInputVoltage(MathUtil.clamp(voltage, -12.0, 12.0)); } + @Override + public void setCurrent(double amps) { + setVoltage( + DCMotor.getKrakenX60Foc(2) + .getVoltage( + amps, + physicsSim.getVelocityMetersPerSecond() / ElevatorSubsystem.DRUM_RADIUS_METERS)); + } + @Override public void resetEncoder(final double position) { // sim always has a perfectly accurate encoder diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 55f94890..c8027015 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -4,6 +4,9 @@ package frc.robot.subsystems.elevator; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose3d; @@ -13,9 +16,13 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Robot; import frc.robot.Robot.RobotType; import java.util.function.DoubleSupplier; +import java.util.function.Function; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; @@ -24,7 +31,7 @@ /** Cascading elevator */ public class ElevatorSubsystem extends SubsystemBase { // Constants - public static final double GEAR_RATIO = 4.5 / 1.0; + public static final double GEAR_RATIO = 2.5 / 1.0; public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(1.751 / 2.0); public static final Rotation2d ELEVATOR_ANGLE = Rotation2d.fromDegrees(90.0); @@ -56,6 +63,9 @@ public class ElevatorSubsystem extends SubsystemBase { private double setpoint = 0.0; + private final SysIdRoutine voltageSysid; + private final SysIdRoutine currentSysid; + // For dashboard private final LoggedMechanism2d mech2d = new LoggedMechanism2d(3.0, Units.feetToMeters(4.0)); private final LoggedMechanismRoot2d @@ -68,6 +78,23 @@ public class ElevatorSubsystem extends SubsystemBase { /** Creates a new ElevatorSubsystem. */ public ElevatorSubsystem(ElevatorIO io) { this.io = io; + voltageSysid = + new SysIdRoutine( + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Elevator/SysIdTestStateVolts", state.toString())), + new Mechanism((volts) -> io.setVoltage(volts.in(Volts)), null, this)); + + currentSysid = + new SysIdRoutine( + new Config( + Volts.of(30.0).per(Second), + Volts.of(120.0), + null, + (state) -> Logger.recordOutput("Elevator/SysIdTestStateCurrent", state.toString())), + new Mechanism((volts) -> io.setCurrent(volts.in(Volts)), null, this)); } @Override @@ -109,12 +136,12 @@ public Command hold() { public Command runCurrentZeroing() { return this.run( () -> { - io.setVoltage(-0.5); + io.setVoltage(-2.0); setpoint = 0.0; if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Setpoint", Double.NaN); }) - .until(() -> Math.abs(currentFilterValue) > 19.0) + .until(() -> Math.abs(currentFilterValue) > 50.0) .finallyDo( (interrupted) -> { if (!interrupted) { @@ -124,6 +151,29 @@ public Command runCurrentZeroing() { }); } + public Command runSysid() { + final Function runSysid = + (routine) -> + Commands.sequence( + routine + .quasistatic(SysIdRoutine.Direction.kForward) + .until(() -> inputs.positionMeters > Units.inchesToMeters(50.0)), + Commands.waitUntil(() -> inputs.velocityMetersPerSec < 0.1), + routine + .quasistatic(SysIdRoutine.Direction.kReverse) + .until(() -> inputs.positionMeters < Units.inchesToMeters(10.0)), + Commands.waitUntil(() -> Math.abs(inputs.velocityMetersPerSec) < 0.1), + routine + .dynamic(SysIdRoutine.Direction.kForward) + .until(() -> inputs.positionMeters > Units.inchesToMeters(50.0)), + Commands.waitUntil(() -> inputs.velocityMetersPerSec < 0.1), + routine + .dynamic(SysIdRoutine.Direction.kReverse) + .until(() -> inputs.positionMeters < Units.inchesToMeters(10.0))); + return Commands.sequence( + runCurrentZeroing(), runSysid.apply(voltageSysid), runSysid.apply(currentSysid)); + } + public Command setVoltage(double voltage) { return this.run( () -> { @@ -135,6 +185,13 @@ public Command setVoltage(DoubleSupplier voltage) { return this.setVoltage(voltage.getAsDouble()); } + public Command setCurrent(double amps) { + return this.run( + () -> { + io.setCurrent(amps); + }); + } + public Pose3d getCarriagePose() { return new Pose3d( Units.inchesToMeters(4.5) + inputs.positionMeters * ELEVATOR_ANGLE.getCos(), diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java index 70d82a98..3fe028c8 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java @@ -66,8 +66,7 @@ public void registerSimulationCallback(Consumer callba @Override public void setPosition(Rotation2d rot) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); + // TODO Actually simulate } @Override diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java index e1ff92ed..d7d2fe49 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shoulder; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; @@ -26,4 +27,6 @@ public default void resetEncoder(final Rotation2d rotation) {} public default void resetEncoder() { resetEncoder(Rotation2d.kZero); } + + public void setMotionMagicConfigs(final MotionMagicConfigs configs); } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 58abca98..f0c57f6c 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; @@ -133,4 +134,9 @@ public void setMotorPosition(final Rotation2d targetPosition) { public void resetEncoder(final Rotation2d rotation) { motor.setPosition(rotation.getRotations()); } + + @Override + public void setMotionMagicConfigs(final MotionMagicConfigs configs) { + motor.getConfigurator().apply(configs); + } } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java index ff7bb0f9..12086ad4 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -57,4 +58,11 @@ public void setMotorPosition(final Rotation2d targetPosition) { pid.calculate(armSim.getAngleRads(), targetPosition.getRadians()) + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); } + + @Override + public void setMotionMagicConfigs(MotionMagicConfigs configs) { + pid.setConstraints( + new TrapezoidProfile.Constraints( + configs.MotionMagicCruiseVelocity, configs.MotionMagicAcceleration)); + } } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 407b0aa8..caa9175e 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shoulder; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -40,9 +41,14 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_SCORE_L4_POS = Rotation2d.fromDegrees(55); public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(30); public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); - public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = SHOULDER_RETRACTED_POS; + public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(75.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); + private static final MotionMagicConfigs DEFAULT_CONFIGS = + new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); + private static final MotionMagicConfigs TOSS_CONFIGS = + new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.6).withMotionMagicAcceleration(4.0); + private final ShoulderIO io; private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); @@ -96,6 +102,23 @@ public Command setTargetAngle(final Rotation2d target) { return setTargetAngle(() -> target); } + public Command setTargetAngleSlow(final Supplier target) { + return Commands.sequence( + this.runOnce(() -> io.setMotionMagicConfigs(TOSS_CONFIGS)), + this.run( + () -> { + io.setMotorPosition(target.get()); + setpoint = target.get(); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); + })) + .finallyDo(() -> io.setMotionMagicConfigs(DEFAULT_CONFIGS)); + } + + public Command setTargetAngleSlow(final Rotation2d target) { + return setTargetAngle(() -> target); + } + public Rotation2d getAngle() { return inputs.position; } diff --git a/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java index b46aea55..436ad47c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java @@ -24,6 +24,16 @@ import frc.robot.subsystems.vision.Vision.VisionConstants; public class AlphaSwerveConstants extends SwerveConstants { + private static boolean instantiated = false; + + public AlphaSwerveConstants() { + super(); + if (instantiated) { + SwerveConstants.multipleInstancesAlert.set(true); + } + instantiated = true; + } + @Override public double getMaxLinearSpeed() { // motor speed (RPM) / gear ratio * pi * wheel diameter (inches) / 12 / 60 diff --git a/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java index d6c94ac3..008c71d8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java @@ -25,6 +25,15 @@ import frc.robot.subsystems.vision.Vision.VisionConstants; public class BansheeSwerveConstants extends SwerveConstants { + private static boolean instantiated = false; + + public BansheeSwerveConstants() { + super(); + if (instantiated) { + SwerveConstants.multipleInstancesAlert.set(true); + } + instantiated = true; + } @Override public double getMaxLinearSpeed() { diff --git a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java index b5a78768..c74fc848 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java @@ -24,6 +24,16 @@ import frc.robot.subsystems.vision.Vision.VisionConstants; public class KelpieSwerveConstants extends SwerveConstants { + private static boolean instantiated = false; + + public KelpieSwerveConstants() { + super(); + if (instantiated) { + SwerveConstants.multipleInstancesAlert.set(true); + } + instantiated = true; + } + @Override public double getMaxLinearSpeed() { // From https://www.swervedrivespecialties.com/products/mk4n-swerve-module, L2+ with KrakenX60 @@ -181,10 +191,16 @@ public VisionConstants[] getVisionConstants() { final Matrix BACK_RIGHT_DIST_COEFFS = MatBuilder.fill( Nat.N8(), Nat.N1(), 0.058, -0.09, 0.006, -0.003, 0.022, -0.002, 0.004, -0.001); - final Matrix FRONT_CAMERA_MATRIX = + final Matrix FRONT_RIGHT_CAMERA_MATRIX = + MatBuilder.fill( + Nat.N3(), Nat.N3(), 911.67, 0.0, 663.03, 0.0, 909.82, 408.72, 0.0, 0.0, 1.0); + final Matrix FRONT_RIGHT_DIST_COEFFS = + MatBuilder.fill( + Nat.N8(), Nat.N1(), 0.044, -0.069, 0.001, 0.001, 0.013, -0.002, 0.004, 0.001); + final Matrix FRONT_LEFT_CAMERA_MATRIX = MatBuilder.fill( Nat.N3(), Nat.N3(), 911.67, 0.0, 663.03, 0.0, 909.82, 408.72, 0.0, 0.0, 1.0); - final Matrix FRONT_DIST_COEFFS = + final Matrix FRONT_LEFT_DIST_COEFFS = MatBuilder.fill( Nat.N8(), Nat.N1(), 0.044, -0.069, 0.001, 0.001, 0.013, -0.002, 0.004, 0.001); final VisionConstants backLeftCamConstants = @@ -197,7 +213,7 @@ public VisionConstants[] getVisionConstants() { Units.inchesToMeters(9.052)), new Rotation3d( Units.degreesToRadians(0.0), - Units.degreesToRadians(-28.125), + Units.degreesToRadians(-(90 - 76.875000)), Units.degreesToRadians(150))), BACK_LEFT_CAMERA_MATRIX, BACK_LEFT_DIST_COEFFS); @@ -207,23 +223,37 @@ public VisionConstants[] getVisionConstants() { new Transform3d( new Translation3d( Units.inchesToMeters(-11.600), - Units.inchesToMeters(-11.400488), - Units.inchesToMeters(9.052)), - new Rotation3d(0, Units.degreesToRadians(-28.125), Units.degreesToRadians(210))), + Units.inchesToMeters(-11.416), + Units.inchesToMeters(9.061)), + new Rotation3d( + 0, Units.degreesToRadians(-(90 - 76.875000)), Units.degreesToRadians(210))), BACK_RIGHT_CAMERA_MATRIX, BACK_RIGHT_DIST_COEFFS); - final VisionConstants frontCamConstants = + final VisionConstants frontRightCamConstants = new VisionConstants( - "Front_Camera", + "Front_Right_Camera", new Transform3d( new Translation3d( Units.inchesToMeters(6.664129), Units.inchesToMeters(-12.320709), Units.inchesToMeters(8.885504)), new Rotation3d(0, Units.degreesToRadians(-10), Units.degreesToRadians(30))), - FRONT_CAMERA_MATRIX, - FRONT_DIST_COEFFS); - return new VisionConstants[] {frontCamConstants, backLeftCamConstants, backRightCamConstants}; + FRONT_RIGHT_CAMERA_MATRIX, + FRONT_RIGHT_DIST_COEFFS); + final VisionConstants frontLeftCamConstants = + new VisionConstants( + "Front_Left_Camera", + new Transform3d( + new Translation3d( + Units.inchesToMeters(6.664129), + Units.inchesToMeters(12.320709), + Units.inchesToMeters(8.885504)), + new Rotation3d(0, Units.degreesToRadians(-10), Units.degreesToRadians(-30))), + FRONT_LEFT_CAMERA_MATRIX, + FRONT_LEFT_DIST_COEFFS); + return new VisionConstants[] { + backLeftCamConstants, backRightCamConstants, frontRightCamConstants + }; } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 4965d5c7..69c06755 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -24,14 +24,17 @@ public static class ModuleIOInputs { public double drivePositionMeters = 0.0; public double driveVelocityMetersPerSec = 0.0; public double driveAppliedVolts = 0.0; - public double driveCurrentAmps = 0.0; + public double driveStatorCurrentAmps = 0.0; public double driveSupplyCurrentAmps = 0.0; + public double driveTempC = 0.0; public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; - public double turnCurrentAmps = 0.0; + public double turnStatorCurrentAmps = 0.0; + public double turnSupplyCurrentAmps = 0.0; + public double turnTempC = 0.0; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOMapleSim.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOMapleSim.java index ae43eaa1..5731c2e8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOMapleSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOMapleSim.java @@ -60,12 +60,15 @@ public class ModuleIOMapleSim implements ModuleIO { private final BaseStatusSignal driveAppliedVolts; private final BaseStatusSignal driveCurrent; private final BaseStatusSignal driveSupplyCurrent; + private final BaseStatusSignal driveTempC; private final BaseStatusSignal turnAbsolutePosition; private final BaseStatusSignal turnPosition; private final BaseStatusSignal turnVelocity; private final BaseStatusSignal turnAppliedVolts; - private final BaseStatusSignal turnCurrent; + private final BaseStatusSignal turnStatorCurrent; + private final BaseStatusSignal turnSupplyCurrent; + private final BaseStatusSignal turnTempC; // Control modes private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); @@ -99,12 +102,15 @@ public ModuleIOMapleSim( driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveTempC = driveTalon.getDeviceTemp(); turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); + turnStatorCurrent = turnTalon.getStatorCurrent(); + turnSupplyCurrent = turnTalon.getSupplyCurrent(); + turnTempC = turnTalon.getDeviceTemp(); PhoenixOdometryThread.getInstance() .registerSignals( @@ -127,10 +133,13 @@ public ModuleIOMapleSim( driveAppliedVolts, driveCurrent, driveSupplyCurrent, + driveTempC, turnAbsolutePosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnStatorCurrent, + turnSupplyCurrent, + turnTempC); driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); cancoder.optimizeBusUtilization(); @@ -155,25 +164,31 @@ public void updateInputs(final ModuleIOInputs inputs) { driveAppliedVolts, driveCurrent, driveSupplyCurrent, + driveTempC, turnAbsolutePosition, turnPosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnStatorCurrent, + turnSupplyCurrent, + turnTempC); inputs.prefix = constants.prefix(); inputs.drivePositionMeters = drivePosition.getValueAsDouble(); inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + inputs.driveStatorCurrentAmps = driveCurrent.getValueAsDouble(); inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); + inputs.driveTempC = driveTempC.getValueAsDouble(); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + inputs.turnStatorCurrentAmps = turnStatorCurrent.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); + inputs.turnTempC = turnTempC.getValueAsDouble(); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index ec97a6ca..9e9b394a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -56,12 +56,15 @@ public class ModuleIOReal implements ModuleIO { private final BaseStatusSignal driveAppliedVolts; private final BaseStatusSignal driveCurrent; private final BaseStatusSignal driveSupplyCurrent; + private final BaseStatusSignal driveTempC; private final BaseStatusSignal turnAbsolutePosition; private final BaseStatusSignal turnPosition; private final BaseStatusSignal turnVelocity; private final BaseStatusSignal turnAppliedVolts; - private final BaseStatusSignal turnCurrent; + private final BaseStatusSignal turnStatorCurrent; + private final BaseStatusSignal turnSupplyCurrent; + private final BaseStatusSignal turnTempC; // Control modes private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); @@ -91,12 +94,15 @@ public ModuleIOReal(ModuleConstants moduleConstants, SwerveConstants swerveConst driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveTempC = driveTalon.getDeviceTemp(); turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); + turnStatorCurrent = turnTalon.getStatorCurrent(); + turnSupplyCurrent = turnTalon.getSupplyCurrent(); + turnTempC = turnTalon.getDeviceTemp(); PhoenixOdometryThread.getInstance() .registerSignals( @@ -119,10 +125,13 @@ public ModuleIOReal(ModuleConstants moduleConstants, SwerveConstants swerveConst driveAppliedVolts, driveCurrent, driveSupplyCurrent, + driveTempC, turnAbsolutePosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnStatorCurrent, + turnSupplyCurrent, + turnTempC); driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); cancoder.optimizeBusUtilization(); @@ -136,25 +145,31 @@ public void updateInputs(final ModuleIOInputs inputs) { driveAppliedVolts, driveCurrent, driveSupplyCurrent, + driveTempC, turnAbsolutePosition, turnPosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnStatorCurrent, + turnSupplyCurrent, + turnTempC); inputs.prefix = constants.prefix(); inputs.drivePositionMeters = drivePosition.getValueAsDouble(); inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + inputs.driveStatorCurrentAmps = driveCurrent.getValueAsDouble(); inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); + inputs.driveTempC = driveTempC.getValueAsDouble(); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + inputs.turnStatorCurrentAmps = turnStatorCurrent.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); + inputs.turnTempC = turnTempC.getValueAsDouble(); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java index 7458296c..0019eeae 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java @@ -96,14 +96,14 @@ public void updateInputs(final ModuleIOInputs inputs) { inputs.driveVelocityMetersPerSec = driveSim.getAngularVelocityRadPerSec() * swerveConstants.getWheelRadiusMeters(); inputs.driveAppliedVolts = driveSimState.getMotorVoltage(); - inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + inputs.driveStatorCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.turnStatorCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index 61ed970e..ba2c98a2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -25,19 +25,13 @@ * same gear ratios Subclass this to make constants for a specific robot. */ public abstract class SwerveConstants { - private static boolean instantiated = false; - private static final Alert multipleInstancesAlert = + protected static final Alert multipleInstancesAlert = new Alert("Multiple Swerve Constants Files", AlertType.kError); private static final Alert tagLoadFailureAlert = new Alert("Failed to load custom tag map", AlertType.kWarning); protected AprilTagFieldLayout fieldTags; public SwerveConstants() { - if (instantiated) { - multipleInstancesAlert.set(true); - } - instantiated = true; - try { fieldTags = new AprilTagFieldLayout( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 3939f7cc..c49ccc62 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -313,26 +313,31 @@ private void updateVision() { var visionPose = estPose.get().estimatedPose; // Sets the pose on the sim field camera.setSimPose(estPose, camera, !camera.inputs.stale); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/Vision Pose From " + camera.getName(), visionPose); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/Vision Pose2d From " + camera.getName(), visionPose.toPose2d()); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/Vision Pose From " + camera.getName(), visionPose); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "Vision/Vision Pose2d From " + camera.getName(), visionPose.toPose2d()); final var deviations = VisionHelper.findVisionMeasurementStdDevs(estPose.get()); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Deviations", deviations.getData()); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + camera.getName() + "/Deviations", deviations.getData()); Tracer.trace( "Add Measurement From " + camera.getName(), () -> { estimator.addVisionMeasurement( visionPose.toPose2d(), camera.inputs.captureTimestampMicros / 1.0e6, - deviations.times(DriverStation.isAutonomous() ? 2.0 : 1.0)); + deviations + .times(DriverStation.isAutonomous() ? 2.0 : 1.0) + .times( + camera.getName().equals("Front_Left_Camera") + || camera.getName().equals("Front_Right_Camera") + ? 1.0 + : 1.5)); }); lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/" + camera.getName() + "/Invalid Pose Result", "Good Update"); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Good Update"); cameraPoses[i] = visionPose; Tracer.trace( "Log Tag Poses", @@ -346,19 +351,17 @@ private void updateVision() { .getTagPose(result.targets.get(j).getFiducialId()) .get(); } - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/" + camera.getName() + "/Target Poses", targetPose3ds); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + camera.getName() + "/Target Poses", targetPose3ds); }); } else { - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Stale"); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Stale"); } } catch (NoSuchElementException e) { - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/" + camera.getName() + "/Invalid Pose Result", "Bad Estimate"); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Bad Estimate"); } i++; } @@ -512,7 +515,9 @@ private void drive( forceSetpoints[i] = new SwerveModuleState( Math.hypot(robotRelForceX, robotRelForceY), - new Rotation2d(robotRelForceX, robotRelForceY)); + robotRelForceX == 0 && robotRelForceY == 0 + ? Rotation2d.kZero + : new Rotation2d(robotRelForceX, robotRelForceY)); optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i], robotRelForceX, robotRelForceY); } @@ -541,14 +546,14 @@ public Consumer choreoDriveController() { thetaController.enableContinuousInput(-Math.PI, Math.PI); return (sample) -> { final var pose = getPose(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Choreo/Target Pose", - new Pose2d(sample.x, sample.y, Rotation2d.fromRadians(sample.heading))); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Choreo/Target Speeds Field Relative", - new ChassisSpeeds(sample.vx, sample.vy, sample.omega)); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "Choreo/Target Pose", + new Pose2d(sample.x, sample.y, Rotation2d.fromRadians(sample.heading))); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "Choreo/Target Speeds Field Relative", + new ChassisSpeeds(sample.vx, sample.vy, sample.omega)); var feedback = new ChassisSpeeds( xController.calculate(pose.getX(), sample.x), diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 10f1c9da..4bcbfe30 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -42,6 +42,7 @@ public record VisionConstants( public Vision(final VisionIO io) { this.io = io; + io.updateInputs(inputs); } public void setSimPose(Optional simEst, Vision camera, boolean newResult) { @@ -53,7 +54,7 @@ public void updateInputs() { } public void processInputs() { - Logger.processInputs("Apriltag Vision/" + io.getName(), inputs); + Logger.processInputs("Apriltag Vision/" + inputs.constants.cameraName, inputs); } public Optional update(PhotonPipelineResult result) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java index 080e7f82..40638c36 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java @@ -23,7 +23,7 @@ public class VisionIOSim implements VisionIO { private final PhotonCameraSim simCamera; private final VisionConstants constants; - public static Supplier pose; + public static Supplier pose = () -> Pose3d.kZero; public VisionIOSim(VisionConstants constants) { this.sim = new VisionSystemSim(constants.cameraName()); @@ -67,11 +67,11 @@ public void setSimPose(Optional simEst, Vision camera, boole public void updateInputs(VisionIOInputs inputs) { sim.update(pose.get()); var results = camera.getAllUnreadResults(); + inputs.constants = constants; if (results.size() > 0) { final var result = results.get(results.size() - 1); inputs.latency = result.metadata.getLatencyMillis(); inputs.targets = result.targets; - inputs.constants = constants; inputs.sequenceID = result.metadata.getSequenceID(); inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index b96042b9..72a256fc 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -24,8 +24,8 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(-33.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); - public static final Rotation2d WRIST_SCORE_L1_POS = Rotation2d.fromDegrees(-40); - public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-30); + public static final Rotation2d WRIST_SCORE_L1_POS = Rotation2d.fromDegrees(-50); + public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-20); public static final Rotation2d WRIST_SCORE_L2_POS = Rotation2d.fromDegrees(-35); public static final Rotation2d WRIST_SCORE_L3_POS = Rotation2d.fromDegrees(-35); public static final Rotation2d WRIST_SCORE_L4_POS = Rotation2d.fromDegrees(-45); diff --git a/src/main/java/frc/robot/utils/Tracer.java b/src/main/java/frc/robot/utils/Tracer.java index 6f93096a..19986966 100644 --- a/src/main/java/frc/robot/utils/Tracer.java +++ b/src/main/java/frc/robot/utils/Tracer.java @@ -2,8 +2,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; import java.util.ArrayList; @@ -107,19 +105,18 @@ private void endCycle() { // if the entry isn't found, time will null-cast to 0.0 Double time = traceTimes.remove(entry); if (time == null) time = 0.0; - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Tracer/" + entry, time); + Logger.recordOutput("Tracer/" + entry, time); } // log all new entries for (var traceTime : traceTimes.entrySet()) { - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Tracer/" + traceTime.getKey(), traceTime.getValue()); + + Logger.recordOutput("Tracer/" + traceTime.getKey(), traceTime.getValue()); entryArray.add(traceTime.getKey()); } // log gc time - if (gcs.size() > 0) - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Tracer/GCTime", gcTimeThisCycle); + if (gcs.size() > 0) Logger.recordOutput("Tracer/GCTime", gcTimeThisCycle); + gcTimeThisCycle = 0.0; // clean up state diff --git a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java index a9ac3e1b..bfb2d116 100644 --- a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java +++ b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java @@ -5,31 +5,37 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import frc.robot.Robot; +import frc.robot.Robot.AlgaeIntakeTarget; import java.util.Arrays; +import java.util.Collections; +import java.util.Comparator; import java.util.List; +import java.util.stream.Stream; public enum AlgaeIntakeTargets { // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls // were extended beyond the coral station // All angles from the center of the coral with 0° across the width of the field, counterclockwise - BLUE_AB(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180))), - BLUE_CD(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240))), - BLUE_EF(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300))), - BLUE_GH(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0))), - BLUE_IJ(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60))), - BLUE_KL(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120))), - - RED_AB(ChoreoAllianceFlipUtil.flip(BLUE_AB.location)), - RED_CD(ChoreoAllianceFlipUtil.flip(BLUE_CD.location)), - RED_EF(ChoreoAllianceFlipUtil.flip(BLUE_EF.location)), - RED_GH(ChoreoAllianceFlipUtil.flip(BLUE_GH.location)), - RED_IJ(ChoreoAllianceFlipUtil.flip(BLUE_IJ.location)), - RED_KL(ChoreoAllianceFlipUtil.flip(BLUE_KL.location)); + BLUE_AB(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AlgaeIntakeTarget.HIGH), + BLUE_CD(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AlgaeIntakeTarget.LOW), + BLUE_EF(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AlgaeIntakeTarget.HIGH), + BLUE_GH(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), AlgaeIntakeTarget.LOW), + BLUE_IJ(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), AlgaeIntakeTarget.HIGH), + BLUE_KL(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), AlgaeIntakeTarget.LOW), + + RED_AB(ChoreoAllianceFlipUtil.flip(BLUE_AB.location), AlgaeIntakeTarget.HIGH), + RED_CD(ChoreoAllianceFlipUtil.flip(BLUE_CD.location), AlgaeIntakeTarget.LOW), + RED_EF(ChoreoAllianceFlipUtil.flip(BLUE_EF.location), AlgaeIntakeTarget.HIGH), + RED_GH(ChoreoAllianceFlipUtil.flip(BLUE_GH.location), AlgaeIntakeTarget.LOW), + RED_IJ(ChoreoAllianceFlipUtil.flip(BLUE_IJ.location), AlgaeIntakeTarget.HIGH), + RED_KL(ChoreoAllianceFlipUtil.flip(BLUE_KL.location), AlgaeIntakeTarget.LOW); public final Pose2d location; + public final AlgaeIntakeTarget height; - private AlgaeIntakeTargets(Pose2d location) { + private AlgaeIntakeTargets(Pose2d location, AlgaeIntakeTarget height) { this.location = location; + this.height = height; } private static final List transformedPoses = @@ -53,7 +59,18 @@ public static Pose2d getOffsetLocation(Pose2d original) { } /** Gets the closest offset target to the given pose. */ - public static Pose2d getClosestTarget(Pose2d pose) { + public static Pose2d getClosestTargetPose(Pose2d pose) { return pose.nearest(transformedPoses); } + + public static AlgaeIntakeTargets getClosestTarget(Pose2d pose) { + return Collections.min( + Stream.of(AlgaeIntakeTargets.values()).toList(), + Comparator.comparing( + (AlgaeIntakeTargets other) -> + pose.getTranslation().getDistance(other.location.getTranslation())) + .thenComparing( + (AlgaeIntakeTargets other) -> + Math.abs(pose.getRotation().minus(other.location.getRotation()).getRadians()))); + } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index c97c5dc3..bbdf7919 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -32,6 +32,9 @@ public class AutoAim { public static double BLUE_NET_X = 8.08; public static double RED_NET_X = ChoreoAllianceFlipUtil.flipX(BLUE_NET_X); + public static Pose2d BLUE_PROCESSOR_POS = new Pose2d(5.973, 0, Rotation2d.fromDegrees(270)); + public static Pose2d RED_PROCESSOR_POS = ChoreoAllianceFlipUtil.flip(BLUE_PROCESSOR_POS); + public static final double TRANSLATION_TOLERANCE_METERS = Units.inchesToMeters(2.0); public static final double ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(2.0); public static final double VELOCITY_TOLERANCE_METERSPERSECOND = 0.5; @@ -225,7 +228,7 @@ public static boolean isInToleranceCoral(Pose2d pose) { } public static boolean isInToleranceAlgaeIntake(Pose2d pose) { - final var diff = pose.minus(AlgaeIntakeTargets.getClosestTarget(pose)); + final var diff = pose.minus(AlgaeIntakeTargets.getClosestTargetPose(pose)); return MathUtil.isNear( 0.0, Math.hypot(diff.getX(), diff.getY()), AutoAim.TRANSLATION_TOLERANCE_METERS) && MathUtil.isNear( diff --git a/src/main/java/frc/robot/utils/autoaim/CageTargets.java b/src/main/java/frc/robot/utils/autoaim/CageTargets.java new file mode 100644 index 00000000..f585a615 --- /dev/null +++ b/src/main/java/frc/robot/utils/autoaim/CageTargets.java @@ -0,0 +1,86 @@ +package frc.robot.utils.autoaim; + +import choreo.util.ChoreoAllianceFlipUtil; +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Robot; +import java.util.Arrays; +import java.util.List; + +public enum CageTargets { + RED_OUTSIDE(new Pose2d(8.760, 0.799, Rotation2d.fromDegrees(0)), Alliance.Red), + RED_MIDDLE(new Pose2d(8.760, 1.889 + 0.15, Rotation2d.fromDegrees(0)), Alliance.Red), + RED_INSIDE(new Pose2d(8.760, 2.980, Rotation2d.fromDegrees(0)), Alliance.Red), + + BLUE_OUTSIDE(ChoreoAllianceFlipUtil.flip(RED_OUTSIDE.getLocation()), Alliance.Blue), + BLUE_MIDDLE(ChoreoAllianceFlipUtil.flip(RED_MIDDLE.getLocation()), Alliance.Blue), + BLUE_INSIDE(ChoreoAllianceFlipUtil.flip(RED_INSIDE.getLocation()), Alliance.Blue); + + private static final List poses = + Arrays.stream(values()).map((CageTargets target) -> target.getLocation()).toList(); + + private final Pose2d location; + private final Alliance alliance; + + private CageTargets(Pose2d location, Alliance alliance) { + this.location = location; + this.alliance = alliance; + } + + public static Pose2d getOffsetClosestTarget(Pose2d robotPose) { + if (DriverStation.getAlliance().isPresent()) { + // If it's across the field, x > 8.76 on blue and x < 8.76 on red + return getOffsetClosestTarget( + robotPose, + (DriverStation.getAlliance().get() == Alliance.Blue && robotPose.getX() > 8.76) + || (DriverStation.getAlliance().get() == Alliance.Red && robotPose.getX() < 8.76)); + } + return getOffsetClosestTarget(robotPose, false); + } + + public static Pose2d getOffsetClosestTarget(Pose2d robotPose, boolean far) { + Pose2d nearestPose; + if (DriverStation.getAlliance().isPresent()) { + nearestPose = + robotPose.nearest( + Arrays.stream(values()) + .filter(target -> target.getAlliance() == DriverStation.getAlliance().get()) + .map(target -> target.getLocation()) + .toList()); + } else { + nearestPose = robotPose.nearest(poses); + } + if (far) { + return getFarRobotTargetLocation(nearestPose); + } else { + return getCloseRobotTargetLocation(nearestPose); + } + } + + public static Pose2d getCloseRobotTargetLocation(Pose2d pose) { + return pose.transformBy( + new Transform2d( + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) + 0.087, + 0, + Rotation2d.kZero)); + } + + public static Pose2d getFarRobotTargetLocation(Pose2d pose) { + return pose.transformBy( + new Transform2d( + -(Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) - 0.087, + 0, + Rotation2d.k180deg)); + } + + public Pose2d getLocation() { + return this.location; + } + + public Alliance getAlliance() { + return this.alliance; + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index c7f5262f..79bdf3e4 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.0.0", + "version": "4.1.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,21 +12,22 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.0.0" + "version": "4.1.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.0.0", + "version": "4.1.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ "linuxathena", - "windowsx86-64", "linuxx86-64", - "osxuniversal" + "linuxarm64", + "osxuniversal", + "windowsx86-64" ] } ], diff --git a/vendordeps/ChoreoLib2025.json b/vendordeps/ChoreoLib2025.json index faef0111..2c2d3c38 100644 --- a/vendordeps/ChoreoLib2025.json +++ b/vendordeps/ChoreoLib2025.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2025.json", "name": "ChoreoLib", - "version": "2025.0.1", + "version": "2025.0.3", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2025.0.1" + "version": "2025.0.3" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2025.0.1", + "version": "2025.0.3", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 80% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-frc2025-latest.json index 18462c6f..f378c137 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.3.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.3.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +365,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.3.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index 39a6c80b..a863f408 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.3.1", + "version": "0.3.10", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.3.1" + "version": "0.3.10" }, { "groupId": "org.dyn4j", diff --git a/vendordeps/photonlib-v2025.1.1.json b/vendordeps/photonlib.json similarity index 90% rename from vendordeps/photonlib-v2025.1.1.json rename to vendordeps/photonlib.json index a9089236..32a648cc 100644 --- a/vendordeps/photonlib-v2025.1.1.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.1.1", + "version": "v2025.2.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", + "version": "v2025.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.1.1", + "version": "v2025.2.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", + "version": "v2025.2.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.1.1" + "version": "v2025.2.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.1.1" + "version": "v2025.2.1" } ] }