From fb26593905ecdc53a2958804d03541164ea569a5 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Tue, 17 Feb 2026 11:54:53 -0500 Subject: [PATCH 01/22] added left, right, mid trajectories --- src/main/deploy/choreo/LeftToTower.traj | 102 +++++++++++++++++++++++ src/main/deploy/choreo/MidToTower.traj | 91 ++++++++++++++++++++ src/main/deploy/choreo/RightToTower.traj | 94 +++++++++++++++++++++ 3 files changed, 287 insertions(+) create mode 100644 src/main/deploy/choreo/LeftToTower.traj create mode 100644 src/main/deploy/choreo/MidToTower.traj create mode 100644 src/main/deploy/choreo/RightToTower.traj diff --git a/src/main/deploy/choreo/LeftToTower.traj b/src/main/deploy/choreo/LeftToTower.traj new file mode 100644 index 0000000..21ea972 --- /dev/null +++ b/src/main/deploy/choreo/LeftToTower.traj @@ -0,0 +1,102 @@ +{ + "name":"LeftToTower", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.635439157485962, "y":7.431572914123535, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.020376205444336, "y":3.180727243423462, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "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":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.635439157485962 m", "val":3.635439157485962}, "y":{"exp":"7.431572914123535 m", "val":7.431572914123535}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.020376205444336 m", "val":2.020376205444336}, "y":{"exp":"3.180727243423462 m", "val":3.180727243423462}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.24933,1.67027], + "samples":[ + {"t":0.0, "x":3.63544, "y":7.43157, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.00557, "ay":-8.79207, "alpha":0.0, "fx":[-34.11416,-34.11416,-34.11416,-34.11416], "fy":[-149.5506,-149.5506,-149.5506,-149.5506]}, + {"t":0.04164, "x":3.6337, "y":7.42395, "heading":0.0, "vx":-0.08352, "vy":-0.36614, "omega":0.0, "ax":-2.0054, "ay":-8.79134, "alpha":0.0, "fx":[-34.11133,-34.11133,-34.11133,-34.11133], "fy":[-149.53818,-149.53818,-149.53818,-149.53818]}, + {"t":0.08329, "x":3.62848, "y":7.40108, "heading":0.0, "vx":-0.16703, "vy":-0.73225, "omega":0.0, "ax":-2.00521, "ay":-8.79048, "alpha":0.0, "fx":[-34.10798,-34.10798,-34.10798,-34.10798], "fy":[-149.52347,-149.52347,-149.52347,-149.52347]}, + {"t":0.12493, "x":3.61979, "y":7.36296, "heading":0.0, "vx":-0.25054, "vy":-1.09833, "omega":0.0, "ax":-2.00497, "ay":-8.78944, "alpha":0.0, "fx":[-34.10394,-34.10394,-34.10394,-34.10394], "fy":[-149.50578,-149.50578,-149.50578,-149.50578]}, + {"t":0.16658, "x":3.60762, "y":7.3096, "heading":0.0, "vx":-0.33404, "vy":-1.46436, "omega":0.0, "ax":-2.00468, "ay":-8.78816, "alpha":0.0, "fx":[-34.099,-34.099,-34.099,-34.099], "fy":[-149.48411,-149.48411,-149.48411,-149.48411]}, + {"t":0.20822, "x":3.59197, "y":7.241, "heading":0.0, "vx":-0.41752, "vy":-1.83034, "omega":0.0, "ax":-2.00431, "ay":-8.78656, "alpha":0.0, "fx":[-34.0928,-34.0928,-34.0928,-34.0928], "fy":[-149.45694,-149.45694,-149.45694,-149.45694]}, + {"t":0.24987, "x":3.57284, "y":7.15716, "heading":0.0, "vx":-0.50099, "vy":-2.19625, "omega":0.0, "ax":-2.00384, "ay":-8.7845, "alpha":0.0, "fx":[-34.0848,-34.0848,-34.0848,-34.0848], "fy":[-149.42186,-149.42186,-149.42186,-149.42186]}, + {"t":0.29151, "x":3.55024, "y":7.05808, "heading":0.0, "vx":-0.58444, "vy":-2.56207, "omega":0.0, "ax":-2.00321, "ay":-8.78174, "alpha":0.0, "fx":[-34.07408,-34.07408,-34.07408,-34.07408], "fy":[-149.37485,-149.37485,-149.37485,-149.37485]}, + {"t":0.33316, "x":3.52416, "y":6.94377, "heading":0.0, "vx":-0.66786, "vy":-2.92778, "omega":0.0, "ax":-2.00232, "ay":-8.77784, "alpha":0.0, "fx":[-34.05897,-34.05897,-34.05897,-34.05897], "fy":[-149.30859,-149.30859,-149.30859,-149.30859]}, + {"t":0.3748, "x":3.49462, "y":6.81423, "heading":0.0, "vx":-0.75125, "vy":-3.29333, "omega":0.0, "ax":-2.00098, "ay":-8.77194, "alpha":0.0, "fx":[-34.03608,-34.03608,-34.03608,-34.03608], "fy":[-149.20821,-149.20821,-149.20821,-149.20821]}, + {"t":0.41644, "x":3.4616, "y":6.66947, "heading":0.0, "vx":-0.83458, "vy":-3.65864, "omega":0.0, "ax":-1.9987, "ay":-8.76195, "alpha":0.0, "fx":[-33.99734,-33.99734,-33.99734,-33.99734], "fy":[-149.03835,-149.03835,-149.03835,-149.03835]}, + {"t":0.45809, "x":3.42511, "y":6.50951, "heading":0.0, "vx":-0.91781, "vy":-4.02352, "omega":0.0, "ax":-1.99402, "ay":-8.74142, "alpha":0.0, "fx":[-33.9177,-33.9177,-33.9177,-33.9177], "fy":[-148.68913,-148.68913,-148.68913,-148.68913]}, + {"t":0.49973, "x":3.38516, "y":6.33438, "heading":0.0, "vx":-1.00085, "vy":-4.38755, "omega":0.0, "ax":-1.97898, "ay":-8.67549, "alpha":0.0, "fx":[-33.66192,-33.66192,-33.66192,-33.66192], "fy":[-147.56753,-147.56753,-147.56753,-147.56753]}, + {"t":0.54138, "x":3.34176, "y":6.14414, "heading":0.0, "vx":-1.08326, "vy":-4.74884, "omega":0.0, "ax":-0.18693, "ay":-0.81836, "alpha":0.0, "fx":[-3.17971,-3.17971,-3.17971,-3.17971], "fy":[-13.92011,-13.92011,-13.92011,-13.92011]}, + {"t":0.58302, "x":3.29649, "y":5.94566, "heading":0.0, "vx":-1.09105, "vy":-4.78292, "omega":0.0, "ax":-0.00111, "ay":-0.00003, "alpha":0.0, "fx":[-0.01885,-0.01885,-0.01885,-0.01885], "fy":[-0.00044,-0.00044,-0.00044,-0.00044]}, + {"t":0.62467, "x":3.25105, "y":5.74648, "heading":0.0, "vx":-1.0911, "vy":-4.78292, "omega":0.0, "ax":-0.00502, "ay":0.00115, "alpha":0.0, "fx":[-0.0854,-0.0854,-0.0854,-0.0854], "fy":[0.01948,0.01948,0.01948,0.01948]}, + {"t":0.66631, "x":3.20561, "y":5.5473, "heading":0.0, "vx":-1.0913, "vy":-4.78287, "omega":0.0, "ax":-0.02419, "ay":0.00552, "alpha":0.0, "fx":[-0.41146,-0.41146,-0.41146,-0.41146], "fy":[0.09393,0.09393,0.09393,0.09393]}, + {"t":0.70796, "x":3.16014, "y":5.34812, "heading":0.0, "vx":-1.09231, "vy":-4.78264, "omega":0.0, "ax":-0.11654, "ay":0.02668, "alpha":0.0, "fx":[-1.98231,-1.98231,-1.98231,-1.98231], "fy":[0.45381,0.45381,0.45381,0.45381]}, + {"t":0.7496, "x":3.11455, "y":5.14898, "heading":0.0, "vx":-1.09716, "vy":-4.78153, "omega":0.0, "ax":-0.55903, "ay":0.12972, "alpha":0.0, "fx":[-9.50887,-9.50887,-9.50887,-9.50887], "fy":[2.20649,2.20649,2.20649,2.20649]}, + {"t":0.79124, "x":3.06837, "y":4.94996, "heading":0.0, "vx":-1.12045, "vy":-4.77613, "omega":0.0, "ax":-2.47526, "ay":0.60916, "alpha":0.0, "fx":[-42.10344,-42.10344,-42.10344,-42.10344], "fy":[10.36168,10.36168,10.36168,10.36168]}, + {"t":0.83289, "x":3.01957, "y":4.75159, "heading":0.0, "vx":-1.22353, "vy":-4.75076, "omega":0.0, "ax":-6.12231, "ay":1.75537, "alpha":0.0, "fx":[-104.13881,-104.13881,-104.13881,-104.13881], "fy":[29.85842,29.85842,29.85842,29.85842]}, + {"t":0.87453, "x":2.9633, "y":4.55527, "heading":0.0, "vx":-1.47849, "vy":-4.67766, "omega":0.0, "ax":-7.63967, "ay":2.70927, "alpha":0.0, "fx":[-129.94863,-129.94863,-129.94863,-129.94863], "fy":[46.08387,46.08387,46.08387,46.08387]}, + {"t":0.91618, "x":2.89511, "y":4.36282, "heading":0.0, "vx":-1.79664, "vy":-4.56483, "omega":0.0, "ax":-7.88972, "ay":3.44726, "alpha":0.0, "fx":[-134.2019,-134.2019,-134.2019,-134.2019], "fy":[58.63697,58.63697,58.63697,58.63697]}, + {"t":0.95782, "x":2.81345, "y":4.17571, "heading":0.0, "vx":-2.1252, "vy":-4.42128, "omega":0.0, "ax":-7.76862, "ay":4.12643, "alpha":0.0, "fx":[-132.142,-132.142,-132.142,-132.142], "fy":[70.18936,70.18936,70.18936,70.18936]}, + {"t":0.99947, "x":2.71821, "y":3.99517, "heading":0.0, "vx":-2.44872, "vy":-4.24943, "omega":0.0, "ax":-6.68475, "ay":5.85285, "alpha":0.0, "fx":[-113.70573,-113.70573,-113.70573,-113.70573], "fy":[99.55524,99.55524,99.55524,99.55524]}, + {"t":1.04111, "x":2.61044, "y":3.82328, "heading":0.0, "vx":-2.7271, "vy":-4.00569, "omega":0.0, "ax":-2.50406, "ay":8.59359, "alpha":0.0, "fx":[-42.5934,-42.5934,-42.5934,-42.5934], "fy":[146.17452,146.17452,146.17452,146.17452]}, + {"t":1.08276, "x":2.4947, "y":3.66392, "heading":0.0, "vx":-2.83138, "vy":-3.64782, "omega":0.0, "ax":-0.77091, "ay":8.9464, "alpha":0.0, "fx":[-13.11294,-13.11294,-13.11294,-13.11294], "fy":[152.17579,152.17579,152.17579,152.17579]}, + {"t":1.1244, "x":2.37612, "y":3.51976, "heading":0.0, "vx":-2.86349, "vy":-3.27525, "omega":0.0, "ax":0.0268, "ay":8.99294, "alpha":0.0, "fx":[0.45588,0.45588,0.45588,0.45588], "fy":[152.96738,152.96738,152.96738,152.96738]}, + {"t":1.16604, "x":2.25689, "y":3.39116, "heading":0.0, "vx":-2.86237, "vy":-2.90074, "omega":0.0, "ax":0.4735, "ay":8.98811, "alpha":0.0, "fx":[8.05402,8.05402,8.05402,8.05402], "fy":[152.88512,152.88512,152.88512,152.88512]}, + {"t":1.20769, "x":2.1381, "y":3.27816, "heading":0.0, "vx":-2.84265, "vy":-2.52644, "omega":0.0, "ax":0.75693, "ay":8.97354, "alpha":0.0, "fx":[12.87519,12.87519,12.87519,12.87519], "fy":[152.63737,152.63737,152.63737,152.63737]}, + {"t":1.24933, "x":2.02038, "y":3.18073, "heading":0.0, "vx":-2.81113, "vy":-2.15274, "omega":0.0, "ax":1.67461, "ay":8.84622, "alpha":0.0, "fx":[28.48471,28.48471,28.48471,28.48471], "fy":[150.47163,150.47163,150.47163,150.47163]}, + {"t":1.28441, "x":1.9228, "y":3.11066, "heading":0.0, "vx":-2.75239, "vy":-1.84243, "omega":0.0, "ax":3.26486, "ay":8.39064, "alpha":0.0, "fx":[55.53427,55.53427,55.53427,55.53427], "fy":[142.72235,142.72235,142.72235,142.72235]}, + {"t":1.31949, "x":1.82826, "y":3.05119, "heading":0.0, "vx":-2.63786, "vy":-1.5481, "omega":0.0, "ax":4.68595, "ay":7.68891, "alpha":0.0, "fx":[79.70672,79.70672,79.70672,79.70672], "fy":[130.78619,130.78619,130.78619,130.78619]}, + {"t":1.35457, "x":1.73861, "y":3.00161, "heading":0.0, "vx":-2.47349, "vy":-1.27838, "omega":0.0, "ax":5.84682, "ay":6.8496, "alpha":0.0, "fx":[99.45279,99.45279,99.45279,99.45279], "fy":[116.50965,116.50965,116.50965,116.50965]}, + {"t":1.38965, "x":1.65544, "y":2.96098, "heading":0.0, "vx":-2.26839, "vy":-1.03811, "omega":0.0, "ax":6.7338, "ay":5.98226, "alpha":0.0, "fx":[114.54008,114.54008,114.54008,114.54008], "fy":[101.75654,101.75654,101.75654,101.75654]}, + {"t":1.42473, "x":1.58001, "y":2.92825, "heading":0.0, "vx":-2.03218, "vy":-0.82826, "omega":0.0, "ax":7.3838, "ay":5.16149, "alpha":0.0, "fx":[125.59625,125.59625,125.59625,125.59625], "fy":[87.79553,87.79553,87.79553,87.79553]}, + {"t":1.4598, "x":1.51327, "y":2.90237, "heading":0.0, "vx":-1.77316, "vy":-0.64721, "omega":0.0, "ax":7.85009, "ay":4.42333, "alpha":0.0, "fx":[133.52778,133.52778,133.52778,133.52778], "fy":[75.23954,75.23954,75.23954,75.23954]}, + {"t":1.49488, "x":1.4559, "y":2.88239, "heading":0.0, "vx":-1.4978, "vy":-0.49204, "omega":0.0, "ax":8.18214, "ay":3.77729, "alpha":0.0, "fx":[139.17588,139.17588,139.17588,139.17588], "fy":[64.25069,64.25069,64.25069,64.25069]}, + {"t":1.52996, "x":1.40839, "y":2.86745, "heading":0.0, "vx":-1.21078, "vy":-0.35954, "omega":0.0, "ax":8.41878, "ay":3.21904, "alpha":0.0, "fx":[143.20108,143.20108,143.20108,143.20108], "fy":[54.75498,54.75498,54.75498,54.75498]}, + {"t":1.56504, "x":1.3711, "y":2.85682, "heading":0.0, "vx":-0.91546, "vy":-0.24662, "omega":0.0, "ax":8.58825, "ay":2.73862, "alpha":0.0, "fx":[146.08371,146.08371,146.08371,146.08371], "fy":[46.58314,46.58314,46.58314,46.58314]}, + {"t":1.60012, "x":1.34427, "y":2.84986, "heading":0.0, "vx":-0.6142, "vy":-0.15056, "omega":0.0, "ax":8.71038, "ay":2.32483, "alpha":0.0, "fx":[148.16113,148.16113,148.16113,148.16113], "fy":[39.54461,39.54461,39.54461,39.54461]}, + {"t":1.6352, "x":1.32808, "y":2.846, "heading":0.0, "vx":-0.30865, "vy":-0.069, "omega":0.0, "ax":8.79894, "ay":1.96714, "alpha":0.0, "fx":[149.66739,149.66739,149.66739,149.66739], "fy":[33.46043,33.46043,33.46043,33.46043]}, + {"t":1.67027, "x":1.32267, "y":2.84479, "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]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/MidToTower.traj b/src/main/deploy/choreo/MidToTower.traj new file mode 100644 index 0000000..cc2a51a --- /dev/null +++ b/src/main/deploy/choreo/MidToTower.traj @@ -0,0 +1,91 @@ +{ + "name":"MidToTower", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.609598159790039, "y":4.007639408111572, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.1108198165893555, "y":2.857714891433716, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "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":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.609598159790039 m", "val":3.609598159790039}, "y":{"exp":"4.007639408111572 m", "val":4.007639408111572}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.1108198165893555 m", "val":2.1108198165893555}, "y":{"exp":"2.857714891433716 m", "val":2.857714891433716}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.70219,1.129], + "samples":[ + {"t":0.0, "x":3.6096, "y":4.00764, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.7722, "ay":-6.92692, "alpha":0.0, "fx":[-98.18353,-98.18353,-98.18353,-98.18353], "fy":[-117.82496,-117.82496,-117.82496,-117.82496]}, + {"t":0.03696, "x":3.60566, "y":4.00291, "heading":0.0, "vx":-0.21333, "vy":-0.256, "omega":0.0, "ax":-5.83207, "ay":-6.8755, "alpha":0.0, "fx":[-99.20183,-99.20183,-99.20183,-99.20183], "fy":[-116.95025,-116.95025,-116.95025,-116.95025]}, + {"t":0.07392, "x":3.59379, "y":3.98875, "heading":0.0, "vx":-0.42887, "vy":-0.5101, "omega":0.0, "ax":-5.90213, "ay":-6.81415, "alpha":0.0, "fx":[-100.39357,-100.39357,-100.39357,-100.39357], "fy":[-115.90667,-115.90667,-115.90667,-115.90667]}, + {"t":0.11087, "x":3.57391, "y":3.96525, "heading":0.0, "vx":-0.64699, "vy":-0.76194, "omega":0.0, "ax":-5.98519, "ay":-6.73973, "alpha":0.0, "fx":[-101.80644,-101.80644,-101.80644,-101.80644], "fy":[-114.64086,-114.64086,-114.64086,-114.64086]}, + {"t":0.14783, "x":3.54591, "y":3.93248, "heading":0.0, "vx":-0.86819, "vy":-1.01102, "omega":0.0, "ax":-6.08517, "ay":-6.64766, "alpha":0.0, "fx":[-103.50701,-103.50701,-103.50701,-103.50701], "fy":[-113.07477,-113.07477,-113.07477,-113.07477]}, + {"t":0.18479, "x":3.50967, "y":3.89058, "heading":0.0, "vx":-1.09309, "vy":-1.2567, "omega":0.0, "ax":-6.20767, "ay":-6.53096, "alpha":0.0, "fx":[-105.59073,-105.59073,-105.59073,-105.59073], "fy":[-111.08972,-111.08972,-111.08972,-111.08972]}, + {"t":0.22175, "x":3.46503, "y":3.83967, "heading":0.0, "vx":-1.32251, "vy":-1.49807, "omega":0.0, "ax":-6.361, "ay":-6.37852, "alpha":0.0, "fx":[-108.19872,-108.19872,-108.19872,-108.19872], "fy":[-108.49683,-108.49683,-108.49683,-108.49683]}, + {"t":0.2587, "x":3.41181, "y":3.77995, "heading":0.0, "vx":-1.55759, "vy":-1.73381, "omega":0.0, "ax":-6.55783, "ay":-6.17164, "alpha":0.0, "fx":[-111.54682,-111.54682,-111.54682,-111.54682], "fy":[-104.97779,-104.97779,-104.97779,-104.97779]}, + {"t":0.29566, "x":3.34977, "y":3.71166, "heading":0.0, "vx":-1.79996, "vy":-1.9619, "omega":0.0, "ax":-6.81824, "ay":-5.87646, "alpha":0.0, "fx":[-115.97624,-115.97624,-115.97624,-115.97624], "fy":[-99.9569,-99.9569,-99.9569,-99.9569]}, + {"t":0.33262, "x":3.27859, "y":3.63514, "heading":0.0, "vx":-2.05194, "vy":-2.17908, "omega":0.0, "ax":-7.17457, "ay":-5.42601, "alpha":0.0, "fx":[-122.03743,-122.03743,-122.03743,-122.03743], "fy":[-92.29494,-92.29494,-92.29494,-92.29494]}, + {"t":0.36958, "x":3.19785, "y":3.5509, "heading":0.0, "vx":-2.3171, "vy":-2.37961, "omega":0.0, "ax":-7.67647, "ay":-4.67197, "alpha":0.0, "fx":[-130.57459,-130.57459,-130.57459,-130.57459], "fy":[-79.46887,-79.46887,-79.46887,-79.46887]}, + {"t":0.40653, "x":3.10698, "y":3.45977, "heading":0.0, "vx":-2.6008, "vy":-2.55227, "omega":0.0, "ax":-8.36556, "ay":-3.24198, "alpha":0.0, "fx":[-142.29585,-142.29585,-142.29585,-142.29585], "fy":[-55.14515,-55.14515,-55.14515,-55.14515]}, + {"t":0.44349, "x":3.00514, "y":3.36323, "heading":0.0, "vx":-2.90997, "vy":-2.67209, "omega":0.0, "ax":-8.94655, "ay":-0.18463, "alpha":0.0, "fx":[-152.17829,-152.17829,-152.17829,-152.17829], "fy":[-3.14051,-3.14051,-3.14051,-3.14051]}, + {"t":0.48045, "x":2.89149, "y":3.26435, "heading":0.0, "vx":-3.24061, "vy":-2.67891, "omega":0.0, "ax":-7.2967, "ay":5.15067, "alpha":0.0, "fx":[-124.1147,-124.1147,-124.1147,-124.1147], "fy":[87.6114,87.6114,87.6114,87.6114]}, + {"t":0.51741, "x":2.76674, "y":3.16886, "heading":0.0, "vx":-3.51028, "vy":-2.48856, "omega":0.0, "ax":-2.98654, "ay":8.43748, "alpha":0.0, "fx":[-50.80027,-50.80027,-50.80027,-50.80027], "fy":[143.51918,143.51918,143.51918,143.51918]}, + {"t":0.55436, "x":2.63497, "y":3.08265, "heading":0.0, "vx":-3.62066, "vy":-2.17673, "omega":0.0, "ax":-0.08484, "ay":8.97274, "alpha":0.0, "fx":[-1.44316,-1.44316,-1.44316,-1.44316], "fy":[152.62372,152.62372,152.62372,152.62372]}, + {"t":0.59132, "x":2.5011, "y":3.00833, "heading":0.0, "vx":-3.62379, "vy":-1.84512, "omega":0.0, "ax":1.42977, "ay":8.87273, "alpha":0.0, "fx":[24.31997,24.31997,24.31997,24.31997], "fy":[150.92266,150.92266,150.92266,150.92266]}, + {"t":0.62828, "x":2.36815, "y":2.9462, "heading":0.0, "vx":-3.57095, "vy":-1.5172, "omega":0.0, "ax":2.2881, "ay":8.69997, "alpha":0.0, "fx":[38.91995,38.91995,38.91995,38.91995], "fy":[147.98401,147.98401,147.98401,147.98401]}, + {"t":0.66524, "x":2.23774, "y":2.89607, "heading":0.0, "vx":-3.48639, "vy":-1.19567, "omega":0.0, "ax":2.82631, "ay":8.54628, "alpha":0.0, "fx":[48.07472,48.07472,48.07472,48.07472], "fy":[145.36983,145.36983,145.36983,145.36983]}, + {"t":0.70219, "x":2.11082, "y":2.85771, "heading":0.0, "vx":-3.38194, "vy":-0.87982, "omega":0.0, "ax":3.86823, "ay":8.12858, "alpha":0.0, "fx":[65.79755,65.79755,65.79755,65.79755], "fy":[138.2648,138.2648,138.2648,138.2648]}, + {"t":0.73776, "x":1.99298, "y":2.83156, "heading":0.0, "vx":-3.24435, "vy":-0.59071, "omega":0.0, "ax":5.51487, "ay":7.11424, "alpha":0.0, "fx":[93.80639,93.80639,93.80639,93.80639], "fy":[121.01114,121.01114,121.01114,121.01114]}, + {"t":0.77333, "x":1.88108, "y":2.81505, "heading":0.0, "vx":-3.04821, "vy":-0.33768, "omega":0.0, "ax":6.9095, "ay":5.77021, "alpha":0.0, "fx":[117.52858,117.52858,117.52858,117.52858], "fy":[98.14955,98.14955,98.14955,98.14955]}, + {"t":0.8089, "x":1.77703, "y":2.80669, "heading":0.0, "vx":-2.80246, "vy":-0.13245, "omega":0.0, "ax":7.90863, "ay":4.30321, "alpha":0.0, "fx":[134.52346,134.52346,134.52346,134.52346], "fy":[73.19638,73.19638,73.19638,73.19638]}, + {"t":0.84446, "x":1.68236, "y":2.8047, "heading":0.0, "vx":-2.52117, "vy":0.0206, "omega":0.0, "ax":8.52107, "ay":2.91409, "alpha":0.0, "fx":[144.94092,144.94092,144.94092,144.94092], "fy":[49.5678,49.5678,49.5678,49.5678]}, + {"t":0.88003, "x":1.59808, "y":2.80728, "heading":0.0, "vx":-2.2181, "vy":0.12425, "omega":0.0, "ax":8.84379, "ay":1.71057, "alpha":0.0, "fx":[150.4303,150.4303,150.4303,150.4303], "fy":[29.09637,29.09637,29.09637,29.09637]}, + {"t":0.9156, "x":1.52478, "y":2.81278, "heading":0.0, "vx":-1.90355, "vy":0.18509, "omega":0.0, "ax":8.98125, "ay":0.71515, "alpha":0.0, "fx":[152.76843,152.76843,152.76843,152.76843], "fy":[12.16457,12.16457,12.16457,12.16457]}, + {"t":0.95116, "x":1.46276, "y":2.81982, "heading":0.0, "vx":-1.58411, "vy":0.21053, "omega":0.0, "ax":9.01094, "ay":-0.09297, "alpha":0.0, "fx":[153.27352,153.27352,153.27352,153.27352], "fy":[-1.58145,-1.58145,-1.58145,-1.58145]}, + {"t":0.98673, "x":1.41211, "y":2.82725, "heading":0.0, "vx":-1.26362, "vy":0.20722, "omega":0.0, "ax":8.98189, "ay":-0.74727, "alpha":0.0, "fx":[152.77944,152.77944,152.77944,152.77944], "fy":[-12.71093,-12.71093,-12.71093,-12.71093]}, + {"t":1.0223, "x":1.37285, "y":2.83414, "heading":0.0, "vx":-0.94416, "vy":0.18064, "omega":0.0, "ax":8.92288, "ay":-1.27996, "alpha":0.0, "fx":[151.77559,151.77559,151.77559,151.77559], "fy":[-21.77168,-21.77168,-21.77168,-21.77168]}, + {"t":1.05787, "x":1.34491, "y":2.83976, "heading":0.0, "vx":-0.6268, "vy":0.13512, "omega":0.0, "ax":8.85017, "ay":-1.71768, "alpha":0.0, "fx":[150.53878,150.53878,150.53878,150.53878], "fy":[-29.21721,-29.21721,-29.21721,-29.21721]}, + {"t":1.09343, "x":1.32822, "y":2.84348, "heading":0.0, "vx":-0.31202, "vy":0.07402, "omega":0.0, "ax":8.77276, "ay":-2.08125, "alpha":0.0, "fx":[149.22206,149.22206,149.22206,149.22206], "fy":[-35.40146,-35.40146,-35.40146,-35.40146]}, + {"t":1.129, "x":1.32267, "y":2.84479, "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]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/RightToTower.traj b/src/main/deploy/choreo/RightToTower.traj new file mode 100644 index 0000000..cece1bc --- /dev/null +++ b/src/main/deploy/choreo/RightToTower.traj @@ -0,0 +1,94 @@ +{ + "name":"RightToTower", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.635439157485962, "y":0.7904343605041504, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3226691484451294, "y":2.082484722137451, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "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":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.635439157485962 m", "val":3.635439157485962}, "y":{"exp":"0.7904343605041504 m", "val":0.7904343605041504}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.082484722137451 m", "val":2.082484722137451}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.91196,1.35192], + "samples":[ + {"t":0.0, "x":3.63544, "y":0.79043, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.8525, "ay":1.71849, "alpha":0.0, "fx":[-150.57856,-150.57856,-150.57856,-150.57856], "fy":[29.231,29.231,29.231,29.231]}, + {"t":0.04145, "x":3.62783, "y":0.79191, "heading":0.0, "vx":-0.36696, "vy":0.07124, "omega":0.0, "ax":-8.83658, "ay":1.79466, "alpha":0.0, "fx":[-150.30767,-150.30767,-150.30767,-150.30767], "fy":[30.52668,30.52668,30.52668,30.52668]}, + {"t":0.08291, "x":3.60503, "y":0.79641, "heading":0.0, "vx":-0.73326, "vy":0.14563, "omega":0.0, "ax":-8.81679, "ay":1.88506, "alpha":0.0, "fx":[-149.97103,-149.97103,-149.97103,-149.97103], "fy":[32.06435,32.06435,32.06435,32.06435]}, + {"t":0.12436, "x":3.56706, "y":0.80406, "heading":0.0, "vx":-1.09874, "vy":0.22377, "omega":0.0, "ax":-8.79163, "ay":1.99404, "alpha":0.0, "fx":[-149.54316,-149.54316,-149.54316,-149.54316], "fy":[33.91802,33.91802,33.91802,33.91802]}, + {"t":0.16581, "x":3.51396, "y":0.81505, "heading":0.0, "vx":-1.46317, "vy":0.30643, "omega":0.0, "ax":-8.75878, "ay":2.1279, "alpha":0.0, "fx":[-148.98431,-148.98431,-148.98431,-148.98431], "fy":[36.19496,36.19496,36.19496,36.19496]}, + {"t":0.20726, "x":3.44578, "y":0.82958, "heading":0.0, "vx":-1.82625, "vy":0.39463, "omega":0.0, "ax":-8.7144, "ay":2.29612, "alpha":0.0, "fx":[-148.22943,-148.22943,-148.22943,-148.22943], "fy":[39.05629,39.05629,39.05629,39.05629]}, + {"t":0.24872, "x":3.36259, "y":0.84791, "heading":0.0, "vx":-2.18748, "vy":0.48981, "omega":0.0, "ax":-8.65185, "ay":2.51355, "alpha":0.0, "fx":[-147.16546,-147.16546,-147.16546,-147.16546], "fy":[42.75478,42.75478,42.75478,42.75478]}, + {"t":0.29017, "x":3.26448, "y":0.87038, "heading":0.0, "vx":-2.54612, "vy":0.59401, "omega":0.0, "ax":-8.5587, "ay":2.80477, "alpha":0.0, "fx":[-145.581,-145.581,-145.581,-145.581], "fy":[47.70835,47.70835,47.70835,47.70835]}, + {"t":0.33162, "x":3.15159, "y":0.89741, "heading":0.0, "vx":-2.9009, "vy":0.71027, "omega":0.0, "ax":-8.40939, "ay":3.21306, "alpha":0.0, "fx":[-143.0413,-143.0413,-143.0413,-143.0413], "fy":[54.65316,54.65316,54.65316,54.65316]}, + {"t":0.37307, "x":3.02411, "y":0.92961, "heading":0.0, "vx":-3.24949, "vy":0.84346, "omega":0.0, "ax":-8.14441, "ay":3.82044, "alpha":0.0, "fx":[-138.53402,-138.53402,-138.53402,-138.53402], "fy":[64.98452,64.98452,64.98452,64.98452]}, + {"t":0.41453, "x":2.88242, "y":0.96786, "heading":0.0, "vx":-3.5871, "vy":1.00183, "omega":0.0, "ax":-7.60055, "ay":4.79307, "alpha":0.0, "fx":[-129.28324,-129.28324,-129.28324,-129.28324], "fy":[81.5287,81.5287,81.5287,81.5287]}, + {"t":0.45598, "x":2.72719, "y":1.0135, "heading":0.0, "vx":-3.90216, "vy":1.20051, "omega":0.0, "ax":-6.23599, "ay":6.44461, "alpha":0.0, "fx":[-106.07238,-106.07238,-106.07238,-106.07238], "fy":[109.62092,109.62092,109.62092,109.62092]}, + {"t":0.49743, "x":2.56008, "y":1.06881, "heading":0.0, "vx":-4.16066, "vy":1.46766, "omega":0.0, "ax":-2.28471, "ay":8.64514, "alpha":0.0, "fx":[-38.86233,-38.86233,-38.86233,-38.86233], "fy":[147.05132,147.05132,147.05132,147.05132]}, + {"t":0.53888, "x":2.38565, "y":1.13707, "heading":0.0, "vx":-4.25537, "vy":1.82602, "omega":0.0, "ax":4.23398, "ay":7.87827, "alpha":0.0, "fx":[72.01879,72.01879,72.01879,72.01879], "fy":[134.00718,134.00718,134.00718,134.00718]}, + {"t":0.58034, "x":2.21289, "y":1.21953, "heading":0.0, "vx":-4.07986, "vy":2.1526, "omega":0.0, "ax":7.40591, "ay":5.0602, "alpha":0.0, "fx":[125.9724,125.9724,125.9724,125.9724], "fy":[86.07263,86.07263,86.07263,86.07263]}, + {"t":0.62179, "x":2.05013, "y":1.31311, "heading":0.0, "vx":-3.77286, "vy":2.36236, "omega":0.0, "ax":8.36786, "ay":3.27693, "alpha":0.0, "fx":[142.33493,142.33493,142.33493,142.33493], "fy":[55.73957,55.73957,55.73957,55.73957]}, + {"t":0.66324, "x":1.90092, "y":1.41385, "heading":0.0, "vx":-3.42599, "vy":2.49819, "omega":0.0, "ax":8.7083, "ay":2.25887, "alpha":0.0, "fx":[148.1257,148.1257,148.1257,148.1257], "fy":[38.42278,38.42278,38.42278,38.42278]}, + {"t":0.70469, "x":1.76639, "y":1.51935, "heading":0.0, "vx":-3.06501, "vy":2.59183, "omega":0.0, "ax":8.85407, "ay":1.62886, "alpha":0.0, "fx":[150.60523,150.60523,150.60523,150.60523], "fy":[27.70641,27.70641,27.70641,27.70641]}, + {"t":0.74615, "x":1.64694, "y":1.62819, "heading":0.0, "vx":-2.69799, "vy":2.65935, "omega":0.0, "ax":8.92556, "ay":1.20691, "alpha":0.0, "fx":[151.82122,151.82122,151.82122,151.82122], "fy":[20.52924,20.52924,20.52924,20.52924]}, + {"t":0.7876, "x":1.54277, "y":1.73946, "heading":0.0, "vx":-2.328, "vy":2.70938, "omega":0.0, "ax":8.96402, "ay":0.90651, "alpha":0.0, "fx":[152.47543,152.47543,152.47543,152.47543], "fy":[15.41942,15.41942,15.41942,15.41942]}, + {"t":0.82905, "x":1.45397, "y":1.85255, "heading":0.0, "vx":-1.95642, "vy":2.74696, "omega":0.0, "ax":8.98607, "ay":0.68246, "alpha":0.0, "fx":[152.85043,152.85043,152.85043,152.85043], "fy":[11.60844,11.60844,11.60844,11.60844]}, + {"t":0.8705, "x":1.3806, "y":1.96701, "heading":0.0, "vx":-1.58392, "vy":2.77525, "omega":0.0, "ax":8.99925, "ay":0.50925, "alpha":0.0, "fx":[153.07473,153.07473,153.07473,153.07473], "fy":[8.66227,8.66227,8.66227,8.66227]}, + {"t":0.91196, "x":1.32267, "y":2.08248, "heading":0.0, "vx":-1.21088, "vy":2.79636, "omega":0.0, "ax":9.01145, "ay":-0.06569, "alpha":0.0, "fx":[153.28221,153.28221,153.28221,153.28221], "fy":[-1.11731,-1.11731,-1.11731,-1.11731]}, + {"t":0.94862, "x":1.28433, "y":2.18497, "heading":0.0, "vx":-0.88049, "vy":2.79395, "omega":0.0, "ax":8.91241, "ay":-1.31752, "alpha":0.0, "fx":[151.59755,151.59755,151.59755,151.59755], "fy":[-22.41063,-22.41063,-22.41063,-22.41063]}, + {"t":0.98528, "x":1.25804, "y":2.28652, "heading":0.0, "vx":-0.55372, "vy":2.74564, "omega":0.0, "ax":8.50835, "ay":-2.954, "alpha":0.0, "fx":[144.72454,144.72454,144.72454,144.72454], "fy":[-50.24676,-50.24676,-50.24676,-50.24676]}, + {"t":1.02195, "x":1.24346, "y":2.3852, "heading":0.0, "vx":-0.24177, "vy":2.63734, "omega":0.0, "ax":7.54244, "ay":-4.91788, "alpha":0.0, "fx":[128.29477,128.29477,128.29477,128.29477], "fy":[-83.65171,-83.65171,-83.65171,-83.65171]}, + {"t":1.05861, "x":1.23966, "y":2.47859, "heading":0.0, "vx":0.03476, "vy":2.45703, "omega":0.0, "ax":5.8318, "ay":-6.8588, "alpha":0.0, "fx":[99.19729,99.19729,99.19729,99.19729], "fy":[-116.66617,-116.66617,-116.66617,-116.66617]}, + {"t":1.09528, "x":1.24485, "y":2.56406, "heading":0.0, "vx":0.24858, "vy":2.20556, "omega":0.0, "ax":3.60511, "ay":-8.25052, "alpha":0.0, "fx":[61.32194,61.32194,61.32194,61.32194], "fy":[-140.33906,-140.33906,-140.33906,-140.33906]}, + {"t":1.13194, "x":1.25639, "y":2.63938, "heading":0.0, "vx":0.38076, "vy":1.90306, "omega":0.0, "ax":1.42341, "ay":-8.89289, "alpha":0.0, "fx":[24.21183,24.21183,24.21183,24.21183], "fy":[-151.26545,-151.26545,-151.26545,-151.26545]}, + {"t":1.1686, "x":1.27131, "y":2.70318, "heading":0.0, "vx":0.43295, "vy":1.57701, "omega":0.0, "ax":-0.34545, "ay":-9.00218, "alpha":0.0, "fx":[-5.87599,-5.87599,-5.87599,-5.87599], "fy":[-153.12445,-153.12445,-153.12445,-153.12445]}, + {"t":1.20527, "x":1.28695, "y":2.75495, "heading":0.0, "vx":0.42028, "vy":1.24696, "omega":0.0, "ax":-1.66113, "ay":-8.85686, "alpha":0.0, "fx":[-28.25536,-28.25536,-28.25536,-28.25536], "fy":[-150.65273,-150.65273,-150.65273,-150.65273]}, + {"t":1.24193, "x":1.30124, "y":2.79471, "heading":0.0, "vx":0.35938, "vy":0.92223, "omega":0.0, "ax":-2.61991, "ay":-8.6242, "alpha":0.0, "fx":[-44.56391,-44.56391,-44.56391,-44.56391], "fy":[-146.69526,-146.69526,-146.69526,-146.69526]}, + {"t":1.2786, "x":1.31266, "y":2.82273, "heading":0.0, "vx":0.26332, "vy":0.60603, "omega":0.0, "ax":-3.32569, "ay":-8.3792, "alpha":0.0, "fx":[-56.56901,-56.56901,-56.56901,-56.56901], "fy":[-142.52773,-142.52773,-142.52773,-142.52773]}, + {"t":1.31526, "x":1.32008, "y":2.83932, "heading":0.0, "vx":0.14139, "vy":0.29882, "omega":0.0, "ax":-3.85633, "ay":-8.15011, "alpha":0.0, "fx":[-65.5951,-65.5951,-65.5951,-65.5951], "fy":[-138.63104,-138.63104,-138.63104,-138.63104]}, + {"t":1.35192, "x":1.32267, "y":2.84479, "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]}], + "splits":[0] + }, + "events":[] +} From 83465c2565c48dd6d9ea539f96080806cedaacd0 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 17 Feb 2026 12:09:12 -0500 Subject: [PATCH 02/22] Added Left Sequences --- .../frc/robot/commands/auto/LeftShoot.java | 24 +++++++++++++++++ .../commands/auto/LeftShootandClimb.java | 26 +++++++++++++++++++ 2 files changed, 50 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/LeftShoot.java create mode 100644 src/main/java/frc/robot/commands/auto/LeftShootandClimb.java diff --git a/src/main/java/frc/robot/commands/auto/LeftShoot.java b/src/main/java/frc/robot/commands/auto/LeftShoot.java new file mode 100644 index 0000000..ab4ab1a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/LeftShoot.java @@ -0,0 +1,24 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class LeftShoot extends LoggableSequentialCommandGroup{ + public LeftShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), + new SpinShooter(shooter, Constants.SHOOTER_SPEED) + ); + } +} + diff --git a/src/main/java/frc/robot/commands/auto/LeftShootandClimb.java b/src/main/java/frc/robot/commands/auto/LeftShootandClimb.java new file mode 100644 index 0000000..45d39a3 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/LeftShootandClimb.java @@ -0,0 +1,26 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class LeftShootandClimb extends LoggableSequentialCommandGroup{ + public LeftShootandClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new ClimberUp(climber) + ); + } +} From b9e31a883ccf97bb04036f74673918d5cbd9d64a Mon Sep 17 00:00:00 2001 From: cloudygitalt Date: Tue, 17 Feb 2026 12:22:52 -0500 Subject: [PATCH 03/22] right shoot commands --- .../frc/robot/commands/auto/RightShoot.java | 25 +++++++++++++++++ .../commands/auto/RightShootandClimb.java | 28 +++++++++++++++++++ 2 files changed, 53 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/RightShoot.java create mode 100644 src/main/java/frc/robot/commands/auto/RightShootandClimb.java diff --git a/src/main/java/frc/robot/commands/auto/RightShoot.java b/src/main/java/frc/robot/commands/auto/RightShoot.java new file mode 100644 index 0000000..a201143 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/RightShoot.java @@ -0,0 +1,25 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.PrintCommand; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class RightShoot extends LoggableSequentialCommandGroup { + public RightShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootState) { + super( + new SetShootingState(shootState, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")), + new SpinShooter(shooter, Constants.SHOOTER_SPEED) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/RightShootandClimb.java b/src/main/java/frc/robot/commands/auto/RightShootandClimb.java new file mode 100644 index 0000000..a067bbd --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/RightShootandClimb.java @@ -0,0 +1,28 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.PrintCommand; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class RightShootandClimb extends LoggableSequentialCommandGroup { + public RightShootandClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootState, ClimberSubsystem climber) { + super( + new SetShootingState(shootState, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")), + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new ClimberUp(climber) + ); + } +} From 63874a142ae3708bc1a2bda0eedbabb37c6bbdf0 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Tue, 17 Feb 2026 12:24:00 -0500 Subject: [PATCH 04/22] auto sequence --- src/main/deploy/choreo/NewPath.traj | 29 +++++++++++++++++++ src/main/deploy/choreo/RightToTower.traj | 2 +- .../frc/robot/commands/auto/LeftShoot.java | 2 +- ...ShootandClimb.java => LeftShootClimb.java} | 4 +-- .../frc/robot/commands/auto/MidShoot.java | 24 +++++++++++++++ .../robot/commands/auto/MidShootClimb.java | 26 +++++++++++++++++ .../frc/robot/commands/auto/RightShoot.java | 2 -- .../commands/auto/RightShootandClimb.java | 2 -- 8 files changed, 83 insertions(+), 8 deletions(-) create mode 100644 src/main/deploy/choreo/NewPath.traj rename src/main/java/frc/robot/commands/auto/{LeftShootandClimb.java => LeftShootClimb.java} (81%) create mode 100644 src/main/java/frc/robot/commands/auto/MidShoot.java create mode 100644 src/main/java/frc/robot/commands/auto/MidShootClimb.java diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj new file mode 100644 index 0000000..b6f2120 --- /dev/null +++ b/src/main/deploy/choreo/NewPath.traj @@ -0,0 +1,29 @@ +{ + "name":"NewPath", + "version":3, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"12.828377723693848 m", "val":12.828377723693848}, "y":{"exp":"7.379827976226807 m", "val":7.379827976226807}, "heading":{"exp":"3.1152834101671085 rad", "val":3.1152834101671085}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":null, + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/RightToTower.traj b/src/main/deploy/choreo/RightToTower.traj index cece1bc..a4d3f04 100644 --- a/src/main/deploy/choreo/RightToTower.traj +++ b/src/main/deploy/choreo/RightToTower.traj @@ -14,7 +14,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.635439157485962 m", "val":3.635439157485962}, "y":{"exp":"0.7904343605041504 m", "val":0.7904343605041504}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.6271181106567383 m", "val":3.6271181106567383}, "y":{"exp":"0.7985455989837646 m", "val":0.7985455989837646}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.082484722137451 m", "val":2.082484722137451}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/java/frc/robot/commands/auto/LeftShoot.java b/src/main/java/frc/robot/commands/auto/LeftShoot.java index ab4ab1a..f33568b 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShoot.java +++ b/src/main/java/frc/robot/commands/auto/LeftShoot.java @@ -14,7 +14,7 @@ public class LeftShoot extends LoggableSequentialCommandGroup{ public LeftShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate) { super( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), new SpinShooter(shooter, Constants.SHOOTER_SPEED) diff --git a/src/main/java/frc/robot/commands/auto/LeftShootandClimb.java b/src/main/java/frc/robot/commands/auto/LeftShootClimb.java similarity index 81% rename from src/main/java/frc/robot/commands/auto/LeftShootandClimb.java rename to src/main/java/frc/robot/commands/auto/LeftShootClimb.java index 45d39a3..057ddb5 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShootandClimb.java +++ b/src/main/java/frc/robot/commands/auto/LeftShootClimb.java @@ -13,8 +13,8 @@ import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class LeftShootandClimb extends LoggableSequentialCommandGroup{ - public LeftShootandClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber) { +public class LeftShootClimb extends LoggableSequentialCommandGroup{ + public LeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), diff --git a/src/main/java/frc/robot/commands/auto/MidShoot.java b/src/main/java/frc/robot/commands/auto/MidShoot.java new file mode 100644 index 0000000..112c8ba --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/MidShoot.java @@ -0,0 +1,24 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class MidShoot extends LoggableSequentialCommandGroup{ + public MidShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), + new SpinShooter(shooter, Constants.SHOOTER_SPEED) + ); + } +} + diff --git a/src/main/java/frc/robot/commands/auto/MidShootClimb.java b/src/main/java/frc/robot/commands/auto/MidShootClimb.java new file mode 100644 index 0000000..a35bc3e --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/MidShootClimb.java @@ -0,0 +1,26 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class MidShootClimb extends LoggableSequentialCommandGroup{ + public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new ClimberUp(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/RightShoot.java b/src/main/java/frc/robot/commands/auto/RightShoot.java index a201143..edc0ddd 100644 --- a/src/main/java/frc/robot/commands/auto/RightShoot.java +++ b/src/main/java/frc/robot/commands/auto/RightShoot.java @@ -1,7 +1,6 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; -import frc.robot.commands.PrintCommand; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; import frc.robot.constants.Constants; @@ -10,7 +9,6 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; -import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class RightShoot extends LoggableSequentialCommandGroup { diff --git a/src/main/java/frc/robot/commands/auto/RightShootandClimb.java b/src/main/java/frc/robot/commands/auto/RightShootandClimb.java index a067bbd..fca9247 100644 --- a/src/main/java/frc/robot/commands/auto/RightShootandClimb.java +++ b/src/main/java/frc/robot/commands/auto/RightShootandClimb.java @@ -1,7 +1,6 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; -import frc.robot.commands.PrintCommand; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; @@ -12,7 +11,6 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; -import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class RightShootandClimb extends LoggableSequentialCommandGroup { From 4d812a6a97908213e3c3ab78d2e50c4b056e440f Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Tue, 17 Feb 2026 12:27:30 -0500 Subject: [PATCH 05/22] do nothing --- src/main/java/frc/robot/commands/auto/DoNothing.java | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/DoNothing.java diff --git a/src/main/java/frc/robot/commands/auto/DoNothing.java b/src/main/java/frc/robot/commands/auto/DoNothing.java new file mode 100644 index 0000000..71d7c0f --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DoNothing.java @@ -0,0 +1,11 @@ +package frc.robot.commands.auto; + +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class DoNothing extends LoggableSequentialCommandGroup{ + public DoNothing() { + super( + new DoNothing() + ); + } +} \ No newline at end of file From 1e822c6818316cfd68d985f3cd1e3f85eedba8dd Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 17 Feb 2026 12:41:29 -0500 Subject: [PATCH 06/22] AutoChooser Edits --- src/main/java/frc/robot/autochooser/AutoAction.java | 6 ++---- src/main/java/frc/robot/autochooser/AutoChooser.java | 8 ++++++++ .../java/frc/robot/autochooser/FieldLocation.java | 12 ++++++------ 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index cbb8da9..1afc730 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -7,10 +7,8 @@ public enum AutoAction { DO_NOTHING("Do Nothing"), - TWO_PIECE_HIGH("2 Piece L4"), - TWO_PIECE_LOW("2 Piece L2"), - ONE_PIECE("1 Piece"), - CROSS_THE_LINE("Cross The Line"), + SHOOT("Shoot"), + SHOOT_AND_CLIMB("Shoot and Climb"), INVALID("INVALID"); private final String name; private static final HashMap nameMap = diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 877b9c6..0d130cd 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -59,6 +59,14 @@ private void populateMap() { AutoCommand.DoNothing); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.RIGHT), AutoCommand.DoSomething); + commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MIDDLE), + AutoCommand.DoNothing); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.RIGHT), + AutoCommand.DoSomething); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.LEFT), + AutoCommand.DoNothing); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), + AutoCommand.DoSomething); } private AutoCommand get() { diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index 43e21ef..8f001e0 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -14,13 +14,13 @@ public enum FieldLocation { ZERO(0, 0, 0, "Zero"), INVALID(-1, -1, -1, "INVALID"), - LEFT(7.150, 7.000, 180, "NON Processor Side"), - MIDDLE(7.150, 4.500, 180, "Middle"), - RIGHT(7.150, 2.000, 180, "Processor Side"); + LEFT(3.622, 7.662, 0, "Depot Side"), + MIDDLE(3.622, 4.024, 0, "Middle"), + RIGHT(3.622, 0.407, 0, "NON Depot Side"); - private static final double RED_X_POS = 2.3876; // meters - public static final double HEIGHT_OF_FIELD = 8.05; - public static final double LENGTH_OF_FIELD = 17.548225; + private static final double RED_X_POS = 9.338; // meters + public static final double HEIGHT_OF_FIELD = 8.043; + public static final double LENGTH_OF_FIELD = 16.411; private final double yPose; private final double xPose; private final double angle; From 0a6c8fd2b965977e8d734aa550a0fbf32d7054af Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 17 Feb 2026 14:51:34 -0500 Subject: [PATCH 07/22] Fixed Auto Chooser to fit path planning --- src/main/java/frc/robot/RobotContainer.java | 12 +++- .../frc/robot/autochooser/AutoChooser.java | 72 ++++++++++++++----- .../frc/robot/autochooser/AutoCommand.java | 47 ------------ 3 files changed, 62 insertions(+), 69 deletions(-) delete mode 100644 src/main/java/frc/robot/autochooser/AutoCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 55bdad6..31cb56c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,7 +80,7 @@ */ public class RobotContainer { // Instantiate the autochooser. - private final AutoChooser autoChooser = new AutoChooser(); + private final AutoChooser autoChooser; // The robot's subsystems and commands are defined here... // private final TiltSubsystem tiltSubsystem; private final AnglerSubsystem anglerSubsystem; @@ -133,6 +133,8 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; + autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); + } case REPLAY -> { // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); @@ -150,6 +152,8 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; + autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); + } case SIM -> { robotVisualizer = new RobotVisualizer(); @@ -172,6 +176,8 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; + autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); + } default -> { @@ -459,9 +465,9 @@ public void putShuffleboardCommands() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // return autoChooser.getCommand(); + return autoChooser.getCommand(); // return straightRoutine.cmd(straightTrajectory.done()); - return new ExampleAuto(drivebase, autoFactory); + // return new ExampleAuto(drivebase, autoFactory); } public ClimberSubsystem getClimberSubsystem() { diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 0d130cd..70bf4b1 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -5,7 +5,19 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import frc.robot.utils.logging.commands.LoggableCommand; +import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.auto.LeftShoot; +import frc.robot.commands.auto.LeftShootClimb; +import frc.robot.commands.auto.MidShoot; +import frc.robot.commands.auto.MidShootClimb; +import frc.robot.commands.auto.RightShoot; +import frc.robot.commands.auto.RightShootandClimb; +import frc.robot.constants.enums.ShootingState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.DoNothingCommand; public class AutoChooser { @@ -14,11 +26,20 @@ public class AutoChooser { /** Drop-down chooser for the action. */ private LoggedDashboardChooser actionChooser; /** Structure for mapping possible choices to commands. */ - private final Map commandMap = new HashMap<>(); + private final Map commandMap = new HashMap<>(); - private final AutoCommand DEFAULT_COMMAND = AutoCommand.Invalid; + private final SwerveSubsystem subsystem; + private final ShootingState state; + private final ShooterSubsystem shooter; + private final AutoFactory auto; + private final ClimberSubsystem climber; - public AutoChooser() { + public AutoChooser(SwerveSubsystem subsystem, ShootingState state, AutoFactory auto, ShooterSubsystem shooter, ClimberSubsystem climber) { + this.subsystem = subsystem; + this.auto = auto; + this.state = state; + this.shooter = shooter; + this.climber = climber; this.locationChooser = new LoggedDashboardChooser<>( "Location Chooser" ); @@ -56,34 +77,47 @@ private void populateChoosers() { private void populateMap() { // Currently, we have some example mappings. commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.LEFT), - AutoCommand.DoNothing); + new DoNothingCommand()); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.RIGHT), - AutoCommand.DoSomething); + new DoNothingCommand()); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MIDDLE), - AutoCommand.DoNothing); - commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.RIGHT), - AutoCommand.DoSomething); - commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.LEFT), - AutoCommand.DoNothing); + new DoNothingCommand()); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.LEFT), + new LeftShoot(subsystem, auto, shooter, state)); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.RIGHT), + new RightShoot(subsystem, auto, shooter, state)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), - AutoCommand.DoSomething); + new MidShoot(subsystem, auto, shooter, state)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), + new LeftShootClimb(subsystem, auto, shooter, state, climber)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), + new RightShootandClimb(subsystem, auto, shooter, state, climber)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), + new MidShootClimb(subsystem, auto, shooter, state, climber)); } - private AutoCommand get() { + public AutoEvent getSelectedEvent() { AutoAction chosenAction = actionChooser.get(); FieldLocation chosenLocation = locationChooser.get(); - AutoEvent event = new AutoEvent(chosenAction, chosenLocation); + return new AutoEvent(chosenAction, chosenLocation); + } - return commandMap.getOrDefault(event, DEFAULT_COMMAND); + public Command getSelectedCommand() { + AutoEvent event = getSelectedEvent(); + return commandMap.getOrDefault(event, new DoNothingCommand()); } - public LoggableCommand getCommand() { - return get().getCommand(); + public Command getCommand() { + return getSelectedCommand(); } - /** @return A human-readable description of the selected command. */ public String getCommandDescription() { - return get().getDescription(); + AutoEvent event = getSelectedEvent(); + Command command = commandMap.get(event); + if (command == null) { + return "No auto mapped for " + event.getAction() + " at " + event.getLocation(); + } + return event.getAction() + " at " + event.getLocation() + " -> " + command.getName(); } public FieldLocation getLocation() { diff --git a/src/main/java/frc/robot/autochooser/AutoCommand.java b/src/main/java/frc/robot/autochooser/AutoCommand.java deleted file mode 100644 index 6d31ab4..0000000 --- a/src/main/java/frc/robot/autochooser/AutoCommand.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.autochooser; - -import frc.robot.utils.logging.commands.LoggableCommand; -import frc.robot.utils.logging.commands.DoNothingCommand; -import frc.robot.utils.logging.commands.DoSomethingCommand; - -/** An enum to associate commands with human-readable descriptions. */ -public enum AutoCommand { - - // Add commands here. Importantly, you should give each command - // a readable description so that the drive team can tell what - // the robot will actually do. This will be used to give the - // drive team visual feedback on the elastic dashboard when - // selecting an autonoumous command. - Invalid( - "The selection is invalid. (The robot won't do anything.)", - new DoNothingCommand() - ), - DoNothing("The robot won't do anything.", new DoNothingCommand()), - DoSomething( - "Something will be printed to the terminal.", - new DoSomethingCommand(""" - SUCCESSFULLY - - DID - - SOMETHING - """) - ); - - private String description; - private LoggableCommand command; - - AutoCommand(String description, LoggableCommand command) { - this.description = description; - this.command = command; - } - - public String getDescription() { - return description; - } - - public LoggableCommand getCommand() { - return command; - } - -} From 911aee89641733b129d1abab838630b35b80c854 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 17 Feb 2026 14:58:29 -0500 Subject: [PATCH 08/22] fixed null exception --- src/main/java/frc/robot/RobotContainer.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 31cb56c..13b0de0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -133,7 +133,6 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; - autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); } case REPLAY -> { @@ -152,7 +151,6 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); } case SIM -> { @@ -176,7 +174,6 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); } @@ -185,9 +182,10 @@ public RobotContainer() { } } + setUpAutoFactory(); + autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); configureBindings(); putShuffleboardCommands(); - setUpAutoFactory(); } /** @@ -205,6 +203,11 @@ public RobotContainer() { * joysticks}. */ private void setUpAutoFactory() { + if (drivebase == null) { + autoFactory = null; + drive = null; + return; + } drive = new Drive(drivebase); From 1042fbfdd4acc8b3c6802ec60caae9db617bd3ea Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 17 Feb 2026 15:58:40 -0500 Subject: [PATCH 09/22] Elasstic change --- .../elastic-4048-2026-dev-v1.0.json | 71 +++++++++++++++---- src/main/java/frc/robot/Robot.java | 1 - 2 files changed, 56 insertions(+), 16 deletions(-) diff --git a/elastic_layouts/elastic-4048-2026-dev-v1.0.json b/elastic_layouts/elastic-4048-2026-dev-v1.0.json index 903fd43..b61f834 100644 --- a/elastic_layouts/elastic-4048-2026-dev-v1.0.json +++ b/elastic_layouts/elastic-4048-2026-dev-v1.0.json @@ -62,18 +62,6 @@ } } ] - }, - { - "title": "camera goes here", - "x": 768.0, - "y": 0.0, - "width": 512.0, - "height": 384.0, - "type": "List Layout", - "properties": { - "label_position": "TOP" - }, - "children": [] } ], "containers": [ @@ -117,16 +105,56 @@ }, { "title": "Auto Chooser", + "x": 1024.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Action Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "DriverCam", "x": 768.0, - "y": 384.0, + "y": 0.0, "width": 512.0, - "height": 256.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/DriverCam", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Location Chooser", + "x": 768.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, "type": "ComboBox Chooser", "properties": { - "topic": "/SmartDashboard/Action Chooser", + "topic": "/SmartDashboard/Location Chooser", "period": 0.06, "sort_options": false } + }, + { + "title": "Selected Action", + "x": 768.0, + "y": 384.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Selected Action", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } } ] } @@ -661,6 +689,19 @@ "show_type": false, "maximize_button_space": true } + }, + { + "title": "Location Chooser", + "x": 512.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Location Chooser", + "period": 0.06, + "sort_options": false + } } ] } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce87288..447c680 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -132,7 +132,6 @@ public void robotPeriodic() { SmartDashboard.putBoolean("Hub Active?", hubActive()); SmartDashboard.putString("Selected Action", robotContainer.getAutoChooser().getCommandDescription()); - SmartDashboard.putString("Starting Location", location().toString()); // Gets the alliance color. if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { From 6724c3b16811e38a20d11c19b9dd767b5d5a53db Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 17 Feb 2026 16:17:40 -0500 Subject: [PATCH 10/22] new climb path --- src/main/deploy/choreo/NewPath.traj | 29 ----- src/main/deploy/choreo/TowerToClimb.traj | 120 ++++++++++++++++++ .../robot/commands/auto/LeftShootClimb.java | 11 +- .../robot/commands/auto/MidShootClimb.java | 12 +- .../commands/auto/RightShootandClimb.java | 11 +- 5 files changed, 147 insertions(+), 36 deletions(-) delete mode 100644 src/main/deploy/choreo/NewPath.traj create mode 100644 src/main/deploy/choreo/TowerToClimb.traj diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj deleted file mode 100644 index b6f2120..0000000 --- a/src/main/deploy/choreo/NewPath.traj +++ /dev/null @@ -1,29 +0,0 @@ -{ - "name":"NewPath", - "version":3, - "snapshot":{ - "waypoints":[], - "constraints":[], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"12.828377723693848 m", "val":12.828377723693848}, "y":{"exp":"7.379827976226807 m", "val":7.379827976226807}, "heading":{"exp":"3.1152834101671085 rad", "val":3.1152834101671085}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "config":null, - "sampleType":null, - "waypoints":[], - "samples":[], - "splits":[] - }, - "events":[] -} diff --git a/src/main/deploy/choreo/TowerToClimb.traj b/src/main/deploy/choreo/TowerToClimb.traj new file mode 100644 index 0000000..c6b7695 --- /dev/null +++ b/src/main/deploy/choreo/TowerToClimb.traj @@ -0,0 +1,120 @@ +{ + "name":"TowerToClimb", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.7245495319366455, "y":3.234717607498169, "heading":0.7853981633974483, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.4321849346160889, "y":3.5758097171783447, "heading":1.5707963267948966, "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":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.7245495319366455 m", "val":1.7245495319366455}, "y":{"exp":"3.234717607498169 m", "val":3.234717607498169}, "heading":{"exp":"45 deg", "val":0.7853981633974483}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.4321849346160889 m", "val":1.4321849346160889}, "y":{"exp":"3.5758097171783447 m", "val":3.5758097171783447}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.1565,2.09057], + "samples":[ + {"t":0.0, "x":1.32267, "y":2.84479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.56194, "ay":6.17797, "alpha":0.00535, "fx":[111.58893,111.61752,111.64452,111.61594], "fy":[105.11504,105.08459,105.056,105.08646]}, + {"t":0.03731, "x":1.32724, "y":2.84909, "heading":0.0, "vx":0.2448, "vy":0.23048, "omega":0.0002, "ax":3.19367, "ay":3.00679, "alpha":1.85827, "fx":[48.27034,50.54209,60.44809,58.03287], "fy":[57.42498,47.20668,44.96406,54.98285]}, + {"t":0.07461, "x":1.33859, "y":2.85978, "heading":0.00001, "vx":0.36395, "vy":0.34265, "omega":0.06953, "ax":0.00001, "ay":0.0, "alpha":2.66386, "fx":[-7.15062,-7.15052,7.1508,7.1507], "fy":[7.15069,-7.15063,-7.15052,7.1508]}, + {"t":0.11192, "x":1.35217, "y":2.87257, "heading":0.0026, "vx":0.36395, "vy":0.34265, "omega":0.1689, "ax":0.0, "ay":0.0, "alpha":2.36366, "fx":[-6.36131,-6.3283,6.36131,6.3283], "fy":[6.3283,-6.36131,-6.3283,6.36131]}, + {"t":0.14923, "x":1.36575, "y":2.88535, "heading":0.0089, "vx":0.36395, "vy":0.34265, "omega":0.25708, "ax":0.0, "ay":0.0, "alpha":2.09801, "fx":[-5.68164,-5.58137,5.68164,5.58137], "fy":[5.58137,-5.68164,-5.58137,5.68164]}, + {"t":0.18653, "x":1.37932, "y":2.89813, "heading":0.01849, "vx":0.36395, "vy":0.34265, "omega":0.33535, "ax":0.0, "ay":0.0, "alpha":1.86325, "fx":[-5.09319,-4.90821,5.09319,4.90821], "fy":[4.90821,-5.09319,-4.90821,5.09319]}, + {"t":0.22384, "x":1.3929, "y":2.91092, "heading":0.031, "vx":0.36395, "vy":0.34265, "omega":0.40486, "ax":0.0, "ay":0.0, "alpha":1.6561, "fx":[-4.58117,-4.30556,4.58117,4.30556], "fy":[4.30556,-4.58117,-4.30556,4.58117]}, + {"t":0.26114, "x":1.40648, "y":2.9237, "heading":0.04611, "vx":0.36395, "vy":0.34265, "omega":0.46665, "ax":0.0, "ay":0.0, "alpha":1.47361, "fx":[-4.13377,-3.76912,4.13377,3.76912], "fy":[3.76912,-4.13377,-3.76912,4.13377]}, + {"t":0.29845, "x":1.42006, "y":2.93648, "heading":0.06352, "vx":0.36395, "vy":0.34265, "omega":0.52162, "ax":0.0, "ay":0.0, "alpha":1.31317, "fx":[-3.7416,-3.29411,3.7416,3.29411], "fy":[3.29411,-3.7416,-3.29411,3.7416]}, + {"t":0.33576, "x":1.43363, "y":2.94927, "heading":0.08298, "vx":0.36395, "vy":0.34265, "omega":0.57061, "ax":0.0, "ay":0.0, "alpha":1.17243, "fx":[-3.39721,-2.87552,3.39721,2.87552], "fy":[2.87552,-3.39721,-2.87552,3.39721]}, + {"t":0.37306, "x":1.44721, "y":2.96205, "heading":0.10426, "vx":0.36395, "vy":0.34265, "omega":0.61435, "ax":0.0, "ay":0.0, "alpha":1.04935, "fx":[-3.09467,-2.50835,3.09467,2.50835], "fy":[2.50835,-3.09467,-2.50835,3.09467]}, + {"t":0.41037, "x":1.46079, "y":2.97483, "heading":0.12718, "vx":0.36395, "vy":0.34265, "omega":0.6535, "ax":0.0, "ay":0.0, "alpha":0.94212, "fx":[-2.82929,-2.18774,2.82929,2.18774], "fy":[2.18774,-2.82929,-2.18774,2.82929]}, + {"t":0.44768, "x":1.47437, "y":2.98761, "heading":0.15156, "vx":0.36395, "vy":0.34265, "omega":0.68865, "ax":0.0, "ay":0.0, "alpha":0.84913, "fx":[-2.59735,-1.90907,2.59735,1.90907], "fy":[1.90907,-2.59735,-1.90907,2.59735]}, + {"t":0.48498, "x":1.48794, "y":3.0004, "heading":0.17725, "vx":0.36395, "vy":0.34265, "omega":0.72032, "ax":0.0, "ay":0.0, "alpha":0.76901, "fx":[-2.39593,-1.66795,2.39593,1.66795], "fy":[1.66795,-2.39593,-1.66795,2.39593]}, + {"t":0.52229, "x":1.50152, "y":3.01318, "heading":0.20413, "vx":0.36395, "vy":0.34265, "omega":0.74901, "ax":0.0, "ay":0.0, "alpha":0.70057, "fx":[-2.22273,-1.4603,2.22273,1.4603], "fy":[1.4603,-2.22273,-1.4603,2.22273]}, + {"t":0.5596, "x":1.5151, "y":3.02596, "heading":0.23207, "vx":0.36395, "vy":0.34265, "omega":0.77515, "ax":0.0, "ay":0.0, "alpha":0.64278, "fx":[-2.07601,-1.28234,2.07601,1.28234], "fy":[1.28234,-2.07601,-1.28234,2.07601]}, + {"t":0.5969, "x":1.52868, "y":3.03875, "heading":0.26099, "vx":0.36395, "vy":0.34265, "omega":0.79913, "ax":0.0, "ay":0.0, "alpha":0.59477, "fx":[-1.95446,-1.13052,1.95446,1.13052], "fy":[1.13052,-1.95446,-1.13052,1.95446]}, + {"t":0.63421, "x":1.54225, "y":3.05153, "heading":0.2908, "vx":0.36395, "vy":0.34265, "omega":0.82132, "ax":0.0, "ay":0.0, "alpha":0.55582, "fx":[-1.85715,-1.00158,1.85715,1.00158], "fy":[1.00158,-1.85715,-1.00158,1.85715]}, + {"t":0.67152, "x":1.55583, "y":3.06431, "heading":0.32144, "vx":0.36395, "vy":0.34265, "omega":0.84205, "ax":0.0, "ay":0.0, "alpha":0.52535, "fx":[-1.78353,-0.89245,1.78353,0.89245], "fy":[0.89245,-1.78353,-0.89245,1.78353]}, + {"t":0.70882, "x":1.56941, "y":3.0771, "heading":0.35286, "vx":0.36395, "vy":0.34265, "omega":0.86165, "ax":0.0, "ay":0.0, "alpha":0.5029, "fx":[-1.7333,-0.80027,1.7333,0.80027], "fy":[0.80027,-1.7333,-0.80027,1.7333]}, + {"t":0.74613, "x":1.58299, "y":3.08988, "heading":0.385, "vx":0.36395, "vy":0.34265, "omega":0.88041, "ax":0.0, "ay":0.0, "alpha":0.48814, "fx":[-1.7065,-0.7223,1.7065,0.7223], "fy":[0.7223,-1.7065,-0.7223,1.7065]}, + {"t":0.78343, "x":1.59656, "y":3.10266, "heading":0.41785, "vx":0.36395, "vy":0.34265, "omega":0.89862, "ax":0.0, "ay":0.0, "alpha":0.48083, "fx":[-1.70342,-0.6559,1.70342,0.6559], "fy":[0.6559,-1.70342,-0.6559,1.70342]}, + {"t":0.82074, "x":1.61014, "y":3.11545, "heading":0.45137, "vx":0.36395, "vy":0.34265, "omega":0.91656, "ax":0.0, "ay":0.0, "alpha":0.48088, "fx":[-1.72464,-0.5985,1.72464,0.5985], "fy":[0.5985,-1.72464,-0.5985,1.72464]}, + {"t":0.85805, "x":1.62372, "y":3.12823, "heading":0.48556, "vx":0.36395, "vy":0.34265, "omega":0.9345, "ax":0.0, "ay":0.0, "alpha":0.4883, "fx":[-1.77098,-0.54751,1.77098,0.54751], "fy":[0.54751,-1.77098,-0.54751,1.77098]}, + {"t":0.89535, "x":1.6373, "y":3.14101, "heading":0.52043, "vx":0.36395, "vy":0.34265, "omega":0.95272, "ax":0.0, "ay":0.0, "alpha":0.50319, "fx":[-1.84355,-0.50025,1.84355,0.50025], "fy":[0.50025,-1.84355,-0.50025,1.84355]}, + {"t":0.93266, "x":1.65087, "y":3.15379, "heading":0.55597, "vx":0.36395, "vy":0.34265, "omega":0.97149, "ax":0.0, "ay":0.0, "alpha":0.5258, "fx":[-1.94374,-0.45394,1.94374,0.45394], "fy":[0.45394,-1.94374,-0.45394,1.94374]}, + {"t":0.96997, "x":1.66445, "y":3.16658, "heading":0.59221, "vx":0.36395, "vy":0.34265, "omega":0.99111, "ax":0.0, "ay":0.0, "alpha":0.55647, "fx":[-2.07318,-0.40557,2.07318,0.40557], "fy":[0.40557,-2.07318,-0.40557,2.07318]}, + {"t":1.00727, "x":1.67803, "y":3.17936, "heading":0.62919, "vx":0.36395, "vy":0.34265, "omega":1.01187, "ax":-0.00002, "ay":0.00002, "alpha":0.59568, "fx":[-2.23415,-0.35216,2.23346,0.35147], "fy":[0.35218,-2.23344,-0.35145,2.23417]}, + {"t":1.04458, "x":1.69161, "y":3.19214, "heading":0.66694, "vx":0.36395, "vy":0.34265, "omega":1.03409, "ax":-0.00436, "ay":0.00463, "alpha":0.64404, "fx":[-2.50196,-0.36317,2.35362,0.21474], "fy":[0.36773,-2.34904,-0.21018,2.50654]}, + {"t":1.08189, "x":1.70518, "y":3.20493, "heading":0.70551, "vx":0.36378, "vy":0.34282, "omega":1.05812, "ax":-0.96642, "ay":0.92764, "alpha":0.67204, "fx":[-18.96955,-16.70303,-13.89692,-16.18427], "fy":[15.93042,13.22532,15.63484,18.32501]}, + {"t":1.11919, "x":1.71808, "y":3.21837, "heading":0.74499, "vx":0.32773, "vy":0.37743, "omega":1.08319, "ax":-8.27377, "ay":3.26411, "alpha":0.0114, "fx":[-140.74582,-140.76357,-140.72301,-140.70524], "fy":[55.49592,55.44649,55.54726,55.59661]}, + {"t":1.1565, "x":1.72455, "y":3.23472, "heading":0.7854, "vx":0.01907, "vy":0.4992, "omega":1.08361, "ax":-8.58, "ay":-2.41753, "alpha":0.00742, "fx":[-145.94812,-145.92904,-145.93864,-145.95772], "fy":[-41.10711,-41.17304,-41.13579,-41.06984]}, + {"t":1.19109, "x":1.72007, "y":3.25054, "heading":0.82289, "vx":-0.27776, "vy":0.41557, "omega":1.08387, "ax":-1.56821, "ay":-1.21133, "alpha":0.42326, "fx":[-28.25512,-26.54358,-25.08855,-26.8123], "fy":[-20.59277,-22.23015,-20.62062,-18.97418]}, + {"t":1.22569, "x":1.70953, "y":3.26419, "heading":0.86038, "vx":-0.33201, "vy":0.37366, "omega":1.09851, "ax":-0.00899, "ay":-0.00799, "alpha":0.34733, "fx":[-1.46774,-0.05416,1.16192,-0.25171], "fy":[-0.23477,-1.45081,-0.03722,1.17886]}, + {"t":1.26028, "x":1.69804, "y":3.27711, "heading":0.89839, "vx":-0.33233, "vy":0.37339, "omega":1.11053, "ax":-0.00005, "ay":-0.00005, "alpha":0.23314, "fx":[-0.88031,0.0989,0.87854,-0.10067], "fy":[-0.10058,-0.88021,0.099,0.87863]}, + {"t":1.29488, "x":1.68654, "y":3.29003, "heading":0.93681, "vx":-0.33233, "vy":0.37338, "omega":1.11859, "ax":0.0, "ay":0.0, "alpha":0.12204, "fx":[-0.458,0.06987,0.45799,-0.06988], "fy":[-0.06988,-0.458,0.06987,0.45799]}, + {"t":1.32948, "x":1.67504, "y":3.30295, "heading":0.9755, "vx":-0.33233, "vy":0.37338, "omega":1.12282, "ax":0.0, "ay":0.0, "alpha":0.01255, "fx":[-0.04679,0.009,0.04679,-0.009], "fy":[-0.009,-0.04679,0.009,0.04679]}, + {"t":1.36407, "x":1.66354, "y":3.31587, "heading":1.01435, "vx":-0.33233, "vy":0.37338, "omega":1.12325, "ax":0.0, "ay":0.0, "alpha":-0.09677, "fx":[0.35778,-0.08337,-0.35778,0.08337], "fy":[0.08337,0.35778,-0.08337,-0.35778]}, + {"t":1.39867, "x":1.65205, "y":3.32878, "heading":1.05321, "vx":-0.33233, "vy":0.37338, "omega":1.1199, "ax":0.0, "ay":0.0, "alpha":-0.20737, "fx":[0.75917,-0.20832,-0.75917,0.20832], "fy":[0.20832,0.75917,-0.20832,-0.75917]}, + {"t":1.43326, "x":1.64055, "y":3.3417, "heading":1.09195, "vx":-0.33233, "vy":0.37338, "omega":1.11273, "ax":0.0, "ay":0.0, "alpha":-0.32072, "fx":[1.16076,-0.36741,-1.16076,0.36741], "fy":[0.36741,1.16076,-0.36741,-1.16076]}, + {"t":1.46786, "x":1.62905, "y":3.35462, "heading":1.13044, "vx":-0.33233, "vy":0.37338, "omega":1.10163, "ax":0.0, "ay":0.0, "alpha":-0.43831, "fx":[1.56583,-0.5628,-1.56583,0.5628], "fy":[0.5628,1.56583,-0.5628,-1.56583]}, + {"t":1.50245, "x":1.61756, "y":3.36754, "heading":1.16856, "vx":-0.33233, "vy":0.37338, "omega":1.08647, "ax":0.0, "ay":0.0, "alpha":-0.56168, "fx":[1.97764,-0.79714,-1.97764,0.79714], "fy":[0.79714,1.97764,-0.79714,-1.97764]}, + {"t":1.53705, "x":1.60606, "y":3.38045, "heading":1.20614, "vx":-0.33233, "vy":0.37338, "omega":1.06704, "ax":0.0, "ay":0.0, "alpha":-0.69246, "fx":[2.39945,-1.07367,-2.39945,1.07367], "fy":[1.07367,2.39945,-1.07367,-2.39945]}, + {"t":1.57164, "x":1.59456, "y":3.39337, "heading":1.24306, "vx":-0.33233, "vy":0.37338, "omega":1.04308, "ax":0.0, "ay":0.0, "alpha":-0.83235, "fx":[2.83459,-1.39614,-2.83459,1.39614], "fy":[1.39614,2.83459,-1.39614,-2.83459]}, + {"t":1.60624, "x":1.58307, "y":3.40629, "heading":1.27914, "vx":-0.33233, "vy":0.37338, "omega":1.01429, "ax":0.0, "ay":0.0, "alpha":-0.98317, "fx":[3.28653,-1.76884,-3.28653,1.76884], "fy":[1.76884,3.28653,-1.76884,-3.28653]}, + {"t":1.64083, "x":1.57157, "y":3.41921, "heading":1.31423, "vx":-0.33233, "vy":0.37338, "omega":0.98027, "ax":0.0, "ay":0.0, "alpha":-1.14686, "fx":[3.75897,-2.19656,-3.75897,2.19656], "fy":[2.19656,3.75897,-2.19656,-3.75897]}, + {"t":1.67543, "x":1.56007, "y":3.43212, "heading":1.34815, "vx":-0.33233, "vy":0.37338, "omega":0.9406, "ax":0.0, "ay":0.0, "alpha":-1.32551, "fx":[4.25596,-2.68459,-4.25596,2.68459], "fy":[2.68459,4.25596,-2.68459,-4.25596]}, + {"t":1.71002, "x":1.54858, "y":3.44504, "heading":1.38069, "vx":-0.33233, "vy":0.37338, "omega":0.89474, "ax":0.0, "ay":0.0, "alpha":-1.52139, "fx":[4.78206,-3.2386,-4.78206,3.2386], "fy":[3.2386,4.78206,-3.2386,-4.78206]}, + {"t":1.74462, "x":1.53708, "y":3.45796, "heading":1.41164, "vx":-0.33233, "vy":0.37338, "omega":0.84211, "ax":0.0, "ay":0.0, "alpha":-1.73695, "fx":[5.34254,-3.86465,-5.34254,3.86465], "fy":[3.86465,5.34254,-3.86465,-5.34254]}, + {"t":1.77921, "x":1.52558, "y":3.47087, "heading":1.44077, "vx":-0.33233, "vy":0.37338, "omega":0.78202, "ax":0.0, "ay":0.0, "alpha":-1.97483, "fx":[5.94366,-4.569,-5.94366,4.569], "fy":[4.569,5.94366,-4.569,-5.94366]}, + {"t":1.81381, "x":1.51408, "y":3.48379, "heading":1.46783, "vx":-0.33233, "vy":0.37338, "omega":0.7137, "ax":0.0, "ay":0.0, "alpha":-2.23792, "fx":[6.59296,-5.358,-6.59296,5.358], "fy":[5.358,6.59296,-5.358,-6.59296]}, + {"t":1.8484, "x":1.50259, "y":3.49671, "heading":1.49252, "vx":-0.33233, "vy":0.37338, "omega":0.63628, "ax":0.0, "ay":0.0, "alpha":-2.52934, "fx":[7.29973,-6.23784,-7.29973,6.23784], "fy":[6.23784,7.29973,-6.23784,-7.29973]}, + {"t":1.883, "x":1.49109, "y":3.50963, "heading":1.51453, "vx":-0.33233, "vy":0.37338, "omega":0.54878, "ax":0.0, "ay":0.0, "alpha":-2.85247, "fx":[8.07545,-7.21423,-8.07545,7.21423], "fy":[7.21423,8.07545,-7.21423,-8.07545]}, + {"t":1.91759, "x":1.47959, "y":3.52254, "heading":1.53351, "vx":-0.33233, "vy":0.37338, "omega":0.45009, "ax":0.0, "ay":0.0, "alpha":-3.21093, "fx":[8.93445,-8.29192,-8.93445,8.29192], "fy":[8.29192,8.93445,-8.29192,-8.93445]}, + {"t":1.95219, "x":1.4681, "y":3.53546, "heading":1.54909, "vx":-0.33233, "vy":0.37338, "omega":0.33901, "ax":0.0, "ay":0.0, "alpha":-3.6086, "fx":[9.89466,-9.47407,-9.89466,9.47407], "fy":[9.47407,9.89466,-9.47407,-9.89466]}, + {"t":1.98678, "x":1.4566, "y":3.54838, "heading":1.56081, "vx":-0.33233, "vy":0.37338, "omega":0.21417, "ax":0.00001, "ay":-0.00001, "alpha":-4.04959, "fx":[10.97855,-10.76118,-10.97822,10.76151], "fy":[10.76116,10.9782,-10.76153,-10.97857]}, + {"t":2.02138, "x":1.4451, "y":3.5613, "heading":1.56822, "vx":-0.33233, "vy":0.37338, "omega":0.07407, "ax":3.61567, "ay":-4.06237, "alpha":-2.1318, "fx":[69.5779,57.56627,53.51959,65.34257], "fy":[-61.56365,-65.66334,-76.62474,-72.54711]}, + {"t":2.05597, "x":1.43577, "y":3.57178, "heading":1.57079, "vx":-0.20724, "vy":0.23285, "omega":0.00032, "ax":5.99048, "ay":-6.73056, "alpha":-0.00936, "fx":[101.9492,101.89315,101.84341,101.89948], "fy":[-114.43784,-114.48758,-114.53201,-114.4823]}, + {"t":2.09057, "x":1.43218, "y":3.57581, "heading":1.5708, "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":[] +} diff --git a/src/main/java/frc/robot/commands/auto/LeftShootClimb.java b/src/main/java/frc/robot/commands/auto/LeftShootClimb.java index 057ddb5..610e9b7 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/LeftShootClimb.java @@ -1,6 +1,7 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; @@ -11,6 +12,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class LeftShootClimb extends LoggableSequentialCommandGroup{ @@ -19,8 +21,13 @@ public LeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsys new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), - new SpinShooter(shooter, Constants.SHOOTER_SPEED), - new ClimberUp(climber) + new SpinShooter(shooter, Constants.SHOOTER_SPEED), //Should be a shoot command + LoggableCommandWrapper.wrap(auto.resetOdometry("TowerToClimb")), + new LoggableParallelCommandGroup( + LoggableCommandWrapper.wrap(auto.trajectoryCmd("TowerToClimb")), + new ClimberUp(climber) + ), + new ClimberDown(climber) ); } } diff --git a/src/main/java/frc/robot/commands/auto/MidShootClimb.java b/src/main/java/frc/robot/commands/auto/MidShootClimb.java index a35bc3e..2e6d730 100644 --- a/src/main/java/frc/robot/commands/auto/MidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/MidShootClimb.java @@ -1,6 +1,7 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; @@ -11,6 +12,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class MidShootClimb extends LoggableSequentialCommandGroup{ @@ -18,9 +20,13 @@ public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsyst super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), - LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), - new SpinShooter(shooter, Constants.SHOOTER_SPEED), - new ClimberUp(climber) + new SpinShooter(shooter, Constants.SHOOTER_SPEED), //Should be a shoot command + LoggableCommandWrapper.wrap(auto.resetOdometry("TowerToClimb")), + new LoggableParallelCommandGroup( + LoggableCommandWrapper.wrap(auto.trajectoryCmd("TowerToClimb")), + new ClimberUp(climber) + ), + new ClimberDown(climber) ); } } diff --git a/src/main/java/frc/robot/commands/auto/RightShootandClimb.java b/src/main/java/frc/robot/commands/auto/RightShootandClimb.java index fca9247..46022f1 100644 --- a/src/main/java/frc/robot/commands/auto/RightShootandClimb.java +++ b/src/main/java/frc/robot/commands/auto/RightShootandClimb.java @@ -1,6 +1,7 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; @@ -11,6 +12,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class RightShootandClimb extends LoggableSequentialCommandGroup { @@ -19,8 +21,13 @@ public RightShootandClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSu new SetShootingState(shootState, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")), - new SpinShooter(shooter, Constants.SHOOTER_SPEED), - new ClimberUp(climber) + new SpinShooter(shooter, Constants.SHOOTER_SPEED), //Should be a shoot command + LoggableCommandWrapper.wrap(auto.resetOdometry("TowerToClimb")), + new LoggableParallelCommandGroup( + LoggableCommandWrapper.wrap(auto.trajectoryCmd("TowerToClimb")), + new ClimberUp(climber) + ), + new ClimberDown(climber) ); } } From 709e482ed3873485a0b275a6cc66da4c549951bd Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Wed, 18 Feb 2026 20:20:58 -0500 Subject: [PATCH 11/22] added stuff --- src/main/deploy/choreo/LeftToTower.traj | 125 +++++++++++------- src/main/deploy/choreo/MidToTower.traj | 2 +- src/main/deploy/choreo/RightToTower.traj | 81 ++++++------ .../frc/robot/autochooser/FieldLocation.java | 10 +- 4 files changed, 122 insertions(+), 96 deletions(-) diff --git a/src/main/deploy/choreo/LeftToTower.traj b/src/main/deploy/choreo/LeftToTower.traj index 21ea972..894d991 100644 --- a/src/main/deploy/choreo/LeftToTower.traj +++ b/src/main/deploy/choreo/LeftToTower.traj @@ -3,9 +3,10 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.635439157485962, "y":7.431572914123535, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.020376205444336, "y":3.180727243423462, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.733068466186523, "y":7.429088115692139, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.9623348712921143, "y":5.500389575958252, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.955095887184143, "y":4.855756759643555, "heading":-3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.8350459933280945, "y":4.654308795928955, "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":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,9 +15,10 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.635439157485962 m", "val":3.635439157485962}, "y":{"exp":"7.431572914123535 m", "val":7.431572914123535}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.020376205444336 m", "val":2.020376205444336}, "y":{"exp":"3.180727243423462 m", "val":3.180727243423462}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.7330684661865234 m", "val":3.733068466186523}, "y":{"exp":"7.429088115692139 m", "val":7.429088115692139}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.9623348712921143 m", "val":2.9623348712921143}, "y":{"exp":"5.500389575958252 m", "val":5.500389575958252}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.955095887184143 m", "val":1.955095887184143}, "y":{"exp":"4.855756759643555 m", "val":4.855756759643555}, "heading":{"exp":"-180 deg", "val":-3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.8350459933280945 m", "val":0.8350459933280945}, "y":{"exp":"4.654308795928955 m", "val":4.654308795928955}, "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":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -51,51 +53,74 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.24933,1.67027], + "waypoints":[0.0,0.73843,1.23731,1.75874], "samples":[ - {"t":0.0, "x":3.63544, "y":7.43157, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.00557, "ay":-8.79207, "alpha":0.0, "fx":[-34.11416,-34.11416,-34.11416,-34.11416], "fy":[-149.5506,-149.5506,-149.5506,-149.5506]}, - {"t":0.04164, "x":3.6337, "y":7.42395, "heading":0.0, "vx":-0.08352, "vy":-0.36614, "omega":0.0, "ax":-2.0054, "ay":-8.79134, "alpha":0.0, "fx":[-34.11133,-34.11133,-34.11133,-34.11133], "fy":[-149.53818,-149.53818,-149.53818,-149.53818]}, - {"t":0.08329, "x":3.62848, "y":7.40108, "heading":0.0, "vx":-0.16703, "vy":-0.73225, "omega":0.0, "ax":-2.00521, "ay":-8.79048, "alpha":0.0, "fx":[-34.10798,-34.10798,-34.10798,-34.10798], "fy":[-149.52347,-149.52347,-149.52347,-149.52347]}, - {"t":0.12493, "x":3.61979, "y":7.36296, "heading":0.0, "vx":-0.25054, "vy":-1.09833, "omega":0.0, "ax":-2.00497, "ay":-8.78944, "alpha":0.0, "fx":[-34.10394,-34.10394,-34.10394,-34.10394], "fy":[-149.50578,-149.50578,-149.50578,-149.50578]}, - {"t":0.16658, "x":3.60762, "y":7.3096, "heading":0.0, "vx":-0.33404, "vy":-1.46436, "omega":0.0, "ax":-2.00468, "ay":-8.78816, "alpha":0.0, "fx":[-34.099,-34.099,-34.099,-34.099], "fy":[-149.48411,-149.48411,-149.48411,-149.48411]}, - {"t":0.20822, "x":3.59197, "y":7.241, "heading":0.0, "vx":-0.41752, "vy":-1.83034, "omega":0.0, "ax":-2.00431, "ay":-8.78656, "alpha":0.0, "fx":[-34.0928,-34.0928,-34.0928,-34.0928], "fy":[-149.45694,-149.45694,-149.45694,-149.45694]}, - {"t":0.24987, "x":3.57284, "y":7.15716, "heading":0.0, "vx":-0.50099, "vy":-2.19625, "omega":0.0, "ax":-2.00384, "ay":-8.7845, "alpha":0.0, "fx":[-34.0848,-34.0848,-34.0848,-34.0848], "fy":[-149.42186,-149.42186,-149.42186,-149.42186]}, - {"t":0.29151, "x":3.55024, "y":7.05808, "heading":0.0, "vx":-0.58444, "vy":-2.56207, "omega":0.0, "ax":-2.00321, "ay":-8.78174, "alpha":0.0, "fx":[-34.07408,-34.07408,-34.07408,-34.07408], "fy":[-149.37485,-149.37485,-149.37485,-149.37485]}, - {"t":0.33316, "x":3.52416, "y":6.94377, "heading":0.0, "vx":-0.66786, "vy":-2.92778, "omega":0.0, "ax":-2.00232, "ay":-8.77784, "alpha":0.0, "fx":[-34.05897,-34.05897,-34.05897,-34.05897], "fy":[-149.30859,-149.30859,-149.30859,-149.30859]}, - {"t":0.3748, "x":3.49462, "y":6.81423, "heading":0.0, "vx":-0.75125, "vy":-3.29333, "omega":0.0, "ax":-2.00098, "ay":-8.77194, "alpha":0.0, "fx":[-34.03608,-34.03608,-34.03608,-34.03608], "fy":[-149.20821,-149.20821,-149.20821,-149.20821]}, - {"t":0.41644, "x":3.4616, "y":6.66947, "heading":0.0, "vx":-0.83458, "vy":-3.65864, "omega":0.0, "ax":-1.9987, "ay":-8.76195, "alpha":0.0, "fx":[-33.99734,-33.99734,-33.99734,-33.99734], "fy":[-149.03835,-149.03835,-149.03835,-149.03835]}, - {"t":0.45809, "x":3.42511, "y":6.50951, "heading":0.0, "vx":-0.91781, "vy":-4.02352, "omega":0.0, "ax":-1.99402, "ay":-8.74142, "alpha":0.0, "fx":[-33.9177,-33.9177,-33.9177,-33.9177], "fy":[-148.68913,-148.68913,-148.68913,-148.68913]}, - {"t":0.49973, "x":3.38516, "y":6.33438, "heading":0.0, "vx":-1.00085, "vy":-4.38755, "omega":0.0, "ax":-1.97898, "ay":-8.67549, "alpha":0.0, "fx":[-33.66192,-33.66192,-33.66192,-33.66192], "fy":[-147.56753,-147.56753,-147.56753,-147.56753]}, - {"t":0.54138, "x":3.34176, "y":6.14414, "heading":0.0, "vx":-1.08326, "vy":-4.74884, "omega":0.0, "ax":-0.18693, "ay":-0.81836, "alpha":0.0, "fx":[-3.17971,-3.17971,-3.17971,-3.17971], "fy":[-13.92011,-13.92011,-13.92011,-13.92011]}, - {"t":0.58302, "x":3.29649, "y":5.94566, "heading":0.0, "vx":-1.09105, "vy":-4.78292, "omega":0.0, "ax":-0.00111, "ay":-0.00003, "alpha":0.0, "fx":[-0.01885,-0.01885,-0.01885,-0.01885], "fy":[-0.00044,-0.00044,-0.00044,-0.00044]}, - {"t":0.62467, "x":3.25105, "y":5.74648, "heading":0.0, "vx":-1.0911, "vy":-4.78292, "omega":0.0, "ax":-0.00502, "ay":0.00115, "alpha":0.0, "fx":[-0.0854,-0.0854,-0.0854,-0.0854], "fy":[0.01948,0.01948,0.01948,0.01948]}, - {"t":0.66631, "x":3.20561, "y":5.5473, "heading":0.0, "vx":-1.0913, "vy":-4.78287, "omega":0.0, "ax":-0.02419, "ay":0.00552, "alpha":0.0, "fx":[-0.41146,-0.41146,-0.41146,-0.41146], "fy":[0.09393,0.09393,0.09393,0.09393]}, - {"t":0.70796, "x":3.16014, "y":5.34812, "heading":0.0, "vx":-1.09231, "vy":-4.78264, "omega":0.0, "ax":-0.11654, "ay":0.02668, "alpha":0.0, "fx":[-1.98231,-1.98231,-1.98231,-1.98231], "fy":[0.45381,0.45381,0.45381,0.45381]}, - {"t":0.7496, "x":3.11455, "y":5.14898, "heading":0.0, "vx":-1.09716, "vy":-4.78153, "omega":0.0, "ax":-0.55903, "ay":0.12972, "alpha":0.0, "fx":[-9.50887,-9.50887,-9.50887,-9.50887], "fy":[2.20649,2.20649,2.20649,2.20649]}, - {"t":0.79124, "x":3.06837, "y":4.94996, "heading":0.0, "vx":-1.12045, "vy":-4.77613, "omega":0.0, "ax":-2.47526, "ay":0.60916, "alpha":0.0, "fx":[-42.10344,-42.10344,-42.10344,-42.10344], "fy":[10.36168,10.36168,10.36168,10.36168]}, - {"t":0.83289, "x":3.01957, "y":4.75159, "heading":0.0, "vx":-1.22353, "vy":-4.75076, "omega":0.0, "ax":-6.12231, "ay":1.75537, "alpha":0.0, "fx":[-104.13881,-104.13881,-104.13881,-104.13881], "fy":[29.85842,29.85842,29.85842,29.85842]}, - {"t":0.87453, "x":2.9633, "y":4.55527, "heading":0.0, "vx":-1.47849, "vy":-4.67766, "omega":0.0, "ax":-7.63967, "ay":2.70927, "alpha":0.0, "fx":[-129.94863,-129.94863,-129.94863,-129.94863], "fy":[46.08387,46.08387,46.08387,46.08387]}, - {"t":0.91618, "x":2.89511, "y":4.36282, "heading":0.0, "vx":-1.79664, "vy":-4.56483, "omega":0.0, "ax":-7.88972, "ay":3.44726, "alpha":0.0, "fx":[-134.2019,-134.2019,-134.2019,-134.2019], "fy":[58.63697,58.63697,58.63697,58.63697]}, - {"t":0.95782, "x":2.81345, "y":4.17571, "heading":0.0, "vx":-2.1252, "vy":-4.42128, "omega":0.0, "ax":-7.76862, "ay":4.12643, "alpha":0.0, "fx":[-132.142,-132.142,-132.142,-132.142], "fy":[70.18936,70.18936,70.18936,70.18936]}, - {"t":0.99947, "x":2.71821, "y":3.99517, "heading":0.0, "vx":-2.44872, "vy":-4.24943, "omega":0.0, "ax":-6.68475, "ay":5.85285, "alpha":0.0, "fx":[-113.70573,-113.70573,-113.70573,-113.70573], "fy":[99.55524,99.55524,99.55524,99.55524]}, - {"t":1.04111, "x":2.61044, "y":3.82328, "heading":0.0, "vx":-2.7271, "vy":-4.00569, "omega":0.0, "ax":-2.50406, "ay":8.59359, "alpha":0.0, "fx":[-42.5934,-42.5934,-42.5934,-42.5934], "fy":[146.17452,146.17452,146.17452,146.17452]}, - {"t":1.08276, "x":2.4947, "y":3.66392, "heading":0.0, "vx":-2.83138, "vy":-3.64782, "omega":0.0, "ax":-0.77091, "ay":8.9464, "alpha":0.0, "fx":[-13.11294,-13.11294,-13.11294,-13.11294], "fy":[152.17579,152.17579,152.17579,152.17579]}, - {"t":1.1244, "x":2.37612, "y":3.51976, "heading":0.0, "vx":-2.86349, "vy":-3.27525, "omega":0.0, "ax":0.0268, "ay":8.99294, "alpha":0.0, "fx":[0.45588,0.45588,0.45588,0.45588], "fy":[152.96738,152.96738,152.96738,152.96738]}, - {"t":1.16604, "x":2.25689, "y":3.39116, "heading":0.0, "vx":-2.86237, "vy":-2.90074, "omega":0.0, "ax":0.4735, "ay":8.98811, "alpha":0.0, "fx":[8.05402,8.05402,8.05402,8.05402], "fy":[152.88512,152.88512,152.88512,152.88512]}, - {"t":1.20769, "x":2.1381, "y":3.27816, "heading":0.0, "vx":-2.84265, "vy":-2.52644, "omega":0.0, "ax":0.75693, "ay":8.97354, "alpha":0.0, "fx":[12.87519,12.87519,12.87519,12.87519], "fy":[152.63737,152.63737,152.63737,152.63737]}, - {"t":1.24933, "x":2.02038, "y":3.18073, "heading":0.0, "vx":-2.81113, "vy":-2.15274, "omega":0.0, "ax":1.67461, "ay":8.84622, "alpha":0.0, "fx":[28.48471,28.48471,28.48471,28.48471], "fy":[150.47163,150.47163,150.47163,150.47163]}, - {"t":1.28441, "x":1.9228, "y":3.11066, "heading":0.0, "vx":-2.75239, "vy":-1.84243, "omega":0.0, "ax":3.26486, "ay":8.39064, "alpha":0.0, "fx":[55.53427,55.53427,55.53427,55.53427], "fy":[142.72235,142.72235,142.72235,142.72235]}, - {"t":1.31949, "x":1.82826, "y":3.05119, "heading":0.0, "vx":-2.63786, "vy":-1.5481, "omega":0.0, "ax":4.68595, "ay":7.68891, "alpha":0.0, "fx":[79.70672,79.70672,79.70672,79.70672], "fy":[130.78619,130.78619,130.78619,130.78619]}, - {"t":1.35457, "x":1.73861, "y":3.00161, "heading":0.0, "vx":-2.47349, "vy":-1.27838, "omega":0.0, "ax":5.84682, "ay":6.8496, "alpha":0.0, "fx":[99.45279,99.45279,99.45279,99.45279], "fy":[116.50965,116.50965,116.50965,116.50965]}, - {"t":1.38965, "x":1.65544, "y":2.96098, "heading":0.0, "vx":-2.26839, "vy":-1.03811, "omega":0.0, "ax":6.7338, "ay":5.98226, "alpha":0.0, "fx":[114.54008,114.54008,114.54008,114.54008], "fy":[101.75654,101.75654,101.75654,101.75654]}, - {"t":1.42473, "x":1.58001, "y":2.92825, "heading":0.0, "vx":-2.03218, "vy":-0.82826, "omega":0.0, "ax":7.3838, "ay":5.16149, "alpha":0.0, "fx":[125.59625,125.59625,125.59625,125.59625], "fy":[87.79553,87.79553,87.79553,87.79553]}, - {"t":1.4598, "x":1.51327, "y":2.90237, "heading":0.0, "vx":-1.77316, "vy":-0.64721, "omega":0.0, "ax":7.85009, "ay":4.42333, "alpha":0.0, "fx":[133.52778,133.52778,133.52778,133.52778], "fy":[75.23954,75.23954,75.23954,75.23954]}, - {"t":1.49488, "x":1.4559, "y":2.88239, "heading":0.0, "vx":-1.4978, "vy":-0.49204, "omega":0.0, "ax":8.18214, "ay":3.77729, "alpha":0.0, "fx":[139.17588,139.17588,139.17588,139.17588], "fy":[64.25069,64.25069,64.25069,64.25069]}, - {"t":1.52996, "x":1.40839, "y":2.86745, "heading":0.0, "vx":-1.21078, "vy":-0.35954, "omega":0.0, "ax":8.41878, "ay":3.21904, "alpha":0.0, "fx":[143.20108,143.20108,143.20108,143.20108], "fy":[54.75498,54.75498,54.75498,54.75498]}, - {"t":1.56504, "x":1.3711, "y":2.85682, "heading":0.0, "vx":-0.91546, "vy":-0.24662, "omega":0.0, "ax":8.58825, "ay":2.73862, "alpha":0.0, "fx":[146.08371,146.08371,146.08371,146.08371], "fy":[46.58314,46.58314,46.58314,46.58314]}, - {"t":1.60012, "x":1.34427, "y":2.84986, "heading":0.0, "vx":-0.6142, "vy":-0.15056, "omega":0.0, "ax":8.71038, "ay":2.32483, "alpha":0.0, "fx":[148.16113,148.16113,148.16113,148.16113], "fy":[39.54461,39.54461,39.54461,39.54461]}, - {"t":1.6352, "x":1.32808, "y":2.846, "heading":0.0, "vx":-0.30865, "vy":-0.069, "omega":0.0, "ax":8.79894, "ay":1.96714, "alpha":0.0, "fx":[149.66739,149.66739,149.66739,149.66739], "fy":[33.46043,33.46043,33.46043,33.46043]}, - {"t":1.67027, "x":1.32267, "y":2.84479, "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]}], + {"t":0.0, "x":3.73307, "y":7.42909, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.60046, "ay":-8.51755, "alpha":-4.50152, "fx":[-18.88916,-25.99334,-74.74813,-57.30151], "fy":[-152.21975,-151.10919,-133.90243,-142.29305]}, + {"t":0.03692, "x":3.7313, "y":7.42328, "heading":0.0, "vx":-0.09601, "vy":-0.31448, "omega":-0.1662, "ax":-2.63073, "ay":-8.51369, "alpha":-4.37655, "fx":[-19.95841,-27.21722,-74.39745,-57.41863], "fy":[-152.07031,-150.87773,-134.08115,-142.2328]}, + {"t":0.07384, "x":3.72596, "y":7.40587, "heading":-0.00614, "vx":-0.19314, "vy":-0.62882, "omega":-0.32779, "ax":-2.66603, "ay":-8.50872, "alpha":-4.23408, "fx":[-21.31911,-28.43579,-73.94111,-57.69769], "fy":[-151.87043,-150.63474,-134.31392,-142.10436]}, + {"t":0.11076, "x":3.71701, "y":7.37685, "heading":-0.01824, "vx":-0.29158, "vy":-0.94297, "omega":-0.48412, "ax":-2.70772, "ay":-8.50229, "alpha":-4.06763, "fx":[-23.01025,-29.72748,-73.36619,-58.12651], "fy":[-151.60552,-150.3642,-134.60568,-141.91087]}, + {"t":0.14769, "x":3.7044, "y":7.33624, "heading":-0.03611, "vx":-0.39155, "vy":-1.25689, "omega":-0.6343, "ax":-2.7577, "ay":-8.49389, "alpha":-3.86837, "fx":[-25.08997,-31.1982,-72.6538,-58.68912], "fy":[-151.25357,-150.04126,-134.96372,-141.65629]}, + {"t":0.18461, "x":3.68806, "y":7.28405, "heading":-0.05953, "vx":-0.49337, "vy":-1.5705, "omega":-0.77713, "ax":-2.81871, "ay":-8.48267, "alpha":-3.62388, "fx":[-27.64587,-32.99222,-71.7791,-59.3644], "fy":[-150.78048,-149.6274,-135.39707,-141.34603]}, + {"t":0.22153, "x":3.66793, "y":7.22028, "heading":-0.08823, "vx":-0.59744, "vy":-1.88369, "omega":-0.91093, "ax":-2.89481, "ay":-8.46716, "alpha":-3.31606, "fx":[-30.814,-35.31063,-70.71114,-60.12372], "fy":[-150.13075,-149.06097,-135.91599,-140.98795]}, + {"t":0.25845, "x":3.64389, "y":7.14496, "heading":-0.12186, "vx":-0.70432, "vy":-2.19631, "omega":-1.03336, "ax":-2.99238, "ay":-8.44475, "alpha":-2.91735, "fx":[-34.81617,-38.44349,-69.41215,-60.92659], "fy":[-149.20791,-148.23809,-136.53116,-140.59395]}, + {"t":0.29537, "x":3.61585, "y":7.05811, "heading":-0.16001, "vx":-0.8148, "vy":-2.5081, "omega":-1.14108, "ax":-3.12197, "ay":-8.41043, "alpha":-2.38277, "fx":[-40.03901,-42.82868,-67.83572,-61.71185], "fy":[-147.82864,-146.97135,-137.25299,-140.18302]}, + {"t":0.33229, "x":3.58364, "y":6.95978, "heading":-0.20214, "vx":-0.93007, "vy":-2.81863, "omega":-1.22905, "ax":-3.30225, "ay":-8.35365, "alpha":-1.6322, "fx":[-47.21819,-49.1641,-65.92159,-62.37729], "fy":[-145.60007,-144.89373,-138.09133,-139.78758]}, + {"t":0.36921, "x":3.54705, "y":6.85001, "heading":-0.24752, "vx":-1.052, "vy":-3.12706, "omega":-1.28931, "ax":-3.56935, "ay":-8.2492, "alpha":-0.50347, "fx":[-57.92253,-58.62742,-63.58096,-62.7236], "fy":[-141.52711,-141.21269,-139.05683,-139.46944]}, + {"t":0.40614, "x":3.50577, "y":6.72894, "heading":-0.29512, "vx":-1.18378, "vy":-3.43163, "omega":-1.3079, "ax":-4.0006, "ay":-8.024, "alpha":1.40219, "fx":[-76.00561,-73.28324,-60.6529,-62.25462], "fy":[-132.39397,-134.01081,-140.16742,-139.37184]}, + {"t":0.44306, "x":3.45934, "y":6.59677, "heading":-0.34341, "vx":-1.33149, "vy":-3.72789, "omega":-1.25613, "ax":-4.75993, "ay":-7.39164, "alpha":5.42202, "fx":[-111.5647,-96.47327,-56.76302,-59.05938], "fy":[-103.38096,-118.10001,-141.46867,-139.96941]}, + {"t":0.47998, "x":3.40693, "y":6.45409, "heading":-0.38979, "vx":-1.50723, "vy":-4.0008, "omega":-1.05594, "ax":-5.25386, "ay":-5.17746, "alpha":16.73659, "fx":[-150.63137,-129.66516,-50.97813,-26.19228], "fy":[14.10994,-79.60446,-143.00909,-143.76513]}, + {"t":0.5169, "x":3.3477, "y":6.30284, "heading":-0.42878, "vx":-1.70121, "vy":-4.19196, "omega":-0.438, "ax":-2.77364, "ay":1.55626, "alpha":35.32807, "fx":[-83.97038,-151.93183,-34.88551,82.07227], "fy":[126.74707,-1.54945,-145.74971,126.43848]}, + {"t":0.55382, "x":3.283, "y":6.14913, "heading":-0.44495, "vx":-1.80362, "vy":-4.1345, "omega":0.86636, "ax":0.61295, "ay":5.33644, "alpha":26.73946, "fx":[-41.30034,-133.74863,140.56096,76.19209], "fy":[146.95534,72.61329,11.57727,131.93966]}, + {"t":0.59074, "x":3.21683, "y":6.00012, "heading":-0.41296, "vx":-1.78099, "vy":-3.93747, "omega":1.85362, "ax":0.65085, "ay":7.25195, "alpha":18.10652, "fx":[-26.7659,-111.29041,107.93537,74.40416], "fy":[150.56751,104.26326,105.11452,133.46921]}, + {"t":0.62767, "x":3.15151, "y":5.85968, "heading":-0.34452, "vx":-1.75696, "vy":-3.66972, "omega":2.52214, "ax":0.83665, "ay":7.61808, "alpha":16.22061, "fx":[-22.01947,-95.10245,103.02243,71.02402], "fy":[151.48254,119.46308,111.84433,135.53577]}, + {"t":0.66459, "x":3.08722, "y":5.72938, "heading":-0.2514, "vx":-1.72607, "vy":-3.38845, "omega":3.12103, "ax":0.94451, "ay":7.77342, "alpha":15.35854, "fx":[-22.0332,-84.20355,103.53747,66.96259], "fy":[151.56972,127.52025,112.08182,137.72308]}, + {"t":0.70151, "x":3.02413, "y":5.60958, "heading":-0.13617, "vx":-1.6912, "vy":-3.10144, "omega":3.68809, "ax":0.94795, "ay":7.81111, "alpha":15.21545, "fx":[-25.9019,-78.69013,106.72881,62.36085], "fy":[151.01601,131.06663,109.4219,139.95419]}, + {"t":0.73843, "x":2.96233, "y":5.50039, "heading":0.0, "vx":-1.6562, "vy":-2.81304, "omega":4.24987, "ax":0.68215, "ay":7.85328, "alpha":14.82477, "fx":[-34.44657,-76.55744,104.11881,53.2979], "fy":[148.88237,131.22059,110.97581,143.24908]}, + {"t":0.75506, "x":2.93489, "y":5.4547, "heading":0.07067, "vx":-1.64485, "vy":-2.68245, "omega":4.4964, "ax":0.52831, "ay":7.76398, "alpha":15.45946, "fx":[-40.37949,-80.65665,107.07686,49.9049], "fy":[147.32371,128.43189,108.06996,144.42704]}, + {"t":0.77169, "x":2.90761, "y":5.41116, "heading":0.14545, "vx":-1.63607, "vy":-2.55334, "omega":4.75348, "ax":0.33382, "ay":7.63601, "alpha":16.2779, "fx":[-47.30132,-86.53902,110.35288,46.20046], "fy":[145.17811,124.09699,104.65955,145.61049]}, + {"t":0.78832, "x":2.88045, "y":5.36976, "heading":0.22449, "vx":-1.63052, "vy":-2.42635, "omega":5.02417, "ax":0.08741, "ay":7.45314, "alpha":17.32266, "fx":[-55.29751,-94.70586,113.8383,42.11236], "fy":[142.23843,117.28216,100.78714,146.79507]}, + {"t":0.80495, "x":2.85334, "y":5.33044, "heading":0.30804, "vx":-1.62906, "vy":-2.30241, "omega":5.31224, "ax":-0.21747, "ay":7.19786, "alpha":18.60794, "fx":[-64.24279,-105.30041,117.18462,37.56194], "fy":[138.32075,106.64669,96.80035,147.96658]}, + {"t":0.82158, "x":2.82622, "y":5.29315, "heading":0.39638, "vx":-1.63268, "vy":-2.18272, "omega":5.62168, "ax":-0.57608, "ay":6.84535, "alpha":20.15839, "fx":[-73.76659,-117.87433,119.92377,32.52162], "fy":[133.34417,90.01674,93.30377,149.08513]}, + {"t":0.83821, "x":2.79899, "y":5.2578, "heading":0.48987, "vx":-1.64226, "vy":-2.06888, "omega":5.9569, "ax":-0.94916, "ay":6.36066, "alpha":22.02504, "fx":[-83.11618,-130.06603,121.51103,27.09149], "fy":[127.53078,64.02965,91.13357,150.07803]}, + {"t":0.85484, "x":2.77155, "y":5.22427, "heading":0.58893, "vx":-1.65804, "vy":-1.96311, "omega":6.32316, "ax":-1.16727, "ay":5.68996, "alpha":24.31152, "fx":[-90.91954,-131.48167,121.33971,21.64184], "fy":[121.80109,23.25323,91.24228,150.84184]}, + {"t":0.87147, "x":2.74382, "y":5.19241, "heading":0.69408, "vx":-1.67745, "vy":-1.86849, "omega":6.72745, "ax":2.14516, "ay":5.75498, "alpha":21.73147, "fx":[-93.37551,104.08075,117.98992,17.25889], "fy":[119.46879,25.48879,95.36839,151.23639]}, + {"t":0.88809, "x":2.71622, "y":5.16214, "heading":0.80595, "vx":-1.64178, "vy":-1.77278, "omega":7.08883, "ax":2.04219, "ay":7.96585, "alpha":10.76318, "fx":[-48.2641,72.35797,93.71661,21.13777], "fy":[143.10013,129.46227,119.03026,150.39494]}, + {"t":0.90472, "x":2.6892, "y":5.13376, "heading":0.92384, "vx":-1.60782, "vy":-1.64032, "omega":7.26782, "ax":2.5077, "ay":8.43476, "alpha":3.76667, "fx":[16.30237,55.89779,64.92125,33.49972], "fy":[149.95799,139.74538,136.56749,147.62027]}, + {"t":0.92135, "x":2.66281, "y":5.10765, "heading":1.0447, "vx":-1.56612, "vy":-1.50005, "omega":7.33046, "ax":1.56143, "ay":8.60679, "alpha":4.4319, "fx":[-5.13459,43.2758,52.69883,15.39804], "fy":[150.31296,143.55866,141.40681,150.31778]}, + {"t":0.93798, "x":2.63698, "y":5.08389, "heading":1.1666, "vx":-1.54015, "vy":-1.35692, "omega":7.40416, "ax":0.2, "ay":8.61607, "alpha":6.1122, "fx":[-41.45624,26.70679,38.97321,-10.61612], "fy":[143.94939,146.43141,145.39225,150.45453]}, + {"t":0.95461, "x":2.6114, "y":5.06252, "heading":1.28972, "vx":-1.53683, "vy":-1.21364, "omega":7.5058, "ax":-1.41237, "ay":8.36927, "alpha":7.33244, "fx":[-77.00979,0.84997,18.72184,-38.65824], "fy":[127.7722,147.40778,148.83522,145.42005]}, + {"t":0.97124, "x":2.58565, "y":5.04349, "heading":1.41454, "vx":-1.56031, "vy":-1.07447, "omega":7.62773, "ax":-3.33547, "ay":7.70823, "alpha":7.78637, "fx":[-108.20214,-39.6874,-10.44588,-68.60594], "fy":[101.90404,140.20961,148.93308,133.41219]}, + {"t":0.98787, "x":2.55924, "y":5.02669, "heading":1.54139, "vx":-1.61578, "vy":-0.94628, "omega":7.75722, "ax":-5.47746, "ay":6.36763, "alpha":7.23528, "fx":[-130.60413,-91.44401,-51.67217,-98.95989], "fy":[70.08441,112.07621,139.08384,112.00184]}, + {"t":1.0045, "x":2.53161, "y":5.01183, "heading":1.67038, "vx":-1.70687, "vy":-0.84039, "omega":7.87754, "ax":-7.40701, "ay":4.13612, "alpha":5.81685, "fx":[-143.7258,-131.42488,-101.94206,-126.87144], "fy":[35.36884,61.61694,106.56796,77.86316]}, + {"t":1.02113, "x":2.5022, "y":4.99843, "heading":1.80138, "vx":-1.83004, "vy":-0.77161, "omega":7.97427, "ax":-8.46514, "ay":1.83894, "alpha":0.65527, "fx":[-144.97489,-144.18416,-142.93263,-143.86642], "fy":[26.93383,29.6377,35.70627,32.84179]}, + {"t":1.03776, "x":2.4706, "y":4.98585, "heading":1.93399, "vx":-1.97081, "vy":-0.74103, "omega":7.98516, "ax":-4.91235, "ay":0.0809, "alpha":-27.02992, "fx":[-58.83682,-140.96051,-133.16472,-1.26892], "fy":[135.84879,51.07289,-66.42826,-114.98902]}, + {"t":1.05439, "x":2.43715, "y":4.97354, "heading":2.06678, "vx":-2.0525, "vy":-0.73969, "omega":7.53567, "ax":-4.87251, "ay":0.26973, "alpha":-26.43448, "fx":[-72.54349,-145.39377,-129.18139,15.59863], "fy":[130.72149,39.78177,-75.60147,-76.54975]}, + {"t":1.07102, "x":2.40234, "y":4.96128, "heading":2.19209, "vx":-2.13353, "vy":-0.7352, "omega":7.09608, "ax":-5.33823, "ay":2.0248, "alpha":-22.82856, "fx":[-85.88295,-148.75242,-124.06923,-4.50215], "fy":[123.41443,27.33592,-84.73458,71.74898]}, + {"t":1.08765, "x":2.36612, "y":4.94933, "heading":2.3101, "vx":-2.2223, "vy":-0.70153, "omega":6.71645, "ax":-6.05089, "ay":2.2268, "alpha":-20.43014, "fx":[-98.5015,-150.85389,-118.93923,-43.40099], "fy":[114.33452,14.86356,-92.43538,114.74605]}, + {"t":1.10428, "x":2.32833, "y":4.93797, "heading":2.42179, "vx":-2.32292, "vy":-0.6645, "omega":6.37671, "ax":-6.66197, "ay":1.79409, "alpha":-18.65798, "fx":[-110.43162,-151.79927,-114.84885,-76.19274], "fy":[103.42679,2.86707,-97.90641,113.68051]}, + {"t":1.12091, "x":2.28878, "y":4.92717, "heading":2.52783, "vx":-2.43371, "vy":-0.63466, "omega":6.06644, "ax":-7.12027, "ay":1.23464, "alpha":-17.23855, "fx":[-120.96158,-151.77894,-111.96125,-99.7534], "fy":[91.36441,-8.31473,-101.51465,102.46817]}, + {"t":1.13754, "x":2.24733, "y":4.91679, "heading":2.62871, "vx":-2.55212, "vy":-0.61413, "omega":5.77977, "ax":-7.44475, "ay":0.67601, "alpha":-16.0848, "fx":[-129.7064,-151.01354,-110.11396,-115.6985], "fy":[78.88474,-18.53081,-103.7638,89.4047]}, + {"t":1.15417, "x":2.20386, "y":4.90667, "heading":2.72483, "vx":-2.67592, "vy":-0.60289, "omega":5.51229, "ax":-7.66355, "ay":0.16421, "alpha":-15.15624, "fx":[-136.58058,-149.70498,-108.93637,-126.19732], "fy":[66.68136,-27.76869,-105.20203,77.4618]}, + {"t":1.1708, "x":2.1583, "y":4.89667, "heading":2.81649, "vx":-2.80336, "vy":-0.60016, "omega":5.26025, "ax":-7.80141, "ay":-0.28728, "alpha":-14.4385, "fx":[-141.73871,-148.01082,-108.01259,-133.03679], "fy":[55.2755,-36.10899,-106.31982,67.60745]}, + {"t":1.18742, "x":2.1106, "y":4.88665, "heading":2.90397, "vx":-2.93309, "vy":-0.60494, "omega":5.02015, "ax":-7.85552, "ay":-0.66054, "alpha":-14.09714, "fx":[-145.32312,-146.01236,-106.37316,-136.77167], "fy":[45.43413,-43.77186,-108.09827,61.4935]}, + {"t":1.20405, "x":2.06074, "y":4.8765, "heading":2.98745, "vx":-3.06372, "vy":-0.61592, "omega":4.78572, "ax":-7.27467, "ay":-0.56049, "alpha":-18.16986, "fx":[-145.14493,-142.93713,-88.39061,-118.48752], "fy":[46.43518,-53.22743,-123.27667,91.93367]}, + {"t":1.22068, "x":2.00878, "y":4.86618, "heading":3.06703, "vx":-3.1847, "vy":-0.62524, "omega":4.48357, "ax":-5.27154, "ay":-0.15292, "alpha":-28.47573, "fx":[-141.64558,-137.7124,-39.96159,-39.34976], "fy":[56.61502,-65.81438,-146.39117,145.18635]}, + {"t":1.23731, "x":1.9551, "y":4.85576, "heading":-3.14159, "vx":-3.27236, "vy":-0.62779, "omega":4.01003, "ax":-4.73582, "ay":-0.29363, "alpha":-30.78277, "fx":[-142.73282,-134.25287,-18.82154,-26.41277], "fy":[55.00641,-73.38294,-151.25405,149.65246]}, + {"t":1.27208, "x":1.83848, "y":4.83376, "heading":-3.0022, "vx":-3.43699, "vy":-0.63799, "omega":2.93996, "ax":-2.49321, "ay":-0.33467, "alpha":-37.25455, "fx":[-140.70416,-119.51507,65.72353,24.86051], "fy":[59.39777,-95.0841,-137.06807,149.98371]}, + {"t":1.30684, "x":1.7175, "y":4.81138, "heading":-2.9, "vx":-3.52366, "vy":-0.64963, "omega":1.64492, "ax":0.09693, "ay":0.12981, "alpha":-40.06, "fx":[-127.27243,-78.21041,132.9055,79.17221], "fy":[83.32272,-130.42008,-74.02746,129.95676]}, + {"t":1.3416, "x":1.59507, "y":4.78887, "heading":-2.84282, "vx":-3.52029, "vy":-0.64512, "omega":0.25235, "ax":4.98764, "ay":1.31634, "alpha":-28.9978, "fx":[-4.5274,74.38444,150.47311,119.02284], "fy":[150.56883,-131.63415,-24.60045,95.22799]}, + {"t":1.37636, "x":1.47571, "y":4.76724, "heading":-2.83404, "vx":-3.34691, "vy":-0.59936, "omega":-0.75567, "ax":8.42937, "ay":1.6374, "alpha":-8.45028, "fx":[132.30814,149.57072,152.70639,138.93931], "fy":[74.94249,-28.20404,1.36638,63.30218]}, + {"t":1.41112, "x":1.36446, "y":4.7474, "heading":-2.86031, "vx":-3.05389, "vy":-0.54244, "omega":-1.04942, "ax":8.79811, "ay":1.62553, "alpha":-2.57688, "fx":[147.15614,152.29242,151.94952,147.21528], "fy":[40.88317,11.76938,16.77848,41.16803]}, + {"t":1.44589, "x":1.26361, "y":4.72952, "heading":-2.89679, "vx":-2.74805, "vy":-0.48593, "omega":-1.13899, "ax":8.84964, "ay":1.60573, "alpha":0.07295, "fx":[150.59618,150.45718,150.46327,150.60272], "fy":[26.95615,27.72048,27.67182,26.90358]}, + {"t":1.48065, "x":1.17343, "y":4.7136, "heading":-2.93639, "vx":-2.44041, "vy":-0.43011, "omega":-1.13646, "ax":8.84411, "ay":1.58725, "alpha":1.66278, "fx":[151.93762,148.95712,148.78162,152.06703], "fy":[19.02215,35.47241,35.98274,17.51752]}, + {"t":1.51541, "x":1.09394, "y":4.69961, "heading":-2.97589, "vx":-2.13298, "vy":-0.37494, "omega":-1.07866, "ax":8.82463, "ay":1.57204, "alpha":2.74863, "fx":[152.60001,147.98851,147.10087,152.72816], "fy":[13.60172,39.64018,42.56626,11.15158]}, + {"t":1.55017, "x":1.02513, "y":4.68752, "heading":-3.01339, "vx":-1.82621, "vy":-0.32029, "omega":-0.98311, "ax":8.80245, "ay":1.55935, "alpha":3.54417, "fx":[152.96407,147.39904,145.51158,153.034], "fy":[9.50993,41.98509,47.88057,6.72055]}, + {"t":1.58493, "x":0.96696, "y":4.67733, "heading":-3.04756, "vx":-1.52022, "vy":-0.26608, "omega":-0.85991, "ax":8.78113, "ay":1.54838, "alpha":4.15241, "fx":[153.17095,147.05266,144.05802,153.17657], "fy":[6.24993,43.32663,52.21095,3.56232]}, + {"t":1.6197, "x":0.91942, "y":4.66902, "heading":-3.07746, "vx":-1.21497, "vy":-0.21226, "omega":-0.71556, "ax":8.76177, "ay":1.53866, "alpha":4.63118, "fx":[153.28592,146.85353,142.75884,153.24231], "fy":[3.58268,44.10327,55.75668,1.24605]}, + {"t":1.65446, "x":0.88248, "y":4.66257, "heading":-3.10233, "vx":-0.9104, "vy":-0.15877, "omega":-0.55457, "ax":8.74454, "ay":1.52996, "alpha":5.01691, "fx":[153.34454,146.73597,141.61707,153.27078], "fy":[1.38125,44.57467,58.66758,-0.5267]}, + {"t":1.68922, "x":0.85612, "y":4.65798, "heading":-3.12161, "vx":-0.60642, "vy":-0.10559, "omega":-0.38017, "ax":8.72927, "ay":1.52218, "alpha":5.33451, "fx":[153.3687,146.65454,140.6267,153.27955], "fy":[-0.42667,44.90668,61.06041,-1.97336]}, + {"t":1.72398, "x":0.84031, "y":4.65522, "heading":-3.13482, "vx":-0.30297, "vy":-0.05267, "omega":-0.19474, "ax":8.71565, "ay":1.51525, "alpha":5.602, "fx":[153.37305,146.57708,139.77727,153.2754], "fy":[-1.88413,45.21128,63.02675,-3.2577]}, + {"t":1.75874, "x":0.83505, "y":4.65431, "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":[] diff --git a/src/main/deploy/choreo/MidToTower.traj b/src/main/deploy/choreo/MidToTower.traj index cc2a51a..98c3199 100644 --- a/src/main/deploy/choreo/MidToTower.traj +++ b/src/main/deploy/choreo/MidToTower.traj @@ -14,7 +14,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.609598159790039 m", "val":3.609598159790039}, "y":{"exp":"4.007639408111572 m", "val":4.007639408111572}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.636425733566284 m", "val":3.636425733566284}, "y":{"exp":"4.022153377532959 m", "val":4.022153377532959}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"2.1108198165893555 m", "val":2.1108198165893555}, "y":{"exp":"2.857714891433716 m", "val":2.857714891433716}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/RightToTower.traj b/src/main/deploy/choreo/RightToTower.traj index a4d3f04..f56d2bc 100644 --- a/src/main/deploy/choreo/RightToTower.traj +++ b/src/main/deploy/choreo/RightToTower.traj @@ -3,8 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.635439157485962, "y":0.7904343605041504, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.3226691484451294, "y":2.082484722137451, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.6271181106567383, "y":0.7985455989837646, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.5972683429718018, "y":2.493582248687744, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,8 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.6271181106567383 m", "val":3.6271181106567383}, "y":{"exp":"0.7985455989837646 m", "val":0.7985455989837646}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.082484722137451 m", "val":2.082484722137451}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.6271181106567383 m", "val":3.6271181106567383}, "y":{"exp":"0.7985455989837646 m", "val":0.7985455989837646}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.5972683429718018 m", "val":2.5972683429718018}, "y":{"exp":"2.493582248687744 m", "val":2.493582248687744}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -51,43 +51,44 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.91196,1.35192], + "waypoints":[0.0,0.71185,1.27531], "samples":[ - {"t":0.0, "x":3.63544, "y":0.79043, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.8525, "ay":1.71849, "alpha":0.0, "fx":[-150.57856,-150.57856,-150.57856,-150.57856], "fy":[29.231,29.231,29.231,29.231]}, - {"t":0.04145, "x":3.62783, "y":0.79191, "heading":0.0, "vx":-0.36696, "vy":0.07124, "omega":0.0, "ax":-8.83658, "ay":1.79466, "alpha":0.0, "fx":[-150.30767,-150.30767,-150.30767,-150.30767], "fy":[30.52668,30.52668,30.52668,30.52668]}, - {"t":0.08291, "x":3.60503, "y":0.79641, "heading":0.0, "vx":-0.73326, "vy":0.14563, "omega":0.0, "ax":-8.81679, "ay":1.88506, "alpha":0.0, "fx":[-149.97103,-149.97103,-149.97103,-149.97103], "fy":[32.06435,32.06435,32.06435,32.06435]}, - {"t":0.12436, "x":3.56706, "y":0.80406, "heading":0.0, "vx":-1.09874, "vy":0.22377, "omega":0.0, "ax":-8.79163, "ay":1.99404, "alpha":0.0, "fx":[-149.54316,-149.54316,-149.54316,-149.54316], "fy":[33.91802,33.91802,33.91802,33.91802]}, - {"t":0.16581, "x":3.51396, "y":0.81505, "heading":0.0, "vx":-1.46317, "vy":0.30643, "omega":0.0, "ax":-8.75878, "ay":2.1279, "alpha":0.0, "fx":[-148.98431,-148.98431,-148.98431,-148.98431], "fy":[36.19496,36.19496,36.19496,36.19496]}, - {"t":0.20726, "x":3.44578, "y":0.82958, "heading":0.0, "vx":-1.82625, "vy":0.39463, "omega":0.0, "ax":-8.7144, "ay":2.29612, "alpha":0.0, "fx":[-148.22943,-148.22943,-148.22943,-148.22943], "fy":[39.05629,39.05629,39.05629,39.05629]}, - {"t":0.24872, "x":3.36259, "y":0.84791, "heading":0.0, "vx":-2.18748, "vy":0.48981, "omega":0.0, "ax":-8.65185, "ay":2.51355, "alpha":0.0, "fx":[-147.16546,-147.16546,-147.16546,-147.16546], "fy":[42.75478,42.75478,42.75478,42.75478]}, - {"t":0.29017, "x":3.26448, "y":0.87038, "heading":0.0, "vx":-2.54612, "vy":0.59401, "omega":0.0, "ax":-8.5587, "ay":2.80477, "alpha":0.0, "fx":[-145.581,-145.581,-145.581,-145.581], "fy":[47.70835,47.70835,47.70835,47.70835]}, - {"t":0.33162, "x":3.15159, "y":0.89741, "heading":0.0, "vx":-2.9009, "vy":0.71027, "omega":0.0, "ax":-8.40939, "ay":3.21306, "alpha":0.0, "fx":[-143.0413,-143.0413,-143.0413,-143.0413], "fy":[54.65316,54.65316,54.65316,54.65316]}, - {"t":0.37307, "x":3.02411, "y":0.92961, "heading":0.0, "vx":-3.24949, "vy":0.84346, "omega":0.0, "ax":-8.14441, "ay":3.82044, "alpha":0.0, "fx":[-138.53402,-138.53402,-138.53402,-138.53402], "fy":[64.98452,64.98452,64.98452,64.98452]}, - {"t":0.41453, "x":2.88242, "y":0.96786, "heading":0.0, "vx":-3.5871, "vy":1.00183, "omega":0.0, "ax":-7.60055, "ay":4.79307, "alpha":0.0, "fx":[-129.28324,-129.28324,-129.28324,-129.28324], "fy":[81.5287,81.5287,81.5287,81.5287]}, - {"t":0.45598, "x":2.72719, "y":1.0135, "heading":0.0, "vx":-3.90216, "vy":1.20051, "omega":0.0, "ax":-6.23599, "ay":6.44461, "alpha":0.0, "fx":[-106.07238,-106.07238,-106.07238,-106.07238], "fy":[109.62092,109.62092,109.62092,109.62092]}, - {"t":0.49743, "x":2.56008, "y":1.06881, "heading":0.0, "vx":-4.16066, "vy":1.46766, "omega":0.0, "ax":-2.28471, "ay":8.64514, "alpha":0.0, "fx":[-38.86233,-38.86233,-38.86233,-38.86233], "fy":[147.05132,147.05132,147.05132,147.05132]}, - {"t":0.53888, "x":2.38565, "y":1.13707, "heading":0.0, "vx":-4.25537, "vy":1.82602, "omega":0.0, "ax":4.23398, "ay":7.87827, "alpha":0.0, "fx":[72.01879,72.01879,72.01879,72.01879], "fy":[134.00718,134.00718,134.00718,134.00718]}, - {"t":0.58034, "x":2.21289, "y":1.21953, "heading":0.0, "vx":-4.07986, "vy":2.1526, "omega":0.0, "ax":7.40591, "ay":5.0602, "alpha":0.0, "fx":[125.9724,125.9724,125.9724,125.9724], "fy":[86.07263,86.07263,86.07263,86.07263]}, - {"t":0.62179, "x":2.05013, "y":1.31311, "heading":0.0, "vx":-3.77286, "vy":2.36236, "omega":0.0, "ax":8.36786, "ay":3.27693, "alpha":0.0, "fx":[142.33493,142.33493,142.33493,142.33493], "fy":[55.73957,55.73957,55.73957,55.73957]}, - {"t":0.66324, "x":1.90092, "y":1.41385, "heading":0.0, "vx":-3.42599, "vy":2.49819, "omega":0.0, "ax":8.7083, "ay":2.25887, "alpha":0.0, "fx":[148.1257,148.1257,148.1257,148.1257], "fy":[38.42278,38.42278,38.42278,38.42278]}, - {"t":0.70469, "x":1.76639, "y":1.51935, "heading":0.0, "vx":-3.06501, "vy":2.59183, "omega":0.0, "ax":8.85407, "ay":1.62886, "alpha":0.0, "fx":[150.60523,150.60523,150.60523,150.60523], "fy":[27.70641,27.70641,27.70641,27.70641]}, - {"t":0.74615, "x":1.64694, "y":1.62819, "heading":0.0, "vx":-2.69799, "vy":2.65935, "omega":0.0, "ax":8.92556, "ay":1.20691, "alpha":0.0, "fx":[151.82122,151.82122,151.82122,151.82122], "fy":[20.52924,20.52924,20.52924,20.52924]}, - {"t":0.7876, "x":1.54277, "y":1.73946, "heading":0.0, "vx":-2.328, "vy":2.70938, "omega":0.0, "ax":8.96402, "ay":0.90651, "alpha":0.0, "fx":[152.47543,152.47543,152.47543,152.47543], "fy":[15.41942,15.41942,15.41942,15.41942]}, - {"t":0.82905, "x":1.45397, "y":1.85255, "heading":0.0, "vx":-1.95642, "vy":2.74696, "omega":0.0, "ax":8.98607, "ay":0.68246, "alpha":0.0, "fx":[152.85043,152.85043,152.85043,152.85043], "fy":[11.60844,11.60844,11.60844,11.60844]}, - {"t":0.8705, "x":1.3806, "y":1.96701, "heading":0.0, "vx":-1.58392, "vy":2.77525, "omega":0.0, "ax":8.99925, "ay":0.50925, "alpha":0.0, "fx":[153.07473,153.07473,153.07473,153.07473], "fy":[8.66227,8.66227,8.66227,8.66227]}, - {"t":0.91196, "x":1.32267, "y":2.08248, "heading":0.0, "vx":-1.21088, "vy":2.79636, "omega":0.0, "ax":9.01145, "ay":-0.06569, "alpha":0.0, "fx":[153.28221,153.28221,153.28221,153.28221], "fy":[-1.11731,-1.11731,-1.11731,-1.11731]}, - {"t":0.94862, "x":1.28433, "y":2.18497, "heading":0.0, "vx":-0.88049, "vy":2.79395, "omega":0.0, "ax":8.91241, "ay":-1.31752, "alpha":0.0, "fx":[151.59755,151.59755,151.59755,151.59755], "fy":[-22.41063,-22.41063,-22.41063,-22.41063]}, - {"t":0.98528, "x":1.25804, "y":2.28652, "heading":0.0, "vx":-0.55372, "vy":2.74564, "omega":0.0, "ax":8.50835, "ay":-2.954, "alpha":0.0, "fx":[144.72454,144.72454,144.72454,144.72454], "fy":[-50.24676,-50.24676,-50.24676,-50.24676]}, - {"t":1.02195, "x":1.24346, "y":2.3852, "heading":0.0, "vx":-0.24177, "vy":2.63734, "omega":0.0, "ax":7.54244, "ay":-4.91788, "alpha":0.0, "fx":[128.29477,128.29477,128.29477,128.29477], "fy":[-83.65171,-83.65171,-83.65171,-83.65171]}, - {"t":1.05861, "x":1.23966, "y":2.47859, "heading":0.0, "vx":0.03476, "vy":2.45703, "omega":0.0, "ax":5.8318, "ay":-6.8588, "alpha":0.0, "fx":[99.19729,99.19729,99.19729,99.19729], "fy":[-116.66617,-116.66617,-116.66617,-116.66617]}, - {"t":1.09528, "x":1.24485, "y":2.56406, "heading":0.0, "vx":0.24858, "vy":2.20556, "omega":0.0, "ax":3.60511, "ay":-8.25052, "alpha":0.0, "fx":[61.32194,61.32194,61.32194,61.32194], "fy":[-140.33906,-140.33906,-140.33906,-140.33906]}, - {"t":1.13194, "x":1.25639, "y":2.63938, "heading":0.0, "vx":0.38076, "vy":1.90306, "omega":0.0, "ax":1.42341, "ay":-8.89289, "alpha":0.0, "fx":[24.21183,24.21183,24.21183,24.21183], "fy":[-151.26545,-151.26545,-151.26545,-151.26545]}, - {"t":1.1686, "x":1.27131, "y":2.70318, "heading":0.0, "vx":0.43295, "vy":1.57701, "omega":0.0, "ax":-0.34545, "ay":-9.00218, "alpha":0.0, "fx":[-5.87599,-5.87599,-5.87599,-5.87599], "fy":[-153.12445,-153.12445,-153.12445,-153.12445]}, - {"t":1.20527, "x":1.28695, "y":2.75495, "heading":0.0, "vx":0.42028, "vy":1.24696, "omega":0.0, "ax":-1.66113, "ay":-8.85686, "alpha":0.0, "fx":[-28.25536,-28.25536,-28.25536,-28.25536], "fy":[-150.65273,-150.65273,-150.65273,-150.65273]}, - {"t":1.24193, "x":1.30124, "y":2.79471, "heading":0.0, "vx":0.35938, "vy":0.92223, "omega":0.0, "ax":-2.61991, "ay":-8.6242, "alpha":0.0, "fx":[-44.56391,-44.56391,-44.56391,-44.56391], "fy":[-146.69526,-146.69526,-146.69526,-146.69526]}, - {"t":1.2786, "x":1.31266, "y":2.82273, "heading":0.0, "vx":0.26332, "vy":0.60603, "omega":0.0, "ax":-3.32569, "ay":-8.3792, "alpha":0.0, "fx":[-56.56901,-56.56901,-56.56901,-56.56901], "fy":[-142.52773,-142.52773,-142.52773,-142.52773]}, - {"t":1.31526, "x":1.32008, "y":2.83932, "heading":0.0, "vx":0.14139, "vy":0.29882, "omega":0.0, "ax":-3.85633, "ay":-8.15011, "alpha":0.0, "fx":[-65.5951,-65.5951,-65.5951,-65.5951], "fy":[-138.63104,-138.63104,-138.63104,-138.63104]}, - {"t":1.35192, "x":1.32267, "y":2.84479, "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]}], + {"t":0.0, "x":3.62712, "y":0.79855, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.27304, "ay":8.72562, "alpha":0.0, "fx":[-38.66378,-38.66378,-38.66378,-38.66378], "fy":[148.42029,148.42029,148.42029,148.42029]}, + {"t":0.03747, "x":3.62552, "y":0.80467, "heading":0.0, "vx":-0.08516, "vy":0.32691, "omega":0.0, "ax":-2.40365, "ay":8.68971, "alpha":0.0, "fx":[-40.88537,-40.88537,-40.88537,-40.88537], "fy":[147.80956,147.80956,147.80956,147.80956]}, + {"t":0.07493, "x":3.62065, "y":0.82302, "heading":0.0, "vx":-0.17522, "vy":0.65248, "omega":0.0, "ax":-2.55637, "ay":8.64504, "alpha":0.0, "fx":[-43.48315,-43.48315,-43.48315,-43.48315], "fy":[147.04962,147.04962,147.04962,147.04962]}, + {"t":0.1124, "x":3.61229, "y":0.85353, "heading":0.0, "vx":-0.27099, "vy":0.97638, "omega":0.0, "ax":-2.73715, "ay":8.58834, "alpha":0.0, "fx":[-46.5581,-46.5581,-46.5581,-46.5581], "fy":[146.08527,146.08527,146.08527,146.08527]}, + {"t":0.14986, "x":3.60021, "y":0.89614, "heading":0.0, "vx":-0.37354, "vy":1.29815, "omega":0.0, "ax":-2.95414, "ay":8.51472, "alpha":0.0, "fx":[-50.24911,-50.24911,-50.24911,-50.24911], "fy":[144.83301,144.83301,144.83301,144.83301]}, + {"t":0.18733, "x":3.58414, "y":0.95075, "heading":0.0, "vx":-0.48422, "vy":1.61716, "omega":0.0, "ax":-3.21882, "ay":8.41648, "alpha":0.0, "fx":[-54.75128,-54.75128,-54.75128,-54.75128], "fy":[143.16189,143.16189,143.16189,143.16189]}, + {"t":0.2248, "x":3.56374, "y":1.01725, "heading":0.0, "vx":-0.60482, "vy":1.93249, "omega":0.0, "ax":-3.54765, "ay":8.28102, "alpha":0.0, "fx":[-60.34447,-60.34447,-60.34447,-60.34447], "fy":[140.85786,140.85786,140.85786,140.85786]}, + {"t":0.26226, "x":3.53859, "y":1.09546, "heading":0.0, "vx":-0.73774, "vy":2.24275, "omega":0.0, "ax":-3.96463, "ay":8.08679, "alpha":0.0, "fx":[-67.43714,-67.43714,-67.43714,-67.43714], "fy":[137.55394,137.55394,137.55394,137.55394]}, + {"t":0.29973, "x":3.50817, "y":1.18516, "heading":0.0, "vx":-0.88627, "vy":2.54573, "omega":0.0, "ax":-4.505, "ay":7.79477, "alpha":0.0, "fx":[-76.6288,-76.6288,-76.6288,-76.6288], "fy":[132.58677,132.58677,132.58677,132.58677]}, + {"t":0.33719, "x":3.4718, "y":1.28601, "heading":0.0, "vx":-1.05506, "vy":2.83777, "omega":0.0, "ax":-5.2188, "ay":7.33052, "alpha":0.0, "fx":[-88.77023,-88.77023,-88.77023,-88.77023], "fy":[124.6901,124.6901,124.6901,124.6901]}, + {"t":0.37466, "x":3.42861, "y":1.39748, "heading":0.0, "vx":-1.25059, "vy":3.11241, "omega":0.0, "ax":-6.16556, "ay":6.54596, "alpha":0.0, "fx":[-104.87435,-104.87435,-104.87435,-104.87435], "fy":[111.34488,111.34488,111.34488,111.34488]}, + {"t":0.41213, "x":3.37743, "y":1.51868, "heading":0.0, "vx":-1.48158, "vy":3.35766, "omega":0.0, "ax":-7.35998, "ay":5.15308, "alpha":0.0, "fx":[-125.19116,-125.19116,-125.19116,-125.19116], "fy":[87.65239,87.65239,87.65239,87.65239]}, + {"t":0.44959, "x":3.31676, "y":1.6481, "heading":0.0, "vx":-1.75733, "vy":3.55073, "omega":0.0, "ax":-8.55256, "ay":2.72575, "alpha":0.0, "fx":[-145.4766,-145.4766,-145.4766,-145.4766], "fy":[46.36422,46.36422,46.36422,46.36422]}, + {"t":0.48706, "x":3.24491, "y":1.78304, "heading":0.0, "vx":-2.07776, "vy":3.65285, "omega":0.0, "ax":-8.94127, "ay":-0.74959, "alpha":0.0, "fx":[-152.08847,-152.08847,-152.08847,-152.08847], "fy":[-12.75026,-12.75026,-12.75026,-12.75026]}, + {"t":0.52452, "x":3.16079, "y":1.91937, "heading":0.0, "vx":-2.41276, "vy":3.62476, "omega":0.0, "ax":-8.00128, "ay":-4.07008, "alpha":0.0, "fx":[-136.09955,-136.09955,-136.09955,-136.09955], "fy":[-69.23097,-69.23097,-69.23097,-69.23097]}, + {"t":0.56199, "x":3.06478, "y":2.05232, "heading":0.0, "vx":-2.71253, "vy":3.47228, "omega":0.0, "ax":-6.48977, "ay":-6.21431, "alpha":0.0, "fx":[-110.38918,-110.38918,-110.38918,-110.38918], "fy":[-105.70368,-105.70368,-105.70368,-105.70368]}, + {"t":0.59946, "x":2.9586, "y":2.17805, "heading":0.0, "vx":-2.95568, "vy":3.23945, "omega":0.0, "ax":-5.14654, "ay":-7.37465, "alpha":0.0, "fx":[-87.54117,-87.54117,-87.54117,-87.54117], "fy":[-125.44074,-125.44074,-125.44074,-125.44074]}, + {"t":0.63692, "x":2.84425, "y":2.29424, "heading":0.0, "vx":-3.1485, "vy":2.96315, "omega":0.0, "ax":-4.12694, "ay":-7.99668, "alpha":0.0, "fx":[-70.19803,-70.19803,-70.19803,-70.19803], "fy":[-136.02132,-136.02132,-136.02132,-136.02132]}, + {"t":0.67439, "x":2.72339, "y":2.39965, "heading":0.0, "vx":-3.30312, "vy":2.66355, "omega":0.0, "ax":-3.37292, "ay":-8.34753, "alpha":0.0, "fx":[-57.3724,-57.3724,-57.3724,-57.3724], "fy":[-141.98909,-141.98909,-141.98909,-141.98909]}, + {"t":0.71185, "x":2.59727, "y":2.49358, "heading":0.0, "vx":-3.42949, "vy":2.3508, "omega":0.0, "ax":-2.47926, "ay":-8.65406, "alpha":0.0, "fx":[-42.17154,-42.17154,-42.17154,-42.17154], "fy":[-147.20311,-147.20311,-147.20311,-147.20311]}, + {"t":0.74707, "x":2.47496, "y":2.571, "heading":0.0, "vx":-3.5168, "vy":2.04604, "omega":0.0, "ax":-1.03423, "ay":-8.93974, "alpha":0.0, "fx":[-17.59196,-17.59196,-17.59196,-17.59196], "fy":[-152.06235,-152.06235,-152.06235,-152.06235]}, + {"t":0.78229, "x":2.35047, "y":2.63751, "heading":0.0, "vx":-3.55322, "vy":1.73122, "omega":0.0, "ax":0.75877, "ay":-8.96486, "alpha":0.0, "fx":[12.9065,12.9065,12.9065,12.9065], "fy":[-152.48969,-152.48969,-152.48969,-152.48969]}, + {"t":0.8175, "x":2.22581, "y":2.69292, "heading":0.0, "vx":-3.5265, "vy":1.41551, "omega":0.0, "ax":2.76311, "ay":-8.5607, "alpha":0.0, "fx":[46.99977,46.99977,46.99977,46.99977], "fy":[-145.61505,-145.61505,-145.61505,-145.61505]}, + {"t":0.85272, "x":2.10333, "y":2.73746, "heading":0.0, "vx":-3.42919, "vy":1.11404, "omega":0.0, "ax":4.68681, "ay":-7.67853, "alpha":0.0, "fx":[79.72138,79.72138,79.72138,79.72138], "fy":[-130.6096,-130.6096,-130.6096,-130.6096]}, + {"t":0.88793, "x":1.98548, "y":2.77193, "heading":0.0, "vx":-3.26414, "vy":0.84363, "omega":0.0, "ax":6.2465, "ay":-6.47613, "alpha":0.0, "fx":[106.25121,106.25121,106.25121,106.25121], "fy":[-110.1571,-110.1571,-110.1571,-110.1571]}, + {"t":0.92315, "x":1.8744, "y":2.79762, "heading":0.0, "vx":-3.04416, "vy":0.61557, "omega":0.0, "ax":7.34361, "ay":-5.20369, "alpha":0.0, "fx":[124.91278,124.91278,124.91278,124.91278], "fy":[-88.51327,-88.51327,-88.51327,-88.51327]}, + {"t":0.95837, "x":1.77175, "y":2.81607, "heading":0.0, "vx":-2.78555, "vy":0.43231, "omega":0.0, "ax":8.04627, "ay":-4.03927, "alpha":0.0, "fx":[136.86476,136.86476,136.86476,136.86476], "fy":[-68.70679,-68.70679,-68.70679,-68.70679]}, + {"t":0.99358, "x":1.67864, "y":2.82879, "heading":0.0, "vx":-2.50219, "vy":0.29006, "omega":0.0, "ax":8.4735, "ay":-3.05046, "alpha":0.0, "fx":[144.13189,144.13189,144.13189,144.13189], "fy":[-51.88741,-51.88741,-51.88741,-51.88741]}, + {"t":1.0288, "x":1.59578, "y":2.83712, "heading":0.0, "vx":-2.20379, "vy":0.18264, "omega":0.0, "ax":8.7259, "ay":-2.23731, "alpha":0.0, "fx":[148.4251,148.4251,148.4251,148.4251], "fy":[-38.05598,-38.05598,-38.05598,-38.05598]}, + {"t":1.06401, "x":1.52358, "y":2.84216, "heading":0.0, "vx":-1.8965, "vy":0.10385, "omega":0.0, "ax":8.87146, "ay":-1.57454, "alpha":0.0, "fx":[150.90105,150.90105,150.90105,150.90105], "fy":[-26.7825,-26.7825,-26.7825,-26.7825]}, + {"t":1.09923, "x":1.4623, "y":2.84484, "heading":0.0, "vx":-1.58408, "vy":0.0484, "omega":0.0, "ax":8.95236, "ay":-1.03288, "alpha":0.0, "fx":[152.27715,152.27715,152.27715,152.27715], "fy":[-17.56891,-17.56891,-17.56891,-17.56891]}, + {"t":1.13445, "x":1.41206, "y":2.84591, "heading":0.0, "vx":-1.26881, "vy":0.01203, "omega":0.0, "ax":8.99403, "ay":-0.5866, "alpha":0.0, "fx":[152.98585,152.98585,152.98585,152.98585], "fy":[-9.9779,-9.9779,-9.9779,-9.9779]}, + {"t":1.16966, "x":1.37296, "y":2.84597, "heading":0.0, "vx":-0.95208, "vy":-0.00863, "omega":0.0, "ax":9.01175, "ay":-0.21516, "alpha":0.0, "fx":[153.28721,153.28721,153.28721,153.28721], "fy":[-3.65982,-3.65982,-3.65982,-3.65982]}, + {"t":1.20488, "x":1.34502, "y":2.84553, "heading":0.0, "vx":-0.63472, "vy":-0.01621, "omega":0.0, "ax":9.01479, "ay":0.09731, "alpha":0.0, "fx":[153.33905,153.33905,153.33905,153.33905], "fy":[1.65514,1.65514,1.65514,1.65514]}, + {"t":1.24009, "x":1.32826, "y":2.84502, "heading":0.0, "vx":-0.31726, "vy":-0.01278, "omega":0.0, "ax":9.00888, "ay":0.36291, "alpha":0.0, "fx":[153.23843,153.23843,153.23843,153.23843], "fy":[6.17291,6.17291,6.17291,6.17291]}, + {"t":1.27531, "x":1.32267, "y":2.84479, "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]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index 43e21ef..a453889 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -12,11 +12,11 @@ import java.util.stream.Collectors; public enum FieldLocation { - ZERO(0, 0, 0, "Zero"), - INVALID(-1, -1, -1, "INVALID"), - LEFT(7.150, 7.000, 180, "NON Processor Side"), - MIDDLE(7.150, 4.500, 180, "Middle"), - RIGHT(7.150, 2.000, 180, "Processor Side"); + ZERO(0, 0, 0, "zero"), + INVALID(-1, -1, -1, "invalid"), + LEFT(7.150, 7.000, 180, "left (from driver station pov)"), + MIDDLE(7.150, 4.500, 180, "middle"), + RIGHT(7.150, 2.000, 180, "right (from driver station pov)"); private static final double RED_X_POS = 2.3876; // meters public static final double HEIGHT_OF_FIELD = 8.05; From b642962de8117f7778db0e745a600d654ed9d8b5 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Fri, 20 Feb 2026 15:19:53 -0500 Subject: [PATCH 12/22] fixed sequences for autos --- src/main/deploy/choreo/LeftToTower.traj | 169 +++++++++--------- src/main/deploy/choreo/NewPath.traj | 29 +++ src/main/deploy/choreo/path.chor | 40 ++--- .../frc/robot/commands/auto/LeftShoot.java | 19 +- .../robot/commands/auto/LeftShootClimb.java | 19 +- .../frc/robot/commands/auto/MidShoot.java | 19 +- .../robot/commands/auto/MidShootClimb.java | 20 ++- .../frc/robot/commands/auto/RightShoot.java | 21 ++- ...hootandClimb.java => RightShootClimb.java} | 23 ++- 9 files changed, 223 insertions(+), 136 deletions(-) create mode 100644 src/main/deploy/choreo/NewPath.traj rename src/main/java/frc/robot/commands/auto/{RightShootandClimb.java => RightShootClimb.java} (56%) diff --git a/src/main/deploy/choreo/LeftToTower.traj b/src/main/deploy/choreo/LeftToTower.traj index 894d991..907510a 100644 --- a/src/main/deploy/choreo/LeftToTower.traj +++ b/src/main/deploy/choreo/LeftToTower.traj @@ -3,9 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.733068466186523, "y":7.429088115692139, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.9623348712921143, "y":5.500389575958252, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.955095887184143, "y":4.855756759643555, "heading":-3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.733068466186523, "y":7.429088115692139, "heading":0.0, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.268970489501953, "y":5.231799125671387, "heading":-3.141592653589793, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":0.8350459933280945, "y":4.654308795928955, "heading":-3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,9 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.7330684661865234 m", "val":3.733068466186523}, "y":{"exp":"7.429088115692139 m", "val":7.429088115692139}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.9623348712921143 m", "val":2.9623348712921143}, "y":{"exp":"5.500389575958252 m", "val":5.500389575958252}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.955095887184143 m", "val":1.955095887184143}, "y":{"exp":"4.855756759643555 m", "val":4.855756759643555}, "heading":{"exp":"-180 deg", "val":-3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.7330684661865234 m", "val":3.733068466186523}, "y":{"exp":"7.429088115692139 m", "val":7.429088115692139}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.268970489501953 m", "val":2.268970489501953}, "y":{"exp":"5.231799125671387 m", "val":5.231799125671387}, "heading":{"exp":"-180 deg", "val":-3.141592653589793}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"0.8350459933280945 m", "val":0.8350459933280945}, "y":{"exp":"4.654308795928955 m", "val":4.654308795928955}, "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}, @@ -31,96 +29,101 @@ "trajectory":{ "config":{ "frontLeft":{ - "x":0.2794, - "y":0.2794 + "x":0.2286, + "y":0.2286 }, "backLeft":{ - "x":-0.2794, - "y":0.2794 + "x":-0.2286, + "y":0.2286 }, - "mass":68.0388555, - "inertia":6.0, + "mass":52.16312255, + "inertia":3.0, "gearing":6.5, - "radius":0.0508, + "radius":0.1016, "vmax":628.3185307179587, "tmax":1.2, "cof":1.5, "bumper":{ - "front":0.4064, - "side":0.4064, - "back":0.4064 + "front":0.6858, + "side":0.6858, + "back":0.6858 }, "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.73843,1.23731,1.75874], + "waypoints":[0.0,1.03333,1.77656], "samples":[ - {"t":0.0, "x":3.73307, "y":7.42909, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.60046, "ay":-8.51755, "alpha":-4.50152, "fx":[-18.88916,-25.99334,-74.74813,-57.30151], "fy":[-152.21975,-151.10919,-133.90243,-142.29305]}, - {"t":0.03692, "x":3.7313, "y":7.42328, "heading":0.0, "vx":-0.09601, "vy":-0.31448, "omega":-0.1662, "ax":-2.63073, "ay":-8.51369, "alpha":-4.37655, "fx":[-19.95841,-27.21722,-74.39745,-57.41863], "fy":[-152.07031,-150.87773,-134.08115,-142.2328]}, - {"t":0.07384, "x":3.72596, "y":7.40587, "heading":-0.00614, "vx":-0.19314, "vy":-0.62882, "omega":-0.32779, "ax":-2.66603, "ay":-8.50872, "alpha":-4.23408, "fx":[-21.31911,-28.43579,-73.94111,-57.69769], "fy":[-151.87043,-150.63474,-134.31392,-142.10436]}, - {"t":0.11076, "x":3.71701, "y":7.37685, "heading":-0.01824, "vx":-0.29158, "vy":-0.94297, "omega":-0.48412, "ax":-2.70772, "ay":-8.50229, "alpha":-4.06763, "fx":[-23.01025,-29.72748,-73.36619,-58.12651], "fy":[-151.60552,-150.3642,-134.60568,-141.91087]}, - {"t":0.14769, "x":3.7044, "y":7.33624, "heading":-0.03611, "vx":-0.39155, "vy":-1.25689, "omega":-0.6343, "ax":-2.7577, "ay":-8.49389, "alpha":-3.86837, "fx":[-25.08997,-31.1982,-72.6538,-58.68912], "fy":[-151.25357,-150.04126,-134.96372,-141.65629]}, - {"t":0.18461, "x":3.68806, "y":7.28405, "heading":-0.05953, "vx":-0.49337, "vy":-1.5705, "omega":-0.77713, "ax":-2.81871, "ay":-8.48267, "alpha":-3.62388, "fx":[-27.64587,-32.99222,-71.7791,-59.3644], "fy":[-150.78048,-149.6274,-135.39707,-141.34603]}, - {"t":0.22153, "x":3.66793, "y":7.22028, "heading":-0.08823, "vx":-0.59744, "vy":-1.88369, "omega":-0.91093, "ax":-2.89481, "ay":-8.46716, "alpha":-3.31606, "fx":[-30.814,-35.31063,-70.71114,-60.12372], "fy":[-150.13075,-149.06097,-135.91599,-140.98795]}, - {"t":0.25845, "x":3.64389, "y":7.14496, "heading":-0.12186, "vx":-0.70432, "vy":-2.19631, "omega":-1.03336, "ax":-2.99238, "ay":-8.44475, "alpha":-2.91735, "fx":[-34.81617,-38.44349,-69.41215,-60.92659], "fy":[-149.20791,-148.23809,-136.53116,-140.59395]}, - {"t":0.29537, "x":3.61585, "y":7.05811, "heading":-0.16001, "vx":-0.8148, "vy":-2.5081, "omega":-1.14108, "ax":-3.12197, "ay":-8.41043, "alpha":-2.38277, "fx":[-40.03901,-42.82868,-67.83572,-61.71185], "fy":[-147.82864,-146.97135,-137.25299,-140.18302]}, - {"t":0.33229, "x":3.58364, "y":6.95978, "heading":-0.20214, "vx":-0.93007, "vy":-2.81863, "omega":-1.22905, "ax":-3.30225, "ay":-8.35365, "alpha":-1.6322, "fx":[-47.21819,-49.1641,-65.92159,-62.37729], "fy":[-145.60007,-144.89373,-138.09133,-139.78758]}, - {"t":0.36921, "x":3.54705, "y":6.85001, "heading":-0.24752, "vx":-1.052, "vy":-3.12706, "omega":-1.28931, "ax":-3.56935, "ay":-8.2492, "alpha":-0.50347, "fx":[-57.92253,-58.62742,-63.58096,-62.7236], "fy":[-141.52711,-141.21269,-139.05683,-139.46944]}, - {"t":0.40614, "x":3.50577, "y":6.72894, "heading":-0.29512, "vx":-1.18378, "vy":-3.43163, "omega":-1.3079, "ax":-4.0006, "ay":-8.024, "alpha":1.40219, "fx":[-76.00561,-73.28324,-60.6529,-62.25462], "fy":[-132.39397,-134.01081,-140.16742,-139.37184]}, - {"t":0.44306, "x":3.45934, "y":6.59677, "heading":-0.34341, "vx":-1.33149, "vy":-3.72789, "omega":-1.25613, "ax":-4.75993, "ay":-7.39164, "alpha":5.42202, "fx":[-111.5647,-96.47327,-56.76302,-59.05938], "fy":[-103.38096,-118.10001,-141.46867,-139.96941]}, - {"t":0.47998, "x":3.40693, "y":6.45409, "heading":-0.38979, "vx":-1.50723, "vy":-4.0008, "omega":-1.05594, "ax":-5.25386, "ay":-5.17746, "alpha":16.73659, "fx":[-150.63137,-129.66516,-50.97813,-26.19228], "fy":[14.10994,-79.60446,-143.00909,-143.76513]}, - {"t":0.5169, "x":3.3477, "y":6.30284, "heading":-0.42878, "vx":-1.70121, "vy":-4.19196, "omega":-0.438, "ax":-2.77364, "ay":1.55626, "alpha":35.32807, "fx":[-83.97038,-151.93183,-34.88551,82.07227], "fy":[126.74707,-1.54945,-145.74971,126.43848]}, - {"t":0.55382, "x":3.283, "y":6.14913, "heading":-0.44495, "vx":-1.80362, "vy":-4.1345, "omega":0.86636, "ax":0.61295, "ay":5.33644, "alpha":26.73946, "fx":[-41.30034,-133.74863,140.56096,76.19209], "fy":[146.95534,72.61329,11.57727,131.93966]}, - {"t":0.59074, "x":3.21683, "y":6.00012, "heading":-0.41296, "vx":-1.78099, "vy":-3.93747, "omega":1.85362, "ax":0.65085, "ay":7.25195, "alpha":18.10652, "fx":[-26.7659,-111.29041,107.93537,74.40416], "fy":[150.56751,104.26326,105.11452,133.46921]}, - {"t":0.62767, "x":3.15151, "y":5.85968, "heading":-0.34452, "vx":-1.75696, "vy":-3.66972, "omega":2.52214, "ax":0.83665, "ay":7.61808, "alpha":16.22061, "fx":[-22.01947,-95.10245,103.02243,71.02402], "fy":[151.48254,119.46308,111.84433,135.53577]}, - {"t":0.66459, "x":3.08722, "y":5.72938, "heading":-0.2514, "vx":-1.72607, "vy":-3.38845, "omega":3.12103, "ax":0.94451, "ay":7.77342, "alpha":15.35854, "fx":[-22.0332,-84.20355,103.53747,66.96259], "fy":[151.56972,127.52025,112.08182,137.72308]}, - {"t":0.70151, "x":3.02413, "y":5.60958, "heading":-0.13617, "vx":-1.6912, "vy":-3.10144, "omega":3.68809, "ax":0.94795, "ay":7.81111, "alpha":15.21545, "fx":[-25.9019,-78.69013,106.72881,62.36085], "fy":[151.01601,131.06663,109.4219,139.95419]}, - {"t":0.73843, "x":2.96233, "y":5.50039, "heading":0.0, "vx":-1.6562, "vy":-2.81304, "omega":4.24987, "ax":0.68215, "ay":7.85328, "alpha":14.82477, "fx":[-34.44657,-76.55744,104.11881,53.2979], "fy":[148.88237,131.22059,110.97581,143.24908]}, - {"t":0.75506, "x":2.93489, "y":5.4547, "heading":0.07067, "vx":-1.64485, "vy":-2.68245, "omega":4.4964, "ax":0.52831, "ay":7.76398, "alpha":15.45946, "fx":[-40.37949,-80.65665,107.07686,49.9049], "fy":[147.32371,128.43189,108.06996,144.42704]}, - {"t":0.77169, "x":2.90761, "y":5.41116, "heading":0.14545, "vx":-1.63607, "vy":-2.55334, "omega":4.75348, "ax":0.33382, "ay":7.63601, "alpha":16.2779, "fx":[-47.30132,-86.53902,110.35288,46.20046], "fy":[145.17811,124.09699,104.65955,145.61049]}, - {"t":0.78832, "x":2.88045, "y":5.36976, "heading":0.22449, "vx":-1.63052, "vy":-2.42635, "omega":5.02417, "ax":0.08741, "ay":7.45314, "alpha":17.32266, "fx":[-55.29751,-94.70586,113.8383,42.11236], "fy":[142.23843,117.28216,100.78714,146.79507]}, - {"t":0.80495, "x":2.85334, "y":5.33044, "heading":0.30804, "vx":-1.62906, "vy":-2.30241, "omega":5.31224, "ax":-0.21747, "ay":7.19786, "alpha":18.60794, "fx":[-64.24279,-105.30041,117.18462,37.56194], "fy":[138.32075,106.64669,96.80035,147.96658]}, - {"t":0.82158, "x":2.82622, "y":5.29315, "heading":0.39638, "vx":-1.63268, "vy":-2.18272, "omega":5.62168, "ax":-0.57608, "ay":6.84535, "alpha":20.15839, "fx":[-73.76659,-117.87433,119.92377,32.52162], "fy":[133.34417,90.01674,93.30377,149.08513]}, - {"t":0.83821, "x":2.79899, "y":5.2578, "heading":0.48987, "vx":-1.64226, "vy":-2.06888, "omega":5.9569, "ax":-0.94916, "ay":6.36066, "alpha":22.02504, "fx":[-83.11618,-130.06603,121.51103,27.09149], "fy":[127.53078,64.02965,91.13357,150.07803]}, - {"t":0.85484, "x":2.77155, "y":5.22427, "heading":0.58893, "vx":-1.65804, "vy":-1.96311, "omega":6.32316, "ax":-1.16727, "ay":5.68996, "alpha":24.31152, "fx":[-90.91954,-131.48167,121.33971,21.64184], "fy":[121.80109,23.25323,91.24228,150.84184]}, - {"t":0.87147, "x":2.74382, "y":5.19241, "heading":0.69408, "vx":-1.67745, "vy":-1.86849, "omega":6.72745, "ax":2.14516, "ay":5.75498, "alpha":21.73147, "fx":[-93.37551,104.08075,117.98992,17.25889], "fy":[119.46879,25.48879,95.36839,151.23639]}, - {"t":0.88809, "x":2.71622, "y":5.16214, "heading":0.80595, "vx":-1.64178, "vy":-1.77278, "omega":7.08883, "ax":2.04219, "ay":7.96585, "alpha":10.76318, "fx":[-48.2641,72.35797,93.71661,21.13777], "fy":[143.10013,129.46227,119.03026,150.39494]}, - {"t":0.90472, "x":2.6892, "y":5.13376, "heading":0.92384, "vx":-1.60782, "vy":-1.64032, "omega":7.26782, "ax":2.5077, "ay":8.43476, "alpha":3.76667, "fx":[16.30237,55.89779,64.92125,33.49972], "fy":[149.95799,139.74538,136.56749,147.62027]}, - {"t":0.92135, "x":2.66281, "y":5.10765, "heading":1.0447, "vx":-1.56612, "vy":-1.50005, "omega":7.33046, "ax":1.56143, "ay":8.60679, "alpha":4.4319, "fx":[-5.13459,43.2758,52.69883,15.39804], "fy":[150.31296,143.55866,141.40681,150.31778]}, - {"t":0.93798, "x":2.63698, "y":5.08389, "heading":1.1666, "vx":-1.54015, "vy":-1.35692, "omega":7.40416, "ax":0.2, "ay":8.61607, "alpha":6.1122, "fx":[-41.45624,26.70679,38.97321,-10.61612], "fy":[143.94939,146.43141,145.39225,150.45453]}, - {"t":0.95461, "x":2.6114, "y":5.06252, "heading":1.28972, "vx":-1.53683, "vy":-1.21364, "omega":7.5058, "ax":-1.41237, "ay":8.36927, "alpha":7.33244, "fx":[-77.00979,0.84997,18.72184,-38.65824], "fy":[127.7722,147.40778,148.83522,145.42005]}, - {"t":0.97124, "x":2.58565, "y":5.04349, "heading":1.41454, "vx":-1.56031, "vy":-1.07447, "omega":7.62773, "ax":-3.33547, "ay":7.70823, "alpha":7.78637, "fx":[-108.20214,-39.6874,-10.44588,-68.60594], "fy":[101.90404,140.20961,148.93308,133.41219]}, - {"t":0.98787, "x":2.55924, "y":5.02669, "heading":1.54139, "vx":-1.61578, "vy":-0.94628, "omega":7.75722, "ax":-5.47746, "ay":6.36763, "alpha":7.23528, "fx":[-130.60413,-91.44401,-51.67217,-98.95989], "fy":[70.08441,112.07621,139.08384,112.00184]}, - {"t":1.0045, "x":2.53161, "y":5.01183, "heading":1.67038, "vx":-1.70687, "vy":-0.84039, "omega":7.87754, "ax":-7.40701, "ay":4.13612, "alpha":5.81685, "fx":[-143.7258,-131.42488,-101.94206,-126.87144], "fy":[35.36884,61.61694,106.56796,77.86316]}, - {"t":1.02113, "x":2.5022, "y":4.99843, "heading":1.80138, "vx":-1.83004, "vy":-0.77161, "omega":7.97427, "ax":-8.46514, "ay":1.83894, "alpha":0.65527, "fx":[-144.97489,-144.18416,-142.93263,-143.86642], "fy":[26.93383,29.6377,35.70627,32.84179]}, - {"t":1.03776, "x":2.4706, "y":4.98585, "heading":1.93399, "vx":-1.97081, "vy":-0.74103, "omega":7.98516, "ax":-4.91235, "ay":0.0809, "alpha":-27.02992, "fx":[-58.83682,-140.96051,-133.16472,-1.26892], "fy":[135.84879,51.07289,-66.42826,-114.98902]}, - {"t":1.05439, "x":2.43715, "y":4.97354, "heading":2.06678, "vx":-2.0525, "vy":-0.73969, "omega":7.53567, "ax":-4.87251, "ay":0.26973, "alpha":-26.43448, "fx":[-72.54349,-145.39377,-129.18139,15.59863], "fy":[130.72149,39.78177,-75.60147,-76.54975]}, - {"t":1.07102, "x":2.40234, "y":4.96128, "heading":2.19209, "vx":-2.13353, "vy":-0.7352, "omega":7.09608, "ax":-5.33823, "ay":2.0248, "alpha":-22.82856, "fx":[-85.88295,-148.75242,-124.06923,-4.50215], "fy":[123.41443,27.33592,-84.73458,71.74898]}, - {"t":1.08765, "x":2.36612, "y":4.94933, "heading":2.3101, "vx":-2.2223, "vy":-0.70153, "omega":6.71645, "ax":-6.05089, "ay":2.2268, "alpha":-20.43014, "fx":[-98.5015,-150.85389,-118.93923,-43.40099], "fy":[114.33452,14.86356,-92.43538,114.74605]}, - {"t":1.10428, "x":2.32833, "y":4.93797, "heading":2.42179, "vx":-2.32292, "vy":-0.6645, "omega":6.37671, "ax":-6.66197, "ay":1.79409, "alpha":-18.65798, "fx":[-110.43162,-151.79927,-114.84885,-76.19274], "fy":[103.42679,2.86707,-97.90641,113.68051]}, - {"t":1.12091, "x":2.28878, "y":4.92717, "heading":2.52783, "vx":-2.43371, "vy":-0.63466, "omega":6.06644, "ax":-7.12027, "ay":1.23464, "alpha":-17.23855, "fx":[-120.96158,-151.77894,-111.96125,-99.7534], "fy":[91.36441,-8.31473,-101.51465,102.46817]}, - {"t":1.13754, "x":2.24733, "y":4.91679, "heading":2.62871, "vx":-2.55212, "vy":-0.61413, "omega":5.77977, "ax":-7.44475, "ay":0.67601, "alpha":-16.0848, "fx":[-129.7064,-151.01354,-110.11396,-115.6985], "fy":[78.88474,-18.53081,-103.7638,89.4047]}, - {"t":1.15417, "x":2.20386, "y":4.90667, "heading":2.72483, "vx":-2.67592, "vy":-0.60289, "omega":5.51229, "ax":-7.66355, "ay":0.16421, "alpha":-15.15624, "fx":[-136.58058,-149.70498,-108.93637,-126.19732], "fy":[66.68136,-27.76869,-105.20203,77.4618]}, - {"t":1.1708, "x":2.1583, "y":4.89667, "heading":2.81649, "vx":-2.80336, "vy":-0.60016, "omega":5.26025, "ax":-7.80141, "ay":-0.28728, "alpha":-14.4385, "fx":[-141.73871,-148.01082,-108.01259,-133.03679], "fy":[55.2755,-36.10899,-106.31982,67.60745]}, - {"t":1.18742, "x":2.1106, "y":4.88665, "heading":2.90397, "vx":-2.93309, "vy":-0.60494, "omega":5.02015, "ax":-7.85552, "ay":-0.66054, "alpha":-14.09714, "fx":[-145.32312,-146.01236,-106.37316,-136.77167], "fy":[45.43413,-43.77186,-108.09827,61.4935]}, - {"t":1.20405, "x":2.06074, "y":4.8765, "heading":2.98745, "vx":-3.06372, "vy":-0.61592, "omega":4.78572, "ax":-7.27467, "ay":-0.56049, "alpha":-18.16986, "fx":[-145.14493,-142.93713,-88.39061,-118.48752], "fy":[46.43518,-53.22743,-123.27667,91.93367]}, - {"t":1.22068, "x":2.00878, "y":4.86618, "heading":3.06703, "vx":-3.1847, "vy":-0.62524, "omega":4.48357, "ax":-5.27154, "ay":-0.15292, "alpha":-28.47573, "fx":[-141.64558,-137.7124,-39.96159,-39.34976], "fy":[56.61502,-65.81438,-146.39117,145.18635]}, - {"t":1.23731, "x":1.9551, "y":4.85576, "heading":-3.14159, "vx":-3.27236, "vy":-0.62779, "omega":4.01003, "ax":-4.73582, "ay":-0.29363, "alpha":-30.78277, "fx":[-142.73282,-134.25287,-18.82154,-26.41277], "fy":[55.00641,-73.38294,-151.25405,149.65246]}, - {"t":1.27208, "x":1.83848, "y":4.83376, "heading":-3.0022, "vx":-3.43699, "vy":-0.63799, "omega":2.93996, "ax":-2.49321, "ay":-0.33467, "alpha":-37.25455, "fx":[-140.70416,-119.51507,65.72353,24.86051], "fy":[59.39777,-95.0841,-137.06807,149.98371]}, - {"t":1.30684, "x":1.7175, "y":4.81138, "heading":-2.9, "vx":-3.52366, "vy":-0.64963, "omega":1.64492, "ax":0.09693, "ay":0.12981, "alpha":-40.06, "fx":[-127.27243,-78.21041,132.9055,79.17221], "fy":[83.32272,-130.42008,-74.02746,129.95676]}, - {"t":1.3416, "x":1.59507, "y":4.78887, "heading":-2.84282, "vx":-3.52029, "vy":-0.64512, "omega":0.25235, "ax":4.98764, "ay":1.31634, "alpha":-28.9978, "fx":[-4.5274,74.38444,150.47311,119.02284], "fy":[150.56883,-131.63415,-24.60045,95.22799]}, - {"t":1.37636, "x":1.47571, "y":4.76724, "heading":-2.83404, "vx":-3.34691, "vy":-0.59936, "omega":-0.75567, "ax":8.42937, "ay":1.6374, "alpha":-8.45028, "fx":[132.30814,149.57072,152.70639,138.93931], "fy":[74.94249,-28.20404,1.36638,63.30218]}, - {"t":1.41112, "x":1.36446, "y":4.7474, "heading":-2.86031, "vx":-3.05389, "vy":-0.54244, "omega":-1.04942, "ax":8.79811, "ay":1.62553, "alpha":-2.57688, "fx":[147.15614,152.29242,151.94952,147.21528], "fy":[40.88317,11.76938,16.77848,41.16803]}, - {"t":1.44589, "x":1.26361, "y":4.72952, "heading":-2.89679, "vx":-2.74805, "vy":-0.48593, "omega":-1.13899, "ax":8.84964, "ay":1.60573, "alpha":0.07295, "fx":[150.59618,150.45718,150.46327,150.60272], "fy":[26.95615,27.72048,27.67182,26.90358]}, - {"t":1.48065, "x":1.17343, "y":4.7136, "heading":-2.93639, "vx":-2.44041, "vy":-0.43011, "omega":-1.13646, "ax":8.84411, "ay":1.58725, "alpha":1.66278, "fx":[151.93762,148.95712,148.78162,152.06703], "fy":[19.02215,35.47241,35.98274,17.51752]}, - {"t":1.51541, "x":1.09394, "y":4.69961, "heading":-2.97589, "vx":-2.13298, "vy":-0.37494, "omega":-1.07866, "ax":8.82463, "ay":1.57204, "alpha":2.74863, "fx":[152.60001,147.98851,147.10087,152.72816], "fy":[13.60172,39.64018,42.56626,11.15158]}, - {"t":1.55017, "x":1.02513, "y":4.68752, "heading":-3.01339, "vx":-1.82621, "vy":-0.32029, "omega":-0.98311, "ax":8.80245, "ay":1.55935, "alpha":3.54417, "fx":[152.96407,147.39904,145.51158,153.034], "fy":[9.50993,41.98509,47.88057,6.72055]}, - {"t":1.58493, "x":0.96696, "y":4.67733, "heading":-3.04756, "vx":-1.52022, "vy":-0.26608, "omega":-0.85991, "ax":8.78113, "ay":1.54838, "alpha":4.15241, "fx":[153.17095,147.05266,144.05802,153.17657], "fy":[6.24993,43.32663,52.21095,3.56232]}, - {"t":1.6197, "x":0.91942, "y":4.66902, "heading":-3.07746, "vx":-1.21497, "vy":-0.21226, "omega":-0.71556, "ax":8.76177, "ay":1.53866, "alpha":4.63118, "fx":[153.28592,146.85353,142.75884,153.24231], "fy":[3.58268,44.10327,55.75668,1.24605]}, - {"t":1.65446, "x":0.88248, "y":4.66257, "heading":-3.10233, "vx":-0.9104, "vy":-0.15877, "omega":-0.55457, "ax":8.74454, "ay":1.52996, "alpha":5.01691, "fx":[153.34454,146.73597,141.61707,153.27078], "fy":[1.38125,44.57467,58.66758,-0.5267]}, - {"t":1.68922, "x":0.85612, "y":4.65798, "heading":-3.12161, "vx":-0.60642, "vy":-0.10559, "omega":-0.38017, "ax":8.72927, "ay":1.52218, "alpha":5.33451, "fx":[153.3687,146.65454,140.6267,153.27955], "fy":[-0.42667,44.90668,61.06041,-1.97336]}, - {"t":1.72398, "x":0.84031, "y":4.65522, "heading":-3.13482, "vx":-0.30297, "vy":-0.05267, "omega":-0.19474, "ax":8.71565, "ay":1.51525, "alpha":5.602, "fx":[153.37305,146.57708,139.77727,153.2754], "fy":[-1.88413,45.21128,63.02675,-3.2577]}, - {"t":1.75874, "x":0.83505, "y":4.65431, "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]}], + {"t":0.0, "x":3.73307, "y":7.42909, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.76606, "ay":-5.14588, "alpha":9.15613, "fx":[-59.70005,-36.68432,1.30046,2.96059], "fy":[-48.00468,-67.32441,-76.64508,-76.45106]}, + {"t":0.02199, "x":3.73264, "y":7.42784, "heading":0.0, "vx":-0.03883, "vy":-0.11314, "omega":0.2013, "ax":-1.78871, "ay":-5.13947, "alpha":9.13253, "fx":[-59.88458,-36.87673,1.0539,2.40252], "fy":[-47.76724,-67.21588,-76.64534,-76.46222]}, + {"t":0.04397, "x":3.73136, "y":7.42411, "heading":0.00443, "vx":-0.07815, "vy":-0.22613, "omega":0.40209, "ax":-1.81329, "ay":-5.13447, "alpha":9.08028, "fx":[-60.04832,-36.97684,0.87515,1.56326], "fy":[-47.55444,-67.15734,-76.64368,-76.47434]}, + {"t":0.06596, "x":3.7292, "y":7.4179, "heading":0.01327, "vx":-0.11802, "vy":-0.33902, "omega":0.60173, "ax":-1.84019, "ay":-5.13077, "alpha":8.99856, "fx":[-60.18911,-36.98615,0.75976,0.42522], "fy":[-47.36961,-67.14851,-76.64055,-76.47815]}, + {"t":0.08794, "x":3.72616, "y":7.40921, "heading":0.0265, "vx":-0.15848, "vy":-0.45182, "omega":0.79957, "ax":-1.86998, "ay":-5.12818, "alpha":8.88678, "fx":[-60.30388,-36.9066,0.70153,-1.03487], "fy":[-47.21718,-67.18835,-76.63627,-76.45996]}, + {"t":0.10993, "x":3.72222, "y":7.39804, "heading":0.04407, "vx":-0.19959, "vy":-0.56457, "omega":0.99495, "ax":-1.90329, "ay":-5.12639, "alpha":8.74498, "fx":[-60.38892,-36.74067,0.69245,-2.84459], "fy":[-47.10236,-67.27508,-76.63096,-76.40021]}, + {"t":0.13191, "x":3.71738, "y":7.38438, "heading":0.06595, "vx":-0.24144, "vy":-0.67727, "omega":1.18721, "ax":-1.94089, "ay":-5.12495, "alpha":8.57444, "fx":[-60.44027,-36.49151,0.72251,-5.03353], "fy":[-47.03064,-67.40612,-76.62466,-76.272]}, + {"t":0.1539, "x":3.7116, "y":7.36826, "heading":0.09205, "vx":-0.28411, "vy":-0.78995, "omega":1.37573, "ax":-1.98353, "ay":-5.1232, "alpha":8.37837, "fx":[-60.45402,-36.16313,0.7795,-7.62966], "fy":[-47.00728,-67.57811,-76.61737,-76.03949]}, + {"t":0.17589, "x":3.70487, "y":7.34965, "heading":0.1223, "vx":-0.32772, "vy":-0.90259, "omega":1.55993, "ax":-2.03194, "ay":-5.12028, "alpha":8.16267, "fx":[-60.42668,-35.76052,0.84892,-10.65401], "fy":[-47.0368,-67.7869,-76.6091,-75.65704]}, + {"t":0.19787, "x":3.69718, "y":7.32857, "heading":0.15659, "vx":-0.37239, "vy":-1.01516, "omega":1.73939, "ax":-2.08663, "ay":-5.1151, "alpha":7.93659, "fx":[-60.35544,-35.28976,0.9137,-14.11369], "fy":[-47.12249,-68.02763,-76.59987,-75.06958]}, + {"t":0.21986, "x":3.68848, "y":7.30501, "heading":0.19484, "vx":-0.41827, "vy":-1.12762, "omega":1.91389, "ax":-2.14781, "ay":-5.10641, "alpha":7.71297, "fx":[-60.23847,-34.75818,0.95398,-17.99377], "fy":[-47.26609,-68.29473,-76.58983,-74.21545]}, + {"t":0.24184, "x":3.67877, "y":7.27899, "heading":0.23691, "vx":-0.46549, "vy":-1.23989, "omega":2.08346, "ax":-2.21521, "ay":-5.0929, "alpha":7.50778, "fx":[-60.075,-34.17441,0.9467,-22.2493], "fy":[-47.46758,-68.58212,-76.57911,-73.03263]}, + {"t":0.26383, "x":3.668, "y":7.2505, "heading":0.28272, "vx":-0.51419, "vy":-1.35186, "omega":2.24853, "ax":-2.28799, "ay":-5.07341, "alpha":7.33856, "fx":[-59.86536,-33.54845,0.86508,-26.80018], "fy":[-47.72518,-68.88324,-76.5678,-71.46858]}, + {"t":0.28581, "x":3.65614, "y":7.21955, "heading":0.33216, "vx":-0.5645, "vy":-1.4634, "omega":2.40987, "ax":-2.36482, "ay":-5.04715, "alpha":7.2217, "fx":[-59.61095,-32.89173,0.67769,-31.53155], "fy":[-48.03549,-69.19122,-76.5557,-69.49253]}, + {"t":0.3078, "x":3.64316, "y":7.18616, "heading":0.38514, "vx":-0.61649, "vy":-1.57437, "omega":2.56864, "ax":-2.44399, "ay":-5.01392, "alpha":7.16889, "fx":[-59.31406,-32.2173,0.34712,-36.30212], "fy":[-48.39381,-69.4989,-76.5419,-67.10726]}, + {"t":0.32979, "x":3.62902, "y":7.15033, "heading":0.44161, "vx":-0.67022, "vy":-1.6846, "omega":2.72626, "ax":-2.52381, "ay":-4.97427, "alpha":7.18354, "fx":[-58.97777,-31.54019,-0.172,-40.95991], "fy":[-48.79441,-69.79882,-76.52409,-64.35605]}, + {"t":0.35177, "x":3.61367, "y":7.11209, "heading":0.50155, "vx":-0.72571, "vy":-1.79396, "omega":2.88419, "ax":-2.60302, "ay":-4.92939, "alpha":7.25818, "fx":[-58.60589,-30.87808,-0.93578,-45.36195], "fy":[-49.23069,-70.08301,-76.49729,-61.32156]}, + {"t":0.37376, "x":3.59709, "y":7.07146, "heading":0.56496, "vx":-0.78294, "vy":-1.90234, "omega":3.04377, "ax":-2.68125, "ay":-4.88095, "alpha":7.37368, "fx":[-58.20312,-30.25242,-2.01464,-49.39243], "fy":[-49.69516,-70.34259,-76.45177,-58.11605]}, + {"t":0.39574, "x":3.57922, "y":7.02845, "heading":0.63188, "vx":-0.84189, "vy":-2.00965, "omega":3.20588, "ax":-2.75937, "ay":-4.83064, "alpha":7.50041, "fx":[-57.77556,-29.69025,-3.49757,-52.9741], "fy":[-50.17897,-70.56711,-76.36945,-54.86562]}, + {"t":0.41773, "x":3.56005, "y":6.9831, "heading":0.70236, "vx":-0.90255, "vy":-2.11586, "omega":3.37079, "ax":-2.83966, "ay":-4.77973, "alpha":7.60092, "fx":[-57.3316,-29.22673,-5.49639,-56.07101], "fy":[-50.67107,-70.74349,-76.21787,-51.69329]}, + {"t":0.43971, "x":3.53952, "y":6.93543, "heading":0.77647, "vx":-0.96499, "vy":-2.22094, "omega":3.5379, "ax":-2.9259, "ay":-4.72856, "alpha":7.63363, "fx":[-56.88339,-28.90866,-8.14837,-58.6838], "fy":[-51.15683,-70.85438,-75.94046,-48.70485]}, + {"t":0.4617, "x":3.5176, "y":6.88546, "heading":0.85426, "vx":-1.02931, "vy":-2.3249, "omega":3.70573, "ax":-3.02325, "ay":-4.67597, "alpha":7.55761, "fx":[-56.44893,-28.79926,-11.61326,-60.84086], "fy":[-51.61601,-70.87569,-75.44179,-45.97967]}, + {"t":0.48369, "x":3.49423, "y":6.83321, "heading":0.93573, "vx":-1.09578, "vy":-2.42771, "omega":3.87189, "ax":-3.13796, "ay":-4.61872, "alpha":7.33885, "fx":[-56.05501,-28.98441,-16.05738,-62.58885], "fy":[-52.02005,-70.77244,-74.56821,-43.56622]}, + {"t":0.50567, "x":3.46938, "y":6.77872, "heading":1.02086, "vx":-1.16477, "vy":-2.52925, "omega":4.03324, "ax":-3.27663, "ay":-4.55092, "alpha":6.9593, "fx":[-55.74103,-29.58064,-21.61266,-63.98469], "fy":[-52.32814,-70.49176,-73.08954,-41.481]}, + {"t":0.52766, "x":3.44298, "y":6.72201, "heading":1.10953, "vx":-1.23681, "vy":-2.62931, "omega":4.18624, "ax":-3.44494, "ay":-4.46373, "alpha":6.42784, "fx":[-55.56418,-30.74497,-28.2991,-65.09034], "fy":[-52.48154,-69.95064,-70.70068,-39.70933]}, + {"t":0.54964, "x":3.41496, "y":6.66313, "heading":1.20157, "vx":-1.31255, "vy":-2.72745, "omega":4.32756, "ax":-3.64587, "ay":-4.34586, "alpha":5.78746, "fx":[-55.6061,-32.6854,-35.91798,-65.97024], "fy":[-52.39458,-69.01416,-67.0783,-38.20645]}, + {"t":0.57163, "x":3.38522, "y":6.60211, "heading":1.29671, "vx":-1.39271, "vy":-2.82299, "omega":4.45481, "ax":-3.87853, "ay":-4.18526, "alpha":5.10452, "fx":[-55.98176,-35.66702,-43.97627,-66.69114], "fy":[-51.9392,-67.45726,-62.02198,-36.89792]}, + {"t":0.59361, "x":3.35366, "y":6.53904, "heading":1.39465, "vx":-1.47798, "vy":-2.91501, "omega":4.56703, "ax":-4.13938, "ay":-3.97083, "alpha":4.42763, "fx":[-56.85084,-39.9969,-51.75139,-67.32411], "fy":[-50.91667,-64.90214,-55.63512,-35.67703]}, + {"t":0.6156, "x":3.32017, "y":6.47399, "heading":1.49506, "vx":-1.56899, "vy":-3.00231, "omega":4.66438, "ax":-4.42572, "ay":-3.69086, "alpha":3.73239, "fx":[-58.42956,-45.94232,-58.53883,-67.94869], "fy":[-49.00342,-60.7355,-48.39215,-34.39575]}, + {"t":0.63759, "x":3.2846, "y":6.40709, "heading":1.59761, "vx":-1.66629, "vy":-3.08346, "omega":4.74644, "ax":-4.7365, "ay":-3.32686, "alpha":2.90787, "fx":[-60.98947,-53.493,-63.92836,-68.65955], "fy":[-45.64728,-54.07537,-40.97494,-32.84203]}, + {"t":0.65957, "x":3.24682, "y":6.33849, "heading":1.70197, "vx":-1.77043, "vy":-3.1566, "omega":4.81037, "ax":-5.0643, "ay":-2.84856, "alpha":1.82995, "fx":[-64.78637,-61.91878,-67.89048,-69.57408], "fy":[-39.88231,-44.02983,-33.98705,-30.69055]}, + {"t":0.68156, "x":3.20668, "y":6.2684, "heading":1.80773, "vx":-1.88177, "vy":-3.21923, "omega":4.8506, "ax":-5.38153, "ay":-2.22039, "alpha":0.46321, "fx":[-69.73365,-69.49698,-70.65455,-70.83235], "fy":[-30.09806,-30.56041,-27.76485,-27.39939]}, + {"t":0.70354, "x":3.164, "y":6.19709, "heading":1.91437, "vx":-2.00008, "vy":-3.26805, "omega":4.86078, "ax":-5.63387, "ay":-1.41879, "alpha":-1.17646, "fx":[-74.463,-74.33406,-72.53313,-72.55022], "fy":[-14.27913,-15.38072,-22.3588,-21.98985]}, + {"t":0.72553, "x":3.11867, "y":6.1249, "heading":2.02124, "vx":-2.12395, "vy":-3.29924, "omega":4.83492, "ax":-5.74448, "ay":-0.45594, "alpha":-3.05565, "fx":[-75.35698,-75.93187,-73.80743,-74.55394], "fy":[7.61281,-1.19557,-17.60431,-12.59601]}, + {"t":0.74751, "x":3.07058, "y":6.05225, "heading":2.12754, "vx":-2.25025, "vy":-3.30926, "omega":4.76774, "ax":-5.65058, "ay":0.5991, "alpha":-4.82858, "fx":[-69.53438,-75.30492,-74.68328,-75.2294], "fy":[30.1124,10.23284,-13.19044,4.09613]}, + {"t":0.7695, "x":3.01974, "y":5.97964, "heading":2.23236, "vx":-2.37448, "vy":-3.29609, "omega":4.66158, "ax":-5.31782, "ay":1.67443, "alpha":-6.52038, "fx":[-59.83591,-73.73013,-75.28078,-68.54722], "fy":[46.68598,18.66786,-8.67556,30.66509]}, + {"t":0.79149, "x":2.96625, "y":5.90757, "heading":2.33485, "vx":-2.49139, "vy":-3.25928, "omega":4.51822, "ax":-4.72337, "ay":2.59976, "alpha":-9.03587, "fx":[-50.79915,-72.0004,-75.61072,-47.97559], "fy":[56.56638,24.67282,-3.45549,57.82776]}, + {"t":0.81347, "x":2.91034, "y":5.83655, "heading":2.43418, "vx":-2.59524, "vy":-3.20212, "omega":4.31956, "ax":-4.10513, "ay":3.17852, "alpha":-11.38699, "fx":[-44.09139,-70.44403,-75.48886,-24.11204], "fy":[62.07749,28.93561,3.29273,71.49596]}, + {"t":0.83546, "x":2.85229, "y":5.76691, "heading":2.52915, "vx":-2.6855, "vy":-3.13224, "omega":4.06921, "ax":-3.65623, "ay":3.55209, "alpha":-12.24342, "fx":[-39.52398,-69.136,-74.29916,-7.76116], "fy":[65.18516,32.0211,12.72122,75.36086]}, + {"t":0.85744, "x":2.79236, "y":5.69891, "heading":2.61862, "vx":-2.76588, "vy":-3.05414, "omega":3.80003, "ax":-3.31824, "ay":3.90222, "alpha":-11.73279, "fx":[-36.51023,-68.04842,-70.40779,1.87651], "fy":[66.99799,34.33563,26.26215,75.95646]}, + {"t":0.87943, "x":2.73075, "y":5.6327, "heading":2.70216, "vx":-2.83883, "vy":-2.96835, "omega":3.54208, "ax":-2.96095, "ay":4.30271, "alpha":-10.47151, "fx":[-34.55668,-67.12507,-60.29702,7.52642], "fy":[68.08609,36.1555,44.44258,75.7584]}, + {"t":0.90141, "x":2.66762, "y":5.56848, "heading":2.78004, "vx":-2.90393, "vy":-2.87375, "omega":3.31185, "ax":-2.47409, "ay":4.69509, "alpha":-9.50758, "fx":[-33.32291,-66.303,-40.30864,10.87811], "fy":[68.74516,37.67784,63.02927,75.45829]}, + {"t":0.9234, "x":2.60318, "y":5.50643, "heading":2.85285, "vx":-2.95833, "vy":-2.77053, "omega":3.10282, "ax":-1.91815, "ay":4.92575, "alpha":-9.77916, "fx":[-32.58118,-65.52035,-14.79659,12.84132], "fy":[69.13706,39.04949,73.52774,75.22833]}, + {"t":0.94539, "x":2.53767, "y":5.44671, "heading":2.92107, "vx":-3.0005, "vy":-2.66223, "omega":2.88782, "ax":-1.46972, "ay":4.98221, "alpha":-10.86244, "fx":[-32.17123,-64.72292,6.29544,13.93367], "fy":[69.35897,40.37653,75.0588,75.09352]}, + {"t":0.96737, "x":2.47135, "y":5.38939, "heading":2.98456, "vx":-3.03281, "vy":-2.55269, "omega":2.649, "ax":-1.17442, "ay":4.96796, "alpha":-11.93927, "fx":[-31.96776,-63.86416,20.08916,14.48144], "fy":[69.47789,41.73519,72.89489,75.03618]}, + {"t":0.98936, "x":2.40439, "y":5.33446, "heading":3.0428, "vx":-3.05863, "vy":-2.44347, "omega":2.38651, "ax":-0.98705, "ay":4.94565, "alpha":-12.71725, "fx":[-31.87193,-62.90834,28.58829,14.70422], "fy":[69.54281,43.17252,70.23573,75.02957]}, + {"t":1.01134, "x":2.3369, "y":5.28194, "heading":3.09527, "vx":-3.08033, "vy":-2.33473, "omega":2.10691, "ax":-0.86251, "ay":4.93355, "alpha":-13.19889, "fx":[-31.81176,-61.83498,33.90443,14.75118], "fy":[69.58821,44.70451,68.00679,75.05001]}, + {"t":1.03333, "x":2.26897, "y":5.2318, "heading":-3.14159, "vx":-3.0993, "vy":-2.22627, "omega":1.81672, "ax":-0.61842, "ay":5.09915, "alpha":-12.31969, "fx":[-28.74796,-55.24981,36.14089,15.59811], "fy":[70.98947,52.79825,67.22619,74.97387]}, + {"t":1.06564, "x":2.1685, "y":5.16252, "heading":-3.08289, "vx":-3.11928, "vy":-2.06149, "omega":1.41862, "ax":-0.06094, "ay":5.19931, "alpha":-11.77091, "fx":[-23.35682,-44.19332,44.50749,19.86379], "fy":[72.92336,62.25712,62.07639,73.95544]}, + {"t":1.09796, "x":2.06767, "y":5.09862, "heading":-3.03704, "vx":-3.12125, "vy":-1.89348, "omega":1.03825, "ax":0.62669, "ay":5.3, "alpha":-10.26462, "fx":[-16.11932,-26.23587,50.31112,24.73427], "fy":[74.83861,71.6261,57.53088,72.469]}, + {"t":1.13027, "x":1.96713, "y":5.0402, "heading":-3.00349, "vx":-3.101, "vy":-1.72221, "omega":0.70655, "ax":1.44637, "ay":5.29738, "alpha":-8.38117, "fx":[-6.9098,-2.38814,54.63976,30.10549], "fy":[76.22553,76.21313,53.48435,70.40471]}, + {"t":1.16259, "x":1.86768, "y":4.98731, "heading":-2.98066, "vx":-3.05426, "vy":-1.55103, "omega":0.43572, "ax":2.27965, "ay":5.12362, "alpha":-6.8287, "fx":[4.07944,21.03022,58.02515,35.77894], "fy":[76.41563,73.32057,49.83173,67.69634]}, + {"t":1.1949, "x":1.77017, "y":4.93987, "heading":-2.96658, "vx":-2.98059, "vy":-1.38546, "omega":0.21506, "ax":3.01009, "ay":4.8217, "alpha":-5.72648, "fx":[16.19712,38.55982,60.7468,41.51173], "fy":[74.78254,65.8808,46.51117,64.34022]}, + {"t":1.22722, "x":1.67543, "y":4.89762, "heading":-2.95963, "vx":-2.88332, "vy":-1.22965, "omega":0.03001, "ax":3.6114, "ay":4.46181, "alpha":-4.80738, "fx":[28.39347,49.95208,62.9703,47.06585], "fy":[71.05284,57.80496,43.48623,60.39777]}, + {"t":1.25953, "x":1.58414, "y":4.86021, "heading":-2.95866, "vx":-2.76662, "vy":-1.08547, "omega":-0.12534, "ax":4.09766, "ay":4.08362, "alpha":-3.91572, "fx":[39.56328,57.13983,64.80422,52.2392], "fy":[65.50184,50.79193,40.73334,55.98736]}, + {"t":1.29184, "x":1.49688, "y":4.82727, "heading":-2.96271, "vx":-2.63421, "vy":-0.95351, "omega":-0.25187, "ax":4.4852, "ay":3.70801, "alpha":-3.04709, "fx":[48.9635,61.78911,66.32461,56.88463], "fy":[58.8237,45.094,38.23655,51.26704]}, + {"t":1.32416, "x":1.4141, "y":4.79839, "heading":-2.97085, "vx":-2.48927, "vy":-0.83369, "omega":-0.35034, "ax":4.78907, "ay":3.34885, "alpha":-2.23918, "fx":[56.37716,64.92693,67.58808,60.92043], "fy":[51.78471,40.51063,35.98342,46.40796]}, + {"t":1.35647, "x":1.33616, "y":4.7732, "heading":-2.98217, "vx":-2.33452, "vy":-0.72547, "omega":-0.4227, "ax":5.02436, "ay":3.01498, "alpha":-1.51612, "fx":[61.979,67.13992,68.63931,64.32784], "fy":[44.95756,36.78411,33.96146,41.56776]}, + {"t":1.38879, "x":1.26334, "y":4.75133, "heading":-2.99583, "vx":-2.17216, "vy":-0.62805, "omega":-0.47169, "ax":5.20534, "ay":2.7105, "alpha":-0.88206, "fx":[66.11074,68.76403,69.51497,67.13708], "fy":[38.65776,33.70145,32.15625,36.87266]}, + {"t":1.4211, "x":1.19587, "y":4.73245, "heading":-3.01107, "vx":-2.00395, "vy":-0.54046, "omega":-0.50019, "ax":5.34431, "ay":2.43601, "alpha":-0.32982, "fx":[69.12406,69.99728,70.24575,69.40879], "fy":[33.00321,31.10471,30.55115,32.4109]}, + {"t":1.45342, "x":1.1339, "y":4.71626, "heading":-3.02724, "vx":-1.83125, "vy":-0.46174, "omega":-0.51085, "ax":5.45115, "ay":2.19005, "alpha":0.15158, "fx":[71.31345,70.96068,70.85737,71.21728], "fy":[27.99666,28.88054,29.12785,28.23495]}, + {"t":1.48573, "x":1.07757, "y":4.70248, "heading":-3.04375, "vx":-1.6551, "vy":-0.39097, "omega":-0.50595, "ax":5.53349, "ay":1.97017, "alpha":0.57311, "fx":[72.90351,71.73093,71.37133,72.63849], "fy":[23.58699,26.948,27.86724,24.36814]}, + {"t":1.51805, "x":1.02698, "y":4.69087, "heading":-3.0601, "vx":-1.47629, "vy":-0.3273, "omega":-0.48743, "ax":5.59715, "ay":1.7736, "alpha":0.94434, "fx":[74.05826,72.35838,71.8055,73.74281], "fy":[19.70481,25.24909,26.75017,20.81256]}, + {"t":1.55036, "x":0.9822, "y":4.68122, "heading":-3.07585, "vx":-1.29542, "vy":-0.26999, "omega":-0.45692, "ax":5.64649, "ay":1.59761, "alpha":1.27324, "fx":[74.89535,72.87709,72.17466,74.59161], "fy":[16.28009,23.74189,25.75817,17.5561]}, + {"t":1.58267, "x":0.94328, "y":4.67333, "heading":-3.09061, "vx":-1.11296, "vy":-0.21837, "omega":-0.41577, "ax":5.6848, "ay":1.43967, "alpha":1.56631, "fx":[75.49895,73.31081,72.49095,75.23638], "fy":[13.24954,22.39595,24.87381,14.57813]}, + {"t":1.61499, "x":0.91029, "y":4.66703, "heading":-3.10405, "vx":-0.92926, "vy":-0.17184, "omega":-0.36516, "ax":5.71457, "ay":1.29751, "alpha":1.82891, "fx":[75.92954,73.67652,72.76439,75.71914], "fy":[10.55889,21.18894,24.08096,11.8535]}, + {"t":1.6473, "x":0.88324, "y":4.66215, "heading":-3.11585, "vx":-0.7446, "vy":-0.12992, "omega":-0.30606, "ax":5.73766, "ay":1.16917, "alpha":2.06543, "fx":[76.23106,73.98667,73.00318,76.07352], "fy":[8.16286,20.1044,23.36492,9.35533]}, + {"t":1.67962, "x":0.86218, "y":4.65856, "heading":-3.12574, "vx":-0.55919, "vy":-0.09213, "omega":-0.23931, "ax":5.75553, "ay":1.05292, "alpha":2.27951, "fx":[76.43581,74.2506,73.2141,76.32605], "fy":[6.02419,19.13017,22.71234,7.05682]}, + {"t":1.71193, "x":0.84711, "y":4.65614, "heading":-3.13347, "vx":-0.3732, "vy":-0.05811, "omega":-0.16565, "ax":5.76928, "ay":0.94728, "alpha":2.47421, "fx":[76.56787,74.47547,73.40274,76.49741], "fy":[4.11244,18.25722,22.11115,4.93227]}, + {"t":1.74425, "x":0.83806, "y":4.65475, "heading":-3.13882, "vx":-0.18677, "vy":-0.0275, "omega":-0.0857, "ax":5.77975, "ay":0.85098, "alpha":2.65209, "fx":[76.64547,74.66683,73.57379,76.60359], "fy":[2.40284,17.4788,21.55037,2.95782]}, + {"t":1.77656, "x":0.83505, "y":4.65431, "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":[] diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj new file mode 100644 index 0000000..65062d4 --- /dev/null +++ b/src/main/deploy/choreo/NewPath.traj @@ -0,0 +1,29 @@ +{ + "name":"NewPath", + "version":3, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.9904961585998535 m", "val":3.9904961585998535}, "y":{"exp":"8.07487678527832 m", "val":8.07487678527832}, "heading":{"exp":"0 deg", "val":0.0}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":null, + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/path.chor b/src/main/deploy/choreo/path.chor index fdb671e..789265a 100644 --- a/src/main/deploy/choreo/path.chor +++ b/src/main/deploy/choreo/path.chor @@ -9,39 +9,39 @@ "config":{ "frontLeft":{ "x":{ - "exp":"11 in", - "val":0.2794 + "exp":"9 in", + "val":0.2286 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"9 in", + "val":0.2286 } }, "backLeft":{ "x":{ - "exp":"-11 in", - "val":-0.2794 + "exp":"-9 in", + "val":-0.2286 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"9 in", + "val":0.2286 } }, "mass":{ - "exp":"150 lbs", - "val":68.0388555 + "exp":"115 lbs", + "val":52.16312255 }, "inertia":{ - "exp":"6 kg m ^ 2", - "val":6.0 + "exp":"3 kg m ^ 2", + "val":3.0 }, "gearing":{ "exp":"6.5", "val":6.5 }, "radius":{ - "exp":"2 in", - "val":0.0508 + "exp":"4 in", + "val":0.1016 }, "vmax":{ "exp":"6000 RPM", @@ -57,16 +57,16 @@ }, "bumper":{ "front":{ - "exp":"16 in", - "val":0.4064 + "exp":"27 in", + "val":0.6858 }, "side":{ - "exp":"16 in", - "val":0.4064 + "exp":"27 in", + "val":0.6858 }, "back":{ - "exp":"16 in", - "val":0.4064 + "exp":"27 in", + "val":0.6858 } }, "differentialTrackWidth":{ diff --git a/src/main/java/frc/robot/commands/auto/LeftShoot.java b/src/main/java/frc/robot/commands/auto/LeftShoot.java index f33568b..f819405 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShoot.java +++ b/src/main/java/frc/robot/commands/auto/LeftShoot.java @@ -1,23 +1,34 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; +import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class LeftShoot extends LoggableSequentialCommandGroup{ - public LeftShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate) { + public LeftShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), - LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), - new SpinShooter(shooter, Constants.SHOOTER_SPEED) + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ) ); } } diff --git a/src/main/java/frc/robot/commands/auto/LeftShootClimb.java b/src/main/java/frc/robot/commands/auto/LeftShootClimb.java index 610e9b7..d4a7fb8 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/LeftShootClimb.java @@ -3,12 +3,16 @@ import choreo.auto.AutoFactory; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; @@ -16,17 +20,20 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class LeftShootClimb extends LoggableSequentialCommandGroup{ - public LeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber) { + public LeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), - LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), - new SpinShooter(shooter, Constants.SHOOTER_SPEED), //Should be a shoot command - LoggableCommandWrapper.wrap(auto.resetOdometry("TowerToClimb")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), new LoggableParallelCommandGroup( - LoggableCommandWrapper.wrap(auto.trajectoryCmd("TowerToClimb")), - new ClimberUp(climber) + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) ), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } diff --git a/src/main/java/frc/robot/commands/auto/MidShoot.java b/src/main/java/frc/robot/commands/auto/MidShoot.java index 112c8ba..2969a63 100644 --- a/src/main/java/frc/robot/commands/auto/MidShoot.java +++ b/src/main/java/frc/robot/commands/auto/MidShoot.java @@ -1,23 +1,34 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; +import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class MidShoot extends LoggableSequentialCommandGroup{ - public MidShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate) { + public MidShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), - LoggableCommandWrapper.wrap((auto.trajectoryCmd("LeftToTower"))), - new SpinShooter(shooter, Constants.SHOOTER_SPEED) + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ) ); } } diff --git a/src/main/java/frc/robot/commands/auto/MidShootClimb.java b/src/main/java/frc/robot/commands/auto/MidShootClimb.java index 2e6d730..c4f14e0 100644 --- a/src/main/java/frc/robot/commands/auto/MidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/MidShootClimb.java @@ -3,12 +3,16 @@ import choreo.auto.AutoFactory; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; @@ -16,16 +20,20 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class MidShootClimb extends LoggableSequentialCommandGroup{ - public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber) { + public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), - LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), - new SpinShooter(shooter, Constants.SHOOTER_SPEED), //Should be a shoot command - LoggableCommandWrapper.wrap(auto.resetOdometry("TowerToClimb")), + LoggableCommandWrapper.wrap(auto.resetOdometry("MidToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidToTower")/*.withTimeout(n)*/), new LoggableParallelCommandGroup( - LoggableCommandWrapper.wrap(auto.trajectoryCmd("TowerToClimb")), - new ClimberUp(climber) + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) ), + LoggableCommandWrapper.wrap(auto.resetOdometry("MidClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } diff --git a/src/main/java/frc/robot/commands/auto/RightShoot.java b/src/main/java/frc/robot/commands/auto/RightShoot.java index edc0ddd..a654150 100644 --- a/src/main/java/frc/robot/commands/auto/RightShoot.java +++ b/src/main/java/frc/robot/commands/auto/RightShoot.java @@ -1,23 +1,34 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; +import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class RightShoot extends LoggableSequentialCommandGroup { - public RightShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootState) { +public class RightShoot extends LoggableSequentialCommandGroup{ + public RightShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( - new SetShootingState(shootState, ShootState.SHOOTING_HUB), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")), - new SpinShooter(shooter, Constants.SHOOTER_SPEED) + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ) ); } } diff --git a/src/main/java/frc/robot/commands/auto/RightShootandClimb.java b/src/main/java/frc/robot/commands/auto/RightShootClimb.java similarity index 56% rename from src/main/java/frc/robot/commands/auto/RightShootandClimb.java rename to src/main/java/frc/robot/commands/auto/RightShootClimb.java index 46022f1..3ab834b 100644 --- a/src/main/java/frc/robot/commands/auto/RightShootandClimb.java +++ b/src/main/java/frc/robot/commands/auto/RightShootClimb.java @@ -3,30 +3,37 @@ import choreo.auto.AutoFactory; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class RightShootandClimb extends LoggableSequentialCommandGroup { - public RightShootandClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootState, ClimberSubsystem climber) { +public class RightShootClimb extends LoggableSequentialCommandGroup{ + public RightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( - new SetShootingState(shootState, ShootState.SHOOTING_HUB), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")), - new SpinShooter(shooter, Constants.SHOOTER_SPEED), //Should be a shoot command - LoggableCommandWrapper.wrap(auto.resetOdometry("TowerToClimb")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")/*.withTimeout(n)*/), new LoggableParallelCommandGroup( - LoggableCommandWrapper.wrap(auto.trajectoryCmd("TowerToClimb")), - new ClimberUp(climber) + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) ), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } From 2fe91180c9f08c3c015ab3c7a88f70bbfce32c69 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Fri, 20 Feb 2026 15:49:37 -0500 Subject: [PATCH 13/22] some more sequences+autochooser changes --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/autochooser/AutoChooser.java | 28 ++++++++++++------- .../frc/robot/commands/auto/LeftShoot.java | 3 +- .../frc/robot/commands/auto/MidShoot.java | 3 +- .../frc/robot/commands/auto/RightShoot.java | 3 +- 5 files changed, 22 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13b0de0..0e24a90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,7 +183,7 @@ public RobotContainer() { } setUpAutoFactory(); - autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem); + autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem, feederSubsystem, hopperSubsystem); configureBindings(); putShuffleboardCommands(); } diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 70bf4b1..cd56a44 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -12,9 +12,11 @@ import frc.robot.commands.auto.MidShoot; import frc.robot.commands.auto.MidShootClimb; import frc.robot.commands.auto.RightShoot; -import frc.robot.commands.auto.RightShootandClimb; +import frc.robot.commands.auto.RightShootClimb; import frc.robot.constants.enums.ShootingState; import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.DoNothingCommand; @@ -29,17 +31,23 @@ public class AutoChooser { private final Map commandMap = new HashMap<>(); private final SwerveSubsystem subsystem; - private final ShootingState state; + private final ShootingState shootstate; private final ShooterSubsystem shooter; private final AutoFactory auto; private final ClimberSubsystem climber; + private final FeederSubsystem feeder; + private final HopperSubsystem hopper; - public AutoChooser(SwerveSubsystem subsystem, ShootingState state, AutoFactory auto, ShooterSubsystem shooter, ClimberSubsystem climber) { + public AutoChooser(SwerveSubsystem subsystem, ShootingState shootstate, AutoFactory auto, + ShooterSubsystem shooter, ClimberSubsystem climber, FeederSubsystem feeder, HopperSubsystem hopper) { this.subsystem = subsystem; this.auto = auto; - this.state = state; + this.shootstate = shootstate; this.shooter = shooter; this.climber = climber; + this.hopper = hopper; + this. feeder = feeder; + this.locationChooser = new LoggedDashboardChooser<>( "Location Chooser" ); @@ -83,17 +91,17 @@ private void populateMap() { commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MIDDLE), new DoNothingCommand()); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.LEFT), - new LeftShoot(subsystem, auto, shooter, state)); + new LeftShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.RIGHT), - new RightShoot(subsystem, auto, shooter, state)); + new RightShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), - new MidShoot(subsystem, auto, shooter, state)); + new MidShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), - new LeftShootClimb(subsystem, auto, shooter, state, climber)); + new LeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), - new RightShootandClimb(subsystem, auto, shooter, state, climber)); + new RightShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), - new MidShootClimb(subsystem, auto, shooter, state, climber)); + new MidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); } public AutoEvent getSelectedEvent() { diff --git a/src/main/java/frc/robot/commands/auto/LeftShoot.java b/src/main/java/frc/robot/commands/auto/LeftShoot.java index f819405..9a84e3e 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShoot.java +++ b/src/main/java/frc/robot/commands/auto/LeftShoot.java @@ -8,7 +8,6 @@ import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -19,7 +18,7 @@ public class LeftShoot extends LoggableSequentialCommandGroup{ public LeftShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, - ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), diff --git a/src/main/java/frc/robot/commands/auto/MidShoot.java b/src/main/java/frc/robot/commands/auto/MidShoot.java index 2969a63..7e4a77f 100644 --- a/src/main/java/frc/robot/commands/auto/MidShoot.java +++ b/src/main/java/frc/robot/commands/auto/MidShoot.java @@ -8,7 +8,6 @@ import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -19,7 +18,7 @@ public class MidShoot extends LoggableSequentialCommandGroup{ public MidShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, - ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), diff --git a/src/main/java/frc/robot/commands/auto/RightShoot.java b/src/main/java/frc/robot/commands/auto/RightShoot.java index a654150..0e180e3 100644 --- a/src/main/java/frc/robot/commands/auto/RightShoot.java +++ b/src/main/java/frc/robot/commands/auto/RightShoot.java @@ -8,7 +8,6 @@ import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -19,7 +18,7 @@ public class RightShoot extends LoggableSequentialCommandGroup{ public RightShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, - ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), From 4ff73ac6d3225ea5bd3be3e137dd2d1681d5c53e Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 16:00:33 -0500 Subject: [PATCH 14/22] Added isRedAlliance --- src/main/java/frc/robot/commands/auto/MidShootClimb.java | 2 ++ .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/auto/MidShootClimb.java b/src/main/java/frc/robot/commands/auto/MidShootClimb.java index c4f14e0..b7de801 100644 --- a/src/main/java/frc/robot/commands/auto/MidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/MidShootClimb.java @@ -1,6 +1,7 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.feeder.SpinFeeder; @@ -31,6 +32,7 @@ public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsyst new SpinHopper(hopper), new SpinFeeder(feeder) ), + subsystem.isRedAlliance() LoggableCommandWrapper.wrap(auto.resetOdometry("MidClimb")), new ClimberUp(climber), LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidClimb")/*.withTimeout(n)*/), diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 5b9f230..2e3a80d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -327,7 +327,7 @@ public void zeroGyro() { * * @return true if the red alliance, false if blue. Defaults to false if none is available. */ - private boolean isRedAlliance() { + public boolean isRedAlliance() { var alliance = DriverStation.getAlliance(); return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; } From 70af821753c2f57306ba63829de4141a2a833361 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Fri, 20 Feb 2026 16:27:35 -0500 Subject: [PATCH 15/22] added variations based on blue and red for climbing paths --- .../frc/robot/autochooser/AutoChooser.java | 12 +++--- .../commands/auto/BlueLeftShootClimb.java | 40 +++++++++++++++++++ ...ShootClimb.java => BlueMidShootClimb.java} | 6 +-- .../commands/auto/BlueRightShootClimb.java | 40 +++++++++++++++++++ ...ShootClimb.java => RedLeftShootClimb.java} | 4 +- .../robot/commands/auto/RedMidShootClimb.java | 40 +++++++++++++++++++ ...hootClimb.java => RedRightShootClimb.java} | 4 +- 7 files changed, 132 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java rename src/main/java/frc/robot/commands/auto/{MidShootClimb.java => BlueMidShootClimb.java} (87%) create mode 100644 src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java rename src/main/java/frc/robot/commands/auto/{LeftShootClimb.java => RedLeftShootClimb.java} (91%) create mode 100644 src/main/java/frc/robot/commands/auto/RedMidShootClimb.java rename src/main/java/frc/robot/commands/auto/{RightShootClimb.java => RedRightShootClimb.java} (91%) diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index cd56a44..4f0d836 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -8,11 +8,11 @@ import choreo.auto.AutoFactory; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.auto.LeftShoot; -import frc.robot.commands.auto.LeftShootClimb; +import frc.robot.commands.auto.RedLeftShootClimb; import frc.robot.commands.auto.MidShoot; -import frc.robot.commands.auto.MidShootClimb; +import frc.robot.commands.auto.RedMidShootClimb; import frc.robot.commands.auto.RightShoot; -import frc.robot.commands.auto.RightShootClimb; +import frc.robot.commands.auto.RedRightShootClimb; import frc.robot.constants.enums.ShootingState; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.FeederSubsystem; @@ -97,11 +97,11 @@ private void populateMap() { commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), new MidShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), - new LeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new RedLeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), - new RightShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new RedRightShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), - new MidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new RedMidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); } public AutoEvent getSelectedEvent() { diff --git a/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java new file mode 100644 index 0000000..4a38779 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class BlueLeftShootClimb extends LoggableSequentialCommandGroup{ + public BlueLeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/MidShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java similarity index 87% rename from src/main/java/frc/robot/commands/auto/MidShootClimb.java rename to src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java index b7de801..0d9dce4 100644 --- a/src/main/java/frc/robot/commands/auto/MidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java @@ -1,7 +1,6 @@ package frc.robot.commands.auto; import choreo.auto.AutoFactory; -import edu.wpi.first.wpilibj.DriverStation; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.feeder.SpinFeeder; @@ -20,8 +19,8 @@ import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class MidShootClimb extends LoggableSequentialCommandGroup{ - public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, +public class BlueMidShootClimb extends LoggableSequentialCommandGroup{ + public BlueMidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), @@ -32,7 +31,6 @@ public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsyst new SpinHopper(hopper), new SpinFeeder(feeder) ), - subsystem.isRedAlliance() LoggableCommandWrapper.wrap(auto.resetOdometry("MidClimb")), new ClimberUp(climber), LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidClimb")/*.withTimeout(n)*/), diff --git a/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java new file mode 100644 index 0000000..a8e29a6 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class BlueRightShootClimb extends LoggableSequentialCommandGroup{ + public BlueRightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/LeftShootClimb.java b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java similarity index 91% rename from src/main/java/frc/robot/commands/auto/LeftShootClimb.java rename to src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java index d4a7fb8..b39676b 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java @@ -19,8 +19,8 @@ import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class LeftShootClimb extends LoggableSequentialCommandGroup{ - public LeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, +public class RedLeftShootClimb extends LoggableSequentialCommandGroup{ + public RedLeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java new file mode 100644 index 0000000..6d3651a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class RedMidShootClimb extends LoggableSequentialCommandGroup{ + public RedMidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("MidToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("MidClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/RightShootClimb.java b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java similarity index 91% rename from src/main/java/frc/robot/commands/auto/RightShootClimb.java rename to src/main/java/frc/robot/commands/auto/RedRightShootClimb.java index 3ab834b..1c9d29d 100644 --- a/src/main/java/frc/robot/commands/auto/RightShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java @@ -19,8 +19,8 @@ import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class RightShootClimb extends LoggableSequentialCommandGroup{ - public RightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, +public class RedRightShootClimb extends LoggableSequentialCommandGroup{ + public RedRightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), From 657d882aa78a5c2b662f184f56ec99af4d1e1f6f Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 16:29:05 -0500 Subject: [PATCH 16/22] Added Red/Blue autos to chooser --- .../frc/robot/autochooser/AutoChooser.java | 22 ++++++++++++++----- .../robot/commands/auto/MidShootClimb.java | 1 - .../swervedrive/SwerveSubsystem.java | 2 +- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index cd56a44..59363ed 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -6,6 +6,7 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.auto.LeftShoot; import frc.robot.commands.auto.LeftShootClimb; @@ -96,12 +97,21 @@ private void populateMap() { new RightShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), new MidShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); - commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), - new LeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); - commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), - new RightShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); - commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), - new MidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + + if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + //Blue ShootClimbAutos + + } + + if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + //Red ShootClimb Autos + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), + new LeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), + new RightShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), + new MidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + } } public AutoEvent getSelectedEvent() { diff --git a/src/main/java/frc/robot/commands/auto/MidShootClimb.java b/src/main/java/frc/robot/commands/auto/MidShootClimb.java index b7de801..7dee6de 100644 --- a/src/main/java/frc/robot/commands/auto/MidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/MidShootClimb.java @@ -32,7 +32,6 @@ public MidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsyst new SpinHopper(hopper), new SpinFeeder(feeder) ), - subsystem.isRedAlliance() LoggableCommandWrapper.wrap(auto.resetOdometry("MidClimb")), new ClimberUp(climber), LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidClimb")/*.withTimeout(n)*/), diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 2e3a80d..5b9f230 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -327,7 +327,7 @@ public void zeroGyro() { * * @return true if the red alliance, false if blue. Defaults to false if none is available. */ - public boolean isRedAlliance() { + private boolean isRedAlliance() { var alliance = DriverStation.getAlliance(); return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; } From faa8eacbd5e2cc781c1b2b2f23c091d36af7cdf9 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 16:34:58 -0500 Subject: [PATCH 17/22] Added comment --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 447c680..5423a33 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -131,7 +131,7 @@ public void robotPeriodic() { SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); SmartDashboard.putString("Selected Action", - robotContainer.getAutoChooser().getCommandDescription()); + robotContainer.getAutoChooser().getCommandDescription() + " ALLIANCE SWITCH = RESTART RIO"); // Gets the alliance color. if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { From 01020004b9bf618a1f552562d03759a8814a8af7 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Fri, 20 Feb 2026 17:07:03 -0500 Subject: [PATCH 18/22] changed path names --- src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java | 4 ++-- src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java | 4 ++-- .../java/frc/robot/commands/auto/BlueRightShootClimb.java | 4 ++-- src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java | 4 ++-- src/main/java/frc/robot/commands/auto/RedMidShootClimb.java | 4 ++-- src/main/java/frc/robot/commands/auto/RedRightShootClimb.java | 4 ++-- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java index 4a38779..5d29de3 100644 --- a/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java @@ -31,9 +31,9 @@ public BlueLeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSu new SpinHopper(hopper), new SpinFeeder(feeder) ), - LoggableCommandWrapper.wrap(auto.resetOdometry("LeftClimb")), + LoggableCommandWrapper.wrap(auto.resetOdometry("BlueLeftClimb")), new ClimberUp(climber), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftClimb")/*.withTimeout(n)*/), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("BlueLeftClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } diff --git a/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java index 0d9dce4..f433579 100644 --- a/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java @@ -31,9 +31,9 @@ public BlueMidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSub new SpinHopper(hopper), new SpinFeeder(feeder) ), - LoggableCommandWrapper.wrap(auto.resetOdometry("MidClimb")), + LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidClimb")), new ClimberUp(climber), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidClimb")/*.withTimeout(n)*/), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("BlueMidClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } diff --git a/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java index a8e29a6..b8f645b 100644 --- a/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java @@ -31,9 +31,9 @@ public BlueRightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterS new SpinHopper(hopper), new SpinFeeder(feeder) ), - LoggableCommandWrapper.wrap(auto.resetOdometry("RightClimb")), + LoggableCommandWrapper.wrap(auto.resetOdometry("BlueRightClimb")), new ClimberUp(climber), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightClimb")/*.withTimeout(n)*/), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("BlueRightClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } diff --git a/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java index b39676b..8ad23ff 100644 --- a/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java @@ -31,9 +31,9 @@ public RedLeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSub new SpinHopper(hopper), new SpinFeeder(feeder) ), - LoggableCommandWrapper.wrap(auto.resetOdometry("LeftClimb")), + LoggableCommandWrapper.wrap(auto.resetOdometry("RedLeftClimb")), new ClimberUp(climber), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftClimb")/*.withTimeout(n)*/), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RedLeftClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } diff --git a/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java index 6d3651a..b63a4ae 100644 --- a/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java @@ -31,9 +31,9 @@ public RedMidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubs new SpinHopper(hopper), new SpinFeeder(feeder) ), - LoggableCommandWrapper.wrap(auto.resetOdometry("MidClimb")), + LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidClimb")), new ClimberUp(climber), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidClimb")/*.withTimeout(n)*/), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RedMidClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } diff --git a/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java index 1c9d29d..39eaec0 100644 --- a/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java @@ -31,9 +31,9 @@ public RedRightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSu new SpinHopper(hopper), new SpinFeeder(feeder) ), - LoggableCommandWrapper.wrap(auto.resetOdometry("RightClimb")), + LoggableCommandWrapper.wrap(auto.resetOdometry("RedRightClimb")), new ClimberUp(climber), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightClimb")/*.withTimeout(n)*/), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RedRightClimb")/*.withTimeout(n)*/), new ClimberDown(climber) ); } From de28860fd22d8e9217f284af1f54d19070e52b2a Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 17:34:45 -0500 Subject: [PATCH 19/22] Fixed --- src/main/java/frc/robot/RobotContainer.java | 45 ++++++++------------- 1 file changed, 16 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fa304d1..b53e461 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,10 +64,6 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; -import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; - /** * This class is where the bulk of the robot should be declared. Since * Command-based is a @@ -150,19 +146,14 @@ public RobotContainer() { turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo()); - // No GyroSubsystem in REPLAY for now - // create the drive subsystem with null gyro (use default json) - drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null; - } - case SIM -> { - robotVisualizer = new RobotVisualizer(); - //rollerSubsystem = new// RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer)); - // No GyroSubsystem in REPLAY for now - // create the drive subsystem with null gyro (use default json) - drivebase = !Constants.TESTBED ? new SwerveSubsystem( - new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - + // No GyroSubsystem in REPLAY for now. + drivebase = !Constants.TESTBED + ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), + null) + : null; + apriltagSubsystem = !Constants.TESTBED + ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) + : null; } case SIM -> { robotVisualizer = new RobotVisualizer(); @@ -179,20 +170,16 @@ public RobotContainer() { turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer)); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); intakeDeployer = new IntakeDeployerSubsystem( - IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now - // create the drive subsystem with null gyro (use default json) - drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null; - } IntakeDeployerSubsystem.createSimIo(robotVisualizer)); - - // No GyroSubsystem in REPLAY for now - // create the drive subsystem with null gyro (use default json) - drivebase = !Constants.TESTBED ? new SwerveSubsystem( - new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - + // No GyroSubsystem in SIM for now. + drivebase = !Constants.TESTBED + ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), + null) + : null; + apriltagSubsystem = !Constants.TESTBED + ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) + : null; } - default -> { throw new RuntimeException("Did not specify Robot Mode"); } From 2051371093fd61ded4817a9b9cd86d7c298ff883 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 17:40:03 -0500 Subject: [PATCH 20/22] Fixed --- src/main/java/frc/robot/autochooser/AutoChooser.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 29af81c..4221ce3 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -2,6 +2,7 @@ import java.util.HashMap; import java.util.Map; +import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -101,7 +102,8 @@ private void populateMap() { commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), new MidShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); - if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Blue) { //Blue ShootClimbAutos commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), new BlueLeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); @@ -111,7 +113,7 @@ private void populateMap() { new BlueMidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); } - if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { //Red ShootClimb Autos commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), new RedLeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); From a6feef5570d244f12592c19160937731818350f7 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Fri, 20 Feb 2026 18:15:40 -0500 Subject: [PATCH 21/22] more --- src/main/deploy/choreo/LeftToTower.traj | 2 +- src/main/java/frc/robot/autochooser/FieldLocation.java | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/choreo/LeftToTower.traj b/src/main/deploy/choreo/LeftToTower.traj index 907510a..c5e696a 100644 --- a/src/main/deploy/choreo/LeftToTower.traj +++ b/src/main/deploy/choreo/LeftToTower.traj @@ -14,7 +14,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.7330684661865234 m", "val":3.733068466186523}, "y":{"exp":"7.429088115692139 m", "val":7.429088115692139}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.9086382389068604 m", "val":3.908638238906861}, "y":{"exp":"7.501871109008789 m", "val":7.501871109008789}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"2.268970489501953 m", "val":2.268970489501953}, "y":{"exp":"5.231799125671387 m", "val":5.231799125671387}, "heading":{"exp":"-180 deg", "val":-3.141592653589793}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"0.8350459933280945 m", "val":0.8350459933280945}, "y":{"exp":"4.654308795928955 m", "val":4.654308795928955}, "heading":{"exp":"-180 deg", "val":-3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index 8f001e0..d25180a 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -12,11 +12,11 @@ import java.util.stream.Collectors; public enum FieldLocation { - ZERO(0, 0, 0, "Zero"), - INVALID(-1, -1, -1, "INVALID"), - LEFT(3.622, 7.662, 0, "Depot Side"), - MIDDLE(3.622, 4.024, 0, "Middle"), - RIGHT(3.622, 0.407, 0, "NON Depot Side"); + ZERO(0, 0, 0, "zero"), + INVALID(-1, -1, -1, "invalid"), + LEFT(3.622, 7.662, 0, "depot side"), + MIDDLE(3.622, 4.024, 0, "middle"), + RIGHT(3.622, 0.407, 0, "outpost side"); private static final double RED_X_POS = 9.338; // meters public static final double HEIGHT_OF_FIELD = 8.043; From a2af6522203ff0187c32827c806756faa2957151 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Fri, 20 Feb 2026 21:46:23 -0500 Subject: [PATCH 22/22] fixed based on comments --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/autochooser/AutoChooser.java | 24 +++++++++---------- .../commands/auto/BlueLeftShootClimb.java | 2 +- .../commands/auto/BlueMidShootClimb.java | 2 +- .../commands/auto/BlueRightShootClimb.java | 2 +- .../frc/robot/commands/auto/ExampleAuto.java | 2 +- .../frc/robot/commands/auto/LeftShoot.java | 2 +- .../frc/robot/commands/auto/MidShoot.java | 2 +- .../commands/auto/RedLeftShootClimb.java | 2 +- .../robot/commands/auto/RedMidShootClimb.java | 2 +- .../commands/auto/RedRightShootClimb.java | 2 +- .../frc/robot/commands/auto/RightShoot.java | 2 +- 12 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5423a33..447c680 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -131,7 +131,7 @@ public void robotPeriodic() { SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); SmartDashboard.putString("Selected Action", - robotContainer.getAutoChooser().getCommandDescription() + " ALLIANCE SWITCH = RESTART RIO"); + robotContainer.getAutoChooser().getCommandDescription()); // Gets the alliance color. if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 4221ce3..319fc62 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -35,7 +35,7 @@ public class AutoChooser { /** Structure for mapping possible choices to commands. */ private final Map commandMap = new HashMap<>(); - private final SwerveSubsystem subsystem; + private final SwerveSubsystem drivetrain; private final ShootingState shootstate; private final ShooterSubsystem shooter; private final AutoFactory auto; @@ -43,9 +43,9 @@ public class AutoChooser { private final FeederSubsystem feeder; private final HopperSubsystem hopper; - public AutoChooser(SwerveSubsystem subsystem, ShootingState shootstate, AutoFactory auto, + public AutoChooser(SwerveSubsystem drivetrain, ShootingState shootstate, AutoFactory auto, ShooterSubsystem shooter, ClimberSubsystem climber, FeederSubsystem feeder, HopperSubsystem hopper) { - this.subsystem = subsystem; + this.drivetrain = drivetrain; this.auto = auto; this.shootstate = shootstate; this.shooter = shooter; @@ -96,31 +96,31 @@ private void populateMap() { commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MIDDLE), new DoNothingCommand()); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.LEFT), - new LeftShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); + new LeftShoot(drivetrain, auto, shooter, shootstate, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.RIGHT), - new RightShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); + new RightShoot(drivetrain, auto, shooter, shootstate, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), - new MidShoot(subsystem, auto, shooter, shootstate, hopper, feeder)); + new MidShoot(drivetrain, auto, shooter, shootstate, hopper, feeder)); Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Blue) { //Blue ShootClimbAutos commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), - new BlueLeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new BlueLeftShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), - new BlueRightShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new BlueRightShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), - new BlueMidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new BlueMidShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); } if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { //Red ShootClimb Autos commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), - new RedLeftShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new RedLeftShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), - new RedRightShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new RedRightShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), - new RedMidShootClimb(subsystem, auto, shooter, shootstate, climber, hopper, feeder)); + new RedMidShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); } } diff --git a/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java index 5d29de3..0746b90 100644 --- a/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java @@ -20,7 +20,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class BlueLeftShootClimb extends LoggableSequentialCommandGroup{ - public BlueLeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public BlueLeftShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java index f433579..4e5434d 100644 --- a/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java @@ -20,7 +20,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class BlueMidShootClimb extends LoggableSequentialCommandGroup{ - public BlueMidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public BlueMidShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java index b8f645b..ae20ea9 100644 --- a/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java @@ -20,7 +20,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class BlueRightShootClimb extends LoggableSequentialCommandGroup{ - public BlueRightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public BlueRightShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/ExampleAuto.java b/src/main/java/frc/robot/commands/auto/ExampleAuto.java index bece64d..01a28ab 100644 --- a/src/main/java/frc/robot/commands/auto/ExampleAuto.java +++ b/src/main/java/frc/robot/commands/auto/ExampleAuto.java @@ -8,7 +8,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class ExampleAuto extends LoggableSequentialCommandGroup{ - public ExampleAuto(SwerveSubsystem subsystem, AutoFactory auto) { + public ExampleAuto(SwerveSubsystem drivetrain, AutoFactory auto) { super( LoggableCommandWrapper.wrap(auto.resetOdometry("ExamplePathOne")), new LoggableParallelCommandGroup( diff --git a/src/main/java/frc/robot/commands/auto/LeftShoot.java b/src/main/java/frc/robot/commands/auto/LeftShoot.java index 9a84e3e..2de078e 100644 --- a/src/main/java/frc/robot/commands/auto/LeftShoot.java +++ b/src/main/java/frc/robot/commands/auto/LeftShoot.java @@ -17,7 +17,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class LeftShoot extends LoggableSequentialCommandGroup{ - public LeftShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public LeftShoot(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/MidShoot.java b/src/main/java/frc/robot/commands/auto/MidShoot.java index 7e4a77f..10f5324 100644 --- a/src/main/java/frc/robot/commands/auto/MidShoot.java +++ b/src/main/java/frc/robot/commands/auto/MidShoot.java @@ -17,7 +17,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class MidShoot extends LoggableSequentialCommandGroup{ - public MidShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public MidShoot(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java index 8ad23ff..cb783e0 100644 --- a/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java @@ -20,7 +20,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class RedLeftShootClimb extends LoggableSequentialCommandGroup{ - public RedLeftShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public RedLeftShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java index b63a4ae..efc4dc6 100644 --- a/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java @@ -20,7 +20,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class RedMidShootClimb extends LoggableSequentialCommandGroup{ - public RedMidShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public RedMidShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java index 39eaec0..39299fc 100644 --- a/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java @@ -20,7 +20,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class RedRightShootClimb extends LoggableSequentialCommandGroup{ - public RedRightShootClimb(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public RedRightShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB), diff --git a/src/main/java/frc/robot/commands/auto/RightShoot.java b/src/main/java/frc/robot/commands/auto/RightShoot.java index 0e180e3..613335f 100644 --- a/src/main/java/frc/robot/commands/auto/RightShoot.java +++ b/src/main/java/frc/robot/commands/auto/RightShoot.java @@ -17,7 +17,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class RightShoot extends LoggableSequentialCommandGroup{ - public RightShoot(SwerveSubsystem subsystem, AutoFactory auto, ShooterSubsystem shooter, + public RightShoot(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { super( new SetShootingState(shootstate, ShootState.SHOOTING_HUB),