cheerios

diff --git a/src/main/deploy/choreo/project.chor b/src/main/deploy/choreo/project.chor
new file mode 100644 (file)
index 0000000..4573de4
--- /dev/null
+++ b/src/main/deploy/choreo/project.chor
@@ -0,0 +1,84 @@
+{
+ "name":"project",
+ "version":2,
+ "type":"Swerve",
+ "variables":{
+  "expressions":{},
+  "poses":{}
+ },
+ "config":{
+  "frontLeft":{
+   "x":{
+    "exp":"20.75 in",
+    "val":0.52705
+   },
+   "y":{
+    "exp":"20.75 in",
+    "val":0.52705
+   }
+  },
+  "backLeft":{
+   "x":{
+    "exp":"-20.75 in",
+    "val":-0.52705
+   },
+   "y":{
+    "exp":"20.75 in",
+    "val":0.52705
+   }
+  },
+  "mass":{
+   "exp":"125 lbs",
+   "val":56.69904625
+  },
+  "inertia":{
+   "exp":"7 kg m ^ 2",
+   "val":7.0
+  },
+  "gearing":{
+   "exp":"7.03125",
+   "val":7.03125
+  },
+  "radius":{
+   "exp":"2 in",
+   "val":0.0508
+  },
+  "vmax":{
+   "exp":"6000 RPM",
+   "val":628.3185307179587
+  },
+  "tmax":{
+   "exp":"7 N * m",
+   "val":7.0
+  },
+  "cof":{
+   "exp":"1",
+   "val":1.0
+  },
+  "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":"home/moo/Projects/robotics/FRC2026/src/main/java/choreo",
+  "genVars":true,
+  "genTrajData":true,
+  "useChoreoLib":true
+ }
+}

diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj
new file mode 100644 (file)
index 0000000..eaeaa7f
--- /dev/null
+++ b/src/main/deploy/choreo/test.traj
@@ -0,0 +1,144 @@
+{
+ "name":"test",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":4.420742511749268, "y":7.652679920196533, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":7.575807094573975, "y":6.538276672363281, "heading":-1.2256388132346097, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.866848945617676, "y":4.889683723449707, "heading":-2.9967794902671, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.89302921295166, "y":7.210275173187256, "heading":0.4241938953287057, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.742519855499267, "y":7.653775691986084, "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":true},
+    {"from":0, "to":4, "data":{"type":"KeepOutCircle", "props":{"x":4.889016749337316, "y":5.995364322792739, "r":1.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"4.420742511749268 m", "val":4.420742511749268}, "y":{"exp":"7.652679920196533 m", "val":7.652679920196533}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"7.575807094573975 m", "val":7.575807094573975}, "y":{"exp":"6.538276672363281 m", "val":6.538276672363281}, "heading":{"exp":"-1.2256388132346097 rad", "val":-1.2256388132346097}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.866848945617676 m", "val":6.866848945617676}, "y":{"exp":"4.889683723449707 m", "val":4.889683723449707}, "heading":{"exp":"-2.9967794902671 rad", "val":-2.9967794902671}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.89302921295166 m", "val":5.89302921295166}, "y":{"exp":"7.210275173187256 m", "val":7.210275173187256}, "heading":{"exp":"0.4241938953287057 rad", "val":0.4241938953287057}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"3.7425198554992676 m", "val":3.742519855499267}, "y":{"exp":"7.653775691986084 m", "val":7.653775691986084}, "heading":{"exp":"0 deg", "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":true},
+    {"from":0, "to":4, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.889016749337316 m", "val":4.889016749337316}, "y":{"exp":"5.995364322792739 m", "val":5.995364322792739}, "r":{"exp":"1 m", "val":1.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.52705,
+    "y":0.52705
+   },
+   "backLeft":{
+    "x":-0.52705,
+    "y":0.52705
+   },
+   "mass":56.69904625,
+   "inertia":7.0,
+   "gearing":7.03125,
+   "radius":0.0508,
+   "vmax":628.3185307179587,
+   "tmax":7.0,
+   "cof":1.0,
+   "bumper":{
+    "front":0.4064,
+    "side":0.4064,
+    "back":0.4064
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.08751,1.76176,2.77777,3.53655],
+  "samples":[
+    {"t":0.0, "x":4.42074, "y":7.65268, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.08942, "ay":-3.06425, "alpha":-8.48469, "fx":[126.41632,138.01365,136.91842,114.01302], "fy":[-57.34431,-14.80495,-22.46969,-79.12099]},
+    {"t":0.04943, "x":4.43185, "y":7.64894, "heading":-0.01037, "vx":0.44931, "vy":-0.15147, "omega":-0.41942, "ax":9.15675, "ay":-2.82924, "alpha":-8.64188, "fx":[127.36088,138.28988,137.65904,115.86912], "fy":[-55.17339,-11.75577,-17.15358,-76.3326]},
+    {"t":0.09886, "x":4.46525, "y":7.63799, "heading":-0.04166, "vx":0.90195, "vy":-0.29133, "omega":-0.84661, "ax":9.23915, "ay":-2.50923, "alpha":-8.83519, "fx":[128.41465,138.52468,138.36766,118.54428], "fy":[-52.61294,-8.18141,-9.43741,-72.03906]},
+    {"t":0.1483, "x":4.52112, "y":7.62052, "heading":-0.0943, "vx":1.35866, "vy":-0.41537, "omega":-1.28335, "ax":9.34376, "ay":-2.04422, "alpha":-8.99064, "fx":[129.84412,138.68733,138.63573,122.61528], "fy":[-48.8787,-3.57987,1.30178,-64.7484]},
+    {"t":0.19773, "x":4.5997, "y":7.59749, "heading":-0.16873, "vx":1.82055, "vy":-0.51642, "omega":-1.72778, "ax":9.47809, "ay":-1.30627, "alpha":-8.83915, "fx":[132.16322,138.64705,137.68186,128.90636], "fy":[-42.01009,3.08477,15.694,-50.8327]},
+    {"t":0.24716, "x":4.70127, "y":7.57037, "heading":-0.26493, "vx":2.28907, "vy":-0.58099, "omega":-2.16472, "ax":9.68099, "ay":-0.32693, "alpha":-5.53102, "fx":[136.42784,138.39049,137.15484,136.92947], "fy":[-24.13963,6.9237,19.04577,-20.36648]},
+    {"t":0.29659, "x":4.82625, "y":7.54125, "heading":-0.3787, "vx":2.76763, "vy":-0.59715, "omega":-2.43813, "ax":9.3341, "ay":-0.23898, "alpha":12.26548, "fx":[127.5346,132.77227,131.24764,137.68011], "fy":[53.10607,-37.08298,-43.73546,14.16235]},
+    {"t":0.34603, "x":4.97447, "y":7.51144, "heading":-0.48424, "vx":3.22903, "vy":-0.60896, "omega":-1.83182, "ax":9.12312, "ay":-1.1359, "alpha":13.84454, "fx":[129.09162,125.69681,124.39202,138.09167], "fy":[47.82605,-54.63319,-59.74851,2.15124]},
+    {"t":0.39546, "x":5.14523, "y":7.47995, "heading":-0.55787, "vx":3.68001, "vy":-0.66511, "omega":-1.14745, "ax":9.07677, "ay":-1.16032, "alpha":13.21737, "fx":[128.6834,126.13999,122.53382,137.28702], "fy":[45.49224,-48.41948,-61.33977,-1.52209]},
+    {"t":0.44489, "x":5.33824, "y":7.44566, "heading":-0.59844, "vx":4.1287, "vy":-0.72247, "omega":-0.49409, "ax":7.16928, "ay":1.02008, "alpha":9.85817, "fx":[95.30167,92.1739,106.42858,112.58724], "fy":[50.00973,10.87099,-20.45832,17.41491]},
+    {"t":0.49432, "x":5.55109, "y":7.41119, "heading":-0.61082, "vx":4.48309, "vy":-0.67205, "omega":-0.00677, "ax":0.08357, "ay":0.51669, "alpha":0.01071, "fx":[1.1802,1.15978,1.189,1.20942], "fy":[7.34863,7.31961,7.29925,7.32828]},
+    {"t":0.54376, "x":5.7728, "y":7.3786, "heading":-0.61115, "vx":4.48722, "vy":-0.6465, "omega":-0.00624, "ax":-0.01647, "ay":-0.11265, "alpha":-0.00027, "fx":[-0.23336,-0.23285,-0.23358,-0.23409], "fy":[-1.59741,-1.59668,-1.59617,-1.5969]},
+    {"t":0.59319, "x":5.99459, "y":7.3465, "heading":-0.61145, "vx":4.48641, "vy":-0.65207, "omega":-0.00626, "ax":-0.20257, "ay":-1.32077, "alpha":-0.00164, "fx":[-2.87074,-2.86759,-2.87214,-2.87529], "fy":[-18.72533,-18.72098,-18.71789,-18.72225]},
+    {"t":0.64262, "x":6.21612, "y":7.31266, "heading":-0.61177, "vx":4.4764, "vy":-0.71736, "omega":-0.00634, "ax":-1.28159, "ay":-6.44797, "alpha":-0.01702, "fx":[-18.14973,-18.11171,-18.18283,-18.22082], "fy":[-91.42268,-91.40123,-91.3743,-91.39575]},
+    {"t":0.69205, "x":6.43583, "y":7.26932, "heading":-0.6121, "vx":4.41304, "vy":-1.0361, "omega":-0.00718, "ax":-3.61548, "ay":-8.38273, "alpha":-1.77388, "fx":[-47.70725,-44.96285,-55.184,-57.13995], "fy":[-120.89275,-121.15947,-116.66385,-116.57671]},
+    {"t":0.74149, "x":6.64956, "y":7.20786, "heading":-0.61462, "vx":4.23432, "vy":-1.45048, "omega":-0.09487, "ax":-8.64362, "ay":-3.46432, "alpha":-10.64977, "fx":[-104.37043,-118.96385,-136.56389,-130.18691], "fy":[-88.52599,-64.42068,-0.16744,-43.30963]},
+    {"t":0.79092, "x":6.84831, "y":7.13192, "heading":-0.63232, "vx":3.80705, "vy":-1.62173, "omega":-0.62131, "ax":-8.98717, "ay":-2.64246, "alpha":-10.95232, "fx":[-110.83771,-127.91039,-136.94568,-133.87017], "fy":[-81.90209,-48.83712,14.53281,-33.61877]},
+    {"t":0.84035, "x":7.02552, "y":7.04853, "heading":-0.67642, "vx":3.36279, "vy":-1.75235, "omega":-1.16271, "ax":-9.12308, "ay":-2.42652, "alpha":-10.257, "fx":[-115.15669,-129.74033,-137.32771,-135.04532], "fy":[-76.35788,-45.94007,14.57362,-29.8573]},
+    {"t":0.88978, "x":7.18061, "y":6.95894, "heading":-0.74642, "vx":2.91181, "vy":-1.8723, "omega":-1.66974, "ax":-9.20823, "ay":-2.42393, "alpha":-9.09933, "fx":[-118.95916,-129.54452,-138.00479,-135.58934], "fy":[-70.65637,-47.65299,8.84428,-27.96917]},
+    {"t":0.93921, "x":7.3133, "y":6.86343, "heading":-0.84008, "vx":2.45663, "vy":-1.99212, "omega":-2.11954, "ax":-9.25723, "ay":-2.559, "alpha":-7.58981, "fx":[-122.17396,-128.61973,-138.39838,-135.6841], "fy":[-65.18638,-50.76972,-1.22828,-27.90869]},
+    {"t":0.98865, "x":7.42342, "y":6.76183, "heading":-0.95413, "vx":1.99902, "vy":-2.11862, "omega":-2.49472, "ax":-9.26721, "ay":-2.80455, "alpha":-5.6836, "fx":[-124.8165,-127.76542,-137.65808,-135.20188], "fy":[-60.15071,-53.30954,-15.15644,-30.39863]},
+    {"t":1.03808, "x":7.51092, "y":6.65367, "heading":-1.08439, "vx":1.54092, "vy":-2.25725, "omega":-2.77567, "ax":-9.23299, "ay":-3.122, "alpha":-3.30592, "fx":[-127.1037,-127.76742,-134.91212,-133.71843], "fy":[-55.28802,-53.60173,-31.61048,-36.51424]},
+    {"t":1.08751, "x":7.57581, "y":6.53828, "heading":-1.22564, "vx":1.08451, "vy":-2.41158, "omega":-2.93909, "ax":-9.21762, "ay":-3.23022, "alpha":-1.07262, "fx":[-129.51603,-129.45187,-131.85754,-131.80467], "fy":[-49.08196,-49.19063,-42.32195,-42.55603]},
+    {"t":1.12717, "x":7.61157, "y":6.44009, "heading":-1.34305, "vx":0.71893, "vy":-2.5397, "omega":-2.98164, "ax":-9.2072, "ay":-3.23234, "alpha":0.93225, "fx":[-131.42757,-131.61912,-129.65027,-129.34252], "fy":[-43.1805,-42.67381,-48.33386,-49.08239]},
+    {"t":1.16683, "x":7.63284, "y":6.33682, "heading":-1.46057, "vx":0.35376, "vy":-2.6679, "omega":-2.94466, "ax":-9.03239, "ay":-3.56067, "alpha":3.50971, "fx":[-131.42511,-132.80754,-125.68047,-122.21464], "fy":[-42.15051,-38.07017,-57.43884,-64.22706]},
+    {"t":1.2065, "x":7.63977, "y":6.22821, "heading":-1.5746, "vx":-0.00448, "vy":-2.80912, "omega":-2.80546, "ax":-8.54339, "ay":-4.32957, "alpha":6.98621, "fx":[-127.74289,-133.08065,-119.06193,-104.5165], "fy":[-50.49389,-35.79019,-69.62863,-89.56992]},
+    {"t":1.24616, "x":7.63287, "y":6.11339, "heading":-1.68038, "vx":-0.34332, "vy":-2.98083, "omega":-2.52838, "ax":-8.32228, "ay":-4.5306, "alpha":8.63934, "fx":[-124.46573,-133.95463,-118.04778,-95.39722], "fy":[-56.43695,-30.82469,-70.86168,-98.75727]},
+    {"t":1.28582, "x":7.61271, "y":5.9916, "heading":-1.77386, "vx":-0.67339, "vy":-3.16052, "omega":-2.18573, "ax":-8.24282, "ay":-4.45361, "alpha":9.79179, "fx":[-121.97673,-134.85353,-119.20387,-91.32589], "fy":[-59.14311,-23.48154,-68.07512,-101.81568]},
+    {"t":1.32548, "x":7.57952, "y":5.86275, "heading":-1.85285, "vx":-1.00031, "vy":-3.33716, "omega":-1.79737, "ax":-8.42545, "ay":-3.8264, "alpha":10.34129, "fx":[-123.88816,-135.33563,-122.95935,-95.53172], "fy":[-50.05873,-11.43961,-59.12934,-96.32568]},
+    {"t":1.36514, "x":7.53322, "y":5.72738, "heading":-1.916, "vx":-1.33448, "vy":-3.48892, "omega":-1.38722, "ax":-9.10058, "ay":-1.15514, "alpha":8.28607, "fx":[-130.63437,-132.54254,-131.48218,-121.33501], "fy":[-1.377,15.11347,-26.098,-53.13394]},
+    {"t":1.4048, "x":7.47313, "y":5.5881, "heading":-1.9645, "vx":-1.69542, "vy":-3.53474, "omega":-1.05859, "ax":-4.99392, "ay":7.62773, "alpha":-6.31184, "fx":[-92.38642,-75.88844,-46.2198,-68.6556], "fy":[93.26843,103.99378,122.23981,112.98276]},
+    {"t":1.44446, "x":7.40196, "y":5.4539, "heading":-2.01145, "vx":-1.89349, "vy":-3.23221, "omega":-1.30892, "ax":0.14105, "ay":9.12367, "alpha":-12.26648, "fx":[-54.43061,30.87109,44.42257,-12.86576], "fy":[123.60669,129.5779,128.42932,135.68936]},
+    {"t":1.48413, "x":7.32698, "y":5.33289, "heading":-2.07301, "vx":-1.88789, "vy":-2.87035, "omega":-1.79543, "ax":1.04366, "ay":9.14328, "alpha":-12.8385, "fx":[-44.69978,48.03364,57.26372,-1.42313], "fy":[129.23757,126.94936,124.75758,137.47065]},
+    {"t":1.52379, "x":7.25292, "y":5.22623, "heading":-2.15432, "vx":-1.8465, "vy":-2.50772, "omega":-2.30462, "ax":1.12052, "ay":9.19879, "alpha":-12.53854, "fx":[-43.86521,44.20278,60.63786,2.5568], "fy":[130.37498,129.44643,123.77458,137.96645]},
+    {"t":1.56345, "x":7.18057, "y":5.13401, "heading":-2.25559, "vx":-1.80206, "vy":-2.14288, "omega":-2.80192, "ax":1.1445, "ay":9.36307, "alpha":-10.31842, "fx":[-33.61448,32.30638,57.89467,8.30527], "fy":[133.8312,133.67073,125.38707,137.98827]},
+    {"t":1.60311, "x":7.11, "y":5.05638, "heading":-2.37483, "vx":-1.75667, "vy":-1.77153, "omega":-3.21116, "ax":2.34253, "ay":9.21867, "alpha":-9.12291, "fx":[-10.09016,47.12611,69.3851,26.39817], "fy":[137.78168,129.5373,119.56998,135.80092]},
+    {"t":1.64277, "x":7.04217, "y":4.99337, "heading":-2.50936, "vx":-1.66376, "vy":-1.4059, "omega":-3.57299, "ax":3.03924, "ay":9.03079, "alpha":-8.93077, "fx":[1.26895,53.56801,78.88752,38.59759], "fy":[138.27315,127.18443,113.63926,132.94018]},
+    {"t":1.68243, "x":6.97857, "y":4.94472, "heading":-2.6581, "vx":-1.54322, "vy":-1.04773, "omega":-3.92719, "ax":3.37901, "ay":8.92433, "alpha":-8.7458, "fx":[7.90818,52.22618,84.31633,47.1362], "fy":[138.15532,127.86947,109.74979,130.22659]},
+    {"t":1.72209, "x":6.92002, "y":4.91018, "heading":-2.82073, "vx":-1.4092, "vy":-0.69378, "omega":-4.27406, "ax":3.45702, "ay":8.92341, "alpha":-8.30166, "fx":[13.2104,45.04846,84.8713,52.87965], "fy":[137.83157,130.69219,109.37316,128.05176]},
+    {"t":1.76176, "x":6.86685, "y":4.88968, "heading":-2.99678, "vx":-1.27209, "vy":-0.33986, "omega":-4.60332, "ax":3.6741, "ay":8.87851, "alpha":-7.35899, "fx":[23.03943,42.42044,83.51394,59.34401], "fy":[136.47795,131.51754,110.29636,125.11122]},
+    {"t":1.80409, "x":6.81629, "y":4.88325, "heading":3.08493, "vx":-1.11655, "vy":0.036, "omega":-4.91486, "ax":4.21322, "ay":8.75024, "alpha":-0.46075, "fx":[57.94843,58.965,61.51047,60.46164], "fy":[124.88828,124.38333,123.15641,123.70237]},
+    {"t":1.84642, "x":6.7728, "y":4.89262, "heading":2.87645, "vx":-0.93819, "vy":0.40644, "omega":-4.93436, "ax":1.59538, "ay":9.41669, "alpha":6.74328, "fx":[44.98047,42.70472,6.64579,-3.87433], "fy":[129.22118,130.5814,137.25981,136.85488]},
+    {"t":1.88876, "x":6.73451, "y":4.91826, "heading":2.67361, "vx":-0.87065, "vy":0.80508, "omega":-4.64889, "ax":-0.85391, "ay":9.36362, "alpha":9.39339, "fx":[1.58948,26.40477,-20.29135,-56.11894], "fy":[135.87323,134.47537,135.81892,124.74071]},
+    {"t":1.93109, "x":6.69688, "y":4.96073, "heading":2.48522, "vx":-0.9068, "vy":1.20148, "omega":-4.25123, "ax":-1.95976, "ay":9.16836, "alpha":9.54555, "fx":[-33.54107,16.9235,-25.78047,-68.71858], "fy":[131.22667,135.61612,134.73697,118.25759]},
+    {"t":1.97343, "x":6.65674, "y":5.01981, "heading":2.3138, "vx":-0.98976, "vy":1.58962, "omega":-3.84713, "ax":-1.71122, "ay":9.21273, "alpha":9.30321, "fx":[-37.95967,19.6328,-17.33211,-61.36541], "fy":[129.70101,134.82538,135.82046,122.00614]},
+    {"t":2.01576, "x":6.61331, "y":5.09536, "heading":2.15927, "vx":-1.06221, "vy":1.97963, "omega":-3.45328, "ax":-0.11012, "ay":9.33272, "alpha":9.35072, "fx":[-14.28162,42.50484,5.597,-40.06411], "fy":[133.66225,128.94982,136.44532,130.09883]},
+    {"t":2.05809, "x":6.56824, "y":5.18753, "heading":2.02146, "vx":-1.06687, "vy":2.37472, "omega":-3.05743, "ax":3.11967, "ay":8.77354, "alpha":8.98866, "fx":[45.15315,81.75949,45.38986,4.57999], "fy":[125.75531,107.98838,128.35094,135.35686]},
+    {"t":2.10043, "x":6.52587, "y":5.29593, "heading":1.90008, "vx":-0.9348, "vy":2.74614, "omega":-2.6769, "ax":5.54013, "ay":7.41417, "alpha":8.19482, "fx":[86.2868,105.12584,76.20014,46.50719], "fy":[99.92065,83.56105,111.40377,125.49064]},
+    {"t":2.14276, "x":6.49126, "y":5.41883, "heading":1.7941, "vx":-0.70026, "vy":3.06002, "omega":-2.32998, "ax":5.84876, "ay":6.47326, "alpha":8.0537, "fx":[87.33392,106.29558,82.6853,55.30453], "fy":[84.96351,69.82723,98.85698,113.37987]},
+    {"t":2.1851, "x":6.46686, "y":5.55417, "heading":1.70268, "vx":-0.45266, "vy":3.33406, "omega":-1.98904, "ax":6.20225, "ay":1.96171, "alpha":2.17315, "fx":[87.01201,91.2394,88.92439,84.48587], "fy":[22.91851,22.72295,32.37566,33.20979]},
+    {"t":2.22743, "x":6.45325, "y":5.69707, "heading":1.62042, "vx":-0.19009, "vy":3.4171, "omega":-1.89704, "ax":4.45967, "ay":-3.95445, "alpha":-5.35493, "fx":[74.71337,57.34155,51.79006,69.01432], "fy":[-43.31124,-50.57632,-68.91366,-61.4122]},
+    {"t":2.26977, "x":6.4492, "y":5.83819, "heading":1.53531, "vx":-0.0013, "vy":3.2497, "omega":-2.12373, "ax":0.85586, "ay":-5.259, "alpha":-5.46236, "fx":[25.70572,1.01015,0.08899,21.72165], "fy":[-67.32058,-68.67991,-81.64688,-80.53286]},
+    {"t":2.3121, "x":6.44991, "y":5.97105, "heading":1.44051, "vx":0.03494, "vy":3.02706, "omega":-2.35498, "ax":-1.92183, "ay":-4.61333, "alpha":-3.53237, "fx":[-20.11174,-34.69604,-33.83265,-20.3254], "fy":[-62.54988,-59.02525,-68.36915,-71.6269]},
+    {"t":2.35443, "x":6.44967, "y":6.09506, "heading":1.33765, "vx":-0.04642, "vy":2.83176, "omega":-2.50452, "ax":-3.99914, "ay":-3.29647, "alpha":-1.43227, "fx":[-54.44166,-59.16081,-58.89502,-54.25013], "fy":[-45.65832,-43.22323,-47.80309,-50.22203]},
+    {"t":2.39677, "x":6.44412, "y":6.21199, "heading":1.23034, "vx":-0.21572, "vy":2.69221, "omega":-2.56515, "ax":-5.52952, "ay":-2.11064, "alpha":-0.00609, "fx":[-78.37172,-78.38723,-78.38751,-78.37201], "fy":[-29.91318,-29.89997,-29.92256,-29.93577]},
+    {"t":2.4391, "x":6.43003, "y":6.32407, "heading":1.12174, "vx":-0.44981, "vy":2.60285, "omega":-2.56541, "ax":-6.38516, "ay":-1.35717, "alpha":0.56268, "fx":[-91.17212,-90.02267,-89.84627,-90.99134], "fy":[-19.62245,-21.07163,-18.84695,-17.40925]},
+    {"t":2.48144, "x":6.40527, "y":6.43304, "heading":1.01364, "vx":-0.72012, "vy":2.5454, "omega":-2.54159, "ax":-6.3014, "ay":-0.46067, "alpha":1.04894, "fx":[-90.70073,-88.84675,-87.94298,-89.79312], "fy":[-7.15958,-9.96083,-5.87598,-3.1233]},
+    {"t":2.52377, "x":6.36914, "y":6.54039, "heading":0.90698, "vx":-0.98689, "vy":2.5259, "omega":-2.49718, "ax":-5.7839, "ay":1.00523, "alpha":2.23434, "fx":[-85.48676,-82.21939,-78.43131,-81.8043], "fy":[12.84864,7.27685,15.82131,21.0486]},
+    {"t":2.5661, "x":6.32217, "y":6.64822, "heading":0.80327, "vx":-1.23174, "vy":2.56845, "omega":-2.40259, "ax":-6.21312, "ay":2.10786, "alpha":3.5816, "fx":[-93.45525,-90.33648,-82.51406,-85.97197], "fy":[27.65794,18.23129,32.70471,40.91955]},
+    {"t":2.60844, "x":6.26446, "y":6.75884, "heading":0.70477, "vx":-1.49477, "vy":2.65769, "omega":-2.25097, "ax":-7.76071, "ay":2.05254, "alpha":4.36037, "fx":[-114.16197,-113.53241,-106.19876,-106.13183], "fy":[27.36839,12.23758,31.66386,45.10699]},
+    {"t":2.65077, "x":6.19423, "y":6.87319, "heading":0.61338, "vx":-1.82331, "vy":2.74458, "omega":-2.06638, "ax":-8.68714, "ay":0.79315, "alpha":11.19076, "fx":[-130.69393,-124.87069,-122.46457,-114.52316], "fy":[15.16489,-35.06394,5.04742,59.82267]},
+    {"t":2.69311, "x":6.10926, "y":6.99009, "heading":0.53593, "vx":-2.19107, "vy":2.77816, "omega":-1.59263, "ax":-8.63965, "ay":-3.99824, "alpha":6.72112, "fx":[-130.29346,-114.43498,-112.55105,-132.58046], "fy":[-42.46377,-75.31301,-76.86087,-32.05863]},
+    {"t":2.73544, "x":6.00876, "y":7.10412, "heading":0.47453, "vx":-2.55683, "vy":2.60889, "omega":-1.30809, "ax":-8.35396, "ay":-4.78922, "alpha":5.62372, "fx":[-127.02388,-111.40217,-107.3209,-127.9146], "fy":[-53.60555,-81.24549,-86.08742,-50.60561]},
+    {"t":2.77777, "x":5.89303, "y":7.21028, "heading":0.42419, "vx":-2.91048, "vy":2.40615, "omega":-1.07002, "ax":-6.94956, "ay":-6.82339, "alpha":2.98703, "fx":[-106.87096,-94.95739,-89.082,-103.12323], "fy":[-87.98406,-100.75373,-105.90227,-92.23942]},
+    {"t":2.83197, "x":5.72508, "y":7.33066, "heading":0.37059, "vx":-3.28714, "vy":2.03633, "omega":-0.90813, "ax":-5.54839, "ay":-8.04339, "alpha":1.66399, "fx":[-84.82883,-77.57002,-72.21736,-79.97237], "fy":[-109.63514,-114.89995,-118.31621,-113.20097]},
+    {"t":2.88617, "x":5.53878, "y":7.42921, "heading":0.32381, "vx":-3.58785, "vy":1.6004, "omega":-0.81794, "ax":-5.11216, "ay":-8.31954, "alpha":2.32312, "fx":[-81.52965,-71.81454,-63.04867,-73.46154], "fy":[-112.12047,-118.59904,-123.45786,-117.53277]},
+    {"t":2.94037, "x":5.33681, "y":7.50373, "heading":0.28289, "vx":-3.86492, "vy":1.1495, "omega":-0.69203, "ax":-5.01849, "ay":-8.3352, "alpha":3.73542, "fx":[-85.75043,-70.91813,-55.80713,-72.06817], "fy":[-108.7277,-118.98644,-126.72492,-118.1589]},
+    {"t":2.99457, "x":5.11997, "y":7.55379, "heading":0.25087, "vx":-4.13691, "vy":0.69775, "omega":-0.48958, "ax":-4.90102, "ay":-8.31356, "alpha":5.43103, "fx":[-90.70851,-70.17965,-47.08314,-69.9117], "fy":[-103.98897,-118.96562,-129.71154,-118.70463]},
+    {"t":3.04876, "x":4.88856, "y":7.5794, "heading":0.23232, "vx":-4.40253, "vy":0.24717, "omega":-0.19523, "ax":4.07774, "ay":-4.38202, "alpha":-6.67977, "fx":[67.95436,71.75204,46.41575,45.08191], "fy":[-65.56378,-45.64725,-59.57497,-77.67061]},
+    {"t":3.10296, "x":4.65594, "y":7.58636, "heading":0.21192, "vx":-4.18153, "vy":0.00967, "omega":-0.55726, "ax":8.99969, "ay":3.54554, "alpha":-4.2205, "fx":[132.91094,124.26211,120.89449,132.20621], "fy":[36.7664,59.86085,65.97494,38.42647]},
+    {"t":3.15716, "x":4.44253, "y":7.59209, "heading":0.17552, "vx":-3.69376, "vy":0.20183, "omega":-0.786, "ax":9.73588, "ay":0.74609, "alpha":0.40021, "fx":[137.90817,138.10986,138.10009,137.89727], "fy":[11.77077,9.11868,9.40527,12.00765]},
+    {"t":3.21136, "x":4.25664, "y":7.60412, "heading":0.13351, "vx":-3.1661, "vy":0.24227, "omega":-0.76431, "ax":9.76943, "ay":-0.10219, "alpha":1.66238, "fx":[138.53962,138.36133,138.48462,138.53155], "fy":[3.41679,-7.88436,-5.89782,4.57133]},
+    {"t":3.26556, "x":4.09939, "y":7.6171, "heading":0.09453, "vx":-2.63662, "vy":0.23673, "omega":-0.67421, "ax":9.75708, "ay":-0.50866, "alpha":2.18691, "fx":[138.66528,137.77335,138.08838,138.69024], "fy":[-0.75451,-15.75662,-12.95407,0.62459]},
+    {"t":3.31975, "x":3.97082, "y":7.62919, "heading":0.0612, "vx":-2.1078, "vy":0.20916, "omega":-0.55569, "ax":9.74158, "ay":-0.75204, "alpha":2.43812, "fx":[138.67781,137.24958,137.68621,138.72458], "fy":[-3.35183,-20.15642,-17.08466,-2.04712]},
+    {"t":3.37395, "x":3.87089, "y":7.63942, "heading":0.03466, "vx":-1.57983, "vy":0.1684, "omega":-0.42355, "ax":9.72785, "ay":-0.91851, "alpha":2.56037, "fx":[138.65299,136.84682,137.34961,138.71045], "fy":[-5.24216,-22.9316,-19.82149,-4.08351]},
+    {"t":3.42815, "x":3.79955, "y":7.6472, "heading":0.01547, "vx":-1.0526, "vy":0.11862, "omega":-0.28478, "ax":9.71602, "ay":-1.04359, "alpha":2.61551, "fx":[138.60999,136.53191,137.07386,138.67346], "fy":[-6.78507,-24.86869,-21.78138,-5.73553]},
+    {"t":3.48235, "x":3.75677, "y":7.65209, "heading":0.00388, "vx":-0.52601, "vy":0.06206, "omega":-0.14303, "ax":9.70542, "ay":-1.14511, "alpha":2.63897, "fx":[138.55449,136.26544,136.84272,138.62524], "fy":[-8.14559,-26.38165,-23.28237,-7.11706]},
+    {"t":3.53655, "x":3.74252, "y":7.65378, "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":[
+   {"name":"Marker", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"runSpindexer"}}}]
+}

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 46e1aa816ab4813274f3d1c3db1140b8a79d860b..ee7dc7abf742eaca19ddecacebeb417cbf807626 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -9,6 +9,7 @@ import com.pathplanner.lib.auto.AutoBuilderException;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 
+import choreo.auto.AutoFactory;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
@@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.commands.DoNothing;
 import frc.robot.commands.LogCommand;
