Skip to content

Commit 7ea667a

Browse files
author
Dhyan Soni
committed
2 parents 056d0c6 + 5bbddf8 commit 7ea667a

File tree

6 files changed

+125
-134
lines changed

6 files changed

+125
-134
lines changed

‎Template/Sample.traj‎

Lines changed: 0 additions & 107 deletions
This file was deleted.

‎Template/src/main/deploy/choreo/Random.traj‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
{
2+
"name":"Random",
3+
"version":1,
4+
"snapshot":{
5+
"waypoints":[
6+
{"x":7.516858100891113, "y":5.901409149169922, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
7+
{"x":11.185239791870115, "y":5.798364639282227, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
8+
{"x":14.008658409118652, "y":1.635368824005127, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
9+
"constraints":[
10+
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
11+
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
12+
{"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
13+
"targetDt":0.05
14+
},
15+
"params":{
16+
"waypoints":[
17+
{"x":{"exp":"7.516858100891113 m", "val":7.516858100891113}, "y":{"exp":"5.901409149169922 m", "val":5.901409149169922}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
18+
{"x":{"exp":"11.185239791870117 m", "val":11.185239791870115}, "y":{"exp":"5.798364639282227 m", "val":5.798364639282227}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
19+
{"x":{"exp":"14.008658409118652 m", "val":14.008658409118652}, "y":{"exp":"1.635368824005127 m", "val":1.635368824005127}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
20+
"constraints":[
21+
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
22+
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
23+
{"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}],
24+
"targetDt":{
25+
"exp":"0.05 s",
26+
"val":0.05
27+
}
28+
},
29+
"trajectory":{
30+
"sampleType":"Swerve",
31+
"waypoints":[0.0,1.03264,2.34434],
32+
"samples":[
33+
{"t":0.0, "x":7.51686, "y":5.90141, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.97731, "ay":0.84838, "alpha":0.0, "fx":[152.70152,152.70152,152.70152,152.70152], "fy":[14.43078,14.43078,14.43078,14.43078]},
34+
{"t":0.03972, "x":7.52394, "y":5.90208, "heading":0.0, "vx":0.35655, "vy":0.0337, "omega":0.0, "ax":8.97653, "ay":0.84831, "alpha":0.0, "fx":[152.68815,152.68815,152.68815,152.68815], "fy":[14.42947,14.42947,14.42947,14.42947]},
35+
{"t":0.07943, "x":7.54518, "y":5.90409, "heading":0.0, "vx":0.71307, "vy":0.06739, "omega":0.0, "ax":8.9756, "ay":0.84822, "alpha":0.0, "fx":[152.67234,152.67234,152.67234,152.67234], "fy":[14.42792,14.42792,14.42792,14.42792]},
36+
{"t":0.11915, "x":7.58058, "y":5.90743, "heading":0.0, "vx":1.06956, "vy":0.10108, "omega":0.0, "ax":8.97448, "ay":0.84811, "alpha":0.0, "fx":[152.65338,152.65338,152.65338,152.65338], "fy":[14.42606,14.42606,14.42606,14.42606]},
37+
{"t":0.15887, "x":7.63014, "y":5.91211, "heading":0.0, "vx":1.426, "vy":0.13476, "omega":0.0, "ax":8.97312, "ay":0.84797, "alpha":0.0, "fx":[152.63022,152.63022,152.63022,152.63022], "fy":[14.42379,14.42379,14.42379,14.42379]},
38+
{"t":0.19859, "x":7.69385, "y":5.91814, "heading":0.0, "vx":1.78238, "vy":0.16844, "omega":0.0, "ax":8.97142, "ay":0.84781, "alpha":0.0, "fx":[152.60128,152.60128,152.60128,152.60128], "fy":[14.42096,14.42096,14.42096,14.42096]},
39+
{"t":0.2383, "x":7.77172, "y":5.92549, "heading":0.0, "vx":2.1387, "vy":0.20211, "omega":0.0, "ax":8.96923, "ay":0.84759, "alpha":0.0, "fx":[152.5641,152.5641,152.5641,152.5641], "fy":[14.41732,14.41732,14.41732,14.41732]},
40+
{"t":0.27802, "x":7.86374, "y":5.93419, "heading":0.0, "vx":2.49493, "vy":0.23578, "omega":0.0, "ax":8.96632, "ay":0.84731, "alpha":0.0, "fx":[152.51459,152.51459,152.51459,152.51459], "fy":[14.41247,14.41247,14.41247,14.41247]},
41+
{"t":0.31774, "x":7.9699, "y":5.94422, "heading":0.0, "vx":2.85105, "vy":0.26943, "omega":0.0, "ax":8.96225, "ay":0.84691, "alpha":0.0, "fx":[152.44538,152.44538,152.44538,152.44538], "fy":[14.4057,14.4057,14.4057,14.4057]},
42+
{"t":0.35745, "x":8.0902, "y":5.95559, "heading":0.0, "vx":3.207, "vy":0.30306, "omega":0.0, "ax":8.95617, "ay":0.84631, "alpha":0.0, "fx":[152.34183,152.34183,152.34183,152.34183], "fy":[14.39556,14.39556,14.39556,14.39556]},
43+
{"t":0.39717, "x":8.22464, "y":5.9683, "heading":0.0, "vx":3.56271, "vy":0.33668, "omega":0.0, "ax":8.94606, "ay":0.84532, "alpha":0.0, "fx":[152.17,152.17,152.17,152.17], "fy":[14.37873,14.37873,14.37873,14.37873]},
44+
{"t":0.43689, "x":8.3732, "y":5.98233, "heading":0.0, "vx":3.91802, "vy":0.37025, "omega":0.0, "ax":8.92604, "ay":0.84336, "alpha":0.0, "fx":[151.82945,151.82945,151.82945,151.82945], "fy":[14.34539,14.34539,14.34539,14.34539]},
45+
{"t":0.4766, "x":8.53585, "y":5.9977, "heading":0.0, "vx":4.27254, "vy":0.40375, "omega":0.0, "ax":8.86758, "ay":0.83764, "alpha":0.0, "fx":[150.83499,150.83499,150.83499,150.83499], "fy":[14.24804,14.24804,14.24804,14.24804]},
46+
{"t":0.51632, "x":8.71253, "y":6.0144, "heading":0.0, "vx":4.62473, "vy":0.43702, "omega":0.0, "ax":6.5205, "ay":0.60893, "alpha":0.0, "fx":[110.91178,110.91178,110.91178,110.91178], "fy":[10.35766,10.35766,10.35766,10.35766]},
47+
{"t":0.55604, "x":8.90136, "y":6.03224, "heading":0.0, "vx":4.88371, "vy":0.4612, "omega":0.0, "ax":0.00887, "ay":-0.03665, "alpha":0.0, "fx":[0.15081,0.15081,0.15081,0.15081], "fy":[-0.62334,-0.62334,-0.62334,-0.62334]},
48+
{"t":0.59576, "x":9.09533, "y":6.05053, "heading":0.0, "vx":4.88406, "vy":0.45975, "omega":0.0, "ax":0.01488, "ay":-0.15922, "alpha":0.0, "fx":[0.25318,0.25318,0.25318,0.25318], "fy":[-2.70827,-2.70827,-2.70827,-2.70827]},
49+
{"t":0.63547, "x":9.28932, "y":6.06866, "heading":0.0, "vx":4.88465, "vy":0.45342, "omega":0.0, "ax":0.06319, "ay":-0.70277, "alpha":0.0, "fx":[1.07492,1.07492,1.07492,1.07492], "fy":[-11.9539,-11.9539,-11.9539,-11.9539]},
50+
{"t":0.67519, "x":9.48338, "y":6.08612, "heading":0.0, "vx":4.88716, "vy":0.42551, "omega":0.0, "ax":0.21331, "ay":-2.82728, "alpha":0.0, "fx":[3.62842,3.62842,3.62842,3.62842], "fy":[-48.09122,-48.09122,-48.09122,-48.09122]},
51+
{"t":0.71491, "x":9.67765, "y":6.10079, "heading":0.0, "vx":4.89563, "vy":0.31322, "omega":0.0, "ax":0.24273, "ay":-6.44263, "alpha":0.0, "fx":[4.12877,4.12877,4.12877,4.12877], "fy":[-109.58729,-109.58729,-109.58729,-109.58729]},
52+
{"t":0.75462, "x":9.87228, "y":6.10814, "heading":0.0, "vx":4.90527, "vy":0.05734, "omega":0.0, "ax":-0.1716, "ay":-8.07247, "alpha":0.0, "fx":[-2.91889,-2.91889,-2.91889,-2.91889], "fy":[-137.31035,-137.31035,-137.31035,-137.31035]},
53+
{"t":0.79434, "x":10.06697, "y":6.10405, "heading":0.0, "vx":4.89846, "vy":-0.26328, "omega":0.0, "ax":-0.76136, "ay":-8.54713, "alpha":0.0, "fx":[-12.95044,-12.95044,-12.95044,-12.95044], "fy":[-145.38424,-145.38424,-145.38424,-145.38424]},
54+
{"t":0.83406, "x":10.26092, "y":6.08686, "heading":0.0, "vx":4.86822, "vy":-0.60274, "omega":0.0, "ax":-1.39223, "ay":-8.66512, "alpha":0.0, "fx":[-23.68138,-23.68138,-23.68138,-23.68138], "fy":[-147.39115,-147.39115,-147.39115,-147.39115]},
55+
{"t":0.87377, "x":10.45317, "y":6.05608, "heading":0.0, "vx":4.81292, "vy":-0.9469, "omega":0.0, "ax":-2.03021, "ay":-8.63249, "alpha":0.0, "fx":[-34.53334,-34.53334,-34.53334,-34.53334], "fy":[-146.83613,-146.83613,-146.83613,-146.83613]},
56+
{"t":0.91349, "x":10.64273, "y":6.01167, "heading":0.0, "vx":4.73229, "vy":-1.28975, "omega":0.0, "ax":-2.66348, "ay":-8.51053, "alpha":0.0, "fx":[-45.30501,-45.30501,-45.30501,-45.30501], "fy":[-144.76175,-144.76175,-144.76175,-144.76175]},
57+
{"t":0.95321, "x":10.82858, "y":5.95373, "heading":0.0, "vx":4.62651, "vy":-1.62777, "omega":0.0, "ax":-3.28562, "ay":-8.32203, "alpha":0.0, "fx":[-55.88748,-55.88748,-55.88748,-55.88748], "fy":[-141.55541,-141.55541,-141.55541,-141.55541]},
58+
{"t":0.99293, "x":11.00974, "y":5.88252, "heading":0.0, "vx":4.49601, "vy":-1.95829, "omega":0.0, "ax":-3.88611, "ay":-8.08019, "alpha":0.0, "fx":[-66.10165,-66.10165,-66.10165,-66.10165], "fy":[-137.44164,-137.44164,-137.44164,-137.44164]},
59+
{"t":1.03264, "x":11.18524, "y":5.79836, "heading":0.0, "vx":4.34167, "vy":-2.27921, "omega":0.0, "ax":-4.45187, "ay":-7.78475, "alpha":0.0, "fx":[-75.72496,-75.72496,-75.72496,-75.72496], "fy":[-132.41634,-132.41634,-132.41634,-132.41634]},
60+
{"t":1.07363, "x":11.35947, "y":5.6984, "heading":0.0, "vx":4.15918, "vy":-2.59831, "omega":0.0, "ax":-5.01091, "ay":-7.41422, "alpha":0.0, "fx":[-85.23411,-85.23411,-85.23411,-85.23411], "fy":[-126.11376,-126.11376,-126.11376,-126.11376]},
61+
{"t":1.11462, "x":11.52574, "y":5.58566, "heading":0.0, "vx":3.95378, "vy":-2.90223, "omega":0.0, "ax":-5.53411, "ay":-6.9939, "alpha":0.0, "fx":[-94.13368,-94.13368,-94.13368,-94.13368], "fy":[-118.9642,-118.9642,-118.9642,-118.9642]},
62+
{"t":1.15561, "x":11.68316, "y":5.46082, "heading":0.0, "vx":3.72694, "vy":-3.18891, "omega":0.0, "ax":-6.00525, "ay":-6.52385, "alpha":0.0, "fx":[-102.14758,-102.14758,-102.14758,-102.14758], "fy":[-110.9688,-110.9688,-110.9688,-110.9688]},
63+
{"t":1.1966, "x":11.83089, "y":5.32463, "heading":0.0, "vx":3.48078, "vy":-3.45633, "omega":0.0, "ax":-6.39902, "ay":-5.99529, "alpha":0.0, "fx":[-108.84549,-108.84549,-108.84549,-108.84549], "fy":[-101.97816,-101.97816,-101.97816,-101.97816]},
64+
{"t":1.23759, "x":11.96819, "y":5.17791, "heading":0.0, "vx":3.21848, "vy":-3.70208, "omega":0.0, "ax":-6.64705, "ay":-5.37827, "alpha":0.0, "fx":[-113.06446,-113.06446,-113.06446,-113.06446], "fy":[-91.48279,-91.48279,-91.48279,-91.48279]},
65+
{"t":1.27859, "x":12.09453, "y":5.02165, "heading":0.0, "vx":2.94601, "vy":-3.92253, "omega":0.0, "ax":-6.50634, "ay":-4.55903, "alpha":0.0, "fx":[-110.67103,-110.67103,-110.67103,-110.67103], "fy":[-77.54782,-77.54782,-77.54782,-77.54782]},
66+
{"t":1.31958, "x":12.20982, "y":4.85703, "heading":0.0, "vx":2.67931, "vy":-4.10941, "omega":0.0, "ax":-5.00515, "ay":-3.09152, "alpha":0.0, "fx":[-85.13619,-85.13619,-85.13619,-85.13619], "fy":[-52.58589,-52.58589,-52.58589,-52.58589]},
67+
{"t":1.36057, "x":12.31545, "y":4.68599, "heading":0.0, "vx":2.47415, "vy":-4.23613, "omega":0.0, "ax":-1.85189, "ay":-1.05972, "alpha":0.0, "fx":[-31.50012,-31.50012,-31.50012,-31.50012], "fy":[-18.02551,-18.02551,-18.02551,-18.02551]},
68+
{"t":1.40156, "x":12.41531, "y":4.51145, "heading":0.0, "vx":2.39824, "vy":-4.27957, "omega":0.0, "ax":-0.42066, "ay":-0.23463, "alpha":0.0, "fx":[-7.15536,-7.15536,-7.15536,-7.15536], "fy":[-3.99107,-3.99107,-3.99107,-3.99107]},
69+
{"t":1.44255, "x":12.51326, "y":4.33583, "heading":0.0, "vx":2.381, "vy":-4.28919, "omega":0.0, "ax":-0.09013, "ay":-0.04998, "alpha":0.0, "fx":[-1.53316,-1.53316,-1.53316,-1.53316], "fy":[-0.85023,-0.85023,-0.85023,-0.85023]},
70+
{"t":1.48354, "x":12.61078, "y":4.15998, "heading":0.0, "vx":2.3773, "vy":-4.29124, "omega":0.0, "ax":-0.01924, "ay":-0.01066, "alpha":0.0, "fx":[-0.32728,-0.32728,-0.32728,-0.32728], "fy":[-0.18127,-0.18127,-0.18127,-0.18127]},
71+
{"t":1.52453, "x":12.70821, "y":3.98407, "heading":0.0, "vx":2.37651, "vy":-4.29168, "omega":0.0, "ax":-0.00411, "ay":-0.00227, "alpha":0.0, "fx":[-0.06984,-0.06984,-0.06984,-0.06984], "fy":[-0.03867,-0.03867,-0.03867,-0.03867]},
72+
{"t":1.56552, "x":12.80562, "y":3.80815, "heading":0.0, "vx":2.37634, "vy":-4.29177, "omega":0.0, "ax":-0.00088, "ay":-0.00049, "alpha":0.0, "fx":[-0.0149,-0.0149,-0.0149,-0.0149], "fy":[-0.00825,-0.00825,-0.00825,-0.00825]},
73+
{"t":1.60651, "x":12.90303, "y":3.63223, "heading":0.0, "vx":2.37631, "vy":-4.29179, "omega":0.0, "ax":-0.00019, "ay":-0.0001, "alpha":0.0, "fx":[-0.00318,-0.00318,-0.00318,-0.00318], "fy":[-0.00176,-0.00176,-0.00176,-0.00176]},
74+
{"t":1.6475, "x":13.00044, "y":3.4563, "heading":0.0, "vx":2.3763, "vy":-4.29179, "omega":0.0, "ax":-0.00004, "ay":-0.00002, "alpha":0.0, "fx":[-0.00068,-0.00068,-0.00068,-0.00068], "fy":[-0.00038,-0.00038,-0.00038,-0.00038]},
75+
{"t":1.68849, "x":13.09784, "y":3.28038, "heading":0.0, "vx":2.3763, "vy":-4.29179, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00015,-0.00015,-0.00015,-0.00015], "fy":[-0.00008,-0.00008,-0.00008,-0.00008]},
76+
{"t":1.72948, "x":13.19525, "y":3.10446, "heading":0.0, "vx":2.3763, "vy":-4.29179, "omega":0.0, "ax":-0.00049, "ay":0.00089, "alpha":0.0, "fx":[-0.00839,-0.00839,-0.00839,-0.00839], "fy":[0.01508,0.01508,0.01508,0.01508]},
77+
{"t":1.77047, "x":13.29265, "y":2.92853, "heading":0.0, "vx":2.37628, "vy":-4.29176, "omega":0.0, "ax":-1.31255, "ay":2.37058, "alpha":0.0, "fx":[-22.32613,-22.32613,-22.32613,-22.32613], "fy":[40.32283,40.32283,40.32283,40.32283]},
78+
{"t":1.81146, "x":13.38895, "y":2.7546, "heading":0.0, "vx":2.32248, "vy":-4.19459, "omega":0.0, "ax":-4.31413, "ay":7.79168, "alpha":0.0, "fx":[-73.38214,-73.38214,-73.38214,-73.38214], "fy":[132.53427,132.53427,132.53427,132.53427]},
79+
{"t":1.85245, "x":13.48053, "y":2.58921, "heading":0.0, "vx":2.14564, "vy":-3.8752, "omega":0.0, "ax":-4.34356, "ay":7.84483, "alpha":0.0, "fx":[-73.88274,-73.88274,-73.88274,-73.88274], "fy":[133.4384,133.4384,133.4384,133.4384]},
80+
{"t":1.89344, "x":13.56483, "y":2.43696, "heading":0.0, "vx":1.96759, "vy":-3.55364, "omega":0.0, "ax":-4.35329, "ay":7.86241, "alpha":0.0, "fx":[-74.04824,-74.04824,-74.04824,-74.04824], "fy":[133.73731,133.73731,133.73731,133.73731]},
81+
{"t":1.93443, "x":13.64183, "y":2.2979, "heading":0.0, "vx":1.78915, "vy":-3.23135, "omega":0.0, "ax":-4.35813, "ay":7.87115, "alpha":0.0, "fx":[-74.13057,-74.13057,-74.13057,-74.13057], "fy":[133.886,133.886,133.886,133.886]},
82+
{"t":1.97542, "x":13.7115, "y":2.17205, "heading":0.0, "vx":1.61051, "vy":-2.90871, "omega":0.0, "ax":-4.36103, "ay":7.87638, "alpha":0.0, "fx":[-74.17981,-74.17981,-74.17981,-74.17981], "fy":[133.97493,133.97493,133.97493,133.97493]},
83+
{"t":2.01641, "x":13.77386, "y":2.05944, "heading":0.0, "vx":1.43175, "vy":-2.58585, "omega":0.0, "ax":-4.36295, "ay":7.87985, "alpha":0.0, "fx":[-74.21255,-74.21255,-74.21255,-74.21255], "fy":[134.03407,134.03407,134.03407,134.03407]},
84+
{"t":2.05741, "x":13.82888, "y":1.96007, "heading":0.0, "vx":1.25291, "vy":-2.26286, "omega":0.0, "ax":-4.36432, "ay":7.88233, "alpha":0.0, "fx":[-74.2359,-74.2359,-74.2359,-74.2359], "fy":[134.07624,134.07624,134.07624,134.07624]},
85+
{"t":2.0984, "x":13.87657, "y":1.87393, "heading":0.0, "vx":1.07401, "vy":-1.93975, "omega":0.0, "ax":-4.36535, "ay":7.88419, "alpha":0.0, "fx":[-74.25339,-74.25339,-74.25339,-74.25339], "fy":[134.10782,134.10782,134.10782,134.10782]},
86+
{"t":2.13939, "x":13.91693, "y":1.80104, "heading":0.0, "vx":0.89507, "vy":-1.61658, "omega":0.0, "ax":-4.36615, "ay":7.88563, "alpha":0.0, "fx":[-74.26697,-74.26697,-74.26697,-74.26697], "fy":[134.13236,134.13236,134.13236,134.13236]},
87+
{"t":2.18038, "x":13.94995, "y":1.7414, "heading":0.0, "vx":0.7161, "vy":-1.29334, "omega":0.0, "ax":-4.36679, "ay":7.88679, "alpha":0.0, "fx":[-74.27783,-74.27783,-74.27783,-74.27783], "fy":[134.15197,134.15197,134.15197,134.15197]},
88+
{"t":2.22137, "x":13.97563, "y":1.69502, "heading":0.0, "vx":0.53711, "vy":-0.97006, "omega":0.0, "ax":-4.36731, "ay":7.88773, "alpha":0.0, "fx":[-74.28671,-74.28671,-74.28671,-74.28671], "fy":[134.168,134.168,134.168,134.168]},
89+
{"t":2.26236, "x":13.99398, "y":1.66188, "heading":0.0, "vx":0.35809, "vy":-0.64674, "omega":0.0, "ax":-4.36775, "ay":7.88851, "alpha":0.0, "fx":[-74.2941,-74.2941,-74.2941,-74.2941], "fy":[134.18135,134.18135,134.18135,134.18135]},
90+
{"t":2.30335, "x":14.00499, "y":1.642, "heading":0.0, "vx":0.17905, "vy":-0.32338, "omega":0.0, "ax":-4.36811, "ay":7.88918, "alpha":0.0, "fx":[-74.30036,-74.30036,-74.30036,-74.30036], "fy":[134.19265,134.19265,134.19265,134.19265]},
91+
{"t":2.34434, "x":14.00866, "y":1.63537, "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]}],
92+
"splits":[0]
93+
},
94+
"events":[]
95+
}

‎Template/src/main/java/frc/robot/Robot.java‎

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,6 @@ public class Robot extends LoggedRobot {
2929
public static Swerve swerve = new Swerve();
3030
public static ExampleElevator exampleElevator = new ExampleElevator();
3131

32-
Command autoCommand = null;
33-
3432
public Robot() {
3533
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
3634
Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
@@ -97,11 +95,11 @@ public void disabledExit() {
9795

9896
@Override
9997
public void autonomousInit() {
100-
autoCommand = AutoUtils.getChooser().selectedCommand();
98+
AutoUtils.runSelectedCommand();
99+
// Dont use ^ anymore. See AutoUtils.setupAutoTrigger()
101100

102-
if (autoCommand != null) {
103-
autoCommand.schedule();
104-
}
101+
// In here should be just any special setup needed before auto starts
102+
// For example, if we do piece detection, maybe we would want to select a different preset
105103
}
106104

107105
@Override
@@ -111,10 +109,7 @@ public void autonomousPeriodic() {
111109

112110
@Override
113111
public void autonomousExit() {
114-
if (autoCommand != null) {
115-
autoCommand.cancel();
116-
autoCommand = null;
117-
}
112+
118113
}
119114

120115
@Override

‎Template/src/main/java/frc/robot/commands/auto/AutoUtils.java‎

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ public class AutoUtils {
4242
public static void initAutoUtils() {
4343
setupFactory();
4444
setupChooser();
45-
setupAutoTrigger();
45+
// setupAutoTrigger();
4646
}
4747

4848
public static void setupFactory() {
@@ -63,12 +63,22 @@ public static void setupChooser() {
6363

6464
chooser.addCmd("No Auto", () -> Commands.print("[AutoUtils]: No Auto Selected"));
6565
chooser.addCmd("Sample", () -> getSingleTrajectoryCommand("Sample"));
66+
chooser.addCmd("Random", () -> getSingleTrajectoryCommand("Random"));
6667

6768
chooser.select("Sample");
6869
}
6970

7071
public static void setupAutoTrigger() {
71-
RobotModeTriggers.autonomous().whileTrue(chooser.selectedCommand());
72+
RobotModeTriggers.autonomous().whileTrue(chooser.selectedCommandScheduler());
73+
}
74+
75+
public static void runSelectedCommand() {
76+
Command autoCmd = chooser.selectedCommand();
77+
if (autoCmd != null) {
78+
autoCmd.schedule();
79+
} else {
80+
System.out.println("[AutoUtils]: No Auto Command Selected");
81+
}
7282
}
7383

7484
/* Adds routine to the auto chooser */
@@ -87,7 +97,7 @@ public static void applyEventMarkers(AutoTrajectory traj, String... markers) {
8797
System.out.println("[AutoUtils]: No command found for event marker: " + marker);
8898
}
8999

90-
traj.atPose(marker, ACCURACY.DRIVE_TRANSLATE_TOLERANCE_M, ACCURACY.DRIVE_ROTATION_TOLERANCE.getRadians())
100+
traj.atPose(marker, ACCURACY.DRIVE_TRANSLATE_TOLERANCE_M, ACCURACY.DRIVE_ROTATION_TOLERANCE_RAD)
91101
.onTrue(eventMarkerCommands.get(marker));
92102
}
93103
}
@@ -97,21 +107,21 @@ public static AutoChooser getChooser() {
97107
}
98108

99109
// public static AutoFactory getAutoFactory() {
100-
// return factory;
110+
// return factory;
101111
// }
102112

103113
public static Command getSingleTrajectoryCommand(String trajectoryName) {
104114
AutoRoutine routine = factory.newRoutine(trajectoryName);
105115
AutoTrajectory traj = routine.trajectory(trajectoryName);
106-
applyEventMarkers(traj, "score");
116+
// applyEventMarkers(traj, "score");
107117
// traj.atTime("Score").onTrue(eventMarkerCommands.get("score"));
108118

109119
routine.active().onTrue(Commands.sequence(
110120
traj.resetOdometry(),
111121
traj.cmd()));
112-
122+
113123
Logger.recordOutput("AutoUtils/" + trajectoryName + "/InitialPose", traj.getInitialPose().orElse(Pose2d.kZero));
114-
124+
115125
return Commands.sequence(
116126
Commands.print("[AutoUtils]: Running - " + trajectoryName),
117127
routine.cmd(),

0 commit comments

Comments
 (0)