competition code at VAGLE

blair-robot-project · Mar 3, 2025 · c65f22b · c65f22b
1 parent d3bf335
commit c65f22b
Show file tree

Hide file tree

Showing 22 changed files with 909 additions and 675 deletions.
diff --git a/src/main/deploy/choreo/OneL4/1.traj b/src/main/deploy/choreo/OneL4/1.traj
@@ -0,0 +1,82 @@
+{
+ "name":"1",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.22906445618, "y":3.857585430145264, "heading":3.141592653589793, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.257402763366699, "y":3.857585430145264, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":1, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true},
+    {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"7.22906445618 m", "val":7.22906445618}, "y":{"exp":"3.8575854301452637 m", "val":3.857585430145264}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.257402763366699 m", "val":6.257402763366699}, "y":{"exp":"3.8575854301452637 m", "val":3.857585430145264}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":1, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true},
+    {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.79719],
+  "samples":[
+    {"t":0.0, "x":7.22906, "y":3.85759, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.75145, "ay":0.0, "alpha":0.0, "fx":[-12.782,-12.782,-12.782,-12.782], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.03907, "x":7.22849, "y":3.85759, "heading":3.14159, "vx":-0.02936, "vy":0.0, "omega":0.0, "ax":-0.75143, "ay":0.0, "alpha":0.0, "fx":[-12.78156,-12.78156,-12.78156,-12.78156], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.07814, "x":7.22677, "y":3.85759, "heading":3.14159, "vx":-0.05872, "vy":0.0, "omega":0.0, "ax":-0.7514, "ay":0.0, "alpha":0.0, "fx":[-12.78108,-12.78108,-12.78108,-12.78108], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.11721, "x":7.2239, "y":3.85759, "heading":3.14159, "vx":-0.08807, "vy":0.0, "omega":0.0, "ax":-0.75137, "ay":0.0, "alpha":0.0, "fx":[-12.78058,-12.78058,-12.78058,-12.78058], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.15628, "x":7.21989, "y":3.85759, "heading":3.14159, "vx":-0.11743, "vy":0.0, "omega":0.0, "ax":-0.75134, "ay":0.0, "alpha":0.0, "fx":[-12.78003,-12.78003,-12.78003,-12.78003], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.19535, "x":7.21473, "y":3.85759, "heading":3.14159, "vx":-0.14678, "vy":0.0, "omega":0.0, "ax":-0.7513, "ay":0.0, "alpha":0.0, "fx":[-12.77945,-12.77945,-12.77945,-12.77945], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.23442, "x":7.20842, "y":3.85759, "heading":3.14159, "vx":-0.17614, "vy":0.0, "omega":0.0, "ax":-0.75127, "ay":0.0, "alpha":0.0, "fx":[-12.77881,-12.77881,-12.77881,-12.77881], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.27348, "x":7.20096, "y":3.85759, "heading":3.14159, "vx":-0.20549, "vy":0.0, "omega":0.0, "ax":-0.75123, "ay":0.0, "alpha":0.0, "fx":[-12.77813,-12.77813,-12.77813,-12.77813], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.31255, "x":7.19236, "y":3.85759, "heading":3.14159, "vx":-0.23484, "vy":0.0, "omega":0.0, "ax":-0.75118, "ay":0.0, "alpha":0.0, "fx":[-12.77738,-12.77738,-12.77738,-12.77738], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.35162, "x":7.18261, "y":3.85759, "heading":3.14159, "vx":-0.26418, "vy":0.0, "omega":0.0, "ax":-0.75113, "ay":0.0, "alpha":0.0, "fx":[-12.77657,-12.77657,-12.77657,-12.77657], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.39069, "x":7.17172, "y":3.85759, "heading":3.14159, "vx":-0.29353, "vy":0.0, "omega":0.0, "ax":-0.75108, "ay":0.0, "alpha":0.0, "fx":[-12.77568,-12.77568,-12.77568,-12.77568], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.42976, "x":7.15968, "y":3.85759, "heading":3.14159, "vx":-0.32288, "vy":0.0, "omega":0.0, "ax":-0.75102, "ay":0.0, "alpha":0.0, "fx":[-12.7747,-12.7747,-12.7747,-12.7747], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.46883, "x":7.14649, "y":3.85759, "heading":3.14159, "vx":-0.35222, "vy":0.0, "omega":0.0, "ax":-0.75096, "ay":0.0, "alpha":0.0, "fx":[-12.77362,-12.77362,-12.77362,-12.77362], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.5079, "x":7.13216, "y":3.85759, "heading":3.14159, "vx":-0.38156, "vy":0.0, "omega":0.0, "ax":-0.75089, "ay":0.0, "alpha":0.0, "fx":[-12.77242,-12.77242,-12.77242,-12.77242], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.54697, "x":7.11668, "y":3.85759, "heading":3.14159, "vx":-0.41089, "vy":0.0, "omega":0.0, "ax":-0.75081, "ay":0.0, "alpha":0.0, "fx":[-12.77107,-12.77107,-12.77107,-12.77107], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.58604, "x":7.10005, "y":3.85759, "heading":3.14159, "vx":-0.44023, "vy":0.0, "omega":0.0, "ax":-0.75072, "ay":0.0, "alpha":0.0, "fx":[-12.76956,-12.76956,-12.76956,-12.76956], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.62511, "x":7.08228, "y":3.85759, "heading":3.14159, "vx":-0.46956, "vy":0.0, "omega":0.0, "ax":-0.75062, "ay":0.0, "alpha":0.0, "fx":[-12.76785,-12.76785,-12.76785,-12.76785], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.66418, "x":7.06336, "y":3.85759, "heading":3.14159, "vx":-0.49888, "vy":0.0, "omega":0.0, "ax":-0.75051, "ay":0.0, "alpha":0.0, "fx":[-12.76589,-12.76589,-12.76589,-12.76589], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.70325, "x":7.0433, "y":3.85759, "heading":3.14159, "vx":-0.52821, "vy":0.0, "omega":0.0, "ax":-0.75037, "ay":0.0, "alpha":0.0, "fx":[-12.76363,-12.76363,-12.76363,-12.76363], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.74232, "x":7.02209, "y":3.85759, "heading":3.14159, "vx":-0.55752, "vy":0.0, "omega":0.0, "ax":-0.75022, "ay":0.0, "alpha":0.0, "fx":[-12.761,-12.761,-12.761,-12.761], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.78139, "x":6.99973, "y":3.85759, "heading":3.14159, "vx":-0.58683, "vy":0.0, "omega":0.0, "ax":-0.75004, "ay":0.0, "alpha":0.0, "fx":[-12.75789,-12.75789,-12.75789,-12.75789], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.82045, "x":6.97623, "y":3.85759, "heading":3.14159, "vx":-0.61614, "vy":0.0, "omega":0.0, "ax":-0.74982, "ay":0.0, "alpha":0.0, "fx":[-12.75416,-12.75416,-12.75416,-12.75416], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.85952, "x":6.95159, "y":3.85759, "heading":3.14159, "vx":-0.64543, "vy":0.0, "omega":0.0, "ax":-0.74955, "ay":0.0, "alpha":0.0, "fx":[-12.7496,-12.7496,-12.7496,-12.7496], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.89859, "x":6.9258, "y":3.85759, "heading":3.14159, "vx":-0.67471, "vy":0.0, "omega":0.0, "ax":-0.74921, "ay":0.0, "alpha":0.0, "fx":[-12.7439,-12.7439,-12.7439,-12.7439], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.93766, "x":6.89887, "y":3.85759, "heading":3.14159, "vx":-0.70399, "vy":0.0, "omega":0.0, "ax":-0.74878, "ay":0.0, "alpha":0.0, "fx":[-12.73658,-12.73658,-12.73658,-12.73658], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.97673, "x":6.87079, "y":3.85759, "heading":3.14159, "vx":-0.73324, "vy":0.0, "omega":0.0, "ax":-0.74821, "ay":0.0, "alpha":0.0, "fx":[-12.72682,-12.72682,-12.72682,-12.72682], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.0158, "x":6.84157, "y":3.85759, "heading":3.14159, "vx":-0.76247, "vy":0.0, "omega":0.0, "ax":-0.74741, "ay":0.0, "alpha":0.0, "fx":[-12.71318,-12.71318,-12.71318,-12.71318], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.05487, "x":6.81121, "y":3.85759, "heading":3.14159, "vx":-0.79167, "vy":0.0, "omega":0.0, "ax":-0.74621, "ay":0.0, "alpha":0.0, "fx":[-12.69275,-12.69275,-12.69275,-12.69275], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.09394, "x":6.77971, "y":3.85759, "heading":3.14159, "vx":-0.82083, "vy":0.0, "omega":0.0, "ax":-0.74421, "ay":0.0, "alpha":0.0, "fx":[-12.65878,-12.65878,-12.65878,-12.65878], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.13301, "x":6.74708, "y":3.85759, "heading":3.14159, "vx":-0.8499, "vy":0.0, "omega":0.0, "ax":-0.74023, "ay":0.0, "alpha":0.0, "fx":[-12.59116,-12.59116,-12.59116,-12.59116], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.17208, "x":6.71331, "y":3.85759, "heading":3.14159, "vx":-0.87882, "vy":0.0, "omega":0.0, "ax":-0.72846, "ay":0.0, "alpha":0.0, "fx":[-12.39083,-12.39083,-12.39083,-12.39083], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.21115, "x":6.67842, "y":3.85759, "heading":3.14159, "vx":-0.90728, "vy":0.0, "omega":0.0, "ax":-0.02798, "ay":0.0, "alpha":0.0, "fx":[-0.47591,-0.47591,-0.47591,-0.47591], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.25022, "x":6.64295, "y":3.85759, "heading":3.14159, "vx":-0.90838, "vy":0.0, "omega":0.0, "ax":0.72834, "ay":0.0, "alpha":0.0, "fx":[12.38891,12.38891,12.38891,12.38891], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.28929, "x":6.60801, "y":3.85759, "heading":3.14159, "vx":-0.87992, "vy":0.0, "omega":0.0, "ax":0.74021, "ay":0.0, "alpha":0.0, "fx":[12.59068,12.59068,12.59068,12.59068], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.32836, "x":6.5742, "y":3.85759, "heading":3.14159, "vx":-0.851, "vy":0.0, "omega":0.0, "ax":0.7442, "ay":0.0, "alpha":0.0, "fx":[12.65857,12.65857,12.65857,12.65857], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.36742, "x":6.54152, "y":3.85759, "heading":3.14159, "vx":-0.82193, "vy":0.0, "omega":0.0, "ax":0.7462, "ay":0.0, "alpha":0.0, "fx":[12.69263,12.69263,12.69263,12.69263], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.40649, "x":6.50998, "y":3.85759, "heading":3.14159, "vx":-0.79277, "vy":0.0, "omega":0.0, "ax":0.7474, "ay":0.0, "alpha":0.0, "fx":[12.7131,12.7131,12.7131,12.7131], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.44556, "x":6.47958, "y":3.85759, "heading":3.14159, "vx":-0.76357, "vy":0.0, "omega":0.0, "ax":0.74821, "ay":0.0, "alpha":0.0, "fx":[12.72677,12.72677,12.72677,12.72677], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.48463, "x":6.45031, "y":3.85759, "heading":3.14159, "vx":-0.73434, "vy":0.0, "omega":0.0, "ax":0.74878, "ay":0.0, "alpha":0.0, "fx":[12.73654,12.73654,12.73654,12.73654], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.5237, "x":6.4222, "y":3.85759, "heading":3.14159, "vx":-0.70509, "vy":0.0, "omega":0.0, "ax":0.74921, "ay":0.0, "alpha":0.0, "fx":[12.74387,12.74387,12.74387,12.74387], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.56277, "x":6.39522, "y":3.85759, "heading":3.14159, "vx":-0.67581, "vy":0.0, "omega":0.0, "ax":0.74955, "ay":0.0, "alpha":0.0, "fx":[12.74957,12.74957,12.74957,12.74957], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.60184, "x":6.36939, "y":3.85759, "heading":3.14159, "vx":-0.64653, "vy":0.0, "omega":0.0, "ax":0.74981, "ay":0.0, "alpha":0.0, "fx":[12.75414,12.75414,12.75414,12.75414], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.64091, "x":6.3447, "y":3.85759, "heading":3.14159, "vx":-0.61724, "vy":0.0, "omega":0.0, "ax":0.75003, "ay":0.0, "alpha":0.0, "fx":[12.75787,12.75787,12.75787,12.75787], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.67998, "x":6.32116, "y":3.85759, "heading":3.14159, "vx":-0.58793, "vy":0.0, "omega":0.0, "ax":0.75022, "ay":0.0, "alpha":0.0, "fx":[12.76099,12.76099,12.76099,12.76099], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.71905, "x":6.29876, "y":3.85759, "heading":3.14159, "vx":-0.55862, "vy":0.0, "omega":0.0, "ax":0.75037, "ay":0.0, "alpha":0.0, "fx":[12.76362,12.76362,12.76362,12.76362], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.75812, "x":6.27751, "y":3.85759, "heading":3.14159, "vx":-0.52931, "vy":0.0, "omega":0.0, "ax":0.75051, "ay":0.0, "alpha":0.0, "fx":[12.76588,12.76588,12.76588,12.76588], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.79719, "x":6.2574, "y":3.85759, "heading":3.14159, "vx":-0.49998, "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/OneL4/OneL4.chor b/src/main/deploy/choreo/OneL4/OneL4.chor
@@ -0,0 +1,78 @@
+{
+ "name":"OneL4",
+ "version":1,
+ "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":"2000 RPM",
+   "val":209.43951023931953
+  },
+  "tmax":{
+   "exp":"0.1 N * m",
+   "val":0.1
+  },
+  "cof":{
+   "exp":"1.5",
+   "val":1.5
+  },
+  "bumper":{
+   "front":{
+    "exp":"16.6875 in",
+    "val":0.4238625
+   },
+   "side":{
+    "exp":"16.5 in",
+    "val":0.4191
+   },
+   "back":{
+    "exp":"16.5 in",
+    "val":0.4191
+   }
+  },
+  "differentialTrackWidth":{
+   "exp":"22 in",
+   "val":0.5588
+  }
+ },
+ "generationFeatures":[]
+}