3 1 practice changes (#105)

* added feedforward to the elevator

* changed most things to units updated the kV

* added feedforward values to constants

* Changed internal elevator logic to meters

* added kg and changed ks

* changed climber deploy position to 14 inches and created branch for practice testing

* tweakd elevator heights and intake voltages

* more robot tweaks

* added command for retracting the climber to a position

* fixed elevator feed forward, tested and worked

* more tuning

* adjustments for practice

* changed some auto stuff

* set field relative chassis speeds in auto

* worked more on improvments

* auto and robot changes

* working 1 piece auto

* added some extra autos

* added RedMoveAuto

* ran spotless

---------

Co-authored-by: nlaverdure <[email protected]>

Author: DylanTaylor29

src/main/deploy/choreo/Pi.chor

@@ -119,12 +119,12 @@
    },
    "blueL4H":{
     "x":{
-     "exp":"5.85 m",
-     "val":5.85
+     "exp":"5.88 m",
+     "val":5.88
     },
     "y":{
-     "exp":"4.15 m",
-     "val":4.15
+     "exp":"3.95 m",
+     "val":3.95
     },
     "heading":{
      "exp":"180 rad",
@@ -235,8 +235,8 @@
      "val":7.2
     },
     "y":{
-     "exp":"4.15 m",
-     "val":4.15
+     "exp":"4 m",
+     "val":4.0
     },
     "heading":{
      "exp":"180 rad",
@@ -347,8 +347,8 @@
      "val":11.75
     },
     "y":{
-     "exp":"4.15 m",
-     "val":4.15
+     "exp":"4.05 m",
+     "val":4.05
     },
     "heading":{
      "exp":"0 rad",
@@ -473,8 +473,8 @@
      "val":10.35
     },
     "y":{
-     "exp":"4.15 m",
-     "val":4.15
+     "exp":"4.05 m",
+     "val":4.05
     },
     "heading":{
      "exp":"0 rad",

src/main/deploy/choreo/blueCenterToL4F.traj

@@ -0,0 +1,92 @@
+{
+ "name":"blueL4GBack",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.88, "y":3.95, "heading":3.141592653589793, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.424586772918701, "y":2.810075521469116, "heading":-2.67794597008687, "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":17.548, "h":8.052}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"blueL4H.x", "val":5.88}, "y":{"exp":"blueL4H.y", "val":3.95}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.424586772918701 m", "val":6.424586772918701}, "y":{"exp":"2.810075521469116 m", "val":2.810075521469116}, "heading":{"exp":"-2.67794597008687 rad", "val":-2.67794597008687}, "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":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,2.26439],
+  "samples":[
+    {"t":0.0, "x":5.88, "y":3.95, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.43088, "ay":-0.9019, "alpha":1.4823, "fx":[7.8018,8.02111,1.86589,1.85543], "fy":[-14.19638,-6.23691,-6.25059,-14.22565]},
+    {"t":0.0444, "x":5.88042, "y":3.94911, "heading":3.14159, "vx":0.01913, "vy":-0.04004, "omega":0.06581, "ax":0.43097, "ay":-0.9021, "alpha":1.10192, "fx":[7.05882,7.20667,2.64748,2.63555], "fy":[-13.18316,-7.25963,-7.27179,-13.20392]},
+    {"t":0.0888, "x":5.8817, "y":3.94644, "heading":-3.13867, "vx":0.03827, "vy":-0.0801, "omega":0.11474, "ax":0.43097, "ay":-0.90209, "alpha":0.80704, "fx":[6.5432,6.55129,3.22576,3.22803], "fy":[-12.37694,-8.10313,-8.0375,-12.40043]},
+    {"t":0.1332, "x":5.88382, "y":3.942, "heading":-3.13358, "vx":0.0574, "vy":-0.12015, "omega":0.15057, "ax":0.43096, "ay":-0.90208, "alpha":0.60586, "fx":[6.07776,6.17669,3.63814,3.65544], "fy":[-11.84016,-8.58992,-8.61751,-11.86987]},
+    {"t":0.1776, "x":5.8868, "y":3.93577, "heading":-3.12689, "vx":0.07654, "vy":-0.1602, "omega":0.17747, "ax":0.43095, "ay":-0.90206, "alpha":0.43255, "fx":[5.80075,5.77498,3.97183,4.00019], "fy":[-11.36184,-9.12617,-9.03411,-11.39472]},
+    {"t":0.222, "x":5.89062, "y":3.92777, "heading":-3.11901, "vx":0.09567, "vy":-0.20025, "omega":0.19668, "ax":0.43095, "ay":-0.90205, "alpha":0.32512, "fx":[5.52039,5.59795,4.19774,4.23135], "fy":[-11.07937,-9.34444,-9.3786,-11.11375]},
+    {"t":0.2664, "x":5.89529, "y":3.91799, "heading":-3.11028, "vx":0.1148, "vy":-0.2403, "omega":0.21111, "ax":0.43094, "ay":-0.90203, "alpha":0.2187, "fx":[5.37344,5.33838,4.40001,4.43524], "fy":[-10.78399,-9.72393,-9.59147,-10.816]},
+    {"t":0.3108, "x":5.90081, "y":3.90643, "heading":-3.10091, "vx":0.13394, "vy":-0.28035, "omega":0.22082, "ax":0.43093, "ay":-0.90201, "alpha":0.16499, "fx":[5.19497,5.2781,4.52081,4.55279], "fy":[-10.64844,-9.77986,-9.80897,-10.67725]},
+    {"t":0.3552, "x":5.90719, "y":3.8931, "heading":-3.0911, "vx":0.15307, "vy":-0.3204, "omega":0.22815, "ax":0.43092, "ay":-0.90199, "alpha":0.09456, "fx":[5.12175,5.09332,4.65167,4.67947], "fy":[-10.45132,-10.08682,-9.901,-10.47438]},
+    {"t":0.3996, "x":5.91441, "y":3.87798, "heading":-3.08097, "vx":0.1722, "vy":-0.36045, "omega":0.23235, "ax":0.43091, "ay":-0.90196, "alpha":0.07271, "fx":[4.99914,5.10617,4.71,4.73037], "fy":[-10.40285,-10.03558,-10.05357,-10.42037]},
+    {"t":0.444, "x":5.92248, "y":3.86109, "heading":-3.07065, "vx":0.19134, "vy":-0.4005, "omega":0.23557, "ax":0.43089, "ay":-0.90193, "alpha":0.02056, "fx":[4.97015,4.95542,4.80286,4.81662], "fy":[-10.25465,-10.32125,-10.06976,-10.26537]},
+    {"t":0.4884, "x":5.9314, "y":3.84242, "heading":-3.0602, "vx":0.21047, "vy":-0.44054, "omega":0.23649, "ax":0.43088, "ay":-0.9019, "alpha":0.01912, "fx":[4.87667,5.02079,4.8209,4.82596], "fy":[-10.26173,-10.18822,-10.19327,-10.26622]},
+    {"t":0.5328, "x":5.94117, "y":3.82197, "heading":-3.0497, "vx":0.2296, "vy":-0.48059, "omega":0.23734, "ax":0.43086, "ay":-0.90186, "alpha":-0.02536, "fx":[4.87716,4.87797,4.89516,4.89315], "fy":[-10.13252,-10.48871,-10.15578,-10.13054]},
+    {"t":0.5772, "x":5.95178, "y":3.79974, "heading":-3.03916, "vx":0.24873, "vy":-0.52063, "omega":0.23621, "ax":0.43084, "ay":-0.90181, "alpha":-0.01172, "fx":[4.79645,4.98928,4.88352,4.87312], "fy":[-10.18072,-10.27963,-10.27221,-10.17266]},
+    {"t":0.6216, "x":5.96325, "y":3.77574, "heading":-3.02867, "vx":0.26786, "vy":-0.56067, "omega":0.23569, "ax":0.43081, "ay":-0.90174, "alpha":-0.05529, "fx":[4.8203,4.8357,4.95091,4.9341], "fy":[-10.05095,-10.62432,-10.1895,-10.03755]},
+    {"t":0.666, "x":5.97557, "y":3.74995, "heading":-3.01821, "vx":0.28698, "vy":-0.60071, "omega":0.23324, "ax":0.43077, "ay":-0.90166, "alpha":-0.02815, "fx":[4.74152,4.99569,4.91302,4.88905], "fy":[-10.13607,-10.33169,-10.31359,-10.11722]},
+    {"t":0.7104, "x":5.98874, "y":3.72239, "heading":-3.00785, "vx":0.30611, "vy":-0.64074, "omega":0.23199, "ax":0.43072, "ay":-0.90155, "alpha":-0.07583, "fx":[4.78817,4.81536,4.98114,4.9523], "fy":[-9.99042,-10.7506,-10.18452,-9.96803]},
+    {"t":0.7548, "x":6.00275, "y":3.69306, "heading":-2.99755, "vx":0.32523, "vy":-0.68077, "omega":0.22862, "ax":0.43065, "ay":-0.9014, "alpha":-0.03423, "fx":[4.70371,5.03179,4.9161,4.88213], "fy":[-10.11539,-10.35415,-10.32833,-10.08872]},
+    {"t":0.7992, "x":6.01762, "y":3.66194, "heading":-2.9874, "vx":0.34436, "vy":-0.72079, "omega":0.2271, "ax":0.43054, "ay":-0.90116, "alpha":-0.08903, "fx":[4.77569,4.81014,4.98963,4.9534], "fy":[-9.9442,-10.86572,-10.14962,-9.91656]},
+    {"t":0.8436, "x":6.03333, "y":3.62905, "heading":-2.97732, "vx":0.36347, "vy":-0.7608, "omega":0.22315, "ax":0.43036, "ay":-0.90078, "alpha":-0.03227, "fx":[4.68542,5.07519,4.89925,4.86091], "fy":[-10.11114,-10.34774,-10.31856,-10.08115]},
+    {"t":0.888, "x":6.04989, "y":3.59438, "heading":-2.96741, "vx":0.38258, "vy":-0.8008, "omega":0.22171, "ax":0.43, "ay":-0.9, "alpha":-0.08946, "fx":[4.7792,4.81442,4.97378,4.93714], "fy":[-9.92608,-10.89403,-10.10488,-9.89854]},
+    {"t":0.9324, "x":6.0673, "y":3.55794, "heading":-2.95756, "vx":0.40167, "vy":-0.84076, "omega":0.21774, "ax":0.42893, "ay":-0.89769, "alpha":-0.02325, "fx":[4.68538,5.08769,4.85855,4.82431], "fy":[-10.09841,-10.2872,-10.26101,-10.07173]},
+    {"t":0.9768, "x":6.08556, "y":3.51973, "heading":-2.9479, "vx":0.42072, "vy":-0.88061, "omega":0.21671, "ax":0.22699, "ay":-0.4805, "alpha":-0.04811, "fx":[2.507,2.53448,2.64113,2.61339], "fy":[-5.30838,-5.77248,-5.42648,-5.28792]},
+    {"t":1.0212, "x":6.10446, "y":3.48015, "heading":-2.93827, "vx":0.43079, "vy":-0.90195, "omega":0.21457, "ax":0.00232, "ay":0.00102, "alpha":-0.01574, "fx":[-0.03752,0.05682,0.05281,0.03333], "fy":[0.0524,-0.0441,-0.02917,0.06734]},
+    {"t":1.0656, "x":6.12359, "y":3.44011, "heading":-2.92875, "vx":0.4309, "vy":-0.9019, "omega":0.21387, "ax":-0.00012, "ay":-0.00006, "alpha":-0.01576, "fx":[-0.02473,-0.01428,0.02199,0.01154], "fy":[0.04413,-0.10305,0.00451,0.0518]},
+    {"t":1.11, "x":6.14273, "y":3.40006, "heading":-2.91925, "vx":0.43089, "vy":-0.90191, "omega":0.21317, "ax":0.00001, "ay":0.0, "alpha":-0.0008, "fx":[-0.00149,-0.00184,0.00223,0.00137], "fy":[0.00177,-0.00241,-0.0017,0.00248]},
+    {"t":1.1544, "x":6.16186, "y":3.36002, "heading":-2.90979, "vx":0.43089, "vy":-0.90191, "omega":0.21314, "ax":0.00016, "ay":0.00007, "alpha":0.01454, "fx":[0.02128,0.01195,-0.01772,-0.00838], "fy":[-0.04126,0.10184,-0.00897,-0.04822]},
+    {"t":1.1988, "x":6.18099, "y":3.31998, "heading":-2.90032, "vx":0.4309, "vy":-0.9019, "omega":0.21378, "ax":-0.00225, "ay":-0.00099, "alpha":0.01423, "fx":[0.03615,-0.06118,-0.04895,-0.02793], "fy":[-0.04652,0.04013,0.02413,-0.06253]},
+    {"t":1.2432, "x":6.20012, "y":3.27993, "heading":-2.89083, "vx":0.4308, "vy":-0.90195, "omega":0.21442, "ax":-0.22703, "ay":0.48023, "alpha":0.04708, "fx":[-2.50906,-2.54208,-2.64002,-2.60675], "fy":[5.31206,5.76798,5.41533,5.28742]},
+    {"t":1.2876, "x":6.21902, "y":3.24036, "heading":-2.88131, "vx":0.42072, "vy":-0.88062, "omega":0.21651, "ax":-0.42892, "ay":0.89769, "alpha":0.02283, "fx":[-4.6813,-5.10739,-4.85632,-4.81063], "fy":[10.10969,10.28457,10.24981,10.07443]},
+    {"t":1.332, "x":6.23728, "y":3.20214, "heading":-2.8717, "vx":0.40167, "vy":-0.84077, "omega":0.21752, "ax":-0.43, "ay":0.90001, "alpha":0.09038, "fx":[-4.77688,-4.8288,-4.97604,-4.9227], "fy":[9.93683,10.90169,10.08822,9.89686]},
+    {"t":1.3764, "x":6.25469, "y":3.1657, "heading":-2.86204, "vx":0.38258, "vy":-0.80081, "omega":0.22153, "ax":-0.43036, "ay":0.90078, "alpha":0.03263, "fx":[-4.67876,-5.09682,-4.90348,-4.84162], "fy":[10.12908,10.34744,10.30064,10.08146]},
+    {"t":1.42079, "x":6.27125, "y":3.13103, "heading":-2.85221, "vx":0.36347, "vy":-0.76081, "omega":0.22298, "ax":-0.43054, "ay":0.90116, "alpha":0.08972, "fx":[-4.76902,-4.83133,-4.99627,-4.93218], "fy":[9.96322,10.8683,10.1298,9.91481]},
+    {"t":1.46519, "x":6.28697, "y":3.09814, "heading":-2.8423, "vx":0.34436, "vy":-0.7208, "omega":0.22696, "ax":-0.43064, "ay":0.9014, "alpha":0.03488, "fx":[-4.69278,-5.06001,-4.9239,-4.85699], "fy":[10.13793,10.35616,10.30582,10.08671]},
+    {"t":1.50959, "x":6.30183, "y":3.06703, "heading":-2.83223, "vx":0.32524, "vy":-0.68078, "omega":0.22851, "ax":-0.43072, "ay":0.90155, "alpha":0.07696, "fx":[-4.77889,-4.83989,-4.99041,-4.92773], "fy":[10.01113,10.75825,10.16084,9.96339]},
+    {"t":1.55399, "x":6.31585, "y":3.03769, "heading":-2.82208, "vx":0.30612, "vy":-0.64075, "omega":0.23193, "ax":-0.43077, "ay":0.90166, "alpha":0.02898, "fx":[-4.73039,-5.02228,-4.92246,-4.86412], "fy":[10.15875,10.33437,10.29093,10.11454]},
+    {"t":1.59839, "x":6.32901, "y":3.01013, "heading":-2.81178, "vx":0.28699, "vy":-0.60072, "omega":0.23322, "ax":-0.4308, "ay":0.90174, "alpha":0.05636, "fx":[-4.81289,-4.8566,-4.95832,-4.91317], "fy":[10.06906,10.62989,10.16928,10.0341]},
+    {"t":1.64279, "x":6.34133, "y":2.98434, "heading":-2.80143, "vx":0.26786, "vy":-0.56068, "omega":0.23572, "ax":-0.43083, "ay":0.90181, "alpha":0.01271, "fx":[-4.79128,-5.00555,-4.88812,-4.85738], "fy":[10.19568,10.27911,10.25726,10.17318]},
+    {"t":1.68719, "x":6.3528, "y":2.96034, "heading":-2.79096, "vx":0.24873, "vy":-0.52064, "omega":0.23628, "ax":-0.43086, "ay":0.90186, "alpha":0.02623, "fx":[-4.88045,-4.88284,-4.89186,-4.88827], "fy":[10.13822,10.4857,10.14958,10.13405]},
+    {"t":1.73159, "x":6.36342, "y":2.93811, "heading":-2.78047, "vx":0.2296, "vy":-0.4806, "omega":0.23745, "ax":-0.43088, "ay":0.9019, "alpha":-0.01803, "fx":[-4.88896,-5.01155,-4.80842,-4.83537], "fy":[10.2552,10.17695,10.1998,10.2775]},
+    {"t":1.77599, "x":6.37319, "y":2.91766, "heading":-2.76993, "vx":0.21047, "vy":-0.44055, "omega":0.23665, "ax":-0.43089, "ay":0.90193, "alpha":-0.01995, "fx":[-5.00051,-4.92195,-4.77249,-4.85009], "fy":[10.22935,10.30062,10.09542,10.28566]},
+    {"t":1.82039, "x":6.38211, "y":2.89899, "heading":-2.75942, "vx":0.19134, "vy":-0.40051, "omega":0.23576, "ax":-0.43091, "ay":0.90196, "alpha":-0.07157, "fx":[-5.05149,-5.03961,-4.65917,-4.79539], "fy":[10.34985,9.99954,10.10659,10.45641]},
+    {"t":1.86479, "x":6.39018, "y":2.8821, "heading":-2.74895, "vx":0.17221, "vy":-0.36046, "omega":0.23258, "ax":-0.43092, "ay":0.90199, "alpha":-0.09447, "fx":[-5.20929,-4.98043,-4.56411,-4.79237], "fy":[10.36149,10.02937,9.99232,10.53035]},
+    {"t":1.90919, "x":6.3974, "y":2.86698, "heading":-2.73863, "vx":0.15307, "vy":-0.32041, "omega":0.22839, "ax":-0.43093, "ay":0.90201, "alpha":-0.16392, "fx":[-5.32803,-5.09829,-4.3913,-4.72904], "fy":[10.50263,9.69328,9.95492,10.7637]},
+    {"t":1.95359, "x":6.40377, "y":2.85364, "heading":-2.72849, "vx":0.13394, "vy":-0.28036, "omega":0.22111, "ax":-0.43094, "ay":0.90203, "alpha":-0.21957, "fx":[-5.57368,-5.06976,-4.19973,-4.7039], "fy":[10.56712,9.59427,9.81165,10.94235]},
+    {"t":1.99799, "x":6.40929, "y":2.84209, "heading":-2.71867, "vx":0.11481, "vy":-0.24031, "omega":0.21136, "ax":-0.43095, "ay":0.90205, "alpha":-0.32431, "fx":[-5.80952,-5.1997,-3.91563,-4.62257], "fy":[10.7547,9.16011,9.70393,11.29742]},
+    {"t":2.04239, "x":6.41397, "y":2.8323, "heading":-2.70929, "vx":0.09567, "vy":-0.20026, "omega":0.19696, "ax":-0.43095, "ay":0.90206, "alpha":-0.43495, "fx":[-6.21504,-5.20996,-3.55744,-4.5653], "fy":[10.90205,8.86074,9.49998,11.65407]},
+    {"t":2.08679, "x":6.41779, "y":2.8243, "heading":-2.70054, "vx":0.07654, "vy":-0.16021, "omega":0.17765, "ax":-0.43096, "ay":0.90208, "alpha":-0.60561, "fx":[-6.65836,-5.37,-3.06926,-4.4504], "fy":[11.17972,8.22297,9.28059,12.23417]},
+    {"t":2.13119, "x":6.42076, "y":2.81808, "heading":-2.69265, "vx":0.05741, "vy":-0.12016, "omega":0.15076, "ax":-0.43097, "ay":0.90209, "alpha":-0.81173, "fx":[-7.35083,-5.43785,-2.41797,-4.34163], "fy":[11.46609,7.58968,8.95998,12.90225]},
+    {"t":2.17559, "x":6.42289, "y":2.81363, "heading":-2.68596, "vx":0.03827, "vy":-0.08011, "omega":0.11472, "ax":-0.43097, "ay":0.9021, "alpha":-1.10274, "fx":[-8.16388,-5.6599,-1.56053,-4.16419], "fy":[11.91289,6.56449,8.55157,13.88955]},
+    {"t":2.21999, "x":6.42416, "y":2.81096, "heading":-2.68087, "vx":0.01914, "vy":-0.04005, "omega":0.06576, "ax":-0.43098, "ay":0.90211, "alpha":-1.48108, "fx":[-9.30943,-5.88061,-0.40895,-3.94973], "fy":[12.46855,5.30192,8.00042,15.14806]},
+    {"t":2.26439, "x":6.42459, "y":2.81008, "heading":-2.67795, "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":[]
+}