+import frc.robot.commands.auto_comm.ChoreoPathCommand;
 import frc.robot.commands.auto_comm.DynamicAutoBuilder;
 import frc.robot.commands.drive_comm.DefaultDriveCommand;
 import frc.robot.commands.drive_comm.SysIDDriveCommand;
@@ -76,6 +78,8 @@ public class RobotContainer {
   // auto Command selection
   private final SendableChooser<Command> autoChooser = new SendableChooser<>();
 
+  // choreo auto factory
+  AutoFactory autoFactory ; 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    * <p>
@@ -89,6 +93,7 @@ public class RobotContainer {
       SmartDashboard.putNumber("Match Time", 0.0);
     }
 
+
     // Filling the SendableChooser on SmartDashboard
 
     // dispatch on the robot
@@ -130,6 +135,23 @@ public class RobotContainer {
         driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer);
         operator = new Operator(drive);
 
+        // choreo auto factory init
+        
+      autoFactory = new AutoFactory(
+            drive::getPose,
+            drive::resetOdometry,
+            sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
+            true,
+            drive,
+            (trajectory, startOrFinish) -> {
+              Logger.recordOutput(
+                  "Autos/Trajectory", trajectory.getPoses());
+              Logger.recordOutput("Autos/StartingOrFinishing", startOrFinish);
+          });
+
+        // warmup command for choreo, prevents lag on auto startup
+        CommandScheduler.getInstance().schedule(autoFactory.warmupCmd());
+
         // Detected objects need access to the drivetrain
         DetectedObject.setDrive(drive);
 
@@ -326,6 +348,8 @@ public class RobotContainer {
     addAuto(leftDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(true));
     addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(false));
 
+    addAuto("testChoreo", new ChoreoPathCommand("test.traj", true, autoFactory));
+
     // put the Chooser on the SmartDashboard
     SmartDashboard.putData("Auto chooser", autoChooser);
   }

diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java
index 98e5d0172c5f4d155f58ecc4ec0fcdcc9e8a71eb..1f381e65a4402f133da25a2d10739bc93992988b 100644 (file)
--- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java
+++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java
@@ -1,5 +1,7 @@
 package frc.robot.commands.auto_comm;
 
+import org.littletonrobotics.junction.Logger;
+
 import choreo.auto.AutoFactory;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -7,31 +9,11 @@ import frc.robot.commands.DoNothing;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 
 public class ChoreoPathCommand extends SequentialCommandGroup {
-    private static AutoFactory factory;
-
-    public ChoreoPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive) {
-        if(factory == null){
-            factory = new AutoFactory(
-                drive::getPose,
-                drive::resetOdometry,
-                sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
-                true,
-                drive,
-                (trajectory, bool)->{
-                    if(bool){
-                        
-                    }else{
-                        
-                    }
-                }
-            );
-        }
-
-        var command = factory.trajectoryCmd(pathName);
+  public ChoreoPathCommand(String pathName, boolean resetOdemetry, AutoFactory factory) {
+    var command = factory.trajectoryCmd(pathName);
 
-        addCommands(
-            resetOdemetry ? new InstantCommand(()->factory.resetOdometry(pathName)) : new DoNothing(),
-            command
-        );
-    }
+    addCommands(
+        resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
+        command);
+  }
 }

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 9d4d5d45d6c994b8fb7574e76d52a6837e29c1bb..81739496d6c8048ee5577720de7e60b16ff374b2 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -84,8 +84,8 @@ public class Superstructure extends Command {
     }
 
     public void updateSetpoints(Pose2d drivepose) {
-               Pose2d turretPosition = drivepose.transformBy(
-                               new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
+               Pose2d turretPosition = drivepose.transformBy(
+                               new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
         
         // If the robot is moving, adjust the target position based on velocity
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();