From a2d9c6dc5cea7440e7c895812f72fc894ea0a1bd Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 6 Apr 2026 17:40:49 -0700 Subject: [PATCH 01/17] make bump auto (untested) --- notes/canIDs.md | 2 +- src/main/deploy/choreo/LBumpToLCleanup.traj | 175 ++++++++++++++++++++ src/main/deploy/choreo/LBumptoDepot.traj | 2 +- src/main/deploy/choreo/LCleanupToLBump.traj | 130 +++++++++++++++ src/main/deploy/choreo/LNeutralToLBump.traj | 132 +++++++++++++++ src/main/deploy/choreo/rebuiltChoreo.chor | 26 ++- src/main/java/frc/robot/Autos.java | 16 ++ src/main/java/frc/robot/Robot.java | 1 + 8 files changed, 476 insertions(+), 8 deletions(-) create mode 100644 src/main/deploy/choreo/LBumpToLCleanup.traj create mode 100644 src/main/deploy/choreo/LCleanupToLBump.traj create mode 100644 src/main/deploy/choreo/LNeutralToLBump.traj diff --git a/notes/canIDs.md b/notes/canIDs.md index ca6ba0a6..3e0aa83f 100644 --- a/notes/canIDs.md +++ b/notes/canIDs.md @@ -44,7 +44,7 @@ Spindexer Agitator | 9 | X60-046 Shooter Hood | 11 | X44-008 Shooter Flywheel Leader (Left) | 12 | X60-048 Shooter Flywheel Follower (Right) | 13 | X60-050 -intake Pivot | 14 | X44-017 +Intake Pivot | 14 | X44-017 Turret Pivot | 15 | X44-001 Climber | 16 | X60-049 2nd Kicker Roller | 17 | X60-051 diff --git a/src/main/deploy/choreo/LBumpToLCleanup.traj b/src/main/deploy/choreo/LBumpToLCleanup.traj new file mode 100644 index 00000000..2c66a5fc --- /dev/null +++ b/src/main/deploy/choreo/LBumpToLCleanup.traj @@ -0,0 +1,175 @@ +{ + "name":"LBumpToLCleanup", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":2.8895323276519775, "y":5.703888893127441, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.960714817047119, "y":7.404163360595703, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.8285322189331055, "y":7.382072448730469, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.158814430236816, "y":6.248106002807617, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.068130016326904, "y":3.792367458343506, "heading":-1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":2.7}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"LBump.x", "val":2.8895323276519775}, "y":{"exp":"LBump.y", "val":5.703888893127441}, "heading":{"exp":"LBump.heading", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.960714817047119 m", "val":2.960714817047119}, "y":{"exp":"7.404163360595703 m", "val":7.404163360595703}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.8285322189331055 m", "val":5.8285322189331055}, "y":{"exp":"7.382072448730469 m", "val":7.382072448730469}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"6.158814430236816 m", "val":6.158814430236816}, "y":{"exp":"6.248106002807617 m", "val":6.248106002807617}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"LCleanup.x", "val":6.068130016326904}, "y":{"exp":"LCleanup.y", "val":3.792367458343506}, "heading":{"exp":"LCleanup.heading", "val":-1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.7}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2761996, + "y":0.2761996 + }, + "backLeft":{ + "x":-0.2761996, + "y":0.2761996 + }, + "mass":66.133767546, + "inertia":10.42384825260472, + "gearing":7.03, + "radius":0.0508, + "vmax":485.8996637552213, + "tmax":0.9, + "cof":2.25, + "bumper":{ + "front":0.44196, + "side":0.44196, + "back":0.44196 + }, + "differentialTrackWidth":0.5427218 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.98589,2.1244,2.80203,3.89283], + "samples":[ + {"t":0.0, "x":2.88953, "y":5.70389, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.7053, "ay":7.02365, "alpha":0.0, "fx":[-44.72793,-44.72793,-44.72793,-44.72793], "fy":[116.12511,116.12511,116.12511,116.12511]}, + {"t":0.04929, "x":2.88625, "y":5.71242, "heading":0.0, "vx":-0.13336, "vy":0.34623, "omega":0.0, "ax":-2.70498, "ay":7.02263, "alpha":0.0, "fx":[-44.72255,-44.72255,-44.72255,-44.72255], "fy":[116.10824,116.10824,116.10824,116.10824]}, + {"t":0.09859, "x":2.87639, "y":5.73802, "heading":0.0, "vx":-0.2667, "vy":0.69241, "omega":0.0, "ax":-2.70449, "ay":7.02121, "alpha":0.0, "fx":[-44.71449,-44.71449,-44.71449,-44.71449], "fy":[116.08483,116.08483,116.08483,116.08483]}, + {"t":0.14788, "x":2.85995, "y":5.78068, "heading":0.0, "vx":-0.40001, "vy":1.03851, "omega":0.0, "ax":-2.70372, "ay":7.0191, "alpha":0.0, "fx":[-44.70174,-44.70174,-44.70174,-44.70174], "fy":[116.04996,116.04996,116.04996,116.04996]}, + {"t":0.19718, "x":2.83695, "y":5.84041, "heading":0.0, "vx":-0.53329, "vy":1.38452, "omega":0.0, "ax":-2.70238, "ay":7.01561, "alpha":0.0, "fx":[-44.67962,-44.67962,-44.67962,-44.67962], "fy":[115.99219,115.99219,115.99219,115.99219]}, + {"t":0.24647, "x":2.80738, "y":5.91718, "heading":0.0, "vx":-0.6665, "vy":1.73035, "omega":0.0, "ax":-2.69963, "ay":7.00865, "alpha":0.0, "fx":[-44.63413,-44.63413,-44.63413,-44.63413], "fy":[115.87718,115.87718,115.87718,115.87718]}, + {"t":0.29577, "x":2.77124, "y":6.01099, "heading":0.0, "vx":-0.79958, "vy":2.07584, "omega":0.0, "ax":-2.69124, "ay":6.98788, "alpha":0.0, "fx":[-44.49546,-44.49546,-44.49546,-44.49546], "fy":[115.53375,115.53375,115.53375,115.53375]}, + {"t":0.34506, "x":2.72856, "y":6.12181, "heading":0.0, "vx":-0.93225, "vy":2.4203, "omega":0.0, "ax":-0.70819, "ay":2.02739, "alpha":0.0, "fx":[-11.70879,-11.70879,-11.70879,-11.70879], "fy":[33.5197,33.5197,33.5197,33.5197]}, + {"t":0.39436, "x":2.68174, "y":6.24358, "heading":0.0, "vx":-0.96716, "vy":2.52024, "omega":0.0, "ax":0.84502, "ay":0.31635, "alpha":0.0, "fx":[13.97114,13.97114,13.97114,13.97114], "fy":[5.23038,5.23038,5.23038,5.23038]}, + {"t":0.44365, "x":2.63509, "y":6.3682, "heading":0.0, "vx":-0.9255, "vy":2.53583, "omega":0.0, "ax":5.31381, "ay":1.63863, "alpha":0.0, "fx":[87.85557,87.85557,87.85557,87.85557], "fy":[27.09222,27.09222,27.09222,27.09222]}, + {"t":0.49295, "x":2.59593, "y":6.49519, "heading":0.0, "vx":-0.66356, "vy":2.61661, "omega":0.0, "ax":7.09732, "ay":1.3086, "alpha":0.0, "fx":[117.34319,117.34319,117.34319,117.34319], "fy":[21.63566,21.63566,21.63566,21.63566]}, + {"t":0.54224, "x":2.57184, "y":6.62577, "heading":0.0, "vx":-0.3137, "vy":2.68112, "omega":0.0, "ax":7.40996, "ay":0.35509, "alpha":0.0, "fx":[122.51209,122.51209,122.51209,122.51209], "fy":[5.87089,5.87089,5.87089,5.87089]}, + {"t":0.59153, "x":2.56538, "y":6.75836, "heading":0.0, "vx":0.05157, "vy":2.69862, "omega":0.0, "ax":6.98803, "ay":-2.66259, "alpha":0.0, "fx":[115.53619,115.53619,115.53619,115.53619], "fy":[-44.02173,-44.02173,-44.02173,-44.02173]}, + {"t":0.64083, "x":2.57641, "y":6.88815, "heading":0.0, "vx":0.39604, "vy":2.56737, "omega":0.0, "ax":5.08653, "ay":-5.5197, "alpha":0.0, "fx":[84.09785,84.09785,84.09785,84.09785], "fy":[-91.25957,-91.25957,-91.25957,-91.25957]}, + {"t":0.69012, "x":2.60212, "y":7.00801, "heading":0.0, "vx":0.64678, "vy":2.29528, "omega":0.0, "ax":4.26851, "ay":-6.18614, "alpha":0.0, "fx":[70.57312,70.57312,70.57312,70.57312], "fy":[-102.27817,-102.27817,-102.27817,-102.27817]}, + {"t":0.73942, "x":2.63918, "y":7.11363, "heading":0.0, "vx":0.8572, "vy":1.99034, "omega":0.0, "ax":3.86127, "ay":-6.45364, "alpha":0.0, "fx":[63.84004,63.84004,63.84004,63.84004], "fy":[-106.70094,-106.70094,-106.70094,-106.70094]}, + {"t":0.78871, "x":2.68613, "y":7.20391, "heading":0.0, "vx":1.04754, "vy":1.67221, "omega":0.0, "ax":3.62125, "ay":-6.59439, "alpha":0.0, "fx":[59.87171,59.87171,59.87171,59.87171], "fy":[-109.028,-109.028,-109.028,-109.028]}, + {"t":0.83801, "x":2.74217, "y":7.27832, "heading":0.0, "vx":1.22604, "vy":1.34714, "omega":0.0, "ax":3.4637, "ay":-6.68046, "alpha":0.0, "fx":[57.2669,57.2669,57.2669,57.2669], "fy":[-110.45093,-110.45093,-110.45093,-110.45093]}, + {"t":0.8873, "x":2.80681, "y":7.33661, "heading":0.0, "vx":1.39679, "vy":1.01783, "omega":0.0, "ax":3.35254, "ay":-6.73829, "alpha":0.0, "fx":[55.42906,55.42906,55.42906,55.42906], "fy":[-111.40711,-111.40711,-111.40711,-111.40711]}, + {"t":0.9366, "x":2.87974, "y":7.3786, "heading":0.0, "vx":1.56205, "vy":0.68567, "omega":0.0, "ax":3.26998, "ay":-6.77974, "alpha":0.0, "fx":[54.06409,54.06409,54.06409,54.06409], "fy":[-112.09238,-112.09238,-112.09238,-112.09238]}, + {"t":0.98589, "x":2.96071, "y":7.40416, "heading":0.0, "vx":1.72324, "vy":0.35146, "omega":0.0, "ax":3.69974, "ay":-6.55162, "alpha":0.0, "fx":[61.16942,61.16942,61.16942,61.16942], "fy":[-108.32083,-108.32083,-108.32083,-108.32083]}, + {"t":1.02515, "x":3.03122, "y":7.41291, "heading":0.0, "vx":1.86849, "vy":0.09425, "omega":0.0, "ax":5.88713, "ay":-4.66933, "alpha":0.0, "fx":[97.33454,97.33454,97.33454,97.33454], "fy":[-77.20003,-77.20003,-77.20003,-77.20003]}, + {"t":1.06441, "x":3.10911, "y":7.41301, "heading":0.0, "vx":2.09961, "vy":-0.08906, "omega":0.0, "ax":7.4956, "ay":-0.11927, "alpha":0.0, "fx":[123.92812,123.92812,123.92812,123.92812], "fy":[-1.97199,-1.97199,-1.97199,-1.97199]}, + {"t":1.10367, "x":3.19732, "y":7.40943, "heading":0.0, "vx":2.39388, "vy":-0.09374, "omega":0.0, "ax":7.45759, "ay":-0.07802, "alpha":0.0, "fx":[123.29966,123.29966,123.29966,123.29966], "fy":[-1.28994,-1.28994,-1.28994,-1.28994]}, + {"t":1.14293, "x":3.29704, "y":7.40569, "heading":0.0, "vx":2.68666, "vy":-0.09681, "omega":0.0, "ax":0.31306, "ay":1.35567, "alpha":0.0, "fx":[5.17599,5.17599,5.17599,5.17599], "fy":[22.41388,22.41388,22.41388,22.41388]}, + {"t":1.18219, "x":3.40276, "y":7.40293, "heading":0.0, "vx":2.69895, "vy":-0.04358, "omega":0.0, "ax":0.00373, "ay":0.26056, "alpha":0.0, "fx":[0.06161,0.06161,0.06161,0.06161], "fy":[4.30791,4.30791,4.30791,4.30791]}, + {"t":1.22145, "x":3.50872, "y":7.40142, "heading":0.0, "vx":2.6991, "vy":-0.03335, "omega":0.0, "ax":0.00108, "ay":0.09282, "alpha":0.0, "fx":[0.01792,0.01792,0.01792,0.01792], "fy":[1.53462,1.53462,1.53462,1.53462]}, + {"t":1.2607, "x":3.61469, "y":7.40018, "heading":0.0, "vx":2.69914, "vy":-0.02971, "omega":0.0, "ax":0.00062, "ay":0.05912, "alpha":0.0, "fx":[0.01033,0.01033,0.01033,0.01033], "fy":[0.97741,0.97741,0.97741,0.97741]}, + {"t":1.29996, "x":3.72065, "y":7.39906, "heading":0.0, "vx":2.69916, "vy":-0.02739, "omega":0.0, "ax":0.00045, "ay":0.04625, "alpha":0.0, "fx":[0.00749,0.00749,0.00749,0.00749], "fy":[0.76467,0.76467,0.76467,0.76467]}, + {"t":1.33922, "x":3.82662, "y":7.39802, "heading":0.0, "vx":2.69918, "vy":-0.02557, "omega":0.0, "ax":0.00035, "ay":0.03852, "alpha":0.0, "fx":[0.00585,0.00585,0.00585,0.00585], "fy":[0.63693,0.63693,0.63693,0.63693]}, + {"t":1.37848, "x":3.93259, "y":7.39705, "heading":0.0, "vx":2.6992, "vy":-0.02406, "omega":0.0, "ax":0.00029, "ay":0.03344, "alpha":0.0, "fx":[0.00479,0.00479,0.00479,0.00479], "fy":[0.55288,0.55288,0.55288,0.55288]}, + {"t":1.41774, "x":4.03856, "y":7.39613, "heading":0.0, "vx":2.69921, "vy":-0.02275, "omega":0.0, "ax":0.00025, "ay":0.0304, "alpha":0.0, "fx":[0.00412,0.00412,0.00412,0.00412], "fy":[0.50266,0.50266,0.50266,0.50266]}, + {"t":1.457, "x":4.14453, "y":7.39526, "heading":0.0, "vx":2.69922, "vy":-0.02155, "omega":0.0, "ax":0.00023, "ay":0.02918, "alpha":0.0, "fx":[0.00374,0.00374,0.00374,0.00374], "fy":[0.4825,0.4825,0.4825,0.4825]}, + {"t":1.49626, "x":4.2505, "y":7.39443, "heading":0.0, "vx":2.69923, "vy":-0.02041, "omega":0.0, "ax":0.00022, "ay":0.0297, "alpha":0.0, "fx":[0.0036,0.0036,0.0036,0.0036], "fy":[0.49107,0.49107,0.49107,0.49107]}, + {"t":1.53552, "x":4.35646, "y":7.39366, "heading":0.0, "vx":2.69923, "vy":-0.01924, "omega":0.0, "ax":0.00022, "ay":0.03199, "alpha":0.0, "fx":[0.00364,0.00364,0.00364,0.00364], "fy":[0.52891,0.52891,0.52891,0.52891]}, + {"t":1.57478, "x":4.46243, "y":7.39293, "heading":0.0, "vx":2.69924, "vy":-0.01799, "omega":0.0, "ax":0.00023, "ay":0.03619, "alpha":0.0, "fx":[0.00382,0.00382,0.00382,0.00382], "fy":[0.5984,0.5984,0.5984,0.5984]}, + {"t":1.61404, "x":4.5684, "y":7.39225, "heading":0.0, "vx":2.69925, "vy":-0.01657, "omega":0.0, "ax":0.00025, "ay":0.04259, "alpha":0.0, "fx":[0.0041,0.0041,0.0041,0.0041], "fy":[0.70411,0.70411,0.70411,0.70411]}, + {"t":1.6533, "x":4.67437, "y":7.39163, "heading":0.0, "vx":2.69926, "vy":-0.01489, "omega":0.0, "ax":0.00027, "ay":0.05162, "alpha":0.0, "fx":[0.00438,0.00438,0.00438,0.00438], "fy":[0.85342,0.85342,0.85342,0.85342]}, + {"t":1.69255, "x":4.78035, "y":7.39108, "heading":0.0, "vx":2.69927, "vy":-0.01287, "omega":0.0, "ax":0.00027, "ay":0.06399, "alpha":0.0, "fx":[0.00455,0.00455,0.00455,0.00455], "fy":[1.0579,1.0579,1.0579,1.0579]}, + {"t":1.73181, "x":4.88632, "y":7.39063, "heading":0.0, "vx":2.69928, "vy":-0.01036, "omega":0.0, "ax":0.00026, "ay":0.08082, "alpha":0.0, "fx":[0.00434,0.00434,0.00434,0.00434], "fy":[1.33625,1.33625,1.33625,1.33625]}, + {"t":1.77107, "x":4.99229, "y":7.39028, "heading":0.0, "vx":2.69929, "vy":-0.00718, "omega":0.0, "ax":0.0002, "ay":0.10406, "alpha":0.0, "fx":[0.00327,0.00327,0.00327,0.00327], "fy":[1.72052,1.72052,1.72052,1.72052]}, + {"t":1.81033, "x":5.09826, "y":7.39008, "heading":0.0, "vx":2.6993, "vy":-0.0031, "omega":0.0, "ax":0.00002, "ay":0.13723, "alpha":0.0, "fx":[0.00034,0.00034,0.00034,0.00034], "fy":[2.26885,2.26885,2.26885,2.26885]}, + {"t":1.84959, "x":5.20423, "y":7.39007, "heading":0.0, "vx":2.6993, "vy":0.00229, "omega":0.0, "ax":-0.00041, "ay":0.18591, "alpha":0.0, "fx":[-0.00676,-0.00676,-0.00676,-0.00676], "fy":[3.07372,3.07372,3.07372,3.07372]}, + {"t":1.88885, "x":5.3102, "y":7.3903, "heading":0.0, "vx":2.69929, "vy":0.00959, "omega":0.0, "ax":-0.00158, "ay":0.24614, "alpha":0.0, "fx":[-0.02619,-0.02619,-0.02619,-0.02619], "fy":[4.06949,4.06949,4.06949,4.06949]}, + {"t":1.92811, "x":5.41617, "y":7.39087, "heading":0.0, "vx":2.69922, "vy":0.01925, "omega":0.0, "ax":-4.16125, "ay":0.08136, "alpha":0.0, "fx":[-68.79987,-68.79987,-68.79987,-68.79987], "fy":[1.34519,1.34519,1.34519,1.34519]}, + {"t":1.96737, "x":5.51894, "y":7.39168, "heading":0.0, "vx":2.53586, "vy":0.02245, "omega":0.0, "ax":-7.46251, "ay":-0.10112, "alpha":0.0, "fx":[-123.38093,-123.38093,-123.38093,-123.38093], "fy":[-1.67187,-1.67187,-1.67187,-1.67187]}, + {"t":2.00663, "x":5.61274, "y":7.39249, "heading":0.0, "vx":2.24289, "vy":0.01848, "omega":0.0, "ax":-7.49618, "ay":-0.15809, "alpha":0.0, "fx":[-123.93762,-123.93762,-123.93762,-123.93762], "fy":[-2.6137,-2.6137,-2.6137,-2.6137]}, + {"t":2.04589, "x":5.69502, "y":7.39309, "heading":0.0, "vx":1.94859, "vy":0.01227, "omega":0.0, "ax":-6.72859, "ay":-3.3402, "alpha":0.0, "fx":[-111.24675,-111.24675,-111.24675,-111.24675], "fy":[-55.22498,-55.22498,-55.22498,-55.22498]}, + {"t":2.08515, "x":5.76633, "y":7.391, "heading":0.0, "vx":1.68443, "vy":-0.11886, "omega":0.0, "ax":-5.09967, "ay":-5.52822, "alpha":0.0, "fx":[-84.31513,-84.31513,-84.31513,-84.31513], "fy":[-91.40058,-91.40058,-91.40058,-91.40058]}, + {"t":2.1244, "x":5.82853, "y":7.38207, "heading":0.0, "vx":1.48423, "vy":-0.3359, "omega":0.0, "ax":-1.06461, "ay":-2.79425, "alpha":-15.66915, "fx":[16.53541,120.38314,-124.29622,-83.02891], "fy":[-123.2824,26.98835,4.17159,-92.67208]}, + {"t":2.14861, "x":5.86414, "y":7.37313, "heading":0.0, "vx":1.45846, "vy":-0.40352, "omega":-0.37921, "ax":-1.07519, "ay":-2.93055, "alpha":-15.42451, "fx":[14.83541,121.57284,-124.3261,-83.18866], "fy":[-123.48737,19.52855,2.6683,-92.51811]}, + {"t":2.17281, "x":5.89912, "y":7.3625, "heading":-0.00918, "vx":1.43244, "vy":-0.47444, "omega":-0.7525, "ax":-1.10374, "ay":-3.1837, "alpha":-14.9712, "fx":[12.3386,122.81557,-124.32919,-83.81969], "fy":[-123.74999,3.4317,1.70317,-91.93475]}, + {"t":2.19701, "x":5.93346, "y":7.35009, "heading":-0.02739, "vx":1.40573, "vy":-0.55149, "omega":-1.11482, "ax":-1.2025, "ay":-3.56677, "alpha":-14.20853, "fx":[9.07277,120.61226,-124.31831,-84.89241], "fy":[-124.01962,-22.13604,1.20292,-90.93129]}, + {"t":2.22121, "x":5.96713, "y":7.3357, "heading":-0.05437, "vx":1.37663, "vy":-0.63781, "omega":-1.45868, "ax":-1.43873, "ay":-4.01813, "alpha":-13.13504, "fx":[5.07121,110.44331,-124.29934,-86.36407], "fy":[-124.23404,-53.04986,1.0682,-89.51863]}, + {"t":2.24541, "x":6.00003, "y":7.31908, "heading":-0.08967, "vx":1.34181, "vy":-0.73505, "omega":-1.77656, "ax":-1.8119, "ay":-4.40959, "alpha":-11.96003, "fx":[0.36762,92.25584,-124.27407,-88.17684], "fy":[-124.32207,-80.74051,1.15434,-87.71456]}, + {"t":2.26961, "x":6.03197, "y":7.3, "heading":-0.13266, "vx":1.29796, "vy":-0.84177, "omega":-2.066, "ax":-2.24501, "ay":-4.66676, "alpha":-10.91827, "fx":[-5.00796,71.03923,-124.24318,-90.25897], "fy":[-124.20499,-100.13597,1.25837,-85.54763]}, + {"t":2.29381, "x":6.06272, "y":7.27826, "heading":-0.18266, "vx":1.24363, "vy":-0.95471, "omega":-2.33024, "ax":-2.67796, "ay":-4.80512, "alpha":-10.06477, "fx":[-11.03811,50.67367,-124.20789,-92.53119], "fy":[-123.79553,-112.04012,1.11158,-83.05653]}, + {"t":2.31801, "x":6.09204, "y":7.25375, "heading":-0.23906, "vx":1.17882, "vy":-1.071, "omega":-2.57381, "ax":-3.09704, "ay":-4.86625, "alpha":-9.33304, "fx":[-17.72884,31.98836,-124.16695,-94.91158], "fy":[-122.99268,-118.90236,0.36185,-80.29031]}, + {"t":2.34221, "x":6.11966, "y":7.22641, "heading":-0.30135, "vx":1.10387, "vy":-1.18877, "omega":-2.79968, "ax":-3.5102, "ay":-4.88202, "alpha":-8.63153, "fx":[-25.11691,14.3903,-124.10235,-97.31363], "fy":[-121.67073,-122.4148,-1.46483,-77.31612]}, + {"t":2.36641, "x":6.14535, "y":7.19621, "heading":-0.3691, "vx":1.01892, "vy":-1.30692, "omega":-3.00857, "ax":-3.9285, "ay":-4.8724, "alpha":-7.87279, "fx":[-33.27862,-2.95,-123.93926,-99.63851], "fy":[-119.66005,-123.31827,-5.01594,-74.236]}, + {"t":2.39062, "x":6.16885, "y":7.16315, "heading":-0.44191, "vx":0.92384, "vy":-1.42483, "omega":-3.1991, "ax":-4.35874, "ay":-4.84997, "alpha":-6.97171, "fx":[-42.34771,-20.69811,-123.45257,-101.76174], "fy":[-116.71112,-121.67643,-11.14316,-71.21601]}, + {"t":2.41482, "x":6.18994, "y":7.12725, "heading":-0.51933, "vx":0.81836, "vy":-1.54221, "omega":-3.36783, "ax":-4.79904, "ay":-4.82117, "alpha":-5.84319, "fx":[-52.55152,-39.22608,-122.09359,-103.50718], "fy":[-112.41928,-117.08057,-20.80294,-68.53935]}, + {"t":2.43902, "x":6.20833, "y":7.08852, "heading":-0.60084, "vx":0.70221, "vy":-1.65889, "omega":-3.50924, "ax":-5.23445, "ay":-4.78013, "alpha":-4.41665, "fx":[-64.27882,-58.49041,-118.81881,-104.58554], "fy":[-106.05071,-108.79162,-34.57167,-66.71432]}, + {"t":2.46322, "x":6.2238, "y":7.04697, "heading":-0.68576, "vx":0.57554, "vy":-1.77457, "omega":-3.61612, "ax":-5.63765, "ay":-4.6926, "alpha":-2.66889, "fx":[-78.18588,-77.84953,-112.38406,-104.41968], "fy":[-96.09263,-95.94521,-51.58171,-66.71969]}, + {"t":2.48742, "x":6.23607, "y":7.00265, "heading":-0.77328, "vx":0.4391, "vy":-1.88813, "omega":-3.68071, "ax":-5.97772, "ay":-4.48122, "alpha":-0.59312, "fx":[-95.18456,-95.89791,-102.70286,-101.54408], "fy":[-78.9707,-77.97431,-68.78965,-70.62545]}, + {"t":2.51162, "x":6.24495, "y":6.95564, "heading":-0.86236, "vx":0.29443, "vy":-1.99658, "omega":-3.69507, "ax":-6.17266, "ay":-4.0322, "alpha":2.12461, "fx":[-114.60554,-110.5597,-92.00946,-91.04626], "fy":[-45.60941,-55.37152,-82.56526,-83.11815]}, + {"t":2.53582, "x":6.25027, "y":6.90614, "heading":-0.95178, "vx":0.14505, "vy":-2.09417, "omega":-3.64365, "ax":-5.74994, "ay":-3.33419, "alpha":6.45511, "fx":[-122.25073,-119.33335,-83.19119,-55.48992], "fy":[12.98867,-32.60537,-91.51103,-109.37504]}, + {"t":2.56002, "x":6.25209, "y":6.85448, "heading":-1.03996, "vx":0.00589, "vy":-2.17486, "omega":-3.48743, "ax":-5.04943, "ay":-3.44767, "alpha":8.48127, "fx":[-120.373,-119.61955,-75.92125,-18.02415], "fy":[22.72456,-31.51991,-97.7019,-121.51031]}, + {"t":2.58422, "x":6.25076, "y":6.80084, "heading":-1.12436, "vx":-0.11631, "vy":-2.2583, "omega":-3.28217, "ax":-4.63163, "ay":-3.55794, "alpha":9.2247, "fx":[-119.78037,-119.33106,-70.79154,3.59609], "fy":[21.89805,-32.55262,-101.55077,-123.09455]}, + {"t":2.60842, "x":6.24659, "y":6.74515, "heading":-1.20379, "vx":-0.2284, "vy":-2.3444, "omega":-3.05893, "ax":-4.36436, "ay":-3.55909, "alpha":9.59146, "fx":[-118.14476,-119.48337,-67.62727,16.62392], "fy":[22.58123,-31.95585,-103.74732,-122.25385]}, + {"t":2.63263, "x":6.23978, "y":6.68737, "heading":-1.27782, "vx":-0.33402, "vy":-2.43054, "omega":-2.8268, "ax":-4.15157, "ay":-3.50088, "alpha":9.81871, "fx":[-112.99297,-119.92741,-65.94707,24.30849], "fy":[24.70307,-30.21632,-104.87779,-121.13517]}, + {"t":2.65683, "x":6.23048, "y":6.62752, "heading":-1.34623, "vx":-0.43449, "vy":-2.51526, "omega":-2.58918, "ax":-1.4586, "ay":-2.23697, "alpha":15.76129, "fx":[57.87105,-121.84896,-66.04321,33.55871], "fy":[97.13034,-21.17286,-104.86406,-119.03246]}, + {"t":2.68103, "x":6.21954, "y":6.56599, "heading":-1.40889, "vx":-0.46979, "vy":-2.5694, "omega":-2.20774, "ax":-0.87455, "ay":-1.55606, "alpha":17.42605, "fx":[78.53853,-122.80033,-70.14162,56.5661], "fy":[94.22022,15.17444,-102.20817,-110.09431]}, + {"t":2.70523, "x":6.20791, "y":6.50336, "heading":-1.46232, "vx":-0.49096, "vy":-2.60705, "omega":-1.78602, "ax":-0.549, "ay":-0.94914, "alpha":18.15761, "fx":[82.43468,-115.20207,-74.83082,71.29096], "fy":[91.98776,45.41578,-98.86479,-101.30917]}, + {"t":2.72943, "x":6.19587, "y":6.43998, "heading":-1.50555, "vx":-0.50424, "vy":-2.63002, "omega":-1.34658, "ax":-0.27812, "ay":-0.4969, "alpha":18.47063, "fx":[84.92064,-104.50507,-79.31453,80.506], "fy":[90.13445,66.61563,-95.34402,-94.26812]}, + {"t":2.75363, "x":6.18359, "y":6.37619, "heading":-1.53813, "vx":-0.51097, "vy":-2.64205, "omega":-0.89958, "ax":-0.07057, "ay":-0.20542, "alpha":18.57484, "fx":[86.98736,-94.94056,-82.76673,86.05315], "fy":[88.36794,79.78837,-92.40634,-89.33517]}, + {"t":2.77783, "x":6.1712, "y":6.31219, "heading":-1.5599, "vx":-0.51268, "vy":-2.64702, "omega":-0.45005, "ax":0.07224, "ay":-0.07902, "alpha":18.59616, "fx":[89.27064,-88.82777,-84.23748,88.57179], "fy":[86.19323,86.63129,-91.12038,-86.92991]}, + {"t":2.80203, "x":6.15881, "y":6.24811, "heading":-1.5708, "vx":-0.51093, "vy":-2.64893, "omega":0.0, "ax":6.58568, "ay":-0.95825, "alpha":0.0, "fx":[108.88391,108.88391,108.88391,108.88391], "fy":[-15.8431,-15.8431,-15.8431,-15.8431]}, + {"t":2.84399, "x":6.14317, "y":6.13613, "heading":-1.5708, "vx":-0.23464, "vy":-2.68914, "omega":0.0, "ax":3.17636, "ay":-0.19823, "alpha":0.0, "fx":[52.51622,52.51622,52.51622,52.51622], "fy":[-3.27735,-3.27735,-3.27735,-3.27735]}, + {"t":2.88594, "x":6.13613, "y":6.02314, "heading":-1.5708, "vx":-0.10138, "vy":-2.69745, "omega":0.0, "ax":0.42438, "ay":-0.01455, "alpha":0.0, "fx":[7.01644,7.01644,7.01644,7.01644], "fy":[-0.24057,-0.24057,-0.24057,-0.24057]}, + {"t":2.92789, "x":6.13225, "y":5.90996, "heading":-1.5708, "vx":-0.08358, "vy":-2.69806, "omega":0.0, "ax":0.04619, "ay":-0.00141, "alpha":0.0, "fx":[0.76372,0.76372,0.76372,0.76372], "fy":[-0.02337,-0.02337,-0.02337,-0.02337]}, + {"t":2.96985, "x":6.12878, "y":5.79676, "heading":-1.5708, "vx":-0.08164, "vy":-2.69812, "omega":0.0, "ax":0.00449, "ay":-0.00014, "alpha":0.0, "fx":[0.07428,0.07428,0.07428,0.07428], "fy":[-0.00224,-0.00224,-0.00224,-0.00224]}, + {"t":3.0118, "x":6.12536, "y":5.68356, "heading":-1.5708, "vx":-0.08145, "vy":-2.69813, "omega":0.0, "ax":-0.00008, "ay":0.0, "alpha":0.0, "fx":[-0.0014,-0.0014,-0.0014,-0.0014], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":3.05375, "x":6.12194, "y":5.57037, "heading":-1.5708, "vx":-0.08145, "vy":-2.69813, "omega":0.0, "ax":-0.00058, "ay":0.00002, "alpha":0.0, "fx":[-0.00964,-0.00964,-0.00964,-0.00964], "fy":[0.0003,0.0003,0.0003,0.0003]}, + {"t":3.09571, "x":6.11852, "y":5.45717, "heading":-1.5708, "vx":-0.08148, "vy":-2.69813, "omega":0.0, "ax":-0.00063, "ay":0.00002, "alpha":0.0, "fx":[-0.01048,-0.01048,-0.01048,-0.01048], "fy":[0.00032,0.00032,0.00032,0.00032]}, + {"t":3.13766, "x":6.11511, "y":5.34398, "heading":-1.5708, "vx":-0.0815, "vy":-2.69813, "omega":0.0, "ax":-0.00064, "ay":0.00002, "alpha":0.0, "fx":[-0.01051,-0.01051,-0.01051,-0.01051], "fy":[0.00032,0.00032,0.00032,0.00032]}, + {"t":3.17962, "x":6.11169, "y":5.23078, "heading":-1.5708, "vx":-0.08153, "vy":-2.69813, "omega":0.0, "ax":-0.00063, "ay":0.00002, "alpha":0.0, "fx":[-0.01046,-0.01046,-0.01046,-0.01046], "fy":[0.00032,0.00032,0.00032,0.00032]}, + {"t":3.22157, "x":6.10827, "y":5.11758, "heading":-1.5708, "vx":-0.08156, "vy":-2.69812, "omega":0.0, "ax":-0.00063, "ay":0.00002, "alpha":0.0, "fx":[-0.0104,-0.0104,-0.0104,-0.0104], "fy":[0.00032,0.00032,0.00032,0.00032]}, + {"t":3.26352, "x":6.10484, "y":5.00439, "heading":-1.5708, "vx":-0.08158, "vy":-2.69812, "omega":0.0, "ax":-0.00063, "ay":0.00002, "alpha":0.0, "fx":[-0.01034,-0.01034,-0.01034,-0.01034], "fy":[0.00032,0.00032,0.00032,0.00032]}, + {"t":3.30548, "x":6.10142, "y":4.89119, "heading":-1.5708, "vx":-0.08161, "vy":-2.69812, "omega":0.0, "ax":-0.00062, "ay":0.00002, "alpha":0.0, "fx":[-0.01029,-0.01029,-0.01029,-0.01029], "fy":[0.00031,0.00031,0.00031,0.00031]}, + {"t":3.34743, "x":6.098, "y":4.778, "heading":-1.5708, "vx":-0.08164, "vy":-2.69812, "omega":0.0, "ax":-0.00062, "ay":0.00002, "alpha":0.0, "fx":[-0.01024,-0.01024,-0.01024,-0.01024], "fy":[0.00031,0.00031,0.00031,0.00031]}, + {"t":3.38938, "x":6.09457, "y":4.6648, "heading":-1.5708, "vx":-0.08166, "vy":-2.69812, "omega":0.0, "ax":-0.00062, "ay":0.00002, "alpha":0.0, "fx":[-0.01022,-0.01022,-0.01022,-0.01022], "fy":[0.00031,0.00031,0.00031,0.00031]}, + {"t":3.43134, "x":6.09114, "y":4.5516, "heading":-1.5708, "vx":-0.08169, "vy":-2.69812, "omega":0.0, "ax":-0.00063, "ay":0.00002, "alpha":0.0, "fx":[-0.0104,-0.0104,-0.0104,-0.0104], "fy":[0.00032,0.00032,0.00032,0.00032]}, + {"t":3.47329, "x":6.08772, "y":4.43841, "heading":-1.5708, "vx":-0.08171, "vy":-2.69812, "omega":0.0, "ax":-0.00075, "ay":0.00023, "alpha":0.0, "fx":[-0.01238,-0.01238,-0.01238,-0.01238], "fy":[0.00383,0.00383,0.00383,0.00383]}, + {"t":3.51525, "x":6.08429, "y":4.32521, "heading":-1.5708, "vx":-0.08174, "vy":-2.69811, "omega":0.0, "ax":0.12715, "ay":4.24017, "alpha":0.0, "fx":[2.10223,2.10223,2.10223,2.10223], "fy":[70.10464,70.10464,70.10464,70.10464]}, + {"t":3.5572, "x":6.08097, "y":4.21575, "heading":-1.5708, "vx":-0.07641, "vy":-2.52022, "omega":0.0, "ax":0.22631, "ay":7.46917, "alpha":0.0, "fx":[3.74175,3.74175,3.74175,3.74175], "fy":[123.49106,123.49106,123.49106,123.49106]}, + {"t":3.59915, "x":6.07796, "y":4.11659, "heading":-1.5708, "vx":-0.06692, "vy":-2.20686, "omega":0.0, "ax":0.22727, "ay":7.49916, "alpha":0.0, "fx":[3.75762,3.75762,3.75762,3.75762], "fy":[123.98694,123.98694,123.98694,123.98694]}, + {"t":3.64111, "x":6.07536, "y":4.0306, "heading":-1.5708, "vx":-0.05738, "vy":-1.89224, "omega":0.0, "ax":0.22762, "ay":7.50926, "alpha":0.0, "fx":[3.76335,3.76335,3.76335,3.76335], "fy":[124.15393,124.15393,124.15393,124.15393]}, + {"t":3.68306, "x":6.07315, "y":3.95782, "heading":-1.5708, "vx":-0.04783, "vy":-1.5772, "omega":0.0, "ax":0.22781, "ay":7.51433, "alpha":0.0, "fx":[3.76652,3.76652,3.76652,3.76652], "fy":[124.23771,124.23771,124.23771,124.23771]}, + {"t":3.72501, "x":6.07134, "y":3.89827, "heading":-1.5708, "vx":-0.03827, "vy":-1.26195, "omega":0.0, "ax":0.22794, "ay":7.51737, "alpha":0.0, "fx":[3.76865,3.76865,3.76865,3.76865], "fy":[124.28806,124.28806,124.28806,124.28806]}, + {"t":3.76697, "x":6.06994, "y":3.85194, "heading":-1.5708, "vx":-0.02871, "vy":-0.94657, "omega":0.0, "ax":0.22804, "ay":7.51941, "alpha":0.0, "fx":[3.77027,3.77027,3.77027,3.77027], "fy":[124.32166,124.32166,124.32166,124.32166]}, + {"t":3.80892, "x":6.06893, "y":3.81885, "heading":-1.5708, "vx":-0.01914, "vy":-0.6311, "omega":0.0, "ax":0.22812, "ay":7.52086, "alpha":0.0, "fx":[3.77159,3.77159,3.77159,3.77159], "fy":[124.34567,124.34567,124.34567,124.34567]}, + {"t":3.85087, "x":6.06833, "y":3.79899, "heading":-1.5708, "vx":-0.00957, "vy":-0.31557, "omega":0.0, "ax":0.22819, "ay":7.52195, "alpha":0.0, "fx":[3.77273,3.77273,3.77273,3.77273], "fy":[124.36368,124.36368,124.36368,124.36368]}, + {"t":3.89283, "x":6.06813, "y":3.79237, "heading":-1.5708, "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/deploy/choreo/LBumptoDepot.traj b/src/main/deploy/choreo/LBumptoDepot.traj index 9c535115..103b07e2 100644 --- a/src/main/deploy/choreo/LBumptoDepot.traj +++ b/src/main/deploy/choreo/LBumptoDepot.traj @@ -16,7 +16,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"LBump.x", "val":3.631879568099976}, "y":{"exp":"LBump.y", "val":5.975247383117676}, "heading":{"exp":"LBump.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"StartingLBump.x", "val":3.631879568099976}, "y":{"exp":"StartingLBump.y", "val":5.975247383117676}, "heading":{"exp":"StartingLBump.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"Depot.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"Depot.x - 6 in", "val":0.551599767303467}, "y":{"exp":"Depot.y", "val":5.975247383117676}, "heading":{"exp":"Depot.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/LCleanupToLBump.traj b/src/main/deploy/choreo/LCleanupToLBump.traj new file mode 100644 index 00000000..16acc697 --- /dev/null +++ b/src/main/deploy/choreo/LCleanupToLBump.traj @@ -0,0 +1,130 @@ +{ + "name":"LCleanupToLBump", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":6.068130016326904, "y":3.792367458343506, "heading":-1.5707963267948966, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.070739269256592, "y":5.675617694854736, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.8895323276519775, "y":5.703888893127441, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.7}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"LCleanup.x", "val":6.068130016326904}, "y":{"exp":"LCleanup.y", "val":3.792367458343506}, "heading":{"exp":"LCleanup.heading", "val":-1.5707963267948966}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.070739269256592 m", "val":6.070739269256592}, "y":{"exp":"5.675617694854736 m", "val":5.675617694854736}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"LBump.x", "val":2.8895323276519775}, "y":{"exp":"LBump.y", "val":5.703888893127441}, "heading":{"exp":"LBump.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.7}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2761996, + "y":0.2761996 + }, + "backLeft":{ + "x":-0.2761996, + "y":0.2761996 + }, + "mass":66.133767546, + "inertia":10.42384825260472, + "gearing":7.03, + "radius":0.0508, + "vmax":485.8996637552213, + "tmax":0.9, + "cof":2.25, + "bumper":{ + "front":0.44196, + "side":0.44196, + "back":0.44196 + }, + "differentialTrackWidth":0.5427218 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.09216,2.48795], + "samples":[ + {"t":0.0, "x":6.06813, "y":3.79237, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.21201, "ay":6.23721, "alpha":6.62571, "fx":[64.82855,-6.99729,-20.68957,109.14711], "fy":[106.21825,124.22369,122.43665,59.61155]}, + {"t":0.0331, "x":6.06934, "y":3.79578, "heading":-1.5708, "vx":0.07321, "vy":0.20643, "omega":0.21928, "ax":2.20989, "ay":6.23271, "alpha":6.6406, "fx":[64.86324,-7.04832,-20.88949,109.22321], "fy":[106.18475,124.20846,122.36466,59.43487]}, + {"t":0.06619, "x":6.07297, "y":3.80603, "heading":-1.56354, "vx":0.14635, "vy":0.4127, "omega":0.43906, "ax":2.21065, "ay":6.2362, "alpha":6.6127, "fx":[64.58819,-7.36922,-20.19521,109.17495], "fy":[106.33734,124.17439,122.43143,59.48044]}, + {"t":0.09929, "x":6.07903, "y":3.8231, "heading":-1.54901, "vx":0.21951, "vy":0.6191, "omega":0.65791, "ax":2.21581, "ay":6.24784, "alpha":6.53876, "fx":[64.00027,-7.94753,-18.50314,108.98992], "fy":[106.67374,124.11902,122.63294,59.76782]}, + {"t":0.13238, "x":6.08751, "y":3.84701, "heading":-1.52723, "vx":0.29284, "vy":0.82587, "omega":0.87432, "ax":2.22817, "ay":6.26748, "alpha":6.41455, "fx":[63.09778,-8.75813,-15.63168,108.64917], "fy":[107.18654,124.03868,122.94416,60.32287]}, + {"t":0.16548, "x":6.09842, "y":3.87778, "heading":-1.4983, "vx":0.36659, "vy":1.0333, "omega":1.08661, "ax":2.25206, "ay":6.29431, "alpha":6.23578, "fx":[61.88162,-9.75705,-11.31263,108.12519], "fy":[107.86229,123.9292,123.29684,61.17838]}, + {"t":0.19858, "x":6.11179, "y":3.91542, "heading":-1.46233, "vx":0.44112, "vy":1.24162, "omega":1.29299, "ax":2.29345, "ay":6.32631, "alpha":5.99997, "fx":[60.35735,-10.86827,-5.19188,107.37696], "fy":[108.67987,123.78727,123.53811,62.37762]}, + {"t":0.23167, "x":6.12764, "y":3.95998, "heading":-1.41954, "vx":0.51702, "vy":1.45099, "omega":1.49157, "ax":2.35982, "ay":6.35938, "alpha":5.70927, "fx":[58.53928,-11.95166,3.13959,106.33635], "fy":[109.60691,123.61272,123.36317,63.98668]}, + {"t":0.26477, "x":6.14604, "y":4.01149, "heading":-1.37018, "vx":0.59512, "vy":1.66146, "omega":1.68052, "ax":2.45958, "ay":6.3865, "alpha":5.37235, "fx":[56.46093,-12.70882,14.04322,104.86585], "fy":[110.58934,123.41302,122.22458,66.13648]}, + {"t":0.29786, "x":6.16709, "y":4.06997, "heading":-1.31456, "vx":0.67653, "vy":1.87283, "omega":1.85832, "ax":2.6021, "ay":6.39847, "alpha":4.99209, "fx":[54.21294,-12.32162,27.60346,102.59164], "fy":[111.51079,123.20905,119.24834,69.18692]}, + {"t":0.33096, "x":6.1909, "y":4.13546, "heading":-1.25306, "vx":0.76265, "vy":2.08459, "omega":2.02354, "ax":2.81359, "ay":6.39219, "alpha":4.43748, "fx":[52.18082,-7.18817,43.24368,97.83721], "fy":[111.92849,122.94301,113.16426,74.70366]}, + {"t":0.36405, "x":6.21768, "y":4.20795, "heading":-1.18609, "vx":0.85576, "vy":2.29614, "omega":2.1704, "ax":3.62292, "ay":5.986, "alpha":-1.20677, "fx":[61.64353,72.30007,58.98135,46.67253], "fy":[97.11055,91.242,101.19405,106.32982]}, + {"t":0.39715, "x":6.24799, "y":4.28722, "heading":-1.11425, "vx":0.97567, "vy":2.49426, "omega":2.13046, "ax":2.75785, "ay":-0.53462, "alpha":-0.93335, "fx":[43.60122,50.56834,47.71433,40.5032], "fy":[-15.641,-10.75761,-2.134,-6.82392]}, + {"t":0.43025, "x":6.28179, "y":4.36948, "heading":-1.04374, "vx":1.06694, "vy":2.47656, "omega":2.09957, "ax":1.1572, "ay":-0.50592, "alpha":0.79107, "fx":[20.59497,14.11973,17.73943,24.07581], "fy":[-3.14001,-7.09328,-13.5941,-9.63107]}, + {"t":0.46334, "x":6.31774, "y":4.45117, "heading":-0.97426, "vx":1.10524, "vy":2.45982, "omega":2.12575, "ax":-0.17219, "ay":0.07891, "alpha":0.90761, "fx":[-1.71517,-8.78414,-3.99352,3.10497], "fy":[7.25476,2.43946,-4.64625,0.17062]}, + {"t":0.49644, "x":6.35422, "y":4.53262, "heading":-0.9039, "vx":1.09954, "vy":2.46243, "omega":2.15579, "ax":-3.41262, "ay":1.43447, "alpha":0.40584, "fx":[-55.70839,-58.58625,-57.16782,-54.227], "fy":[26.90041,23.64079,20.50503,23.82079]}, + {"t":0.52953, "x":6.38874, "y":4.6149, "heading":-0.83256, "vx":0.9866, "vy":2.50991, "omega":2.16922, "ax":-5.96321, "ay":2.08142, "alpha":-0.0986, "fx":[-98.89343,-98.31076,-98.29067,-98.87465], "fy":[33.37779,34.65544,35.44235,34.17662]}, + {"t":0.56263, "x":6.41813, "y":4.69911, "heading":-0.76076, "vx":0.78924, "vy":2.57879, "omega":2.16596, "ax":-6.81478, "ay":1.76249, "alpha":-0.22465, "fx":[-113.29013,-112.30916,-112.03958,-113.04854], "fy":[26.49416,29.85893,31.7479,28.45915]}, + {"t":0.59573, "x":6.44052, "y":4.78542, "heading":-0.68908, "vx":0.5637, "vy":2.63712, "omega":2.15853, "ax":-7.18003, "ay":1.18644, "alpha":-0.23051, "fx":[-119.14152,-118.4996,-118.25733,-118.94423], "fy":[16.75716,20.3745,22.43534,18.89692]}, + {"t":0.62882, "x":6.45524, "y":4.87335, "heading":-0.61764, "vx":0.32607, "vy":2.67639, "omega":2.1509, "ax":-7.36211, "ay":0.50623, "alpha":-0.16163, "fx":[-121.84469,-121.65211,-121.58336,-121.80423], "fy":[6.31079,8.86943,10.41127,7.88731]}, + {"t":0.66192, "x":6.462, "y":4.9622, "heading":-0.54646, "vx":0.08241, "vy":2.69314, "omega":2.14555, "ax":-7.3332, "ay":-1.07857, "alpha":-0.96465, "fx":[-119.0519,-121.56878,-122.78609,-121.56523], "fy":[-30.40692,-16.54978,-5.40602,-18.96698]}, + {"t":0.69501, "x":6.46071, "y":5.05074, "heading":-0.47545, "vx":-0.16029, "vy":2.65745, "omega":2.11362, "ax":-5.94932, "ay":-4.06728, "alpha":-3.47283, "fx":[-75.82398,-89.23375,-120.76667,-107.62629], "fy":[-97.66509,-84.54605,-25.63888,-61.13425]}, + {"t":0.72811, "x":6.45215, "y":5.13647, "heading":-0.40549, "vx":-0.35718, "vy":2.52284, "omega":1.99869, "ax":-4.86502, "ay":-5.08495, "alpha":-4.62834, "fx":[-51.46915,-54.04512,-118.26452,-97.9632], "fy":[-112.79499,-110.88094,-36.50331,-76.10771]}, + {"t":0.7612, "x":6.43766, "y":5.21718, "heading":-0.33935, "vx":-0.5182, "vy":2.35455, "omega":1.84551, "ax":-4.23866, "ay":-5.49768, "alpha":-5.18531, "fx":[-38.24548,-34.17336,-116.35251,-91.54719], "fy":[-118.10206,-118.81082,-42.75453,-83.91511]}, + {"t":0.7943, "x":6.41819, "y":5.29209, "heading":-0.27827, "vx":-0.65848, "vy":2.1726, "omega":1.67389, "ax":-3.8516, "ay":-5.71505, "alpha":-5.45358, "fx":[-29.63549,-23.38993,-114.91884,-86.77683], "fy":[-120.64194,-121.56805,-46.79858,-88.9492]}, + {"t":0.8274, "x":6.39429, "y":5.36087, "heading":-0.22287, "vx":-0.78595, "vy":1.98345, "omega":1.4934, "ax":-3.59163, "ay":-5.85168, "alpha":-5.57967, "fx":[-23.36967,-17.38028,-113.80019,-82.97793], "fy":[-122.06784,-122.68422,-49.66953,-92.57193]}, + {"t":0.86049, "x":6.36631, "y":5.42331, "heading":-0.17344, "vx":-0.90482, "vy":1.78979, "omega":1.30874, "ax":-3.40606, "ay":-5.94718, "alpha":-5.63202, "fx":[-18.5087,-14.01026,-112.89459,-79.84221], "fy":[-122.93816,-123.19353,-51.84156,-95.33635]}, + {"t":0.89359, "x":6.3345, "y":5.47928, "heading":-0.13013, "vx":-1.01754, "vy":1.59296, "omega":1.12234, "ax":-3.26766, "ay":-6.0181, "alpha":-5.64662, "fx":[-14.598,-12.14471,-112.14658,-77.2136], "fy":[-123.49237,-123.45023,-53.54622,-97.51109]}, + {"t":0.92668, "x":6.29903, "y":5.52871, "heading":-0.09298, "vx":-1.12569, "vy":1.39378, "omega":0.93546, "ax":-3.16073, "ay":-6.07243, "alpha":-5.64472, "fx":[-11.39013,-11.10669,-111.52768,-75.00627], "fy":[-123.85151,-123.59401,-54.90255,-99.2443]}, + {"t":0.95978, "x":6.26005, "y":5.57151, "heading":-0.06202, "vx":-1.2303, "vy":1.19281, "omega":0.74865, "ax":-3.0753, "ay":-6.11447, "alpha":-5.63982, "fx":[-8.73806,-10.44929,-111.02418,-73.16992], "fy":[-124.08437,-123.68891,-55.97428,-100.62552]}, + {"t":0.99288, "x":6.21764, "y":5.60764, "heading":-0.03725, "vx":-1.33208, "vy":0.99045, "omega":0.56199, "ax":-3.00462, "ay":-6.14682, "alpha":-5.64099, "fx":[-6.54877,-9.85468,-110.63012,-71.67358], "fy":[-124.23353,-123.76939,-56.79668,-101.71249]}, + {"t":1.02597, "x":6.17191, "y":5.63705, "heading":-0.01865, "vx":-1.43152, "vy":0.78701, "omega":0.3753, "ax":-2.94386, "ay":-6.17107, "alpha":-5.65455, "fx":[-4.76095,-9.08653,-110.34342,-70.49763], "fy":[-124.32695,-123.85564,-57.38967,-102.54392]}, + {"t":1.05907, "x":6.12292, "y":5.65972, "heading":-0.00623, "vx":-1.52895, "vy":0.58278, "omega":0.18816, "ax":-2.8895, "ay":-6.18824, "alpha":-5.68518, "fx":[-3.33351,-7.96649,-110.16377,-69.6295], "fy":[-124.38375,-123.95712,-57.76462,-103.14609]}, + {"t":1.09216, "x":6.07074, "y":5.67562, "heading":0.0, "vx":-1.62458, "vy":0.37797, "omega":0.0, "ax":-4.18109, "ay":-6.2547, "alpha":0.0, "fx":[-69.12773,-69.12773,-69.12773,-69.12773], "fy":[-103.41173,-103.41173,-103.41173,-103.41173]}, + {"t":1.13719, "x":5.99335, "y":5.6863, "heading":0.0, "vx":-1.81283, "vy":0.09635, "omega":0.0, "ax":-6.64055, "ay":-3.50784, "alpha":0.0, "fx":[-109.79109,-109.79109,-109.79109,-109.79109], "fy":[-57.99662,-57.99662,-57.99662,-57.99662]}, + {"t":1.18221, "x":5.905, "y":5.68708, "heading":0.0, "vx":-2.11183, "vy":-0.06159, "omega":0.0, "ax":-7.48156, "ay":0.01882, "alpha":0.0, "fx":[-123.69597,-123.69597,-123.69597,-123.69597], "fy":[0.31117,0.31117,0.31117,0.31117]}, + {"t":1.22724, "x":5.80233, "y":5.68432, "heading":0.0, "vx":-2.44869, "vy":-0.06074, "omega":0.0, "ax":-5.56773, "ay":1.16602, "alpha":0.0, "fx":[-92.05379,-92.05379,-92.05379,-92.05379], "fy":[19.27834,19.27834,19.27834,19.27834]}, + {"t":1.27227, "x":5.68643, "y":5.68277, "heading":0.0, "vx":-2.69938, "vy":-0.00824, "omega":0.0, "ax":-0.00036, "ay":0.35393, "alpha":0.0, "fx":[-0.00598,-0.00598,-0.00598,-0.00598], "fy":[5.85167,5.85167,5.85167,5.85167]}, + {"t":1.31729, "x":5.56489, "y":5.68276, "heading":0.0, "vx":-2.69939, "vy":0.00769, "omega":0.0, "ax":0.00038, "ay":0.10267, "alpha":0.0, "fx":[0.00629,0.00629,0.00629,0.00629], "fy":[1.69748,1.69748,1.69748,1.69748]}, + {"t":1.36232, "x":5.44335, "y":5.68321, "heading":0.0, "vx":-2.69938, "vy":0.01232, "omega":0.0, "ax":0.00031, "ay":0.06184, "alpha":0.0, "fx":[0.00519,0.00519,0.00519,0.00519], "fy":[1.0224,1.0224,1.0224,1.0224]}, + {"t":1.40734, "x":5.32181, "y":5.68383, "heading":0.0, "vx":-2.69936, "vy":0.0151, "omega":0.0, "ax":0.00027, "ay":0.04555, "alpha":0.0, "fx":[0.0045,0.0045,0.0045,0.0045], "fy":[0.75311,0.75311,0.75311,0.75311]}, + {"t":1.45237, "x":5.20027, "y":5.68455, "heading":0.0, "vx":-2.69935, "vy":0.01715, "omega":0.0, "ax":0.00023, "ay":0.03473, "alpha":0.0, "fx":[0.00382,0.00382,0.00382,0.00382], "fy":[0.57425,0.57425,0.57425,0.57425]}, + {"t":1.49739, "x":5.07873, "y":5.68536, "heading":0.0, "vx":-2.69934, "vy":0.01871, "omega":0.0, "ax":0.00019, "ay":0.02668, "alpha":0.0, "fx":[0.00316,0.00316,0.00316,0.00316], "fy":[0.44119,0.44119,0.44119,0.44119]}, + {"t":1.54242, "x":4.95719, "y":5.68623, "heading":0.0, "vx":-2.69933, "vy":0.01992, "omega":0.0, "ax":0.00016, "ay":0.02056, "alpha":0.0, "fx":[0.00257,0.00257,0.00257,0.00257], "fy":[0.33992,0.33992,0.33992,0.33992]}, + {"t":1.58744, "x":4.83565, "y":5.68715, "heading":0.0, "vx":-2.69933, "vy":0.02084, "omega":0.0, "ax":0.00012, "ay":0.01586, "alpha":0.0, "fx":[0.00206,0.00206,0.00206,0.00206], "fy":[0.26225,0.26225,0.26225,0.26225]}, + {"t":1.63247, "x":4.71411, "y":5.6881, "heading":0.0, "vx":-2.69932, "vy":0.02156, "omega":0.0, "ax":0.0001, "ay":0.01225, "alpha":0.0, "fx":[0.00164,0.00164,0.00164,0.00164], "fy":[0.20249,0.20249,0.20249,0.20249]}, + {"t":1.6775, "x":4.59257, "y":5.68908, "heading":0.0, "vx":-2.69932, "vy":0.02211, "omega":0.0, "ax":0.00008, "ay":0.00946, "alpha":0.0, "fx":[0.00129,0.00129,0.00129,0.00129], "fy":[0.15639,0.15639,0.15639,0.15639]}, + {"t":1.72252, "x":4.47104, "y":5.69009, "heading":0.0, "vx":-2.69931, "vy":0.02253, "omega":0.0, "ax":0.00006, "ay":0.00731, "alpha":0.0, "fx":[0.00102,0.00102,0.00102,0.00102], "fy":[0.12078,0.12078,0.12078,0.12078]}, + {"t":1.76755, "x":4.3495, "y":5.69111, "heading":0.0, "vx":-2.69931, "vy":0.02286, "omega":0.0, "ax":0.00005, "ay":0.00564, "alpha":0.0, "fx":[0.00079,0.00079,0.00079,0.00079], "fy":[0.09325,0.09325,0.09325,0.09325]}, + {"t":1.81257, "x":4.22796, "y":5.69215, "heading":0.0, "vx":-2.69931, "vy":0.02312, "omega":0.0, "ax":0.00004, "ay":0.00435, "alpha":0.0, "fx":[0.00062,0.00062,0.00062,0.00062], "fy":[0.07193,0.07193,0.07193,0.07193]}, + {"t":1.8576, "x":4.10642, "y":5.69319, "heading":0.0, "vx":-2.6993, "vy":0.02331, "omega":0.0, "ax":0.00003, "ay":0.00335, "alpha":0.0, "fx":[0.00048,0.00048,0.00048,0.00048], "fy":[0.0554,0.0554,0.0554,0.0554]}, + {"t":1.90262, "x":3.98489, "y":5.69424, "heading":0.0, "vx":-2.6993, "vy":0.02346, "omega":0.0, "ax":0.00002, "ay":0.00257, "alpha":0.0, "fx":[0.00037,0.00037,0.00037,0.00037], "fy":[0.04257,0.04257,0.04257,0.04257]}, + {"t":1.94765, "x":3.86335, "y":5.6953, "heading":0.0, "vx":-2.6993, "vy":0.02358, "omega":0.0, "ax":0.00002, "ay":0.00197, "alpha":0.0, "fx":[0.00028,0.00028,0.00028,0.00028], "fy":[0.03256,0.03256,0.03256,0.03256]}, + {"t":1.99267, "x":3.74181, "y":5.69637, "heading":0.0, "vx":-2.6993, "vy":0.02367, "omega":0.0, "ax":0.00001, "ay":0.0015, "alpha":0.0, "fx":[0.00022,0.00022,0.00022,0.00022], "fy":[0.02479,0.02479,0.02479,0.02479]}, + {"t":2.0377, "x":3.62027, "y":5.69743, "heading":0.0, "vx":-2.6993, "vy":0.02373, "omega":0.0, "ax":0.00001, "ay":0.00117, "alpha":0.0, "fx":[0.00017,0.00017,0.00017,0.00017], "fy":[0.01928,0.01928,0.01928,0.01928]}, + {"t":2.08272, "x":3.49874, "y":5.6985, "heading":0.0, "vx":-2.6993, "vy":0.02379, "omega":0.0, "ax":0.00475, "ay":0.00127, "alpha":0.0, "fx":[0.07858,0.07858,0.07858,0.07858], "fy":[0.02097,0.02097,0.02097,0.02097]}, + {"t":2.12775, "x":3.3772, "y":5.69958, "heading":0.0, "vx":-2.69909, "vy":0.02384, "omega":0.0, "ax":7.3356, "ay":-0.0645, "alpha":0.0, "fx":[121.2827,121.2827,121.2827,121.2827], "fy":[-1.06647,-1.06647,-1.06647,-1.06647]}, + {"t":2.17278, "x":3.26311, "y":5.70059, "heading":0.0, "vx":-2.3688, "vy":0.02094, "omega":0.0, "ax":7.49108, "ay":-0.06612, "alpha":0.0, "fx":[123.85329,123.85329,123.85329,123.85329], "fy":[-1.09326,-1.09326,-1.09326,-1.09326]}, + {"t":2.2178, "x":3.16405, "y":5.70146, "heading":0.0, "vx":-2.03151, "vy":0.01796, "omega":0.0, "ax":7.50947, "ay":-0.06634, "alpha":0.0, "fx":[124.15735,124.15735,124.15735,124.15735], "fy":[-1.09685,-1.09685,-1.09685,-1.09685]}, + {"t":2.26283, "x":3.08019, "y":5.7022, "heading":0.0, "vx":-1.69339, "vy":0.01498, "omega":0.0, "ax":7.5166, "ay":-0.06644, "alpha":0.0, "fx":[124.27528,124.27528,124.27528,124.27528], "fy":[-1.09844,-1.09844,-1.09844,-1.09844]}, + {"t":2.30785, "x":3.01156, "y":5.70281, "heading":0.0, "vx":-1.35495, "vy":0.01198, "omega":0.0, "ax":7.52039, "ay":-0.06649, "alpha":0.0, "fx":[124.33793,124.33793,124.33793,124.33793], "fy":[-1.09938,-1.09938,-1.09938,-1.09938]}, + {"t":2.35288, "x":2.95818, "y":5.70328, "heading":0.0, "vx":-1.01634, "vy":0.00899, "omega":0.0, "ax":7.52274, "ay":-0.06653, "alpha":0.0, "fx":[124.37678,124.37678,124.37678,124.37678], "fy":[-1.10002,-1.10002,-1.10002,-1.10002]}, + {"t":2.3979, "x":2.92004, "y":5.70362, "heading":0.0, "vx":-0.67763, "vy":0.00599, "omega":0.0, "ax":7.52434, "ay":-0.06656, "alpha":0.0, "fx":[124.40322,124.40322,124.40322,124.40322], "fy":[-1.10047,-1.10047,-1.10047,-1.10047]}, + {"t":2.44293, "x":2.89716, "y":5.70382, "heading":0.0, "vx":-0.33884, "vy":0.003, "omega":0.0, "ax":7.5255, "ay":-0.06658, "alpha":0.0, "fx":[124.42239,124.42239,124.42239,124.42239], "fy":[-1.10081,-1.10081,-1.10081,-1.10081]}, + {"t":2.48795, "x":2.88953, "y":5.70389, "heading":0.0, "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/deploy/choreo/LNeutralToLBump.traj b/src/main/deploy/choreo/LNeutralToLBump.traj new file mode 100644 index 00000000..dc161647 --- /dev/null +++ b/src/main/deploy/choreo/LNeutralToLBump.traj @@ -0,0 +1,132 @@ +{ + "name":"LNeutralToLBump", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":8.254300117492676, "y":4.509925842285156, "heading":-1.5707963267948966, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.177672863006592, "y":5.591793060302734, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.8895323276519775, "y":5.703888893127441, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.7}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"LNeutral.x", "val":8.254300117492676}, "y":{"exp":"LNeutral.y", "val":4.509925842285156}, "heading":{"exp":"LNeutral.heading", "val":-1.5707963267948966}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.177672863006592 m", "val":6.177672863006592}, "y":{"exp":"5.591793060302734 m", "val":5.591793060302734}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"LBump.x", "val":2.8895323276519775}, "y":{"exp":"LBump.y", "val":5.703888893127441}, "heading":{"exp":"LBump.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.7}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2761996, + "y":0.2761996 + }, + "backLeft":{ + "x":-0.2761996, + "y":0.2761996 + }, + "mass":66.133767546, + "inertia":10.42384825260472, + "gearing":7.03, + "radius":0.0508, + "vmax":485.8996637552213, + "tmax":0.9, + "cof":2.25, + "bumper":{ + "front":0.44196, + "side":0.44196, + "back":0.44196 + }, + "differentialTrackWidth":0.5427218 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.05296,2.45332], + "samples":[ + {"t":0.0, "x":8.2543, "y":4.50993, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.47869, "ay":3.52562, "alpha":2.59517, "fx":[-88.00836,-105.25374,-120.34621,-114.85202], "fy":[87.86243,66.32033,31.43991,47.53983]}, + {"t":0.02925, "x":8.25153, "y":4.51143, "heading":-1.5708, "vx":-0.18949, "vy":0.10312, "omega":0.07591, "ax":-6.47516, "ay":3.52409, "alpha":2.62051, "fx":[-87.73098,-105.20542,-120.39724,-114.89311], "fy":[88.11634,66.37444,31.18858,47.38204]}, + {"t":0.0585, "x":8.24322, "y":4.51596, "heading":-1.56858, "vx":-0.37888, "vy":0.2062, "omega":0.15255, "ax":-6.47101, "ay":3.52235, "alpha":2.6493, "fx":[-87.4033,-105.1808,-120.46524,-114.90316], "fy":[88.41423,66.38683,30.85809,47.28725]}, + {"t":0.08775, "x":8.22937, "y":4.5235, "heading":-1.56411, "vx":-0.56815, "vy":0.30922, "omega":0.23004, "ax":-6.46604, "ay":3.52031, "alpha":2.68287, "fx":[-87.01009,-105.17863,-120.55267,-114.88217], "fy":[88.76876,66.35822,30.43263,47.25194]}, + {"t":0.117, "x":8.20998, "y":4.53405, "heading":-1.55739, "vx":-0.75728, "vy":0.41218, "omega":0.30851, "ax":-6.45992, "ay":3.51784, "alpha":2.72316, "fx":[-86.52911,-105.19699,-120.66286,-114.82992], "fy":[89.19807,66.28978,29.88938,47.27077]}, + {"t":0.14624, "x":8.18507, "y":4.54761, "heading":-1.54836, "vx":-0.94622, "vy":0.51508, "omega":0.38816, "ax":-6.45216, "ay":3.51472, "alpha":2.77312, "fx":[-85.92627,-105.23283,-120.8006,-114.74612], "fy":[89.72948,66.18346,29.19372,47.33497]}, + {"t":0.17549, "x":8.15463, "y":4.56417, "heading":-1.53701, "vx":-1.13494, "vy":0.61788, "omega":0.46927, "ax":-6.44196, "ay":3.51059, "alpha":2.83754, "fx":[-85.14608,-105.2811,-120.9731,-114.63057], "fy":[90.40647,66.04261,28.29016,47.42932]}, + {"t":0.20474, "x":8.11868, "y":4.58375, "heading":-1.52328, "vx":-1.32336, "vy":0.72056, "omega":0.55227, "ax":-6.42788, "ay":3.50479, "alpha":2.92457, "fx":[-84.09187,-105.33316,-121.1916,-114.48349], "fy":[91.3027,65.87335,27.08357,47.52525]}, + {"t":0.23399, "x":8.07723, "y":4.60632, "heading":-1.50713, "vx":-1.51137, "vy":0.82307, "omega":0.63781, "ax":-6.40721, "ay":3.49596, "alpha":3.04939, "fx":[-82.57931,-105.37291,-121.47439,-114.3064], "fy":[92.55301,65.68749,25.39638,47.56391]}, + {"t":0.26324, "x":8.03028, "y":4.63189, "heading":-1.48848, "vx":-1.69877, "vy":0.92532, "omega":0.727, "ax":-6.37395, "ay":3.48086, "alpha":3.24385, "fx":[-80.21052,-105.36704,-121.85148,-114.10433], "fy":[94.43151,65.51024,22.85534,47.40543]}, + {"t":0.29249, "x":7.97787, "y":4.66045, "heading":-1.46721, "vx":-1.8852, "vy":1.02713, "omega":0.82188, "ax":-6.31206, "ay":3.44968, "alpha":3.58776, "fx":[-75.94966,-105.23435,-122.3642,-113.89221], "fy":[97.58344,65.40406,18.52525,46.62763]}, + {"t":0.32174, "x":7.92003, "y":4.69196, "heading":-1.44317, "vx":-2.06982, "vy":1.12803, "omega":0.92681, "ax":-6.16077, "ay":3.35477, "alpha":4.35559, "fx":[-66.08565,-104.71319,-122.9354,-113.70045], "fy":[103.88637,65.57009,9.25812,43.14914]}, + {"t":0.35099, "x":7.85685, "y":4.72639, "heading":-1.41606, "vx":-2.25002, "vy":1.22616, "omega":1.05421, "ax":-4.51824, "ay":1.38343, "alpha":10.9459, "fx":[-24.996,-102.26112,-119.11887,-52.43228], "fy":[118.66817,67.14795,-24.3266,-69.99838]}, + {"t":0.38023, "x":7.78911, "y":4.76285, "heading":-1.38523, "vx":-2.38217, "vy":1.26662, "omega":1.37436, "ax":0.05056, "ay":0.16873, "alpha":17.37863, "fx":[64.3766,-92.88202,-66.68376,98.53319], "fy":[96.93444,69.71435,-94.46242,-61.02734]}, + {"t":0.40948, "x":7.71946, "y":4.79997, "heading":-1.34503, "vx":-2.38069, "vy":1.27155, "omega":1.88267, "ax":0.28804, "ay":0.51274, "alpha":6.39974, "fx":[26.71484,-30.93896,-18.47965,41.75315], "fy":[43.57333,32.13738,-28.46671,-13.3348]}, + {"t":0.43873, "x":7.64995, "y":4.83738, "heading":-1.28997, "vx":-2.37226, "vy":1.28655, "omega":2.06985, "ax":0.21774, "ay":0.39728, "alpha":0.11142, "fx":[3.95786,2.94698,3.24172,4.25312], "fy":[7.21658,6.92933,5.91988,6.20806]}, + {"t":0.46798, "x":7.58065, "y":4.87518, "heading":-1.22943, "vx":-2.3659, "vy":1.29817, "omega":2.07311, "ax":0.18307, "ay":0.3321, "alpha":0.31874, "fx":[3.93677,1.10265,2.11552,4.95235], "fy":[7.40486,6.40886,3.57336,4.57609]}, + {"t":0.49723, "x":7.51153, "y":4.91329, "heading":-1.16879, "vx":-2.36054, "vy":1.30788, "omega":2.08243, "ax":0.17631, "ay":0.31684, "alpha":0.55328, "fx":[4.28938,-0.51384,1.53792,6.34646], "fy":[8.64986,6.62941,1.81707,3.85776]}, + {"t":0.52648, "x":7.44257, "y":4.95168, "heading":-1.10788, "vx":-2.35538, "vy":1.31715, "omega":2.09862, "ax":0.17749, "ay":0.31606, "alpha":0.80152, "fx":[4.61982,-2.14523,1.24634,8.01708], "fy":[10.27706,6.93917,0.15235,3.5338]}, + {"t":0.55573, "x":7.37375, "y":4.99034, "heading":-1.0465, "vx":-2.35019, "vy":1.3264, "omega":2.12206, "ax":0.18146, "ay":0.32018, "alpha":1.07092, "fx":[4.83255,-3.91457,1.16751,9.91484], "fy":[12.1642,7.16978,-1.61683,3.4573]}, + {"t":0.58498, "x":7.30509, "y":5.02927, "heading":-0.98443, "vx":-2.34489, "vy":1.33576, "omega":2.15338, "ax":0.18645, "ay":0.32594, "alpha":1.37, "fx":[4.87707,-5.89607,1.29655,12.05292], "fy":[14.30072,7.24614,-3.58885,3.5979]}, + {"t":0.61422, "x":7.23658, "y":5.06848, "heading":-0.92145, "vx":-2.33943, "vy":1.34529, "omega":2.19345, "ax":0.19057, "ay":0.33003, "alpha":1.70854, "fx":[4.68695,-8.17335,1.64125,14.44811], "fy":[16.68175,7.07669,-5.87036,3.93837]}, + {"t":0.64347, "x":7.16824, "y":5.10797, "heading":-0.85729, "vx":-2.33386, "vy":1.35495, "omega":2.24343, "ax":0.18854, "ay":0.32352, "alpha":2.09607, "fx":[4.12226,-10.87858,2.17137,17.05408], "fy":[19.20635,6.45452,-8.65297,4.38744]}, + {"t":0.67272, "x":7.10006, "y":5.14774, "heading":-0.79167, "vx":-2.32834, "vy":1.36441, "omega":2.30473, "ax":0.16358, "ay":0.27836, "alpha":2.53333, "fx":[2.83203,-14.25996,2.67493,19.57092], "fy":[21.4036,4.82106,-12.36954,4.55389]}, + {"t":0.70197, "x":7.03202, "y":5.18777, "heading":-0.72426, "vx":-2.32356, "vy":1.37255, "omega":2.37883, "ax":0.063, "ay":0.10695, "alpha":2.97842, "fx":[-0.14557,-18.82607,2.29253,20.84573], "fy":[21.55836,0.60065,-18.10473,3.01872]}, + {"t":0.73122, "x":6.96409, "y":5.22796, "heading":-0.65469, "vx":-2.32172, "vy":1.37568, "omega":2.46595, "ax":-0.26978, "ay":-0.45766, "alpha":3.22492, "fx":[-7.55604,-25.64111,-1.74148,17.09726], "fy":[13.91129,-10.4518,-28.67769,-5.04889]}, + {"t":0.76047, "x":6.89607, "y":5.268, "heading":-0.58256, "vx":-2.32961, "vy":1.36229, "omega":2.56027, "ax":-1.142, "ay":-2.00891, "alpha":2.48651, "fx":[-24.3281,-35.03269,-14.6426,-1.52122], "fy":[-17.05437,-35.48017,-48.53609,-31.78636]}, + {"t":0.78972, "x":6.82744, "y":5.30698, "heading":-0.50767, "vx":-2.36301, "vy":1.30354, "omega":2.633, "ax":-1.44227, "ay":-2.72323, "alpha":-1.76915, "fx":[-19.18695,-11.4095,-29.43442,-35.35217], "fy":[-55.49612,-43.93211,-34.03669,-46.63259]}, + {"t":0.81897, "x":6.75771, "y":5.34395, "heading":-0.43066, "vx":-2.40519, "vy":1.22388, "omega":2.58125, "ax":-0.46487, "ay":-0.92744, "alpha":-3.21057, "fx":[-0.32156,12.99644,-16.00039,-27.41785], "fy":[-35.09704,-8.4952,4.83296,-22.57558]}, + {"t":0.84821, "x":6.68716, "y":5.37935, "heading":-0.35516, "vx":-2.41879, "vy":1.19676, "omega":2.48735, "ax":-0.25233, "ay":-0.51457, "alpha":-2.90614, "fx":[3.80271,13.70077,-12.59261,-21.59861], "fy":[-26.04233,-0.59104,9.1451,-16.54225]}, + {"t":0.87746, "x":6.61631, "y":5.41413, "heading":-0.28241, "vx":-2.42617, "vy":1.18171, "omega":2.40235, "ax":-0.82311, "ay":-1.72366, "alpha":-2.11606, "fx":[-6.32256,-0.65149,-21.72371,-25.73734], "fy":[-40.6153,-22.67176,-16.221,-34.48424]}, + {"t":0.90671, "x":6.54499, "y":5.44796, "heading":-0.21215, "vx":-2.45025, "vy":1.13129, "omega":2.34045, "ax":-1.05403, "ay":-2.35279, "alpha":-13.074, "fx":[17.75208,83.85759,-93.21845,-78.0981], "fy":[-105.60545,-4.11531,29.85242,-75.73042]}, + {"t":0.93596, "x":6.47287, "y":5.48004, "heading":-0.14369, "vx":-2.48108, "vy":1.06248, "omega":1.95806, "ax":-0.74147, "ay":-1.78219, "alpha":-16.41589, "fx":[34.12818,108.00134,-106.37984,-84.78598], "fy":[-113.91696,32.41491,47.95387,-84.31493]}, + {"t":0.96521, "x":6.39999, "y":5.51035, "heading":-0.08642, "vx":-2.50276, "vy":1.01035, "omega":1.47791, "ax":-0.70827, "ay":-1.8088, "alpha":-16.83764, "fx":[38.98625,109.35775,-112.51266,-82.6722], "fy":[-114.8143,42.4748,42.02714,-89.31012]}, + {"t":0.99446, "x":6.32648, "y":5.53913, "heading":-0.04319, "vx":-2.52348, "vy":0.95744, "omega":0.98543, "ax":-0.70215, "ay":-1.91447, "alpha":-16.89267, "fx":[40.91637,109.3642,-116.58548,-80.13069], "fy":[-115.19238,47.13642,34.22746,-92.78264]}, + {"t":1.02371, "x":6.25237, "y":5.56632, "heading":-0.01437, "vx":-2.54402, "vy":0.90145, "omega":0.49134, "ax":-0.68134, "ay":-2.07698, "alpha":-16.79865, "fx":[41.44023,110.50029,-119.2784,-77.72178], "fy":[-115.59742,46.91522,26.76304,-95.43933]}, + {"t":1.05296, "x":6.17767, "y":5.59179, "heading":0.0, "vx":-2.56394, "vy":0.8407, "omega":0.0, "ax":-1.88681, "ay":-7.06598, "alpha":0.0, "fx":[-31.19542,-31.19542,-31.19542,-31.19542], "fy":[-116.825,-116.825,-116.825,-116.825]}, + {"t":1.09672, "x":6.06366, "y":5.62182, "heading":0.0, "vx":-2.64651, "vy":0.53148, "omega":0.0, "ax":-0.97269, "ay":-6.76353, "alpha":0.0, "fx":[-16.08196,-16.08196,-16.08196,-16.08196], "fy":[-111.82437,-111.82437,-111.82437,-111.82437]}, + {"t":1.14048, "x":5.94692, "y":5.6386, "heading":0.0, "vx":-2.68908, "vy":0.2355, "omega":0.0, "ax":-0.21068, "ay":-3.63898, "alpha":0.0, "fx":[-3.48321,-3.48321,-3.48321,-3.48321], "fy":[-60.16492,-60.16492,-60.16492,-60.16492]}, + {"t":1.18424, "x":5.82904, "y":5.64542, "heading":0.0, "vx":-2.6983, "vy":0.07626, "omega":0.0, "ax":-0.01161, "ay":-0.47574, "alpha":0.0, "fx":[-0.192,-0.192,-0.192,-0.192], "fy":[-7.86559,-7.86559,-7.86559,-7.86559]}, + {"t":1.228, "x":5.71095, "y":5.6483, "heading":0.0, "vx":-2.69881, "vy":0.05544, "omega":0.0, "ax":-0.00096, "ay":-0.04782, "alpha":0.0, "fx":[-0.01593,-0.01593,-0.01593,-0.01593], "fy":[-0.79061,-0.79061,-0.79061,-0.79061]}, + {"t":1.27176, "x":5.59284, "y":5.65068, "heading":0.0, "vx":-2.69885, "vy":0.05335, "omega":0.0, "ax":-0.00009, "ay":-0.00479, "alpha":0.0, "fx":[-0.00156,-0.00156,-0.00156,-0.00156], "fy":[-0.07915,-0.07915,-0.07915,-0.07915]}, + {"t":1.31552, "x":5.47474, "y":5.65301, "heading":0.0, "vx":-2.69885, "vy":0.05314, "omega":0.0, "ax":-0.00001, "ay":-0.00048, "alpha":0.0, "fx":[-0.00016,-0.00016,-0.00016,-0.00016], "fy":[-0.00792,-0.00792,-0.00792,-0.00792]}, + {"t":1.35928, "x":5.35663, "y":5.65534, "heading":0.0, "vx":-2.69885, "vy":0.05311, "omega":0.0, "ax":0.0, "ay":-0.00005, "alpha":0.0, "fx":[-0.00002,-0.00002,-0.00002,-0.00002], "fy":[-0.00079,-0.00079,-0.00079,-0.00079]}, + {"t":1.40305, "x":5.23853, "y":5.65766, "heading":0.0, "vx":-2.69885, "vy":0.05311, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00008,-0.00008,-0.00008,-0.00008]}, + {"t":1.44681, "x":5.12042, "y":5.65999, "heading":0.0, "vx":-2.69885, "vy":0.05311, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.49057, "x":5.00232, "y":5.66231, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.53433, "x":4.88421, "y":5.66463, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.57809, "x":4.76611, "y":5.66696, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.62185, "x":4.648, "y":5.66928, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.66561, "x":4.5299, "y":5.67161, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.70937, "x":4.41179, "y":5.67393, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.75314, "x":4.29369, "y":5.67626, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.7969, "x":4.17558, "y":5.67858, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.84066, "x":4.05748, "y":5.6809, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.88442, "x":3.93937, "y":5.68323, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.92818, "x":3.82127, "y":5.68555, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":1.97194, "x":3.70316, "y":5.68788, "heading":0.0, "vx":-2.69885, "vy":0.05311, "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":2.0157, "x":3.58506, "y":5.6902, "heading":0.0, "vx":-2.69885, "vy":0.05311, "omega":0.0, "ax":0.00005, "ay":0.0, "alpha":0.0, "fx":[0.00078,0.00078,0.00078,0.00078], "fy":[-0.00002,-0.00002,-0.00002,-0.00002]}, + {"t":2.05946, "x":3.46695, "y":5.69253, "heading":0.0, "vx":-2.69885, "vy":0.05311, "omega":0.0, "ax":1.57451, "ay":-0.03099, "alpha":0.0, "fx":[26.03199,26.03199,26.03199,26.03199], "fy":[-0.5123,-0.5123,-0.5123,-0.5123]}, + {"t":2.10323, "x":3.35035, "y":5.69482, "heading":0.0, "vx":-2.62995, "vy":0.05176, "omega":0.0, "ax":7.4745, "ay":-0.1471, "alpha":0.0, "fx":[123.57929,123.57929,123.57929,123.57929], "fy":[-2.43199,-2.43199,-2.43199,-2.43199]}, + {"t":2.14699, "x":3.24242, "y":5.69694, "heading":0.0, "vx":-2.30286, "vy":0.04532, "omega":0.0, "ax":7.50306, "ay":-0.14766, "alpha":0.0, "fx":[124.05147,124.05147,124.05147,124.05147], "fy":[-2.44128,-2.44128,-2.44128,-2.44128]}, + {"t":2.19075, "x":3.14883, "y":5.69879, "heading":0.0, "vx":-1.97451, "vy":0.03886, "omega":0.0, "ax":7.51258, "ay":-0.14784, "alpha":0.0, "fx":[124.20887,124.20887,124.20887,124.20887], "fy":[-2.44438,-2.44438,-2.44438,-2.44438]}, + {"t":2.23451, "x":3.06962, "y":5.70034, "heading":0.0, "vx":-1.64575, "vy":0.03239, "omega":0.0, "ax":7.51734, "ay":-0.14794, "alpha":0.0, "fx":[124.28754,124.28754,124.28754,124.28754], "fy":[-2.44593,-2.44593,-2.44593,-2.44593]}, + {"t":2.27827, "x":3.00479, "y":5.70162, "heading":0.0, "vx":-1.31678, "vy":0.02591, "omega":0.0, "ax":7.5202, "ay":-0.14799, "alpha":0.0, "fx":[124.33472,124.33472,124.33472,124.33472], "fy":[-2.44685,-2.44685,-2.44685,-2.44685]}, + {"t":2.32203, "x":2.95437, "y":5.70261, "heading":0.0, "vx":-0.98769, "vy":0.01944, "omega":0.0, "ax":7.5221, "ay":-0.14803, "alpha":0.0, "fx":[124.36617,124.36617,124.36617,124.36617], "fy":[-2.44747,-2.44747,-2.44747,-2.44747]}, + {"t":2.36579, "x":2.91835, "y":5.70332, "heading":0.0, "vx":-0.65852, "vy":0.01296, "omega":0.0, "ax":7.52346, "ay":-0.14806, "alpha":0.0, "fx":[124.38862,124.38862,124.38862,124.38862], "fy":[-2.44792,-2.44792,-2.44792,-2.44792]}, + {"t":2.40955, "x":2.89674, "y":5.70375, "heading":0.0, "vx":-0.32928, "vy":0.00648, "omega":0.0, "ax":7.52447, "ay":-0.14808, "alpha":0.0, "fx":[124.40546,124.40546,124.40546,124.40546], "fy":[-2.44825,-2.44825,-2.44825,-2.44825]}, + {"t":2.45332, "x":2.88953, "y":5.70389, "heading":0.0, "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/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor index 969c1c06..d8eb73af 100644 --- a/src/main/deploy/choreo/rebuiltChoreo.chor +++ b/src/main/deploy/choreo/rebuiltChoreo.chor @@ -113,16 +113,16 @@ }, "LBump":{ "x":{ - "exp":"3.6318795680999756 m", - "val":3.631879568099976 + "exp":"2.8895323276519775 m", + "val":2.8895323276519775 }, "y":{ - "exp":"Depot.y", - "val":5.975247383117676 + "exp":"5.703888893127441 m", + "val":5.703888893127441 }, "heading":{ - "exp":"90 deg", - "val":1.5707963267948966 + "exp":"0 rad", + "val":0.0 } }, "LCleanup":{ @@ -405,6 +405,20 @@ "val":1.5707963192945 } }, + "StartingLBump":{ + "x":{ + "exp":"3.6318795680999756 m", + "val":3.631879568099976 + }, + "y":{ + "exp":"Depot.y", + "val":5.975247383117676 + }, + "heading":{ + "exp":"90 deg", + "val":1.5707963267948966 + } + }, "StartingLTrench":{ "x":{ "exp":"4.40537166595459 m", diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 0988d203..615c553a 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -116,6 +116,10 @@ public enum Path { StartingRTrenchtoDisrupt("StartingRTrenchtoDisruptR", Action.INTAKE), StartingLTrenchtoDisrupt("StartingLTrenchtoDisruptL", Action.INTAKE), + LNeutralToLBump("LNeutralToLBump", Action.SCORE_AT_END), + LBumptoLCleanup("LBumpToLCleanup", Action.INTAKE), + LCleanupToLBump("LCleanupToLBump", Action.SCORE_AT_END), + HubtoDepot("HubtoDepot", Action.DEPOT), PreDepottoDepot("PreDepottoDepot", Action.DEPOT), @@ -754,6 +758,18 @@ public Command getLeftNeutralScoreTwice() { setLeftClimb()); } + public Command getLeftBumpDoubleDipAuto() { + return createAuto( + "Left Double Dip Bump", + new Path[] { + Path.StartingLTrenchtoLNeutral, + Path.LNeutralToLBump, + Path.LBumptoLCleanup, + Path.LCleanupToLBump + }, + Commands.none()); + } + public Command getTestAuto() { final AutoRoutine routine = factory.newRoutine("test auto"); Path[] paths = {Path.RUNtoTEST, Path.RUNtoTEST, Path.RUNtoTEST, Path.RUNtoTEST}; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 03aa91da..54a55ee2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -859,6 +859,7 @@ private void addAutos() { autoChooser.addOption("X44 Sysid", indexer.runX44Sysid()); autoChooser.addOption("Right Neutral Outpost Score", autos.getRightNeutralOutpostScore()); + autoChooser.addOption("Left Double Dip Bump", autos.getLeftBumpDoubleDipAuto()); haveAutosGenerated = true; System.out.println("Done generating autos"); From 2f328f179450e0e819301fc084618defa4e73ea8 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 6 Apr 2026 21:33:06 -0700 Subject: [PATCH 02/17] agkjsflksjdflkjdls --- src/main/java/frc/robot/Autos.java | 3 +- src/main/java/frc/robot/Robot.java | 14 +- src/main/java/frc/robot/Superstructure.java | 126 ++++++++++-------- .../indexer/SpindexerSubsystem.java | 16 ++- .../java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 5 files changed, 89 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 615c553a..92946534 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -765,7 +765,8 @@ public Command getLeftBumpDoubleDipAuto() { Path.StartingLTrenchtoLNeutral, Path.LNeutralToLBump, Path.LBumptoLCleanup, - Path.LCleanupToLBump + // Path.LCleanupToLBump + Path.EndWScoreLCleanuptoLPreTrench }, Commands.none()); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 54a55ee2..fdcbbb7a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -731,11 +731,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta driver .leftBumper() - .onTrue( - Commands.runOnce( - () -> - shooter - .resetTurretToCalculatedPosition())); // , intake.zeroPivotOffCancoder())); + .onTrue(shooter.resetTurretToCalculatedPosition()); // , intake.zeroPivotOffCancoder())); operator .leftBumper() @@ -748,11 +744,9 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta operator .rightStick() .onTrue( - Commands.runOnce( - () -> - shooter - .resetTurretToPosition(shooter::getCalculatedTurretRotations) - .ignoringDisable(true))); + shooter + .resetTurretToPosition(shooter::getCalculatedTurretRotations) + .ignoringDisable(true)); driver .rightBumper() diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 7d44f086..fc2e95b1 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -554,15 +554,18 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.restExtended(), indexer.rest(), - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), climber.retract()); bindCommands( @@ -580,30 +583,36 @@ private void addCommands() { : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), climber.retract()); bindCommands( SuperState.SPIN_UP_SCORE_FLOW, intake.intake(), indexer.rest(), - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), climber.retract()); bindCommands( @@ -620,15 +629,18 @@ private void addCommands() { : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), climber.retract()); bindCommands( @@ -668,15 +680,18 @@ private void addCommands() { SuperState.SPIN_UP_SCORE_PRE_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), climber.extend()); bindCommands( @@ -693,15 +708,18 @@ private void addCommands() { : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), climber.extend()); bindCommands( diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 9b87bffc..77d71d25 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -66,11 +66,11 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO - private LoggedTunableNumber kickerSpeed = new LoggedTunableNumber("Kicker Speed", 70); + private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 50); - private LoggedTunableNumber spinnerSpeed = new LoggedTunableNumber("Spinner Speed", 13); + private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 10); - private LoggedTunableNumber x60KickSpeed = new LoggedTunableNumber("X60 Kick Speed", 20); + private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 40); private SysIdRoutine x60Sysid = new SysIdRoutine( @@ -119,9 +119,13 @@ public Command kick(DoubleSupplier flywheelRPS) { double x60Speed = flywheelLinearSpeed * 0.9 / (Math.PI * X60_KICKER_DIAMETER_INCHES); double spinnerSpeed = flywheelLinearSpeed * 0.85 / (Math.PI * SPINNER_DIAMETER_INCHES); - spinnerIO.setRollerVelocity(spinnerSpeed); - x44KickerIO.setRollerVelocity(x44Speed); - x60KickerIO.setRollerVelocity(x60Speed); + // spinnerIO.setRollerVelocity(spinnerSpeed); + // x44KickerIO.setRollerVelocity(x44Speed); + // x60KickerIO.setRollerVelocity(x60Speed); + + spinnerIO.setRollerVelocity(spinnerSpeedLTN.get()); + x44KickerIO.setRollerVelocity(x44KickerSpeedLTN.get()); + x60KickerIO.setRollerVelocity(x60KickSpeedLTN.get()); })); } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index cf57d54b..bc08c668 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -153,7 +153,7 @@ public static Rotation2d getTurretTargetRotation(Rotation2d targetRotation, Pose Rotation2d turretTargetRotation = targetRotation.minus(robotPose.getRotation()); Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); // -5 is some insane fudge factor i forgot where it's from - double turretTargetDegrees = turretTargetRotation.getDegrees() - 5; + double turretTargetDegrees = turretTargetRotation.getDegrees() - 5 - 2.5; //fudge factor of doom and despair // If its in the deadzone, clamp to nearest hardstop outOfRange = turretTargetDegrees > TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() From 3ce60e3d71492a211149226d0b6e87836d16b306 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 10:48:04 -0700 Subject: [PATCH 03/17] cassie's secret numbers --- .../frc/robot/subsystems/indexer/SpindexerSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 77d71d25..bdc0ec32 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -66,11 +66,11 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO - private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 50); + private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 70); - private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 10); + private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 11); - private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 40); + private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 50); private SysIdRoutine x60Sysid = new SysIdRoutine( From 375d6657a35c7c299c8f22395bbc8c9081738936 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 13:22:36 -0700 Subject: [PATCH 04/17] go back to old spindexer numbers and do gyro test stuff --- src/main/java/frc/robot/Robot.java | 25 +++++++++++++++++++ .../indexer/SpindexerSubsystem.java | 6 ++--- .../subsystems/swerve/SwerveSubsystem.java | 4 +++ .../java/frc/robot/utils/autoaim/AutoAim.java | 3 ++- 4 files changed, 34 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fdcbbb7a..40be6d40 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -673,6 +673,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta .onTrue( Commands.runOnce( () -> + // swerve.setGyroYaw( swerve.setYaw( DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) ? Rotation2d.kZero @@ -855,6 +856,8 @@ private void addAutos() { autoChooser.addOption("Right Neutral Outpost Score", autos.getRightNeutralOutpostScore()); autoChooser.addOption("Left Double Dip Bump", autos.getLeftBumpDoubleDipAuto()); + autoChooser.addOption("spin", spinTest()); + haveAutosGenerated = true; System.out.println("Done generating autos"); } @@ -945,6 +948,8 @@ public void robotPeriodic() { .getDistance(FieldUtils.getCurrentHubTranslation())); Logger.recordOutput( "AutoAim/Feed Target", FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose()); + + Logger.recordOutput("Wrapped gyro yaw", swerve.getRotation()); } public void updateAlerts() { @@ -1083,4 +1088,24 @@ public void testPeriodic() {} @Override public void testExit() {} + + public Command spinTest() { + return Commands.sequence( + Commands.runOnce(() -> swerve.setGyroYaw(Rotation2d.kZero)), + swerve + .driveOpenLoopFieldRelative( + () -> + new ChassisSpeeds( + 0, 0, SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed() / 4.0)) + .withTimeout(20) + // .until() + // , + // // swerve + // // .driveOpenLoopFieldRelative( + // // () -> + // // new ChassisSpeeds(0, 0, + // -SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed())) + // // .withTimeout(10) + ); + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index bdc0ec32..77d71d25 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -66,11 +66,11 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO - private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 70); + private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 50); - private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 11); + private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 10); - private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 50); + private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 40); private SysIdRoutine x60Sysid = new SysIdRoutine( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 00b4ad4c..03dc8f33 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -907,6 +907,10 @@ public void setYaw(Rotation2d newYaw) { resetPose(new Pose2d(getPose().getTranslation(), newYaw)); } + public void setGyroYaw(Rotation2d yaw) { + gyroIO.setYaw(yaw); + } + @AutoLogOutput(key = "Odometry/Velocity Robot Relative") public ChassisSpeeds getVelocityRobotRelative() { ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index bc08c668..50a0a9ea 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -153,7 +153,8 @@ public static Rotation2d getTurretTargetRotation(Rotation2d targetRotation, Pose Rotation2d turretTargetRotation = targetRotation.minus(robotPose.getRotation()); Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); // -5 is some insane fudge factor i forgot where it's from - double turretTargetDegrees = turretTargetRotation.getDegrees() - 5 - 2.5; //fudge factor of doom and despair + double turretTargetDegrees = + turretTargetRotation.getDegrees() - 5 - 2.5; // fudge factor of doom and despair // If its in the deadzone, clamp to nearest hardstop outOfRange = turretTargetDegrees > TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() From 24571e5cc3c9cd52de481a0aea6f1fc41d0ba115 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 13:24:47 -0700 Subject: [PATCH 05/17] truncate match time --- src/main/java/frc/robot/Superstructure.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index fc2e95b1..55378cc2 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -25,6 +25,7 @@ import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.ShotTrees; +import java.text.DecimalFormat; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -127,8 +128,8 @@ private double timeLeftinMatch() { } @AutoLogOutput(key = "Superstructure/Shift Timer") - private double getTimeStampLeftInShift() { - return getTimeLeftInShift(); + private String getTimeStampLeftInShift() { + return new DecimalFormat("#.#").format(getTimeLeftInShift()); } @AutoLogOutput(key = "Superstructure/Current Shift") From 5196cdfd993da70943510ede97f23022fc0b1af9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 13:45:12 -0700 Subject: [PATCH 06/17] tentatively change it back to estimtaed pose to fix that spinning thing --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 50a0a9ea..73a29fa9 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -130,7 +130,7 @@ public static ShotParams getShotParameters( // Calculate parameters accounted for imparted velocity // Rotation2d turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();\ Rotation2d turretAngle = getTargetRotation(target, lookaheadPose); - turretAngle = getTurretTargetRotation(turretAngle, lookaheadPose); + turretAngle = getTurretTargetRotation(turretAngle, estimatedPose); // Log calculated values Logger.recordOutput("LaunchCalculator/LookaheadPose", lookaheadPose); From fee1634b7ec5a649b1cb05a5e4b531edc9fa235d Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 15:44:37 -0700 Subject: [PATCH 07/17] redo swerve offsets --- .../swerve/constants/comp/R1WispSwerveConstants.java | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java index 53b5e440..76672e9d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java @@ -169,27 +169,24 @@ public Mass getMass() { return Pound.of(145.8); // 136.185702); } - // TODO: CANCODER OFFSETS @Override public ModuleConstants getFrontLeftModuleConstants() { - return new ModuleConstants( - 0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(-0.22656).plus(Rotation2d.k180deg)); + return new ModuleConstants(0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(0.272217)); } @Override public ModuleConstants getFrontRightModuleConstants() { - return new ModuleConstants(1, "Front Right", 2, 3, 1, Rotation2d.fromRotations(-0.388)); + return new ModuleConstants(1, "Front Right", 2, 3, 1, Rotation2d.fromRotations(0.11377 - 0.5)); } @Override public ModuleConstants getBackLeftModuleConstants() { - return new ModuleConstants( - 2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(-0.3).plus(Rotation2d.k180deg)); + return new ModuleConstants(2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.199463)); } @Override public ModuleConstants getBackRightModuleConstants() { - return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(0.3303)); + return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(-0.168945 - 0.5)); } @Override From 3c4f95a6435535f1e5ca1fb70fdc212d91e3f31e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 15:50:46 -0700 Subject: [PATCH 08/17] turn off deadzone correction but it seems like that still happened? --- src/main/java/frc/robot/Robot.java | 81 ++++++++++++++++-------------- 1 file changed, 42 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 40be6d40..5599ae33 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -673,11 +673,14 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta .onTrue( Commands.runOnce( () -> - // swerve.setGyroYaw( - swerve.setYaw( - DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) - ? Rotation2d.kZero - : Rotation2d.k180deg))); + swerve.setGyroYaw( + // swerve.setYaw( + // + // DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) + // ? + Rotation2d.kZero + // : Rotation2d.k180deg + ))); // autoaim (alpha) // autoAimReq @@ -767,40 +770,40 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta // driver // .leftBumper() // .and( - new Trigger(AutoAim::targetInTurretDeadzone) - .and(() -> Superstructure.getState().isAScoreState()) - .and(() -> !Superstructure.getState().isAFlowState()) - .and(() -> !Superstructure.getPoseOverride()) - .and(() -> superstructure.inScoringArea()) - .whileTrue( - swerve.faceHubComp( - () -> - -1 - * modifyJoystick(driver.getLeftY()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - () -> - -1 - * modifyJoystick(driver.getLeftX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - shooter::getTurretPosition)); - - new Trigger(AutoAim::targetInTurretDeadzone) - .and(() -> Superstructure.getState().isAFeedState()) - .and(() -> !Superstructure.getState().isAFlowState()) - .and(() -> !Superstructure.getPoseOverride()) - .and(() -> !superstructure.inScoringArea()) - .whileTrue( - swerve.faceFeedComp( - () -> - -1 - * modifyJoystick(driver.getLeftY()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - () -> - -1 - * modifyJoystick(driver.getLeftX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - shooter::getTurretPosition, - () -> Superstructure.getFeedTarget())); + // new Trigger(AutoAim::targetInTurretDeadzone) + // .and(() -> Superstructure.getState().isAScoreState()) + // .and(() -> !Superstructure.getState().isAFlowState()) + // .and(() -> !Superstructure.getPoseOverride()) + // .and(() -> superstructure.inScoringArea()) + // .whileTrue( + // swerve.faceHubComp( + // () -> + // -1 + // * modifyJoystick(driver.getLeftY()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // () -> + // -1 + // * modifyJoystick(driver.getLeftX()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // shooter::getTurretPosition)); + + // new Trigger(AutoAim::targetInTurretDeadzone) + // .and(() -> Superstructure.getState().isAFeedState()) + // .and(() -> !Superstructure.getState().isAFlowState()) + // .and(() -> !Superstructure.getPoseOverride()) + // .and(() -> !superstructure.inScoringArea()) + // .whileTrue( + // swerve.faceFeedComp( + // () -> + // -1 + // * modifyJoystick(driver.getLeftY()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // () -> + // -1 + // * modifyJoystick(driver.getLeftX()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // shooter::getTurretPosition, + // () -> Superstructure.getFeedTarget())); operator.povRight().onTrue(Commands.runOnce(() -> AutoAim.incrementFudgeFactor())); operator.povLeft().onTrue(Commands.runOnce(() -> AutoAim.decrementFudgeFactor())); From 9f9e916616b00e59ea4620bd5214b400e7174ebe Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 16:04:27 -0700 Subject: [PATCH 09/17] stator limit swerve --- .../subsystems/swerve/constants/comp/R1WispSwerveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java index 76672e9d..cca8d5ea 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java @@ -211,7 +211,7 @@ public TalonFXConfiguration getDriveConfig() { // Current limits driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveConfig.CurrentLimits.StatorCurrentLimit = 120.0; + driveConfig.CurrentLimits.StatorCurrentLimit = 90.0; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Inverts driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; From d510aa8f3866965153502bba7613238b490862df Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 20:27:18 -0700 Subject: [PATCH 10/17] recal gyro + add trim --- src/main/java/frc/robot/Robot.java | 13 +++++-------- .../constants/comp/R1WispSwerveConstants.java | 9 ++++++--- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5599ae33..51e39a21 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -673,14 +673,11 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta .onTrue( Commands.runOnce( () -> - swerve.setGyroYaw( - // swerve.setYaw( - // - // DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) - // ? - Rotation2d.kZero - // : Rotation2d.k180deg - ))); + // swerve.setGyroYaw( + swerve.setYaw( + DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) + ? Rotation2d.kZero + : Rotation2d.k180deg))); // autoaim (alpha) // autoAimReq diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java index cca8d5ea..125457ef 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java @@ -198,10 +198,13 @@ public int getGyroID() { public Pigeon2Configuration getGyroConfig() { Pigeon2Configuration config = new Pigeon2Configuration(); config.MountPose.MountPosePitch = - 0.3399254083633423; // -0.20375922322273254; // -0.420589417219162; - config.MountPose.MountPoseRoll = 179.3086395263672; // -179.13818359375; // -179.8539581298828; + 1.2711083889007568; // 0.3399254083633423; // -0.20375922322273254; // -0.420589417219162; + config.MountPose.MountPoseRoll = + 178.1570281982422; // 179.3086395263672; // -179.13818359375; // -179.8539581298828; config.MountPose.MountPoseYaw = - -90.44647216796875; // -90.93168640136719; // -86.66709899902344; + -175.72914123535156; // -90.44647216796875; // -90.93168640136719; // -86.66709899902344; + + config.GyroTrim.GyroScalarZ = -2.5; return config; } From 3c141bfd3b25b30cebd95d9d3562e41fa980adea Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 21:06:00 -0700 Subject: [PATCH 11/17] retune spinner config --- .../java/frc/robot/subsystems/indexer/SpindexerSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 77d71d25..d89c2ccf 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -185,8 +185,8 @@ public static TalonFXConfiguration getIndexerConfig() { config.Feedback.SensorToMechanismRatio = SPINNER_GEAR_RATIO; - config.Slot0.kS = 0.25181; - config.Slot0.kV = 0.66739; + config.Slot0.kS = 0.15181; + config.Slot0.kV = 0.587389988899231; config.Slot0.kA = 0.038125; config.Slot0.kP = 0.1; config.Slot0.kD = 0; From 3d525c9cd49c573d867b5f9673da77596578a445 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 21:08:14 -0700 Subject: [PATCH 12/17] x60 kicker can only go to 30 i fear --- .../java/frc/robot/subsystems/indexer/SpindexerSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index d89c2ccf..1335af75 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -70,7 +70,7 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 10); - private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 40); + private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 30); private SysIdRoutine x60Sysid = new SysIdRoutine( From 17a8019c35fa1556fe9deea74e577e49c483e705 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 21:37:54 -0700 Subject: [PATCH 13/17] they're coming up with fudge factors you could not imagine --- .../indexer/SpindexerSubsystem.java | 41 +++++++++++-------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 1335af75..a42e8ac9 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; -import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.LoggedTunableNumber; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -66,9 +65,9 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO - private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 50); + private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 49); - private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 10); + private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 12); private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 30); @@ -113,19 +112,29 @@ public Command kick(DoubleSupplier flywheelRPS) { return Commands.sequence( this.run( () -> { - double flywheelLinearSpeed = - flywheelRPS.getAsDouble() * Math.PI * TurretSubsystem.FLYWHEEL_DIAMETER_INCHES; - double x44Speed = flywheelLinearSpeed * 1 / (Math.PI * X44_KICKER_DIAMETER_INCHES); - double x60Speed = flywheelLinearSpeed * 0.9 / (Math.PI * X60_KICKER_DIAMETER_INCHES); - double spinnerSpeed = - flywheelLinearSpeed * 0.85 / (Math.PI * SPINNER_DIAMETER_INCHES); - // spinnerIO.setRollerVelocity(spinnerSpeed); - // x44KickerIO.setRollerVelocity(x44Speed); - // x60KickerIO.setRollerVelocity(x60Speed); - - spinnerIO.setRollerVelocity(spinnerSpeedLTN.get()); - x44KickerIO.setRollerVelocity(x44KickerSpeedLTN.get()); - x60KickerIO.setRollerVelocity(x60KickSpeedLTN.get()); + // double flywheelLinearSpeed = + // flywheelRPS.getAsDouble() * Math.PI * TurretSubsystem.FLYWHEEL_DIAMETER_INCHES; + // double x44Speed = flywheelLinearSpeed * 1 / (Math.PI * X44_KICKER_DIAMETER_INCHES); + // double x60Speed = flywheelLinearSpeed * 0.9 / (Math.PI * + // X60_KICKER_DIAMETER_INCHES); + // double spinnerSpeed = + // flywheelLinearSpeed * 0.85 / (Math.PI * SPINNER_DIAMETER_INCHES); + + // 27 rps of flywheel + // 49 x44 + // 12 spinner + // 30 x60 + double spinnerSpeed = flywheelRPS.getAsDouble() * 12.0 / 27.0; + double x44Speed = flywheelRPS.getAsDouble() * 49.0 / 27.0; + double x60Speed = flywheelRPS.getAsDouble() * 30.0 / 27.0; + + spinnerIO.setRollerVelocity(spinnerSpeed); + x44KickerIO.setRollerVelocity(x44Speed); + x60KickerIO.setRollerVelocity(x60Speed); + + // spinnerIO.setRollerVelocity(spinnerSpeedLTN.get()); + // x44KickerIO.setRollerVelocity(x44KickerSpeedLTN.get()); + // x60KickerIO.setRollerVelocity(x60KickSpeedLTN.get()); })); } From e282c56ca9da2113588bee9889940d25982a50b4 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 7 Apr 2026 22:00:38 -0700 Subject: [PATCH 14/17] add budget shots --- src/main/java/frc/robot/Superstructure.java | 40 ++++++++++--------- .../subsystems/shooter/TurretSubsystem.java | 24 +++++++++++ .../frc/robot/utils/autoaim/ShotTrees.java | 25 ++++++++++++ 3 files changed, 71 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 55378cc2..a64675ac 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -557,16 +557,18 @@ private void addCommands() { indexer.rest(), shooter .resetTurretToCalculatedPosition() + // .andThen( + // shooter.score( + // () -> + // AutoAim.getShotParameters( + // swerve.getPose(), + // swerve.getVelocityRobotRelative(), + // FieldUtils.getCurrentHubTranslation(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? ShotTrees.ALPHA_HUB_SHOT_TREE + // : ShotTrees.COMP_HUB_SHOT_TREE))), .andThen( - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE))), + shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), climber.retract()); bindCommands( @@ -586,16 +588,18 @@ private void addCommands() { .flywheelVelocityRotPerSec()), shooter .resetTurretToCalculatedPosition() + // .andThen( + // shooter.score( + // () -> + // AutoAim.getShotParameters( + // swerve.getPose(), + // swerve.getVelocityRobotRelative(), + // FieldUtils.getCurrentHubTranslation(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? ShotTrees.ALPHA_HUB_SHOT_TREE + // : ShotTrees.COMP_HUB_SHOT_TREE))), .andThen( - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE))), + shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), climber.retract()); bindCommands( diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 72bcd35c..f31044c6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -44,6 +44,7 @@ import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; import frc.robot.utils.FuelSim; +import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.AutoAim.ShotParams; import frc.robot.utils.autoaim.ShotTrees; @@ -76,6 +77,9 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final double CANCODER_24T_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0); public static final double CANCODER_26T_TO_TURRET_GEAR_RATIO = (26.0 / 32.0) * (10.0 / 85.0); + private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Hood Degrees", 40); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Flywheel RPS", 40); + // TODO: REDO THIS HARDSTOP WHEN FIXED?? // logged for ease of graph viewing @AutoLogOutput(key = "Shooter/Turret/Left Hardstop") @@ -637,4 +641,24 @@ public Command runFlywheelSysid() { flywheelSysid.dynamic(Direction.kForward), flywheelSysid.dynamic(Direction.kReverse)); } + + @Override + public Command testShot( + Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + // turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5)); + turretIO.setTurretPosition( + AutoAim.getShotParameters( + robotPoseSupplier.get(), + chassisSpeedsSupplier.get(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE) + .turretAngle()); + }); + } } diff --git a/src/main/java/frc/robot/utils/autoaim/ShotTrees.java b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java index 5b9342ac..8a921356 100644 --- a/src/main/java/frc/robot/utils/autoaim/ShotTrees.java +++ b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java @@ -104,6 +104,31 @@ public class ShotTrees { Rotation2d.fromDegrees(41 - 13.16), 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.13)); + + +//budget shots + COMP_HUB_SHOT_TREE.put( + 4.172776, + new ShotData( + Rotation2d.fromDegrees(36), + 25.5, + 1.13)); + + COMP_HUB_SHOT_TREE.put( + 4.512483, + new ShotData( + Rotation2d.fromDegrees(34), + 25.5, + 1.2)); + + COMP_HUB_SHOT_TREE.put( + 4.974656, + new ShotData( + Rotation2d.fromDegrees(39), + 27, + 1.2)); + + COMP_HUB_SHOT_TREE.put( 4.602258, new ShotData( From 7b2f876aff8acb1da25bd433a467e8960752f856 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 9 Apr 2026 08:49:35 -0700 Subject: [PATCH 15/17] retune shots and have the kick speed match the test speed. also tuned specifically for corners --- src/main/java/frc/robot/Superstructure.java | 57 +++++++------- .../frc/robot/subsystems/shooter/Shooter.java | 4 + .../subsystems/shooter/TurretSubsystem.java | 5 ++ .../frc/robot/utils/autoaim/ShotTrees.java | 75 ++++++++----------- 4 files changed, 69 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index a64675ac..188ec550 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -557,17 +557,16 @@ private void addCommands() { indexer.rest(), shooter .resetTurretToCalculatedPosition() - // .andThen( - // shooter.score( - // () -> - // AutoAim.getShotParameters( - // swerve.getPose(), - // swerve.getVelocityRobotRelative(), - // FieldUtils.getCurrentHubTranslation(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? ShotTrees.ALPHA_HUB_SHOT_TREE - // : ShotTrees.COMP_HUB_SHOT_TREE))), .andThen( + // shooter.score( + // () -> + // AutoAim.getShotParameters( + // swerve.getPose(), + // swerve.getVelocityRobotRelative(), + // FieldUtils.getCurrentHubTranslation(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? ShotTrees.ALPHA_HUB_SHOT_TREE + // : ShotTrees.COMP_HUB_SHOT_TREE))), shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), climber.retract()); @@ -577,28 +576,28 @@ private void addCommands() { // intake.restExtended(), indexer.kick( () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE) - .shotData() - .flywheelVelocityRotPerSec()), + // AutoAim.getShotParameters( + // swerve.getPose(), + // swerve.getVelocityRobotRelative(), + // FieldUtils.getCurrentHubTranslation(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? ShotTrees.ALPHA_HUB_SHOT_TREE + // : ShotTrees.COMP_HUB_SHOT_TREE) + // .shotData() + // .flywheelVelocityRotPerSec() + shooter.getTestVel()), shooter .resetTurretToCalculatedPosition() - // .andThen( - // shooter.score( - // () -> - // AutoAim.getShotParameters( - // swerve.getPose(), - // swerve.getVelocityRobotRelative(), - // FieldUtils.getCurrentHubTranslation(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? ShotTrees.ALPHA_HUB_SHOT_TREE - // : ShotTrees.COMP_HUB_SHOT_TREE))), .andThen( + // shooter.score( + // () -> + // AutoAim.getShotParameters( + // swerve.getPose(), + // swerve.getVelocityRobotRelative(), + // FieldUtils.getCurrentHubTranslation(), + // Robot.ROBOT_EDITION == RobotEdition.ALPHA + // ? ShotTrees.ALPHA_HUB_SHOT_TREE + // : ShotTrees.COMP_HUB_SHOT_TREE))), shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), climber.retract()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 447f7e98..0716df3e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -108,4 +108,8 @@ public default Command runFlywheelSysid() { public default Command runHoodSysid() { return Commands.none(); } + + public default double getTestVel() { + return 0; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index f31044c6..a4d3d043 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -661,4 +661,9 @@ public Command testShot( .turretAngle()); }); } + + @Override + public double getTestVel() { + return testVelocity.get(); + } } diff --git a/src/main/java/frc/robot/utils/autoaim/ShotTrees.java b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java index 8a921356..a20026e8 100644 --- a/src/main/java/frc/robot/utils/autoaim/ShotTrees.java +++ b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java @@ -105,54 +105,43 @@ public class ShotTrees { 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.13)); + // budget shots + COMP_HUB_SHOT_TREE.put(4.172776, new ShotData(Rotation2d.fromDegrees(36), 25.5, 1.13)); -//budget shots - COMP_HUB_SHOT_TREE.put( - 4.172776, - new ShotData( - Rotation2d.fromDegrees(36), - 25.5, - 1.13)); + COMP_HUB_SHOT_TREE.put(4.512483, new ShotData(Rotation2d.fromDegrees(34), 25.5, 1.2)); - COMP_HUB_SHOT_TREE.put( - 4.512483, - new ShotData( - Rotation2d.fromDegrees(34), - 25.5, - 1.2)); + COMP_HUB_SHOT_TREE.put(4.974656, new ShotData(Rotation2d.fromDegrees(39), 27, 1.2)); - COMP_HUB_SHOT_TREE.put( - 4.974656, - new ShotData( - Rotation2d.fromDegrees(39), - 27, - 1.2)); + COMP_HUB_SHOT_TREE.put(5.116029, new ShotData(Rotation2d.fromDegrees(39.5), 27.5, 1.1)); + COMP_HUB_SHOT_TREE.put(5.509574, new ShotData(Rotation2d.fromDegrees(40), 28, 1.12)); - COMP_HUB_SHOT_TREE.put( - 4.602258, - new ShotData( - Rotation2d.fromDegrees(43 - 13.16), - 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.15)); - COMP_HUB_SHOT_TREE.put( - 4.893493, - new ShotData( - Rotation2d.fromDegrees(45 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); - COMP_HUB_SHOT_TREE.put( - 5.225402, - new ShotData( - Rotation2d.fromDegrees(47 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); - COMP_HUB_SHOT_TREE.put( - 5.584793, - new ShotData( - Rotation2d.fromDegrees(49 - 13.16), - 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.17)); + COMP_HUB_SHOT_TREE.put(5.63499, new ShotData(Rotation2d.fromDegrees(40), 28, 1.12)); + + // COMP_HUB_SHOT_TREE.put( + // 4.602258, + // new ShotData( + // Rotation2d.fromDegrees(43 - 13.16), + // 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + // 1.15)); + // COMP_HUB_SHOT_TREE.put( + // 4.893493, + // new ShotData( + // Rotation2d.fromDegrees(45 - 13.16), + // 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + // 1.2)); + // COMP_HUB_SHOT_TREE.put( + // 5.225402, + // new ShotData( + // Rotation2d.fromDegrees(47 - 13.16), + // 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + // 1.2)); + // COMP_HUB_SHOT_TREE.put( + // 5.584793, + // new ShotData( + // Rotation2d.fromDegrees(49 - 13.16), + // 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + // 1.17)); } public static final InterpolatingShotTree FEED_SHOT_TREE = new InterpolatingShotTree(); From 2b69098ae20c0bdabd2e99bb89658c8627936e24 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 9 Apr 2026 08:54:53 -0700 Subject: [PATCH 16/17] switch back to autoaim --- src/main/java/frc/robot/Superstructure.java | 61 +++++++++++---------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 188ec550..0eeefcb4 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -558,16 +558,16 @@ private void addCommands() { shooter .resetTurretToCalculatedPosition() .andThen( - // shooter.score( - // () -> - // AutoAim.getShotParameters( - // swerve.getPose(), - // swerve.getVelocityRobotRelative(), - // FieldUtils.getCurrentHubTranslation(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? ShotTrees.ALPHA_HUB_SHOT_TREE - // : ShotTrees.COMP_HUB_SHOT_TREE))), - shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), + // shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), climber.retract()); bindCommands( @@ -576,29 +576,30 @@ private void addCommands() { // intake.restExtended(), indexer.kick( () -> - // AutoAim.getShotParameters( - // swerve.getPose(), - // swerve.getVelocityRobotRelative(), - // FieldUtils.getCurrentHubTranslation(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? ShotTrees.ALPHA_HUB_SHOT_TREE - // : ShotTrees.COMP_HUB_SHOT_TREE) - // .shotData() - // .flywheelVelocityRotPerSec() - shooter.getTestVel()), + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE) + .shotData() + .flywheelVelocityRotPerSec() + // shooter.getTestVel() + ), shooter .resetTurretToCalculatedPosition() .andThen( - // shooter.score( - // () -> - // AutoAim.getShotParameters( - // swerve.getPose(), - // swerve.getVelocityRobotRelative(), - // FieldUtils.getCurrentHubTranslation(), - // Robot.ROBOT_EDITION == RobotEdition.ALPHA - // ? ShotTrees.ALPHA_HUB_SHOT_TREE - // : ShotTrees.COMP_HUB_SHOT_TREE))), - shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE))), + // shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), climber.retract()); bindCommands( From be66817c8aab22edefba395049b4ffedcec14d61 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 9 Apr 2026 09:02:26 -0700 Subject: [PATCH 17/17] retrim gyro --- .../subsystems/swerve/constants/comp/R1WispSwerveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java index 125457ef..d932f8da 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java @@ -204,7 +204,7 @@ public Pigeon2Configuration getGyroConfig() { config.MountPose.MountPoseYaw = -175.72914123535156; // -90.44647216796875; // -90.93168640136719; // -86.66709899902344; - config.GyroTrim.GyroScalarZ = -2.5; + config.GyroTrim.GyroScalarZ = -4;//-2.5; return config; }