Initial Climb Subsystem

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

@@ -3,9 +3,213 @@
  "version":1,
  "type":"Swerve",
  "variables":{
-  "expressions":{},
+  "expressions":{
+   "endEffectorOffset":{
+    "dimension":"Number",
+    "var":{
+     "exp":"9.408839",
+     "val":9.408839
+    }
+   }
+  },
   "poses":{
-   "startpos":{
+   "A":{
+    "x":{
+     "exp":"3.2654999999999994 m",
+     "val":3.2654999999999994
+    },
+    "y":{
+     "exp":"4.3309 m",
+     "val":4.3309
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
+   "B":{
+    "x":{
+     "exp":"3.2654999999999994 m",
+     "val":3.2654999999999994
+    },
+    "y":{
+     "exp":"4.0009 m",
+     "val":4.0009
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
+   "C":{
+    "x":{
+     "exp":"3.6132737518457456 m",
+     "val":3.6132737518457456
+    },
+    "y":{
+     "exp":"3.1185381922643174 m",
+     "val":3.1185381922643174
+    },
+    "heading":{
+     "exp":"60 deg",
+     "val":1.0471975511965976
+    }
+   },
+   "D":{
+    "x":{
+     "exp":"3.89906213509461 m",
+     "val":3.89906213509461
+    },
+    "y":{
+     "exp":"2.953538192264317 m",
+     "val":2.953538192264317
+    },
+    "heading":{
+     "exp":"60 deg",
+     "val":1.0471975511965976
+    }
+   },
+   "E":{
+    "x":{
+     "exp":"4.837096751845746 m",
+     "val":4.837096751845746
+    },
+    "y":{
+     "exp":"2.813538192264317 m",
+     "val":2.813538192264317
+    },
+    "heading":{
+     "exp":"120 deg",
+     "val":2.0943951023931953
+    }
+   },
+   "F":{
+    "x":{
+     "exp":"5.122885135094611 m",
+     "val":5.122885135094611
+    },
+    "y":{
+     "exp":"2.978538192264317 m",
+     "val":2.978538192264317
+    },
+    "heading":{
+     "exp":"120 deg",
+     "val":2.0943951023931953
+    }
+   },
+   "G":{
+    "x":{
+     "exp":"5.713146 m",
+     "val":5.713146
+    },
+    "y":{
+     "exp":"3.7209 m",
+     "val":3.7209
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "H":{
+    "x":{
+     "exp":"5.713146 m",
+     "val":5.713146
+    },
+    "y":{
+     "exp":"4.0509 m",
+     "val":4.0509
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "I":{
+    "x":{
+     "exp":"5.365372248154253 m",
+     "val":5.365372248154253
+    },
+    "y":{
+     "exp":"4.933261807735684 m",
+     "val":4.933261807735684
+    },
+    "heading":{
+     "exp":"240 deg",
+     "val":4.1887902047863905
+    }
+   },
+   "J":{
+    "x":{
+     "exp":"5.079583864905389 m",
+     "val":5.079583864905389
+    },
+    "y":{
+     "exp":"5.098261807735684 m",
+     "val":5.098261807735684
+    },
+    "heading":{
+     "exp":"240 deg",
+     "val":4.1887902047863905
+    }
+   },
+   "K":{
+    "x":{
+     "exp":"4.141549248154255 m",
+     "val":4.141549248154255
+    },
+    "y":{
+     "exp":"5.238261807735684 m",
+     "val":5.238261807735684
+    },
+    "heading":{
+     "exp":"300 deg",
+     "val":5.235987755982989
+    }
+   },
+   "L":{
+    "x":{
+     "exp":"3.85576086490539 m",
+     "val":3.85576086490539
+    },
+    "y":{
+     "exp":"5.073261807735684 m",
+     "val":5.073261807735684
+    },
+    "heading":{
+     "exp":"300 deg",
+     "val":5.235987755982989
+    }
+   },
+   "barge":{
+    "x":{
+     "exp":"305.5 in",
+     "val":7.7597
+    },
+    "y":{
+     "exp":"4.3597 m",
+     "val":4.3597
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "fetch":{
+    "x":{
+     "exp":"1.58 m",
+     "val":1.58
+    },
+    "y":{
+     "exp":"7.3 m",
+     "val":7.3
+    },
+    "heading":{
+     "exp":"-52.8554 deg",
+     "val":-0.9225007574586108
+    }
+   },
+   "start":{
     "x":{
      "exp":"7.1008875 m",
      "val":7.1008875
@@ -18,6 +222,48 @@
      "exp":"180 deg",
      "val":3.141592653589793
     }
+   },
+   "startDeep":{
+    "x":{
+     "exp":"7.1008875 m",
+     "val":7.1008875
+    },
+    "y":{
+     "exp":"7.2570308 m",
+     "val":7.2570308
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "startG":{
+    "x":{
+     "exp":"7.1008875 m",
+     "val":7.1008875
+    },
+    "y":{
+     "exp":"142.621161 in",
+     "val":3.6225774894
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "startH":{
+    "x":{
+     "exp":"7.1008875 m",
+     "val":7.1008875
+    },
+    "y":{
+     "exp":"155.561161 in",
+     "val":3.9512534894
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
    }
   }
  },
@@ -43,20 +289,20 @@
    }
   },
   "mass":{
-   "exp":"150 lbs",
-   "val":68.0388555
+   "exp":"147 lbs",
+   "val":66.67807839000001
   },
   "inertia":{
    "exp":"6 kg m ^ 2",
    "val":6.0
   },
   "gearing":{
-   "exp":"6.5",
-   "val":6.5
+   "exp":"7.09",
+   "val":7.09
   },
   "radius":{
-   "exp":"1.75 in",
-   "val":0.04445
+   "exp":"1.68 in",
+   "val":0.042671999999999995
   },
   "vmax":{
    "exp":"6000 RPM",
@@ -72,16 +318,16 @@
   },
   "bumper":{
    "front":{
-    "exp":"17.9375 in",
-    "val":0.4556125
+    "exp":"18.5 in",
+    "val":0.4699
    },
    "side":{
-    "exp":"17.9375 in",
-    "val":0.4556125
+    "exp":"18.5 in",
+    "val":0.4699
    },
    "back":{
-    "exp":"17.9375 in",
-    "val":0.4556125
+    "exp":"18.5 in",
+    "val":0.4699
    }
   },
   "differentialTrackWidth":{

src/main/deploy/choreo/ATofetch.traj

@@ -0,0 +1,69 @@
+{
+ "name":"ATofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.2654999999999994, "y":4.3309, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":31, "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},
+    {"from":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"A.x", "val":3.2654999999999994}, "y":{"exp":"A.y", "val":4.3309}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":31, "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},
+    {"from":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.23713],
+  "samples":[
+    {"t":0.0, "x":3.2655, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.30896, "ay":9.88198, "alpha":-14.2027, "fx":[-49.79715,-21.5846,-112.85386,-169.75595], "fy":[192.60411,198.01611,164.17484,104.11653]},
+    {"t":0.03866, "x":3.26153, "y":4.33828, "heading":0.0, "vx":-0.20525, "vy":0.38204, "omega":-0.54908, "ax":-5.39586, "ay":9.93662, "alpha":-13.281, "fx":[-55.53562,-25.77834,-112.19147,-166.2803], "fy":[190.97923,197.47805,164.59131,109.50615]},
+    {"t":0.07732, "x":3.24957, "y":4.36048, "heading":-0.02123, "vx":-0.41385, "vy":0.7662, "omega":-1.06253, "ax":-5.5072, "ay":10.01397, "alpha":-11.89483, "fx":[-65.17468,-31.06793,-110.16236,-160.80484], "fy":[187.84378,196.66126,165.90257,117.30477]},
+    {"t":0.11598, "x":3.22945, "y":4.39759, "heading":-0.06231, "vx":-0.62676, "vy":1.15334, "omega":-1.52239, "ax":-5.63373, "ay":10.11742, "alpha":-9.90408, "fx":[-77.20491,-39.6519,-106.8783,-151.91119], "fy":[183.14628,195.02466,167.95128,128.48823]},
+    {"t":0.15464, "x":3.20101, "y":4.44974, "heading":-0.12116, "vx":-0.84457, "vy":1.54449, "omega":-1.90529, "ax":-5.76169, "ay":10.25597, "alpha":-6.74921, "fx":[-89.14902,-56.67983,-102.54631,-135.80334], "fy":[177.53836,190.61584,170.47022,145.22379]},
+    {"t":0.1933, "x":3.16405, "y":4.51711, "heading":-0.19482, "vx":-1.06732, "vy":1.94099, "omega":-2.16622, "ax":-5.85423, "ay":10.37353, "alpha":-0.31926, "fx":[-97.43777,-95.68462,-97.73927,-99.48724], "fy":[173.00259,173.98955,172.85305,171.84194]},
+    {"t":0.23196, "x":3.11841, "y":4.5999, "heading":-0.27857, "vx":-1.29364, "vy":2.34203, "omega":-2.17856, "ax":-5.42083, "ay":9.46516, "alpha":15.71702, "fx":[-97.44713,-174.89501,-99.69209,10.58371], "fy":[172.8305,92.9213,167.61559,197.75099]},
+    {"t":0.27062, "x":3.06435, "y":4.69752, "heading":-0.36279, "vx":-1.50322, "vy":2.70796, "omega":-1.57093, "ax":-5.65468, "ay":8.01707, "alpha":22.30033, "fx":[-89.35516,-182.61572,-152.2676,47.19515], "fy":[176.21693,73.75,93.61352,190.98216]},
+    {"t":0.30928, "x":3.00201, "y":4.8082, "heading":-0.42353, "vx":-1.72183, "vy":3.0179, "omega":-0.70879, "ax":-3.34642, "ay":7.0843, "alpha":18.15978, "fx":[-66.21869,-133.09216,-50.597,26.77517], "fy":[154.31283,86.08343,78.21784,153.75344]},
+    {"t":0.34794, "x":2.93294, "y":4.93017, "heading":-0.45093, "vx":-1.8512, "vy":3.29178, "omega":-0.00672, "ax":0.1361, "ay":0.08415, "alpha":0.01598, "fx":[2.25059,2.21669,2.28673,2.32063], "fy":[1.45468,1.38464,1.35071,1.42075]},
+    {"t":0.3866, "x":2.86147, "y":5.05749, "heading":-0.45119, "vx":-1.84594, "vy":3.29504, "omega":-0.00611, "ax":-0.00182, "ay":-0.00096, "alpha":0.00009, "fx":[-0.03046,-0.03065,-0.03026,-0.03006], "fy":[-0.01575,-0.01615,-0.01634,-0.01594]},
+    {"t":0.42527, "x":2.79011, "y":5.18488, "heading":-0.45142, "vx":-1.84601, "vy":3.295, "omega":-0.0061, "ax":-0.01944, "ay":-0.01084, "alpha":0.00007, "fx":[-0.32418,-0.32433,-0.32402,-0.32388], "fy":[-0.18052,-0.18082,-0.18097,-0.18067]},
+    {"t":0.46393, "x":2.71873, "y":5.31226, "heading":-0.45166, "vx":-1.84676, "vy":3.29458, "omega":-0.0061, "ax":-0.02407, "ay":-0.01345, "alpha":0.00005, "fx":[-0.40128,-0.40138,-0.40116,-0.40106], "fy":[-0.224,-0.22422,-0.22433,-0.22411]},
+    {"t":0.50259, "x":2.64731, "y":5.43962, "heading":-0.4519, "vx":-1.84769, "vy":3.29406, "omega":-0.0061, "ax":-0.02792, "ay":-0.01562, "alpha":0.00002, "fx":[-0.46538,-0.46543,-0.46533,-0.46529], "fy":[-0.26035,-0.26044,-0.26049,-0.26039]},
+    {"t":0.54125, "x":2.57586, "y":5.56696, "heading":-0.45213, "vx":-1.84877, "vy":3.29346, "omega":-0.0061, "ax":-0.03281, "ay":-0.01839, "alpha":-0.00002, "fx":[-0.54691,-0.54687,-0.54695,-0.547], "fy":[-0.30668,-0.30659,-0.30655,-0.30663]},
+    {"t":0.57991, "x":2.50436, "y":5.69427, "heading":-0.45237, "vx":-1.85004, "vy":3.29275, "omega":-0.0061, "ax":-0.04041, "ay":-0.0227, "alpha":-0.00007, "fx":[-0.67354,-0.67338,-0.67371,-0.67386], "fy":[-0.37858,-0.37825,-0.3781,-0.37842]},
+    {"t":0.61857, "x":2.43281, "y":5.82155, "heading":-0.4526, "vx":-1.8516, "vy":3.29187, "omega":-0.0061, "ax":-0.0548, "ay":-0.03083, "alpha":-0.00011, "fx":[-0.91341,-0.91317,-0.91367,-0.91392], "fy":[-0.51436,-0.51386,-0.51361,-0.51412]},
+    {"t":0.65723, "x":2.36118, "y":5.94879, "heading":-0.45284, "vx":-1.85372, "vy":3.29068, "omega":-0.00611, "ax":-0.08393, "ay":-0.04731, "alpha":-0.00012, "fx":[-1.39887,-1.39861,-1.39914,-1.3994], "fy":[-0.78905,-0.78852,-0.78827,-0.78879]},
+    {"t":0.69589, "x":2.28945, "y":6.07598, "heading":-0.45307, "vx":-1.85697, "vy":3.28885, "omega":-0.00611, "ax":-0.13391, "ay":-0.07567, "alpha":0.00009, "fx":[-2.23234,-2.23254,-2.23213,-2.23193], "fy":[-1.26101,-1.26142,-1.26162,-1.26121]},
+    {"t":0.73455, "x":2.21756, "y":6.20307, "heading":-0.45331, "vx":-1.86214, "vy":3.28592, "omega":-0.00611, "ax":-0.16679, "ay":-0.09458, "alpha":0.00047, "fx":[-2.78092,-2.78193,-2.77985,-2.77885], "fy":[-1.57504,-1.57711,-1.57812,-1.57605]},
+    {"t":0.77321, "x":2.14544, "y":6.33003, "heading":-0.45355, "vx":-1.86859, "vy":3.28227, "omega":-0.00609, "ax":-0.14307, "ay":-0.08153, "alpha":0.00035, "fx":[-2.38528,-2.38602,-2.38449,-2.38375], "fy":[-1.35794,-1.35946,-1.3602,-1.35868]},
+    {"t":0.81187, "x":2.0731, "y":6.45687, "heading":-0.45378, "vx":-1.87412, "vy":3.27911, "omega":-0.00608, "ax":-0.13178, "ay":-0.07546, "alpha":0.00012, "fx":[-2.1969,-2.19714,-2.19664,-2.19639], "fy":[-1.25744,-1.25795,-1.2582,-1.25769]},
+    {"t":0.85053, "x":2.00054, "y":6.58358, "heading":-0.45402, "vx":-1.87922, "vy":3.2762, "omega":-0.00607, "ax":-0.44204, "ay":-0.26348, "alpha":-0.01728, "fx":[-7.34912,-7.31237,-7.38799,-7.42473], "fy":[-4.44847,-4.37285,-4.33579,-4.41141]},
+    {"t":0.88919, "x":1.92756, "y":6.71005, "heading":-0.45425, "vx":-1.89631, "vy":3.26601, "omega":-0.00674, "ax":2.69567, "ay":-7.51981, "alpha":-18.40304, "fx":[56.53034,128.80933,34.33934,-39.93669], "fy":[-160.75486,-96.01529,-89.40517,-155.23087]},
+    {"t":0.92785, "x":1.85627, "y":6.83069, "heading":-0.45451, "vx":-1.79209, "vy":2.97529, "omega":-0.71821, "ax":5.70514, "ay":-7.9955, "alpha":-22.9634, "fx":[81.90445,177.44468,169.4511,-48.39244], "fy":[-179.79677,-85.66873,-77.10406,-190.55508]},
+    {"t":0.96651, "x":1.79125, "y":6.93974, "heading":-0.48228, "vx":-1.57153, "vy":2.66618, "omega":-1.60598, "ax":5.767, "ay":-9.23442, "alpha":-16.33864, "fx":[85.17977,166.78278,144.74787,-12.17788], "fy":[-179.19053,-107.02025,-131.99584,-197.52651]},
+    {"t":1.00517, "x":1.7348, "y":7.03592, "heading":-0.54437, "vx":-1.34857, "vy":2.30917, "omega":-2.23764, "ax":5.97116, "ay":-10.30787, "alpha":0.17994, "fx":[99.83586,98.5077,99.24201,100.55985], "fy":[-171.65007,-172.41967,-172.00515,-171.23415]},
+    {"t":1.04383, "x":1.68713, "y":7.11749, "heading":-0.63088, "vx":-1.11773, "vy":1.91067, "omega":-2.23069, "ax":5.90684, "ay":-10.19527, "alpha":6.39024, "fx":[116.11521,59.72799,88.04782,129.96561], "fy":[-161.24442,-189.62146,-178.3856,-150.54927]},
+    {"t":1.08249, "x":1.64833, "y":7.18374, "heading":-0.71711, "vx":-0.88936, "vy":1.51651, "omega":-1.98364, "ax":5.8396, "ay":-10.0207, "alpha":9.81045, "fx":[132.11588,37.8889,79.86673,139.5017], "fy":[-148.56542,-195.28709,-182.34303,-141.96529]},
+    {"t":1.12115, "x":1.61831, "y":7.23488, "heading":-0.7938, "vx":-0.6636, "vy":1.12911, "omega":-1.60436, "ax":5.78055, "ay":-9.85858, "alpha":12.20174, "fx":[145.26601,24.24682,73.06989,142.85302], "fy":[-135.87087,-197.52027,-185.24836,-138.71147]},
+    {"t":1.15981, "x":1.59697, "y":7.27116, "heading":-0.85583, "vx":-0.44012, "vy":0.74797, "omega":-1.13264, "ax":5.72047, "ay":-9.72537, "alpha":13.98608, "fx":[154.85325,15.08528,67.55032,143.94117], "fy":[-124.9526,-198.48087,-187.37875,-137.65698]},
+    {"t":1.19847, "x":1.58423, "y":7.29281, "heading":-0.89962, "vx":-0.21897, "vy":0.37198, "omega":-0.59193, "ax":5.66388, "ay":-9.62184, "alpha":15.31093, "fx":[161.36875,8.53205,63.42083,144.3352], "fy":[-116.51418,-198.90836,-188.84852,-137.29443]},
+    {"t":1.23713, "x":1.58, "y":7.3, "heading":-0.9225, "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":[]
+}