Merge pull request #73 from strykeforce/barge-auton

Barge Autos - Stealing

Author: mwitcpalek

src/main/deploy/choreo/2025-project.chor

@@ -4,6 +4,20 @@
  "type":"Swerve",
  "variables":{
   "expressions":{
+   "FieldMaxX":{
+    "dimension":"Length",
+    "var":{
+     "exp":"17.526 m",
+     "val":17.526
+    }
+   },
+   "FieldMaxY":{
+    "dimension":"Length",
+    "var":{
+     "exp":"8.0518 m",
+     "val":8.0518
+    }
+   },
    "endEffectorOffset":{
     "dimension":"Number",
     "var":{

src/main/deploy/choreo/HToBarge.traj

@@ -0,0 +1,89 @@
+{
+ "name":"OppGToOppBarge",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":11.698677, "y":4.3309, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":11.333887100219728, "y":4.521003246307373, "heading":-0.9827950870986544, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":10.47326946258545, "y":5.011077404022217, "heading":-3.0750240485246456, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":9.826, "y":5.08871412277, "heading":3.141592653589793, "intervals":27, "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":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"FieldMaxX - G.x", "val":11.698677}, "y":{"exp":"FieldMaxY - G.y", "val":4.3309}, "heading":{"exp":"G.heading - 180 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"11.333887100219727 m", "val":11.333887100219728}, "y":{"exp":"4.521003246307373 m", "val":4.521003246307373}, "heading":{"exp":"-0.9827950870986543 rad", "val":-0.9827950870986544}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"10.47326946258545 m", "val":10.47326946258545}, "y":{"exp":"5.011077404022217 m", "val":5.011077404022217}, "heading":{"exp":"-3.0750240485246456 rad", "val":-3.0750240485246456}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"FieldMaxX - barge.x", "val":9.826}, "y":{"exp":"barge.y", "val":5.08871412277}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":27, "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":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.30677,0.79439,1.12781],
+  "samples":[
+    {"t":0.0, "x":11.69868, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.64651, "ay":1.98617, "alpha":-36.04913, "fx":[-122.88069,-30.09616,-159.82774,-197.04998], "fy":[-154.76678,196.66105,118.76195,-28.22204]},
+    {"t":0.02045, "x":11.69708, "y":4.33132, "heading":0.0, "vx":-0.15638, "vy":0.04062, "omega":-0.73725, "ax":-7.91136, "ay":2.21277, "alpha":-34.3421, "fx":[-137.29655,-33.14697,-159.70876,-197.36174], "fy":[-141.80121,196.11798,118.8684,-25.64173]},
+    {"t":0.0409, "x":11.69223, "y":4.33261, "heading":-0.01508, "vx":-0.31818, "vy":0.08587, "omega":-1.43959, "ax":-8.15259, "ay":2.53431, "alpha":-32.59, "fx":[-151.92816,-35.17625,-158.65432,-197.8403], "fy":[-125.72565,195.69371,120.20486,-21.19007]},
+    {"t":0.06135, "x":11.68401, "y":4.3349, "heading":-0.04452, "vx":-0.48491, "vy":0.1377, "omega":-2.1061, "ax":-8.35563, "ay":2.93822, "alpha":-30.84213, "fx":[-165.33984,-36.73082,-156.70606,-198.36089], "fy":[-107.26448,195.31072,122.64726,-14.77854]},
+    {"t":0.08181, "x":11.67235, "y":4.33833, "heading":-0.08759, "vx":-0.65579, "vy":0.19779, "omega":-2.73686, "ax":-8.52505, "ay":3.42663, "alpha":-28.99761, "fx":[-177.1432,-38.65719,-153.90002,-198.73346], "fy":[-86.17909,194.80074,126.03473,-6.17553]},
+    {"t":0.10226, "x":11.65715, "y":4.34309, "heading":-0.14356, "vx":-0.83014, "vy":0.26787, "omega":-3.3299, "ax":-8.67825, "ay":4.02534, "alpha":-26.77263, "fx":[-187.29678,-42.40951,-150.29548,-198.64744], "fy":[-60.69398,193.81476,130.14906,5.13239]},
+    {"t":0.12271, "x":11.63836, "y":4.34941, "heading":-0.21167, "vx":-1.00762, "vy":0.3502, "omega":-3.87744, "ax":-8.83496, "ay":4.78882, "alpha":-23.63427, "fx":[-194.84118,-50.71964,-146.04632,-197.49107], "fy":[-27.14256,191.49038,134.65024,20.31103]},
+    {"t":0.14316, "x":11.61591, "y":4.35757, "heading":-0.29096, "vx":-1.18831, "vy":0.44813, "omega":-4.36079, "ax":-8.99253, "ay":5.80046, "alpha":-18.60622, "fx":[-195.43385,-68.93664,-141.61699,-193.61683], "fy":[20.30896,185.18424,138.86264,42.4076]},
+    {"t":0.16361, "x":11.58972, "y":4.36795, "heading":-0.38015, "vx":-1.37222, "vy":0.56676, "omega":-4.74131, "ax":-9.00562, "ay":7.12247, "alpha":-9.88873, "fx":[-176.15175,-105.8358,-138.73317,-179.75703], "fy":[86.50478,165.94685,140.73731,81.72365]},
+    {"t":0.18406, "x":11.55978, "y":4.38103, "heading":-0.47711, "vx":-1.5564, "vy":0.71243, "omega":-4.94355, "ax":-7.6892, "ay":8.54654, "alpha":8.09243, "fx":[-117.36153,-159.74974,-149.82879,-85.76087], "fy":[157.33952,113.52506,123.67373,175.32858]},
+    {"t":0.20451, "x":11.52634, "y":4.39739, "heading":-0.57822, "vx":-1.71365, "vy":0.88721, "omega":-4.77805, "ax":-6.19277, "ay":7.2166, "alpha":25.08967, "fx":[-84.88902,-174.97574,-182.49768,29.44067], "fy":[176.13298,85.87586,28.81022,190.36991]},
+    {"t":0.22496, "x":11.49, "y":4.41704, "heading":-0.67593, "vx":-1.8403, "vy":1.0348, "omega":-4.26493, "ax":-6.01527, "ay":7.20236, "alpha":24.57254, "fx":[-79.1792,-168.65509,-177.19013,23.93792], "fy":[175.68958,91.80222,27.25944,185.48807]},
+    {"t":0.24542, "x":11.4511, "y":4.43971, "heading":-0.76316, "vx":-1.96332, "vy":1.1821, "omega":-3.76239, "ax":-4.13788, "ay":7.95373, "alpha":20.61863, "fx":[-49.72627,-140.02764,-129.09532,42.94356], "fy":[175.95895,113.72977,74.22583,166.42507]},
+    {"t":0.26587, "x":11.41008, "y":4.46555, "heading":-0.8401, "vx":-2.04795, "vy":1.34476, "omega":-3.34071, "ax":8.95102, "ay":1.63754, "alpha":-14.46191, "fx":[155.01153,174.33562,140.94444,126.54494], "fy":[-55.48958,16.54134,95.01144,53.12501]},
+    {"t":0.28632, "x":11.37007, "y":4.49339, "heading":-0.90842, "vx":-1.86489, "vy":1.37825, "omega":-3.63648, "ax":9.33978, "ay":-2.76147, "alpha":-19.01615, "fx":[128.20464,187.30644,171.97574,135.27159], "fy":[-138.19624,-37.62589,73.81934,-82.12684]},
+    {"t":0.30677, "x":11.33389, "y":4.521, "heading":-0.9828, "vx":-1.67387, "vy":1.32178, "omega":-4.02538, "ax":8.91402, "ay":-4.04743, "alpha":-19.06022, "fx":[113.90483,182.17009,179.96654,118.32834], "fy":[-150.73251,-57.9263,50.8044,-112.02062]},
+    {"t":0.32797, "x":11.3004, "y":4.54812, "heading":-1.06814, "vx":-1.48489, "vy":1.23597, "omega":-4.42948, "ax":7.57101, "ay":-6.14838, "alpha":-18.78639, "fx":[86.46709,165.03276,182.4806,70.83992], "fy":[-165.90922,-91.93336,-1.42384,-150.69599]},
+    {"t":0.34917, "x":11.27062, "y":4.57294, "heading":-1.16205, "vx":-1.32437, "vy":1.10562, "omega":-4.82777, "ax":5.64447, "ay":-7.88951, "alpha":-17.90314, "fx":[51.98144,137.88276,166.12998,20.36851], "fy":[-177.19079,-124.59053,-58.23583,-166.04037]},
+    {"t":0.37037, "x":11.24381, "y":4.59461, "heading":-1.2644, "vx":-1.2047, "vy":0.93835, "omega":-5.20734, "ax":3.03314, "ay":-9.11647, "alpha":-16.20442, "fx":[10.64028,98.69377,123.6021,-30.69196], "fy":[-180.9175,-152.11616,-111.71303,-163.12184]},
+    {"t":0.39157, "x":11.21895, "y":4.61245, "heading":-1.3748, "vx":-1.1404, "vy":0.74507, "omega":-5.55089, "ax":-0.34208, "ay":-9.44238, "alpha":-13.11778, "fx":[-35.89634,43.50867,46.19524,-76.61697], "fy":[-171.90996,-168.21382,-146.24843,-143.22782]},
+    {"t":0.41278, "x":11.1947, "y":4.62613, "heading":-1.49249, "vx":-1.14765, "vy":0.54488, "omega":-5.829, "ax":-4.43918, "ay":-7.96697, "alpha":-7.28819, "fx":[-85.25111,-42.16832,-60.80109,-107.77577], "fy":[-139.11544,-152.31164,-129.18425,-110.61118]},
+    {"t":0.43398, "x":11.16937, "y":4.63589, "heading":-1.61607, "vx":-1.24177, "vy":0.37597, "omega":-5.98352, "ax":-8.03539, "ay":-1.80495, "alpha":4.25656, "fx":[-130.02811,-141.55474,-138.35841,-125.84284], "fy":[-17.19082,-13.57818,-41.17325,-48.40828]},
+    {"t":0.45518, "x":11.14124, "y":4.64345, "heading":-1.74293, "vx":-1.41213, "vy":0.33771, "omega":-5.89328, "ax":-5.30646, "ay":6.90352, "alpha":10.78034, "fx":[-41.9105,-98.26082,-132.60714,-81.04581], "fy":[148.4258,131.13325,80.8552,99.89922]},
+    {"t":0.47638, "x":11.11011, "y":4.65216, "heading":-1.86787, "vx":-1.52463, "vy":0.48407, "omega":-5.66472, "ax":-1.05956, "ay":9.71744, "alpha":8.97465, "fx":[24.57709,-34.53829,-68.54394,7.85578], "fy":[169.72029,170.89151,150.67228,156.65592]},
+    {"t":0.49758, "x":11.07754, "y":4.66461, "heading":-1.98797, "vx":-1.54709, "vy":0.69009, "omega":-5.47445, "ax":1.28041, "ay":9.81549, "alpha":7.26433, "fx":[54.53004,5.42804,-19.16397,44.58104], "fy":[162.73176,173.32368,164.08313,154.33949]},
+    {"t":0.51878, "x":11.04503, "y":4.68145, "heading":-2.10403, "vx":-1.51995, "vy":0.89819, "omega":-5.32044, "ax":3.25023, "ay":9.16136, "alpha":5.27498, "fx":[76.80563,41.88573,27.5634,70.46445], "fy":[148.2147,162.74462,158.15433,141.74825]},
+    {"t":0.53998, "x":11.01354, "y":4.70255, "heading":-2.21683, "vx":-1.45104, "vy":1.09242, "omega":-5.2086, "ax":4.75189, "ay":8.21206, "alpha":3.78367, "fx":[94.20379,70.72715,62.32354,89.59274], "fy":[131.6129,146.27745,143.11791,126.55628]},
+    {"t":0.56118, "x":10.98384, "y":4.72755, "heading":-2.32726, "vx":-1.35029, "vy":1.26652, "omega":-5.12838, "ax":2.68368, "ay":7.1553, "alpha":9.82109, "fx":[84.94208,36.98383,-2.13475,59.15165], "fy":[112.59744,141.98788,128.62494,93.89122]},
+    {"t":0.58238, "x":10.95582, "y":4.75601, "heading":-2.43599, "vx":-1.2934, "vy":1.41822, "omega":-4.92017, "ax":-7.33675, "ay":-0.90829, "alpha":22.78135, "fx":[-63.32805,-127.07772,-172.23528,-126.55914], "fy":[-48.6879,96.43278,-2.4663,-105.84142]},
+    {"t":0.60359, "x":10.92675, "y":4.78588, "heading":-2.5403, "vx":-1.44894, "vy":1.39897, "omega":-4.43718, "ax":-8.85431, "ay":-2.37146, "alpha":20.9077, "fx":[-105.20003,-156.62413,-185.32435,-143.24009], "fy":[-114.19136,81.44373,-10.74207,-114.63454]},
+    {"t":0.62479, "x":10.89404, "y":4.815, "heading":-2.63437, "vx":-1.63666, "vy":1.34869, "omega":-3.99391, "ax":-9.51426, "ay":-2.27517, "alpha":19.72575, "fx":[-124.52914,-164.87936,-189.83435,-155.14976], "fy":[-119.8477,79.55329,-3.97597,-107.4337]},
+    {"t":0.64599, "x":10.8572, "y":4.84309, "heading":-2.71905, "vx":-1.83838, "vy":1.30045, "omega":-3.5757, "ax":-9.89384, "ay":-2.06805, "alpha":18.94859, "fx":[-135.38863,-169.05515,-191.97113,-163.28749], "fy":[-119.76204,78.54783,2.99002,-99.66957]},
+    {"t":0.66719, "x":10.816, "y":4.87019, "heading":-2.79486, "vx":-2.04814, "vy":1.25661, "omega":-3.17397, "ax":-10.11549, "ay":-1.98205, "alpha":18.46348, "fx":[-140.28542,-172.63902,-193.15286,-168.40392], "fy":[-121.21521,75.50285,7.65992,-94.10703]},
+    {"t":0.68839, "x":10.77031, "y":4.89639, "heading":-2.86215, "vx":-2.2626, "vy":1.21459, "omega":-2.78252, "ax":-10.22787, "ay":-2.07411, "alpha":18.21405, "fx":[-140.34266,-176.5423,-193.99626,-171.09349], "fy":[-125.86539,69.61419,9.53775,-91.58451]},
+    {"t":0.70959, "x":10.72004, "y":4.92167, "heading":-2.92114, "vx":-2.47944, "vy":1.17061, "omega":-2.39637, "ax":-10.24881, "ay":-2.3447, "alpha":18.16893, "fx":[-136.06536,-180.77793,-194.75155,-171.77581], "fy":[-133.73984,60.80223,8.80273,-92.20528]},
+    {"t":0.73079, "x":10.66517, "y":4.94597, "heading":-2.97195, "vx":-2.69672, "vy":1.1209, "omega":-2.01117, "ax":-10.18031, "ay":-2.76436, "alpha":18.31206, "fx":[-127.66404,-184.94348,-195.4574,-170.73859], "fy":[-144.12778,49.41769,6.01758,-95.62953]},
+    {"t":0.75199, "x":10.60571, "y":4.96911, "heading":-3.01459, "vx":-2.91256, "vy":1.0623, "omega":-1.62293, "ax":-10.0196, "ay":-3.28389, "alpha":18.63862, "fx":[-115.31774,-188.49399,-196.06182,-168.21397], "fy":[-155.92055,36.17036,2.011,-101.22429]},
+    {"t":0.77319, "x":10.5417, "y":4.99089, "heading":-3.04899, "vx":-3.12498, "vy":0.99267, "omega":-1.22777, "ax":-9.70805, "ay":-3.82808, "alpha":19.63514, "fx":[-96.37748,-190.4633,-196.5376,-163.93593], "fy":[-169.59111,24.87322,-1.51913,-109.01176]},
+    {"t":0.79439, "x":10.47327, "y":5.01108, "heading":-3.07502, "vx":-3.33081, "vy":0.91151, "omega":-0.81148, "ax":-6.16071, "ay":-9.35881, "alpha":11.3387, "fx":[-41.64858,-90.80289,-163.58205,-114.75105], "fy":[-190.7182,-168.58306,-105.78924,-158.93721]},
+    {"t":0.82218, "x":10.37834, "y":5.03279, "heading":-3.09757, "vx":-3.50198, "vy":0.65148, "omega":-0.49644, "ax":8.35613, "ay":-7.97891, "alpha":6.86858, "fx":[142.46903,169.48472,139.55383,105.66317], "fy":[-134.90586,-97.51039,-135.03334,-164.56872]},
+    {"t":0.84996, "x":10.28427, "y":5.04781, "heading":-3.11136, "vx":-3.26981, "vy":0.42979, "omega":-0.30559, "ax":11.12951, "ay":-3.95074, "alpha":3.22461, "fx":[183.43331,191.71871,188.95559,177.98656], "fy":[-73.63097,-47.73912,-56.67596,-85.38179]},
+    {"t":0.87775, "x":10.19771, "y":5.05823, "heading":-3.11986, "vx":-2.96057, "vy":0.32001, "omega":-0.216, "ax":11.59778, "ay":-2.54951, "alpha":1.97177, "fx":[192.08965,195.67522,194.94305,190.60956], "fy":[-49.0089,-31.65632,-35.24434,-54.08661]},
+    {"t":0.90553, "x":10.11993, "y":5.06614, "heading":-3.12586, "vx":-2.63833, "vy":0.24918, "omega":-0.16121, "ax":11.75136, "ay":-1.88345, "alpha":1.38614, "fx":[195.13807,197.08079,196.78378,194.5556], "fy":[-36.58052,-23.92622,-25.80496,-39.27318]},
+    {"t":0.93332, "x":10.05116, "y":5.07233, "heading":-3.13034, "vx":-2.31182, "vy":0.19684, "omega":-0.1227, "ax":11.82094, "ay":-1.49874, "alpha":1.05111, "fx":[196.55733,197.75766,197.60768,196.27467], "fy":[-29.18337,-19.40574,-20.53893,-30.80532]},
+    {"t":0.9611, "x":9.99149, "y":5.07722, "heading":-3.13375, "vx":-1.98337, "vy":0.1552, "omega":-0.09349, "ax":11.85875, "ay":-1.24913, "alpha":0.83492, "fx":[197.33715,198.14375,198.05727,197.18063], "fy":[-24.29739,-16.44382,-17.1897,-25.35875]},
+    {"t":0.98889, "x":9.94096, "y":5.08105, "heading":-3.13634, "vx":-1.65388, "vy":0.12049, "omega":-0.0703, "ax":11.88184, "ay":-1.07436, "alpha":0.68407, "fx":[197.81476,198.38896,198.33447,197.71988], "fy":[-20.83548,-14.35399,-14.87519,-21.57132]},
+    {"t":1.01667, "x":9.89959, "y":5.08399, "heading":-3.1383, "vx":-1.32374, "vy":0.09064, "omega":-0.05129, "ax":11.89711, "ay":-0.94525, "alpha":0.57289, "fx":[198.13048,198.55668,198.52007,198.06904], "fy":[-18.25643,-12.80081,-13.18146,-18.78892]},
+    {"t":1.04446, "x":9.8674, "y":5.08614, "heading":-3.13972, "vx":-0.99318, "vy":0.06438, "omega":-0.03537, "ax":11.90782, "ay":-0.84603, "alpha":0.48759, "fx":[198.35131,198.67775,198.65194,198.30947], "fy":[-16.26165,-11.60116,-11.88895,-16.66006]},
+    {"t":1.07224, "x":9.8444, "y":5.0876, "heading":-3.1407, "vx":-0.66232, "vy":0.04087, "omega":-0.02182, "ax":11.91568, "ay":-0.76742, "alpha":0.42008, "fx":[198.51263,198.76882,198.74991,198.48298], "fy":[-14.67323,-10.6467,-10.87054,-14.97951]},
+    {"t":1.10003, "x":9.8306, "y":5.08844, "heading":-3.14131, "vx":-0.33124, "vy":0.01955, "omega":-0.01015, "ax":11.92164, "ay":-0.70361, "alpha":0.36533, "fx":[198.6346,198.83955,198.82528,198.61289], "fy":[-13.37873,-9.86925,-10.04758,-13.61959]},
+    {"t":1.12781, "x":9.826, "y":5.08871, "heading":3.14159, "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":[]
+}