diff --git a/src/main/deploy/choreo/CollectFuel.traj b/src/main/deploy/choreo/CollectFuel.traj new file mode 100644 index 0000000..609c047 --- /dev/null +++ b/src/main/deploy/choreo/CollectFuel.traj @@ -0,0 +1,109 @@ +{ + "name":"CollectFuel", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.80876998901367, "y":3.5819289684295654, "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":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "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":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,2.43628], + "samples":[ + {"t":0.0, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":9.01865, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.4046,153.4046,153.4046,153.4046]}, + {"t":0.04972, "x":7.80877, "y":1.27211, "heading":1.5708, "vx":0.0, "vy":0.44841, "omega":0.0, "ax":0.0, "ay":9.01049, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.26592,153.26592,153.26592,153.26592]}, + {"t":0.09944, "x":7.80877, "y":1.30554, "heading":1.5708, "vx":0.0, "vy":0.89641, "omega":0.0, "ax":0.0, "ay":2.07942, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[35.37032,35.37032,35.37032,35.37032]}, + {"t":0.14916, "x":7.80877, "y":1.35268, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00006,0.00006,0.00006,0.00006]}, + {"t":0.19888, "x":7.80877, "y":1.40239, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.2486, "x":7.80877, "y":1.4521, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.29832, "x":7.80877, "y":1.50181, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.34804, "x":7.80877, "y":1.55152, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.39776, "x":7.80877, "y":1.60123, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.44748, "x":7.80877, "y":1.65094, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.4972, "x":7.80877, "y":1.70065, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.54692, "x":7.80877, "y":1.75036, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.59664, "x":7.80877, "y":1.80007, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.64636, "x":7.80877, "y":1.84978, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.69608, "x":7.80877, "y":1.89949, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.7458, "x":7.80877, "y":1.9492, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.79552, "x":7.80877, "y":1.99891, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.84524, "x":7.80877, "y":2.04862, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.89496, "x":7.80877, "y":2.09833, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.94468, "x":7.80877, "y":2.14804, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.9944, "x":7.80877, "y":2.19775, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.04412, "x":7.80877, "y":2.24746, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.09384, "x":7.80877, "y":2.29717, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.14356, "x":7.80877, "y":2.34688, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.19328, "x":7.80877, "y":2.39659, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.243, "x":7.80877, "y":2.4463, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.29272, "x":7.80877, "y":2.49601, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.34244, "x":7.80877, "y":2.54572, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.39216, "x":7.80877, "y":2.59543, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.44188, "x":7.80877, "y":2.64514, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.4916, "x":7.80877, "y":2.69485, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.54132, "x":7.80877, "y":2.74456, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.59104, "x":7.80877, "y":2.79427, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.64076, "x":7.80877, "y":2.84398, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.69048, "x":7.80877, "y":2.89369, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.7402, "x":7.80877, "y":2.9434, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.78992, "x":7.80877, "y":2.99311, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.83964, "x":7.80877, "y":3.04282, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.88936, "x":7.80877, "y":3.09253, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.93908, "x":7.80877, "y":3.14224, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.9888, "x":7.80877, "y":3.19195, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.03852, "x":7.80877, "y":3.24166, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.08824, "x":7.80877, "y":3.29137, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.13796, "x":7.80877, "y":3.34108, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.18768, "x":7.80877, "y":3.39079, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.2374, "x":7.80877, "y":3.4405, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00006,-0.00006,-0.00006,-0.00006]}, + {"t":2.28712, "x":7.80877, "y":3.49021, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":-2.07942, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-35.37032,-35.37032,-35.37032,-35.37032]}, + {"t":2.33684, "x":7.80877, "y":3.53735, "heading":1.5708, "vx":0.0, "vy":0.89641, "omega":0.0, "ax":0.0, "ay":-9.01049, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.26592,-153.26592,-153.26592,-153.26592]}, + {"t":2.38656, "x":7.80877, "y":3.57078, "heading":1.5708, "vx":0.0, "vy":0.44841, "omega":0.0, "ax":0.0, "ay":-9.01865, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.4046,-153.4046,-153.4046,-153.4046]}, + {"t":2.43628, "x":7.80877, "y":3.58193, "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/FaceFuel.traj b/src/main/deploy/choreo/FaceFuel.traj new file mode 100644 index 0000000..036bd2b --- /dev/null +++ b/src/main/deploy/choreo/FaceFuel.traj @@ -0,0 +1,101 @@ +{ + "name":"FaceFuel", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.420536518096924, "y":0.8504827618598938, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.690505504608154, "y":0.6633973717689514, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.420536518096924 m", "val":4.420536518096924}, "y":{"exp":"0.8504827618598938 m", "val":0.8504827618598938}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.690505504608154 m", "val":6.690505504608154}, "y":{"exp":"0.6633973717689514 m", "val":0.6633973717689514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.47972,0.85838], + "samples":[ + {"t":0.0, "x":4.42054, "y":0.85048, "heading":0.0, "vx":4.57021, "vy":-1.19002, "omega":-0.53764, "ax":5.35546, "ay":-1.52639, "alpha":16.06544, "fx":[75.43527,49.63132,113.35305,125.95971], "fy":[43.53688,-102.91841,-64.38465,19.91253]}, + {"t":0.02284, "x":4.52634, "y":0.8229, "heading":-0.01228, "vx":4.69255, "vy":-1.22488, "omega":-0.17064, "ax":1.47303, "ay":-0.92868, "alpha":4.92878, "fx":[12.44825,11.64662,37.2907,38.8378], "fy":[-2.2446,-29.86888,-28.62529,-2.44774]}, + {"t":0.04569, "x":4.63392, "y":0.79468, "heading":-0.01618, "vx":4.7262, "vy":-1.2461, "omega":-0.05805, "ax":0.039, "ay":-0.13689, "alpha":0.21771, "fx":[0.08842,0.06949,1.23824,1.25743], "fy":[-1.73485,-2.90328,-2.92199,-1.75365]}, + {"t":0.06853, "x":4.74189, "y":0.76617, "heading":-0.01751, "vx":4.72709, "vy":-1.24923, "omega":-0.05308, "ax":0.069, "ay":0.2581, "alpha":-0.00021, "fx":[1.17417,1.17429,1.17318,1.17306], "fy":[4.38961,4.39072,4.39084,4.38973]}, + {"t":0.09138, "x":4.8499, "y":0.7377, "heading":-0.01872, "vx":4.72867, "vy":-1.24333, "omega":-0.05308, "ax":0.19405, "ay":0.76177, "alpha":-0.02004, "fx":[3.35414,3.35576,3.24746,3.24582], "fy":[12.90291,13.00977,13.01215,12.90529]}, + {"t":0.11422, "x":4.95797, "y":0.7095, "heading":-0.01993, "vx":4.7331, "vy":-1.22593, "omega":-0.05354, "ax":0.39878, "ay":1.60605, "alpha":-0.04278, "fx":[6.90075,6.90193,6.66545,6.66415], "fy":[27.20299,27.42572,27.43388,27.21113]}, + {"t":0.13706, "x":5.0662, "y":0.68191, "heading":-0.02115, "vx":4.74221, "vy":-1.18924, "omega":-0.05452, "ax":0.70337, "ay":2.98388, "alpha":-0.08731, "fx":[12.22975,12.21604,11.69907,11.71176], "fy":[50.52859,50.94831,50.98136,50.5616]}, + {"t":0.15991, "x":5.17471, "y":0.65553, "heading":-0.0224, "vx":4.75828, "vy":-1.12108, "omega":-0.05651, "ax":1.01244, "ay":4.71998, "alpha":-0.16679, "fx":[17.82833,17.74531,16.61757,16.69394], "fy":[79.89571,80.5555,80.6749,80.01597]}, + {"t":0.18275, "x":5.28367, "y":0.63115, "heading":-0.02369, "vx":4.78141, "vy":-1.01325, "omega":-0.06032, "ax":1.14526, "ay":6.23057, "alpha":-0.29592, "fx":[20.76313,20.52399,18.21407,18.42122], "fy":[105.40399,106.25802,106.55302,105.70597]}, + {"t":0.2056, "x":5.3932, "y":0.60963, "heading":-0.02507, "vx":4.80757, "vy":-0.87092, "omega":-0.06708, "ax":1.05042, "ay":7.24413, "alpha":-0.49754, "fx":[20.27471,19.82604,15.51934,15.84905], "fy":[122.46523,123.45224,123.96315,123.00147]}, + {"t":0.22844, "x":5.5033, "y":0.59162, "heading":-0.0266, "vx":4.83157, "vy":-0.70544, "omega":-0.07845, "ax":0.80213, "ay":7.8633, "alpha":-0.74099, "fx":[17.42561,16.84046,10.01803,10.29159], "fy":[132.9315,133.93112,134.54889,133.59825]}, + {"t":0.25128, "x":5.61388, "y":0.57756, "heading":-0.02839, "vx":4.84989, "vy":-0.52581, "omega":-0.09537, "ax":0.50479, "ay":8.23677, "alpha":-0.87328, "fx":[13.11492,12.6426,4.29612,4.29151], "fy":[139.45007,140.25902,140.73913,139.97221]}, + {"t":0.27413, "x":5.7248, "y":0.5677, "heading":-0.03057, "vx":4.86142, "vy":-0.33765, "omega":-0.11532, "ax":0.2192, "ay":8.46698, "alpha":-0.74668, "fx":[7.56344,7.42682,0.07779,-0.15427], "fy":[143.68018,144.1644,144.35762,143.88144]}, + {"t":0.29697, "x":5.83591, "y":0.56219, "heading":-0.03321, "vx":4.86643, "vy":-0.14423, "omega":-0.13238, "ax":-0.07257, "ay":8.61211, "alpha":-0.44188, "fx":[0.96926,1.08941,-3.37141,-3.62459], "fy":[146.39842,146.60998,146.5837,146.366]}, + {"t":0.31982, "x":5.94706, "y":0.56114, "heading":-0.03623, "vx":4.86477, "vy":0.05251, "omega":-0.14248, "ax":-0.39818, "ay":8.69922, "alpha":-0.22205, "fx":[-5.71077,-5.54324,-7.81807,-8.01945], "fy":[147.97595,148.06482,147.96876,147.87573]}, + {"t":0.34266, "x":6.05809, "y":0.56461, "heading":-0.03949, "vx":4.85568, "vy":0.25123, "omega":-0.14755, "ax":-0.86975, "ay":8.73163, "alpha":0.26162, "fx":[-15.94524,-16.33648,-13.62033,-13.27495], "fy":[148.45334,148.33417,148.59717,148.70547]}, + {"t":0.3655, "x":6.16879, "y":0.57263, "heading":-0.04286, "vx":4.83581, "vy":0.4507, "omega":-0.14157, "ax":-3.19518, "ay":7.9198, "alpha":7.27882, "fx":[-69.86865,-102.66531,-30.07179,-14.79031], "fy":[133.9292,109.36516,145.58133,149.97826]}, + {"t":0.38835, "x":6.27842, "y":0.58499, "heading":-0.04609, "vx":4.76282, "vy":0.63162, "omega":0.02471, "ax":-4.85901, "ay":6.42534, "alpha":11.73322, "fx":[-97.59403,-145.21307,-73.02321,-14.77121], "fy":[116.62634,42.97129,126.7252,150.84986]}, + {"t":0.41119, "x":6.38596, "y":0.6011, "heading":-0.04553, "vx":4.65182, "vy":0.7784, "omega":0.29274, "ax":-6.04111, "ay":4.68691, "alpha":14.47425, "fx":[-108.23307,-151.61383,-135.92965,-15.25396], "fy":[107.4611,12.93573,47.22444,151.27065]}, + {"t":0.43404, "x":6.49065, "y":0.6201, "heading":-0.03884, "vx":4.51381, "vy":0.88547, "omega":0.62339, "ax":-6.16757, "ay":2.92907, "alpha":19.8463, "fx":[-113.96529,-152.51791,-136.60226,-16.5492], "fy":[101.72439,-2.01965,-51.84171,151.42766]}, + {"t":0.45688, "x":6.59215, "y":0.6411, "heading":-0.0246, "vx":4.37292, "vy":0.95238, "omega":1.07676, "ax":-5.90468, "ay":2.09151, "alpha":23.18554, "fx":[-117.79156,-152.31693,-113.65192,-17.98697], "fy":[97.50426,-11.40979,-95.25948,151.46925]}, + {"t":0.47972, "x":6.69051, "y":0.6634, "heading":0.0, "vx":4.23804, "vy":1.00016, "omega":1.60641, "ax":-6.63098, "ay":3.11333, "alpha":17.02397, "fx":[-119.98775,-152.17065,-144.49155,-34.51419], "fy":[94.16214,-4.89741,-25.4041,147.96647]}, + {"t":0.49429, "x":6.75152, "y":0.67829, "heading":0.02339, "vx":4.14147, "vy":1.0455, "omega":1.85434, "ax":-6.64819, "ay":3.07387, "alpha":16.89704, "fx":[-121.09847,-151.98856,-143.83319,-35.41505], "fy":[92.63348,-7.29036,-23.877,147.67661]}, + {"t":0.50885, "x":6.81113, "y":0.69385, "heading":0.0504, "vx":4.04464, "vy":1.09027, "omega":2.10042, "ax":-6.62524, "ay":3.01492, "alpha":16.97382, "fx":[-122.0057,-151.71842,-142.21501,-34.8348], "fy":[91.33771,-10.31201,-23.64093,147.74665]}, + {"t":0.52342, "x":6.86933, "y":0.71004, "heading":0.08099, "vx":3.94816, "vy":1.13417, "omega":2.34762, "ax":-6.57944, "ay":2.97462, "alpha":17.0577, "fx":[-122.87178,-151.34367,-139.68325,-33.75871], "fy":[90.0675,-13.70746,-21.90526,147.93503]}, + {"t":0.53798, "x":6.92613, "y":0.72688, "heading":0.11518, "vx":3.85234, "vy":1.17749, "omega":2.59604, "ax":-6.50216, "ay":3.00384, "alpha":17.03873, "fx":[-123.74636,-150.84577,-135.27116,-32.53593], "fy":[88.75554,-17.39857,-15.13343,148.15446]}, + {"t":0.55254, "x":6.98155, "y":0.74434, "heading":0.15299, "vx":3.75764, "vy":1.22124, "omega":2.84418, "ax":-6.36357, "ay":3.29949, "alpha":16.52947, "fx":[-124.80606,-150.25128,-125.65036,-32.26218], "fy":[87.14282,-21.01777,10.2076,148.16057]}, + {"t":0.56711, "x":7.0356, "y":0.76248, "heading":0.19441, "vx":3.66497, "vy":1.26929, "omega":3.08491, "ax":-6.16221, "ay":3.98889, "alpha":15.35585, "fx":[-126.54549,-149.76679,-106.94908,-36.00845], "fy":[84.44998,-23.27354,63.00802,147.21505]}, + {"t":0.58167, "x":7.08832, "y":0.78139, "heading":0.23934, "vx":3.57523, "vy":1.32739, "omega":3.30854, "ax":-6.0638, "ay":4.35322, "alpha":14.82281, "fx":[-128.67261,-149.43706,-91.98057,-42.48356], "fy":[80.99411,-24.28039,94.09621,145.37826]}, + {"t":0.59623, "x":7.13974, "y":0.80118, "heading":0.28752, "vx":3.48692, "vy":1.39078, "omega":3.52441, "ax":-6.03211, "ay":4.48803, "alpha":14.71054, "fx":[-130.69193,-149.15618,-81.58951,-48.98034], "fy":[77.50009,-24.85971,109.49781,143.22232]}, + {"t":0.6108, "x":7.18989, "y":0.82191, "heading":0.33885, "vx":3.39907, "vy":1.45615, "omega":3.73865, "ax":-6.02714, "ay":4.53616, "alpha":14.76291, "fx":[-132.52803,-148.91461,-73.59421,-55.04314], "fy":[74.11254,-25.10395,118.71226,140.91446]}, + {"t":0.62536, "x":7.23875, "y":0.8436, "heading":0.39329, "vx":3.31129, "vy":1.52221, "omega":3.95365, "ax":-6.18514, "ay":4.47774, "alpha":14.36653, "fx":[-134.44838,-148.86506,-75.06554,-62.45096], "fy":[70.33354,-24.10605,120.74159,137.69122]}, + {"t":0.63992, "x":7.28632, "y":0.86624, "heading":0.45087, "vx":3.22121, "vy":1.58742, "omega":4.16288, "ax":-7.18615, "ay":3.83841, "alpha":10.87935, "fx":[-138.36111,-149.8786,-115.15772,-85.5401], "fy":[61.75281,-14.72657,89.78752,124.3474]}, + {"t":0.65449, "x":7.33247, "y":0.88977, "heading":0.5115, "vx":3.11656, "vy":1.64332, "omega":4.32132, "ax":-7.68969, "ay":3.26823, "alpha":9.0586, "fx":[-141.82518,-149.89715,-128.75053,-102.72492], "fy":[52.57219,-11.62757,71.25051,110.17136]}, + {"t":0.66905, "x":7.37704, "y":0.91405, "heading":0.57443, "vx":3.00457, "vy":1.69092, "omega":4.45324, "ax":-7.83989, "ay":2.88865, "alpha":8.88256, "fx":[-144.14376,-148.96824,-131.23028,-109.07457], "fy":[44.97594,-17.43449,65.54684,103.45235]}, + {"t":0.68361, "x":7.41997, "y":0.93898, "heading":0.63929, "vx":2.89039, "vy":1.73299, "omega":4.5826, "ax":-7.93332, "ay":2.51475, "alpha":8.96028, "fx":[-146.02179,-147.44665,-132.48984,-113.81559], "fy":[37.23594,-24.89296,61.0952,97.66276]}, + {"t":0.69818, "x":7.46122, "y":0.96448, "heading":0.70603, "vx":2.77486, "vy":1.76961, "omega":4.7131, "ax":-7.99878, "ay":2.12454, "alpha":9.10587, "fx":[-147.47556,-145.29364,-133.34029,-118.11828], "fy":[29.12458,-32.90271,56.63331,91.69635]}, + {"t":0.71274, "x":7.50078, "y":0.99048, "heading":0.77467, "vx":2.65837, "vy":1.80055, "omega":4.84571, "ax":-8.03432, "ay":1.71779, "alpha":9.30198, "fx":[-148.43132,-142.40318,-133.7564,-122.05517], "fy":[20.63953,-41.2738,52.08297,85.42746]}, + {"t":0.7273, "x":7.53864, "y":1.01688, "heading":0.84524, "vx":2.54136, "vy":1.82557, "omega":4.98118, "ax":-8.03471, "ay":1.29661, "alpha":9.54262, "fx":[-148.79304,-138.65966,-133.62173,-125.59795], "fy":[11.81218,-49.84062,47.44976,78.79839]}, + {"t":0.74187, "x":7.5748, "y":1.04361, "heading":0.91778, "vx":2.42434, "vy":1.84445, "omega":5.12015, "ax":-7.99278, "ay":0.86507, "alpha":9.81886, "fx":[-148.43795,-133.95536,-132.75493,-128.67133], "fy":[2.71411,-58.35361,42.74799,71.74977]}, + {"t":0.75643, "x":7.60926, "y":1.07056, "heading":0.99235, "vx":2.30794, "vy":1.85705, "omega":5.26315, "ax":-7.89849, "ay":0.43096, "alpha":10.11318, "fx":[-147.2016,-128.2123,-130.86531,-131.12474], "fy":[-6.50589,-66.40117,38.00921,64.22006]}, + {"t":0.77099, "x":7.64204, "y":1.09765, "heading":1.069, "vx":2.19291, "vy":1.86332, "omega":5.41043, "ax":-7.73644, "ay":0.01022, "alpha":10.38784, "fx":[-144.84188,-121.42259,-127.45776,-132.65642], "fy":[-15.5348,-73.24572,33.31745,56.15833]}, + {"t":0.78556, "x":7.67315, "y":1.12479, "heading":1.14779, "vx":2.08024, "vy":1.86347, "omega":5.56172, "ax":-7.47906, "ay":-0.36048, "alpha":10.55065, "fx":[-140.92935,-113.72067,-121.61754,-132.59923], "fy":[-23.64911,-77.36692,28.91772,47.57163]}, + {"t":0.80012, "x":7.70266, "y":1.15189, "heading":1.22879, "vx":1.97132, "vy":1.85822, "omega":5.71537, "ax":-7.06049, "ay":-0.57625, "alpha":10.32425, "fx":[-134.3853,-105.43687,-111.42515,-129.14035], "fy":[-28.7881,-74.76219,25.5977,38.74501]}, + {"t":0.81468, "x":7.73062, "y":1.17889, "heading":1.31202, "vx":1.8685, "vy":1.84983, "omega":5.86573, "ax":-6.20395, "ay":-0.19504, "alpha":8.45115, "fx":[-120.02569,-95.38333,-91.82213,-114.87832], "fy":[-21.92642,-49.24136,26.23648,31.6612]}, + {"t":0.82925, "x":7.75717, "y":1.20581, "heading":1.39745, "vx":1.77815, "vy":1.84699, "omega":5.98881, "ax":-1.5998, "ay":3.03532, "alpha":-5.13844, "fx":[-9.18836,-37.0062,-46.23629,-16.41769], "fy":[64.14144,64.65288,39.85526,37.87]}, + {"t":0.84381, "x":7.7829, "y":1.23303, "heading":1.48467, "vx":1.75485, "vy":1.8912, "omega":5.91397, "ax":2.9877, "ay":3.68303, "alpha":-17.20252, "fx":[89.1827,6.44688,-5.82658,113.47703], "fy":[89.07098,118.6037,35.81823,7.09638]}, + {"t":0.85838, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":1.79836, "vy":1.94483, "omega":5.66344, "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/FuelToShot.traj b/src/main/deploy/choreo/FuelToShot.traj new file mode 100644 index 0000000..74e9b72 --- /dev/null +++ b/src/main/deploy/choreo/FuelToShot.traj @@ -0,0 +1,119 @@ +{ + "name":"FuelToShot", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":7.80876998901367, "y":3.5819289684295654, "heading":1.5707963267948966, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.657511234283447, "y":2.5716681480407715, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.3354413509368896, "y":3.0580899715423584, "heading":3.7524578917878086, "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}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.657511234283447 m", "val":4.657511234283447}, "y":{"exp":"2.5716681480407715 m", "val":2.5716681480407715}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.3354413509368896 m", "val":3.3354413509368896}, "y":{"exp":"3.0580899715423584 m", "val":3.0580899715423584}, "heading":{"exp":"215 deg", "val":3.7524578917878086}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.97983,1.55968], + "samples":[ + {"t":0.0, "x":7.80877, "y":3.58193, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.6936, "ay":-3.51677, "alpha":10.04437, "fx":[-130.15898,-88.89209,-151.40065,-153.01204], "fy":[-81.15222,-124.89633,-22.94009,-10.28804]}, + {"t":0.02882, "x":7.80558, "y":3.58047, "heading":1.5708, "vx":-0.22172, "vy":-0.10135, "omega":0.28946, "ax":-7.7102, "ay":-3.53963, "alpha":9.80757, "fx":[-130.28049,-90.28188,-151.10441,-152.92663], "fy":[-80.93624,-123.87635,-24.7011,-11.31874]}, + {"t":0.05764, "x":7.79598, "y":3.57608, "heading":1.57914, "vx":-0.44392, "vy":-0.20336, "omega":0.5721, "ax":-7.72544, "ay":-3.56164, "alpha":9.58937, "fx":[-130.20168,-91.71596,-150.91391,-152.79855], "fy":[-81.03905,-122.79516,-25.72732,-12.76813]}, + {"t":0.08646, "x":7.77998, "y":3.56874, "heading":1.59563, "vx":-0.66655, "vy":-0.306, "omega":0.84846, "ax":-7.74028, "ay":-3.5839, "alpha":9.37463, "fx":[-129.94384,-93.26599,-150.8142,-152.6156], "fy":[-81.42409,-121.59392,-26.19208,-14.63447]}, + {"t":0.11527, "x":7.75756, "y":3.55843, "heading":1.62008, "vx":-0.88962, "vy":-0.40928, "omega":1.11862, "ax":-7.75572, "ay":-3.60773, "alpha":9.14555, "fx":[-129.53146,-95.02357,-150.77393,-152.3611], "fy":[-82.04605,-120.19122,-26.30443,-16.92388]}, + {"t":0.14409, "x":7.7287, "y":3.54514, "heading":1.65231, "vx":-1.11312, "vy":-0.51325, "omega":1.38218, "ax":-7.77296, "ay":-3.63438, "alpha":8.88008, "fx":[-128.99613,-97.10226,-150.7515,-152.0131], "fy":[-82.84674,-118.4765,-26.30579,-19.65034]}, + {"t":0.17291, "x":7.6934, "y":3.52884, "heading":1.69215, "vx":-1.33713, "vy":-0.61799, "omega":1.63809, "ax":-7.79351, "ay":-3.66488, "alpha":8.55023, "fx":[-128.3823,-99.63751,-150.69864,-151.54307], "fy":[-83.74856,-116.3013,-26.46853,-22.83607]}, + {"t":0.20173, "x":7.65162, "y":3.50951, "heading":1.73935, "vx":-1.56173, "vy":-0.7236, "omega":1.8845, "ax":-7.81922, "ay":-3.69968, "alpha":8.12089, "fx":[-127.75608,-102.78149,-150.55946,-150.91376], "fy":[-84.64408,-113.46911,-27.09613,-26.51298]}, + {"t":0.23055, "x":7.60337, "y":3.48712, "heading":1.79366, "vx":-1.78707, "vy":-0.83022, "omega":2.11853, "ax":-7.85204, "ay":-3.73843, "alpha":7.54942, "fx":[-127.21897,-106.68682,-150.2625,-150.07565], "fy":[-85.37794,-109.72662,-28.52732,-30.72634]}, + {"t":0.25937, "x":7.54861, "y":3.46164, "heading":1.85472, "vx":-2.01335, "vy":-0.93796, "omega":2.3361, "ax":-7.89348, "ay":-3.77976, "alpha":6.78634, "fx":[-126.93045,-111.47098,-149.70179,-148.96009], "fy":[-85.71374,-104.76446,-31.14815,-35.54386]}, + {"t":0.28819, "x":7.48731, "y":3.43304, "heading":1.92204, "vx":-2.24083, "vy":-1.04689, "omega":2.53167, "ax":-7.94346, "ay":-3.82148, "alpha":5.7757, "fx":[-127.14646,-117.15383,-148.69919,-147.46456], "fy":[-85.26745,-98.24475,-35.4197,-41.07734]}, + {"t":0.317, "x":7.41943, "y":3.40128, "heading":1.995, "vx":-2.46975, "vy":-1.15702, "omega":2.69812, "ax":-7.99846, "ay":-3.8612, "alpha":4.4496, "fx":[-128.2876,-123.57209,-146.92973,-145.41662], "fy":[-83.35985,-89.88326,-41.93289,-47.53538]}, + {"t":0.34582, "x":7.34494, "y":3.36634, "heading":2.07275, "vx":-2.70025, "vy":-1.26829, "omega":2.82635, "ax":-8.04837, "ay":-3.89667, "alpha":2.69798, "fx":[-131.05709,-130.30796,-143.76434,-142.47229], "fy":[-78.64893,-79.61002,-51.50389,-55.36188]}, + {"t":0.37464, "x":7.26378, "y":3.32817, "heading":2.1542, "vx":-2.9322, "vy":-1.38059, "omega":2.9041, "ax":-8.06854, "ay":-3.92171, "alpha":0.25819, "fx":[-136.57829,-136.71267,-137.91629,-137.76672], "fy":[-68.08908,-67.78094,-65.30236,-65.65621]}, + {"t":0.40346, "x":7.17593, "y":3.28675, "heading":2.2379, "vx":-3.16472, "vy":-1.49361, "omega":2.91154, "ax":-7.9776, "ay":-3.90405, "alpha":-3.65444, "fx":[-145.80313,-142.09478,-126.64047,-128.24841], "fy":[-43.67842,-55.3093,-84.84427,-81.79487]}, + {"t":0.43228, "x":7.08141, "y":3.24209, "heading":2.3218, "vx":-3.39462, "vy":-1.60611, "omega":2.80622, "ax":-7.27827, "ay":-3.78453, "alpha":-11.73741, "fx":[-150.85755,-145.99941,-104.36734,-93.98054], "fy":[14.54999,-43.56808,-110.81748,-117.65928]}, + {"t":0.4611, "x":6.98056, "y":3.19423, "heading":2.40267, "vx":-3.60437, "vy":-1.71518, "omega":2.46797, "ax":-5.89335, "ay":-3.71453, "alpha":-19.63874, "fx":[-139.70958,-146.70529,-85.92334,-28.63827], "fy":[57.54382,-40.3294,-125.27404,-144.67263]}, + {"t":0.48992, "x":6.87424, "y":3.14326, "heading":2.4738, "vx":-3.77421, "vy":-1.82223, "omega":1.90201, "ax":-5.90736, "ay":-3.71124, "alpha":-18.95585, "fx":[-139.30361,-145.43621,-82.4816,-34.70892], "fy":[56.93394,-43.15418,-126.84503,-139.44293]}, + {"t":0.51873, "x":6.76302, "y":3.0892, "heading":2.52861, "vx":-3.94445, "vy":-1.92918, "omega":1.35573, "ax":-5.92284, "ay":-3.59371, "alpha":-18.31018, "fx":[-137.98948,-144.04611,-79.55219,-41.39554], "fy":[56.80953,-44.71211,-127.31451,-129.29505]}, + {"t":0.54755, "x":6.64689, "y":3.03211, "heading":2.56768, "vx":-4.11514, "vy":-2.03274, "omega":0.82805, "ax":-5.93622, "ay":-3.13679, "alpha":-17.52844, "fx":[-134.08386,-141.69436,-77.62037,-50.49484], "fy":[56.77378,-43.85045,-124.57201,-101.77499]}, + {"t":0.57637, "x":6.52583, "y":2.97223, "heading":2.59154, "vx":-4.28621, "vy":-2.12314, "omega":0.32291, "ax":-4.64913, "ay":0.24833, "alpha":-10.60684, "fx":[-86.88141,-107.07445,-74.99846,-47.3669], "fy":[48.70468,-6.31834,-47.56007,22.0697]}, + {"t":0.60519, "x":6.40038, "y":2.91115, "heading":2.60085, "vx":-4.42019, "vy":-2.11599, "omega":0.01723, "ax":-1.27182, "ay":2.64722, "alpha":-0.08014, "fx":[-21.68753,-21.95414,-21.57935,-21.31249], "fy":[45.29782,44.93531,44.75876,45.12199]}, + {"t":0.63401, "x":6.27246, "y":2.85127, "heading":2.60135, "vx":-4.45685, "vy":-2.0397, "omega":0.01492, "ax":-1.79874, "ay":4.07116, "alpha":-0.00131, "fx":[-30.59662,-30.60192,-30.59556,-30.59026], "fy":[69.25313,69.24744,69.24551,69.2512]}, + {"t":0.66283, "x":6.14328, "y":2.79418, "heading":2.60178, "vx":-4.50868, "vy":-1.92237, "omega":0.01489, "ax":-2.40441, "ay":5.95358, "alpha":0.00236, "fx":[-40.89848,-40.88578,-40.89831,-40.91102], "fy":[101.26393,101.27313,101.27345,101.26425]}, + {"t":0.69165, "x":6.01234, "y":2.74125, "heading":2.60221, "vx":-4.57798, "vy":-1.7508, "omega":0.01495, "ax":-2.55912, "ay":7.18304, "alpha":0.00862, "fx":[-43.53177,-43.47695,-43.52788,-43.5827], "fy":[122.17086,122.19933,122.19205,122.16357]}, + {"t":0.72046, "x":5.87935, "y":2.69378, "heading":2.60264, "vx":-4.65173, "vy":-1.54379, "omega":0.0152, "ax":-2.39129, "ay":7.86152, "alpha":0.01783, "fx":[-40.67763,-40.55673,-40.67281,-40.79367], "fy":[133.70933,133.75759,133.73502,133.6867]}, + {"t":0.74928, "x":5.7443, "y":2.65255, "heading":2.60308, "vx":-4.72064, "vy":-1.31724, "omega":0.01572, "ax":-2.06413, "ay":8.26281, "alpha":0.04795, "fx":[-35.10482,-34.77648,-35.1159,-35.44409], "fy":[140.52896,140.63128,140.5674,140.46442]}, + {"t":0.7781, "x":5.6074, "y":2.61802, "heading":2.60353, "vx":-4.78013, "vy":-1.07911, "omega":0.0171, "ax":-1.56861, "ay":8.53798, "alpha":0.33783, "fx":[-26.50395,-24.2608,-26.85732,-29.10415], "fy":[145.17269,145.67054,145.30313,144.7678]}, + {"t":0.80692, "x":5.46899, "y":2.59047, "heading":2.60402, "vx":-4.82533, "vy":-0.83306, "omega":0.02684, "ax":-0.38595, "ay":8.74393, "alpha":2.28139, "fx":[-2.73729,9.53713,-9.69311,-23.36662], "fy":[148.82934,149.17702,149.46441,147.45627]}, + {"t":0.83574, "x":5.32977, "y":2.57009, "heading":2.60479, "vx":-4.83645, "vy":-0.58107, "omega":0.09258, "ax":1.31637, "ay":8.58332, "alpha":5.48238, "fx":[42.53098,54.68035,10.14774,-17.79509], "fy":[143.39331,140.60363,150.77573,149.22672]}, + {"t":0.86456, "x":5.19094, "y":2.55691, "heading":2.60746, "vx":-4.79852, "vy":-0.33371, "omega":0.25058, "ax":2.46988, "ay":8.2309, "alpha":7.47844, "fx":[77.33111,77.67208,22.72291,-9.67856], "fy":[129.00388,130.27977,150.10142,150.63598]}, + {"t":0.89338, "x":5.05368, "y":2.55071, "heading":2.61468, "vx":-4.72734, "vy":-0.09651, "omega":0.46609, "ax":3.14945, "ay":7.94062, "alpha":8.49559, "fx":[96.69237,88.69928,30.44796,-1.5548], "fy":[116.11669,123.61214,149.13378,151.40787]}, + {"t":0.92219, "x":4.91875, "y":2.55123, "heading":2.62812, "vx":-4.63658, "vy":0.13233, "omega":0.71092, "ax":3.54982, "ay":7.74499, "alpha":8.99984, "fx":[107.24597,94.34079,34.98608,4.95293], "fy":[107.15322,119.73399,148.3975,151.67554]}, + {"t":0.95101, "x":4.78661, "y":2.55826, "heading":2.6486, "vx":-4.53428, "vy":0.35553, "omega":0.97029, "ax":3.79135, "ay":7.62162, "alpha":9.24356, "fx":[113.34029,97.19646,37.43086,9.99144], "fy":[101.22028,117.68403,147.98171,151.6801]}, + {"t":0.97983, "x":4.65751, "y":2.57167, "heading":2.67657, "vx":-4.42501, "vy":0.57517, "omega":1.23667, "ax":4.2214, "ay":7.23872, "alpha":10.47527, "fx":[126.70908,104.13722,41.09032,15.28254], "fy":[83.44697,111.41965,146.84396,150.80336]}, + {"t":1.00303, "x":4.55601, "y":2.58696, "heading":2.70525, "vx":-4.3271, "vy":0.74306, "omega":1.47964, "ax":4.77157, "ay":6.5771, "alpha":12.60323, "fx":[142.63058,114.26805,46.00676,21.74687], "fy":[51.6953,101.00095,145.3138,149.48859]}, + {"t":1.02622, "x":4.45693, "y":2.60596, "heading":2.73957, "vx":-4.21643, "vy":0.89561, "omega":1.77195, "ax":5.15767, "ay":5.88243, "alpha":14.57376, "fx":[150.73136,122.66761,49.25717,28.26588], "fy":[17.97027,90.63673,144.17739,147.44972]}, + {"t":1.04941, "x":4.36053, "y":2.62831, "heading":2.78067, "vx":-4.09681, "vy":1.03205, "omega":2.10998, "ax":6.2956, "ay":4.98944, "alpha":12.48901, "fx":[151.96753,130.37838,63.02833,82.97146], "fy":[-2.15085,79.14145,138.58636,123.89859]}, + {"t":1.07261, "x":4.2672, "y":2.65359, "heading":2.8296, "vx":-3.95079, "vy":1.14777, "omega":2.39964, "ax":7.85098, "ay":3.27467, "alpha":8.62329, "fx":[152.14046,139.05076,100.70041,142.27995], "fy":[-3.36145,62.58218,113.98846,49.59565]}, + {"t":1.0958, "x":4.17768, "y":2.6811, "heading":2.88526, "vx":-3.76869, "vy":1.22373, "omega":2.59965, "ax":8.57521, "ay":1.84536, "alpha":5.62484, "fx":[152.27019,146.44101,133.74192,150.99434], "fy":[-3.93398,42.43311,72.45439,14.60263]}, + {"t":1.119, "x":4.09257, "y":2.70998, "heading":2.94556, "vx":-3.5698, "vy":1.26653, "omega":2.73011, "ax":8.89874, "ay":0.62349, "alpha":2.56348, "fx":[152.34596,151.25514,149.65064,152.20829], "fy":[-4.95498,19.2054,28.17653,-0.00569]}, + {"t":1.14219, "x":4.01217, "y":2.73952, "heading":3.00888, "vx":-3.3634, "vy":1.28099, "omega":2.78957, "ax":8.95838, "ay":-0.35592, "alpha":0.12107, "fx":[152.35534,152.40748,152.40318,152.35161], "fy":[-6.75369,-5.47453,-5.34903,-6.63889]}, + {"t":1.16538, "x":3.93657, "y":2.76913, "heading":3.07358, "vx":-3.15562, "vy":1.27273, "omega":2.79238, "ax":8.88675, "ay":-1.10894, "alpha":-1.76699, "fx":[152.268,149.74407,150.25726,152.37469], "fy":[-9.50005,-29.23734,-27.10562,-9.60785]}, + {"t":1.18858, "x":3.86577, "y":2.79836, "heading":3.13835, "vx":-2.9495, "vy":1.24701, "omega":2.7514, "ax":8.75967, "ay":-1.68515, "alpha":-3.27007, "fx":[152.03042,144.22711,147.31371,152.42675], "fy":[-13.28184,-50.02643,-40.6703,-10.67714]}, + {"t":1.21177, "x":3.79971, "y":2.82683, "heading":-3.08102, "vx":-2.74633, "vy":1.20793, "omega":2.67555, "ax":8.61627, "ay":-2.1283, "alpha":-4.45378, "fx":[151.56525,137.32915,144.83256,152.51414], "fy":[-18.12784,-66.87868,-49.10848,-10.69214]}, + {"t":1.23496, "x":3.73833, "y":2.85427, "heading":-3.01897, "vx":-2.54649, "vy":1.15856, "omega":2.57225, "ax":8.47658, "ay":-2.47416, "alpha":-5.35339, "fx":[150.7761,130.30728,143.03316,152.62012], "fy":[-24.00686,-79.86837,-54.35555,-10.10828]}, + {"t":1.25816, "x":3.68155, "y":2.88048, "heading":-2.95931, "vx":-2.34988, "vy":1.10118, "omega":2.44808, "ax":8.34865, "ay":-2.75133, "alpha":-6.01859, "fx":[149.55887,123.90832,141.8365,152.72918], "fy":[-30.82135,-89.60971,-57.57371,-9.19245]}, + {"t":1.28135, "x":3.62929, "y":2.90528, "heading":-2.90252, "vx":-2.15625, "vy":1.03736, "omega":2.30849, "ax":8.23343, "ay":-2.98096, "alpha":-6.5122, "fx":[147.81902,118.4376,141.10527,152.8311], "fy":[-38.40527,-96.83368,-59.47203,-8.11037]}, + {"t":1.30455, "x":3.58149, "y":2.92853, "heading":-2.84898, "vx":-1.96528, "vy":0.96822, "omega":2.15745, "ax":8.12855, "ay":-3.17733, "alpha":-6.89681, "fx":[145.49214,113.93265,140.71224,152.92043], "fy":[-46.53076,-102.1839,-60.49818,-6.9692]}, + {"t":1.32774, "x":3.5381, "y":2.95014, "heading":-2.79894, "vx":-1.77675, "vy":0.89453, "omega":1.99748, "ax":8.03095, "ay":-3.34918, "alpha":-7.2246, "fx":[142.56309,110.30341,140.55499,152.99513], "fy":[-54.92559,-106.163,-60.94587,-5.84022]}, + {"t":1.35093, "x":3.49905, "y":2.96998, "heading":-2.75261, "vx":-1.59048, "vy":0.81685, "omega":1.82992, "ax":7.93815, "ay":-3.50141, "alpha":-7.5326, "fx":[139.07752,107.41469,140.5549,153.05537], "fy":[-63.30052,-109.14393,-61.01541,-4.77179]}, + {"t":1.37413, "x":3.46429, "y":2.98799, "heading":-2.71017, "vx":-1.40636, "vy":0.73564, "omega":1.65521, "ax":7.84879, "ay":-3.63654, "alpha":-7.84202, "fx":[135.1411,105.12639,140.65252,153.10264], "fy":[-71.38147,-111.39879,-60.84882,-3.79677]}, + {"t":1.39732, "x":3.43379, "y":3.00407, "heading":-2.67178, "vx":-1.22432, "vy":0.65129, "omega":1.47332, "ax":7.76259, "ay":-3.75598, "alpha":-8.1604, "fx":[130.90595,103.31008,140.80288,153.13905], "fy":[-78.93868,-113.12645,-60.55063,-2.93702]}, + {"t":1.42052, "x":3.40748, "y":3.01817, "heading":-2.63761, "vx":-1.04427, "vy":0.56418, "omega":1.28405, "ax":7.68004, "ay":-3.86083, "alpha":-8.48527, "fx":[126.54816,101.85399,140.97172,153.16696], "fy":[-85.80579,-114.47403,-60.20052,-2.20603]}, + {"t":1.44371, "x":3.38532, "y":3.03021, "heading":-2.60783, "vx":-0.86614, "vy":0.47463, "omega":1.08724, "ax":7.60195, "ay":-3.95229, "alpha":-8.80826, "fx":[122.24363,100.66316,141.13263,153.18858], "fy":[-91.885,-115.55228,-59.86105,-1.61077]}, + {"t":1.4669, "x":3.36728, "y":3.04016, "heading":-2.58261, "vx":-0.68982, "vy":0.38296, "omega":0.88294, "ax":7.52919, "ay":-4.03183, "alpha":-9.11872, "fx":[118.14887,99.65775,141.26496,153.20584], "fy":[-97.13999,-116.44596,-59.58244,-1.15304]}, + {"t":1.4901, "x":3.3533, "y":3.04796, "heading":-2.56213, "vx":-0.51519, "vy":0.28945, "omega":0.67144, "ax":7.46241, "ay":-4.10117, "alpha":-9.4063, "fx":[114.39015,98.77106,141.3523,153.22029], "fy":[-101.58107,-117.22097,-59.40562,-0.83093]}, + {"t":1.51329, "x":3.34336, "y":3.05357, "heading":-2.54656, "vx":-0.34211, "vy":0.19432, "omega":0.45328, "ax":7.40199, "ay":-4.16205, "alpha":-9.66237, "fx":[111.06109,97.94758,141.3813,153.23301], "fy":[-105.24789,-117.92895,-59.36414,-0.64021]}, + {"t":1.53649, "x":3.33742, "y":3.05696, "heading":-2.53604, "vx":-0.17043, "vy":0.09779, "omega":0.22917, "ax":7.34806, "ay":-4.2162, "alpha":-9.8805, "fx":[108.22654,97.14143,141.34079,153.2447], "fy":[-108.19366,-118.61043,-59.48542,-0.57592]}, + {"t":1.55968, "x":3.33544, "y":3.05809, "heading":-2.53073, "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/StartCollectShoot.chor b/src/main/deploy/choreo/StartCollectShoot.chor new file mode 100644 index 0000000..9147e2c --- /dev/null +++ b/src/main/deploy/choreo/StartCollectShoot.chor @@ -0,0 +1,84 @@ +{ + "name":"StartCollectShoot", + "version":2, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c13ce1a..c7a6944 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,9 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.commands.DriveCommands; +import frc.robot.commands.autos.AutoConstants; import frc.robot.commands.autos.DriveTesting; +import frc.robot.commands.autos.StartCollectShoot; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; @@ -33,7 +35,10 @@ import frc.robot.subsystems.feeder.FeederConstants; import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncher; import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncherConstants; +import frc.robot.util.AllianceFlipUtil; import frc.robot.util.CANHealthLogger; +import java.util.Arrays; +import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -130,7 +135,14 @@ public RobotContainer() { drive::setPose, // Function that resets the current robot pose to the provided Pose2d drive::followTrajectory, // The drive subsystem trajectory follower true, // If alliance flipping should be enabled - drive // The drive subsystem + drive, + (sample, isStart) -> { + Logger.recordOutput( + "TrajectoryFollowing/ActiveTrajectory", + Arrays.stream(sample.getPoses()) + .map(AllianceFlipUtil::apply) + .toArray(Pose2d[]::new)); + } // The drive subsystem ); // Set up auto routines @@ -138,7 +150,9 @@ public RobotContainer() { autoChooser.addRoutine( "Drive Straight Test", () -> DriveTesting.getRoutine(autoFactory, drive)); - + autoChooser.addRoutine( + "Start, Collect, and Shoot", + () -> StartCollectShoot.getRoutine(autoFactory, drive, intakeAndShooter, feederAndIndexer)); RobotModeTriggers.autonomous() .whileTrue( Commands.sequence( @@ -168,6 +182,9 @@ public RobotContainer() { // Send to elastic SmartDashboard.putData("Auto Chooser", autoChooser); + // Initialize auto tunable numbers so they appear on dashboard + AutoConstants.initialize(); + // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/autos/AutoConstants.java b/src/main/java/frc/robot/commands/autos/AutoConstants.java new file mode 100644 index 0000000..39bb6e2 --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/AutoConstants.java @@ -0,0 +1,16 @@ +package frc.robot.commands.autos; + +import frc.robot.util.LoggedTunableNumber; + +public class AutoConstants { + public static final LoggedTunableNumber launchDuration = + new LoggedTunableNumber("Auto/Launch Length", 10); + public static final LoggedTunableNumber autoSpeed = + new LoggedTunableNumber("Auto/Auto Speed", 1.0); + + /** Initialize all auto constants by accessing them once so they appear on dashboard */ + public static void initialize() { + autoSpeed.get(); + launchDuration.get(); + } +} diff --git a/src/main/java/frc/robot/commands/autos/StartCollectShoot.java b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java new file mode 100644 index 0000000..4d27600 --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java @@ -0,0 +1,66 @@ +package frc.robot.commands.autos; + +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants; +import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncher; +import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncherConstants; + +public class StartCollectShoot { + public static AutoRoutine getRoutine( + AutoFactory factory, + Drive driveSubsystem, + IntakeAndLauncher intakeAndShooter, + Feeder feederAndIndexer) { + AutoRoutine routine = factory.newRoutine("Start Collect Shoot"); + + AutoTrajectory faceFuel = routine.trajectory("FaceFuel"); + AutoTrajectory collectFuel = routine.trajectory("CollectFuel"); + AutoTrajectory fuelToShot = routine.trajectory("FuelToShot"); + + routine + .active() + .onTrue( + Commands.sequence( + new InstantCommand(() -> Commands.print("Going to fuel")), + faceFuel.resetOdometry(), + faceFuel.cmd())); + + faceFuel + .done() + .onTrue( + Commands.sequence( + new InstantCommand(() -> Commands.print("Starting intake and collecting fuel")), + Commands.parallel( + intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.intakeVoltage.get()), + collectFuel.cmd()))); + collectFuel + .done() + .onTrue( + Commands.sequence( + new InstantCommand(() -> Commands.print("Moving to shooting position")), + fuelToShot.cmd())); + fuelToShot + .done() + .onTrue( + Commands.sequence( + new InstantCommand(() -> Commands.print("Starting launch sequence")), + Commands.race( + Commands.parallel( + intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.launchVoltage.get()), + Commands.sequence( + Commands.waitSeconds(IntakeAndLauncherConstants.launchWarmUpTime.get()), + Commands.sequence( + new InstantCommand( + () -> Commands.print("Feeding fuel to launcher")), + feederAndIndexer.voltageCmd(FeederConstants.launchVoltage.get())))), + Commands.waitSeconds(AutoConstants.launchDuration.get())))); + + return routine; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5eff19f..7b6bca7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -362,11 +362,16 @@ public static Translation2d[] getModuleTranslations() { public void followTrajectory(SwerveSample sample) { // Get the current pose of the robot Pose2d pose = getPose(); - + Pose2d samplePose2d = + new Pose2d( + new Translation2d(sample.x, sample.y), new Rotation2d(Radians.of(sample.heading))); + Logger.recordOutput("TrajectoryFollowing/pose2d", samplePose2d); Logger.recordOutput("TrajectoryFollowing/posex", pose.getX()); Logger.recordOutput("TrajectoryFollowing/samplex", sample.x); + Logger.recordOutput("TrajectoryFollowing/posey", pose.getY()); Logger.recordOutput("TrajectoryFollowing/sampley", sample.y); + Logger.recordOutput( "TrajectoryFollowing/heading", pose.getRotation().getRadians() + Math.PI * 2); Logger.recordOutput( diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 78369c9..13aba9b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -13,6 +13,7 @@ public final class AutoConstants { public static final LoggedTunableGains headingTrajectoryController = new LoggedTunableGains("headingTraj", new ControlGains().p(3).d(0.0)); } - public final static LoggedTunableNumber speedScalar = - new LoggedTunableNumber("Drive/Drive Speed Scalar", 1); + + public static final LoggedTunableNumber speedScalar = + new LoggedTunableNumber("Drive/Drive Speed Scalar", 1); } diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..f01b082 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,70 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public class AllianceFlipUtil { + public static double fieldWidth = Units.feetToMeters(26.0) + Units.inchesToMeters(5.0); + public static double fieldLength = Units.feetToMeters(57.0) + Units.inchesToMeters(6.875); + + public static double applyX(double x) { + return shouldFlip() ? fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Rotation2d dontApply(Rotation2d rotation) { + return rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Pose2d flip(Pose2d pose) { + return new Pose2d( + new Translation2d(fieldLength - pose.getX(), fieldWidth - pose.getY()), + pose.getRotation().rotateBy(Rotation2d.kPi)); + } + + public static Translation2d flip(Translation2d pose) { + return new Translation2d(fieldLength - pose.getX(), fieldWidth - pose.getY()); + } + + public static Rotation2d flip(Rotation2d pose) { + return pose.rotateBy(Rotation2d.kPi); + } + + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } + + public static Command apply(Command ifFlip, Command otherwise) { + return Commands.either(ifFlip, otherwise, AllianceFlipUtil::shouldFlip); + } +}