diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj new file mode 100644 index 00000000..aca4ea83 --- /dev/null +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -0,0 +1,126 @@ +{ + "name":"Depot_Fast", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.794022798538208, "y":5.955489635467529, "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}, + {"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":1, "data":{"type":"MaxVelocity", "props":{"max":1.8}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":0.8}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.794022798538208 m", "val":1.794022798538208}, "y":{"exp":"5.955489635467529 m", "val":5.955489635467529}, "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}, + {"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":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.8 m / s", "val":1.8}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"0.8 m / s ^ 2", "val":0.8}}}, "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":45.359237, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.43, + "side":0.43, + "back":0.43 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,3.17257], + "samples":[ + {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.71612, "ay":0.35587, "alpha":0.00095, "fx":[-8.10451,-8.10456,-8.13681,-8.13682], "fy":[4.0466,3.98327,4.06573,4.04658]}, + {"t":0.04957, "x":3.59512, "y":5.06044, "heading":3.14159, "vx":-0.0355, "vy":0.01764, "omega":0.00005, "ax":-0.71626, "ay":0.35594, "alpha":0.00246, "fx":[-8.10663,-8.10665,-8.13792,-8.13794], "fy":[4.03879,4.01043,4.05738,4.03878]}, + {"t":0.09914, "x":3.59248, "y":5.06175, "heading":-3.14159, "vx":-0.07101, "vy":0.03529, "omega":0.00017, "ax":-0.71626, "ay":0.35594, "alpha":0.00108, "fx":[-8.13611,-8.13608,-8.15432,-8.06241], "fy":[3.99559,4.05596,4.05595,4.03776]}, + {"t":0.14871, "x":3.58808, "y":5.06394, "heading":-3.14158, "vx":-0.10651, "vy":0.05293, "omega":0.00022, "ax":-0.71625, "ay":0.35594, "alpha":0.00211, "fx":[-8.10725,-8.10727,-8.13706,-8.13708], "fy":[4.03989,4.00771,4.05766,4.03987]}, + {"t":0.19829, "x":3.58192, "y":5.067, "heading":-3.14157, "vx":-0.14202, "vy":0.07057, "omega":0.00033, "ax":-0.71625, "ay":0.35594, "alpha":0.00075, "fx":[-8.13749,-8.13746,-8.15498,-8.05846], "fy":[3.99556,4.05564,4.05563,4.03816]}, + {"t":0.24786, "x":3.574, "y":5.07093, "heading":-3.14155, "vx":-0.17752, "vy":0.08822, "omega":0.00036, "ax":-0.71624, "ay":0.35593, "alpha":0.00189, "fx":[-8.1077,-8.10773,-8.13633,-8.13635], "fy":[4.04037,4.00661,4.05751,4.04036]}, + {"t":0.29743, "x":3.56432, "y":5.07574, "heading":-3.14154, "vx":-0.21303, "vy":0.10586, "omega":0.00046, "ax":-0.71623, "ay":0.35593, "alpha":0.00057, "fx":[-8.13927,-8.13924,-8.1562,-8.05309], "fy":[3.99191,4.05657,4.05656,4.03965]}, + {"t":0.347, "x":3.55288, "y":5.08143, "heading":-3.14151, "vx":-0.24853, "vy":0.12351, "omega":0.00049, "ax":-0.71623, "ay":0.35593, "alpha":0.0015, "fx":[-8.108,-8.10803,-8.13571,-8.13572], "fy":[4.04194,4.00205,4.0586,4.04193]}, + {"t":0.39657, "x":3.53968, "y":5.08799, "heading":-3.14149, "vx":-0.28404, "vy":0.14115, "omega":0.00056, "ax":-0.71622, "ay":0.35592, "alpha":0.00028, "fx":[-8.14161,-8.14158,-8.15811,-8.0458], "fy":[3.98859,4.05741,4.05741,4.04092]}, + {"t":0.44614, "x":3.52472, "y":5.09542, "heading":-3.14146, "vx":-0.31954, "vy":0.15879, "omega":0.00057, "ax":-0.71621, "ay":0.35592, "alpha":0.00123, "fx":[-8.10818,-8.10821,-8.13515,-8.13517], "fy":[4.04294,3.99904,4.05922,4.04293]}, + {"t":0.49571, "x":3.508, "y":5.10373, "heading":-3.14143, "vx":-0.35504, "vy":0.17644, "omega":0.00064, "ax":-0.7162, "ay":0.35591, "alpha":0.00006, "fx":[-8.14362,-8.14358,-8.1598,-8.03928], "fy":[3.98507,4.05834,4.05833,4.04217]}, + {"t":0.54529, "x":3.48952, "y":5.11291, "heading":-3.1414, "vx":-0.39055, "vy":0.19408, "omega":0.00064, "ax":-0.71619, "ay":0.35591, "alpha":0.00097, "fx":[-8.10825,-8.10828,-8.13463,-8.13464], "fy":[4.04387,3.99605,4.05989,4.04386]}, + {"t":0.59486, "x":3.46928, "y":5.12297, "heading":-3.14137, "vx":-0.42605, "vy":0.21172, "omega":0.00069, "ax":-0.71618, "ay":0.3559, "alpha":-0.00043, "fx":[-8.14698,-8.14695,-8.16293,-8.02843], "fy":[3.9818,4.05918,4.05917,4.04325]}, + {"t":0.64443, "x":3.44728, "y":5.13391, "heading":-3.14134, "vx":-0.46155, "vy":0.22937, "omega":0.00067, "ax":-0.71617, "ay":0.35589, "alpha":0.00047, "fx":[-8.10822,-8.10826,-8.13411,-8.13413], "fy":[4.0462,3.9887,4.06201,4.04619]}, + {"t":0.694, "x":3.42352, "y":5.14571, "heading":-3.1413, "vx":-0.49705, "vy":0.24701, "omega":0.00069, "ax":-0.71615, "ay":0.35589, "alpha":-0.00092, "fx":[-8.15101,-8.15097,-8.16675,-8.01533], "fy":[3.9763,4.06073,4.06073,4.04501]}, + {"t":0.74357, "x":3.398, "y":5.15839, "heading":-3.14127, "vx":-0.53255, "vy":0.26465, "omega":0.00064, "ax":-0.71614, "ay":0.35588, "alpha":-0.0001, "fx":[-8.10813,-8.10817,-8.13352,-8.13354], "fy":[4.04885,3.98026,4.06445,4.04884]}, + {"t":0.79314, "x":3.37072, "y":5.17195, "heading":-3.14124, "vx":-0.56805, "vy":0.28229, "omega":0.00064, "ax":-0.71612, "ay":0.35587, "alpha":-0.00121, "fx":[-8.15547,-8.15543,-8.17096,-8.00067], "fy":[3.96602,4.06381,4.06381,4.04834]}, + {"t":0.84271, "x":3.34168, "y":5.18638, "heading":-3.14121, "vx":-0.60355, "vy":0.29993, "omega":0.00058, "ax":-0.7161, "ay":0.35586, "alpha":-0.00068, "fx":[-8.10799,-8.10803,-8.13278,-8.1328], "fy":[4.05141,3.97194,4.06675,4.05141]}, + {"t":0.89229, "x":3.31088, "y":5.20169, "heading":-3.14118, "vx":-0.63905, "vy":0.31757, "omega":0.00054, "ax":-0.71607, "ay":0.35585, "alpha":-0.00191, "fx":[-8.16122,-8.16117,-8.17639,-7.98177], "fy":[3.95801,4.06603,4.06603,4.05089]}, + {"t":0.94186, "x":3.27833, "y":5.21787, "heading":-3.14115, "vx":-0.67455, "vy":0.33521, "omega":0.00045, "ax":-0.71605, "ay":0.35583, "alpha":-0.00099, "fx":[-8.10783,-8.10788,-8.13179,-8.13181], "fy":[4.05235,3.96835,4.06728,4.05234]}, + {"t":0.99143, "x":3.24401, "y":5.23492, "heading":-3.14113, "vx":-0.71004, "vy":0.35285, "omega":0.0004, "ax":-0.71601, "ay":0.35582, "alpha":-0.00275, "fx":[-8.17012,-8.17006,-8.18481,-7.95286], "fy":[3.94201,4.07074,4.07074,4.05609]}, + {"t":1.041, "x":3.20793, "y":5.25285, "heading":-3.14111, "vx":-0.74554, "vy":0.37049, "omega":0.00026, "ax":-0.71598, "ay":0.3558, "alpha":-0.00245, "fx":[-8.10758,-8.10763,-8.13045,-8.13047], "fy":[4.05926,3.94655,4.07363,4.05925]}, + {"t":1.09057, "x":3.17009, "y":5.27165, "heading":-3.1411, "vx":-0.78103, "vy":0.38813, "omega":0.00014, "ax":-0.71593, "ay":0.35577, "alpha":-0.00432, "fx":[-8.1809,-8.18084,-8.19487,-7.91745], "fy":[3.93096,4.07352,4.07353,4.0596]}, + {"t":1.14014, "x":3.1305, "y":5.29133, "heading":-3.14109, "vx":-0.81652, "vy":0.40576, "omega":-0.00007, "ax":-0.71587, "ay":0.35574, "alpha":-0.00323, "fx":[-8.10727,-8.10733,-8.12843,-8.12845], "fy":[4.06195,3.93702,4.07536,4.06195]}, + {"t":1.18971, "x":3.08914, "y":5.31188, "heading":-3.14109, "vx":-0.852, "vy":0.4234, "omega":-0.00023, "ax":-0.7158, "ay":0.35571, "alpha":-0.00452, "fx":[-8.19111,-8.19104,-8.20385,-7.88222], "fy":[3.89711,4.08339,4.0834,4.0707]}, + {"t":1.23929, "x":3.04603, "y":5.33331, "heading":-3.1411, "vx":-0.88749, "vy":0.44103, "omega":-0.00046, "ax":-0.71571, "ay":0.35566, "alpha":-0.00487, "fx":[-8.10661,-8.10668,-8.12532,-8.12534], "fy":[4.06856,3.91475,4.08054,4.06856]}, + {"t":1.28886, "x":3.00115, "y":5.35561, "heading":-3.14113, "vx":-0.92297, "vy":0.45866, "omega":-0.0007, "ax":-0.71558, "ay":0.35559, "alpha":-0.00482, "fx":[-8.19779,-8.19771,-8.20872,-7.8539], "fy":[3.87126,4.08969,4.08969,4.0788]}, + {"t":1.33843, "x":2.95452, "y":5.37878, "heading":-3.14116, "vx":-0.95844, "vy":0.47629, "omega":-0.00094, "ax":-0.7154, "ay":0.3555, "alpha":-0.00586, "fx":[-8.10483,-8.10489,-8.12002,-8.12004], "fy":[4.0703,3.90418,4.08021,4.07031]}, + {"t":1.388, "x":2.90613, "y":5.40283, "heading":-3.14121, "vx":-0.9939, "vy":0.49391, "omega":-0.00123, "ax":-0.7151, "ay":0.35535, "alpha":-0.00887, "fx":[-8.20001,-8.19996,-8.20867,-7.82796], "fy":[3.90851,4.07276,4.07277,4.06416]}, + {"t":1.43757, "x":2.85598, "y":5.42775, "heading":-3.14127, "vx":-1.02935, "vy":0.51153, "omega":-0.00167, "ax":-0.71458, "ay":0.35509, "alpha":-0.00498, "fx":[-8.09816,-8.09818,-8.10817,-8.10819], "fy":[4.05834,3.92452,4.0652,4.05834]}, + {"t":1.48714, "x":2.80408, "y":5.45354, "heading":-3.14135, "vx":-1.06477, "vy":0.52913, "omega":-0.00191, "ax":-0.71337, "ay":0.35444, "alpha":-0.00875, "fx":[-8.16086,-8.16083,-8.16609,-7.87028], "fy":[3.9514,4.04357,4.04358,4.03837]}, + {"t":1.53671, "x":2.75042, "y":5.48021, "heading":-3.14145, "vx":-1.10014, "vy":0.5467, "omega":-0.00235, "ax":-0.70777, "ay":0.3505, "alpha":-0.02046, "fx":[-8.02372,-8.02374,-8.02813,-8.02816], "fy":[4.08656,3.63352,4.09159,4.08658]}, + {"t":1.58629, "x":2.69501, "y":5.50774, "heading":-3.14156, "vx":-1.13522, "vy":0.56407, "omega":-0.00336, "ax":0.7079, "ay":-0.34993, "alpha":0.02081, "fx":[8.22237,8.22235,8.22314,7.44179], "fy":[-3.71919,-4.05145,-4.05143,-4.0507]}, + {"t":1.63586, "x":2.63961, "y":5.53527, "heading":3.14146, "vx":-1.10013, "vy":0.54673, "omega":-0.00233, "ax":0.71328, "ay":-0.35458, "alpha":0.0051, "fx":[8.08569,8.08574,8.09126,8.09125], "fy":[-4.05107,-3.92838,-4.05317,-4.05106]}, + {"t":1.68543, "x":2.58595, "y":5.56194, "heading":3.14134, "vx":-1.06477, "vy":0.52915, "omega":-0.00208, "ax":0.71455, "ay":-0.35512, "alpha":0.00228, "fx":[8.1646,8.16457,8.16838,7.91409], "fy":[-3.88243,-4.07645,-4.07644,-4.07269]}, + {"t":1.735, "x":2.53405, "y":5.58773, "heading":3.14124, "vx":-1.02935, "vy":0.51155, "omega":-0.00196, "ax":0.71508, "ay":-0.35538, "alpha":0.0092, "fx":[8.1027,8.10279,8.11506,8.11505], "fy":[-4.08556,-3.85728,-4.09159,-4.08555]}, + {"t":1.78457, "x":2.4839, "y":5.61265, "heading":3.14114, "vx":-0.9939, "vy":0.49393, "omega":-0.00151, "ax":0.71538, "ay":-0.35553, "alpha":0.00939, "fx":[8.21173,8.21168,8.21921,7.80636], "fy":[-3.89184,-4.08068,-4.08067,-4.07327]}, + {"t":1.83414, "x":2.43551, "y":5.6367, "heading":3.14107, "vx":-0.95844, "vy":0.4763, "omega":-0.00104, "ax":0.71557, "ay":-0.35562, "alpha":0.00627, "fx":[8.10598,8.10607,8.12274,8.12273], "fy":[-4.07467,-3.89793,-4.08329,-4.07465]}, + {"t":1.88371, "x":2.38888, "y":5.65987, "heading":3.14101, "vx":-0.92297, "vy":0.45868, "omega":-0.00073, "ax":0.7157, "ay":-0.35568, "alpha":0.00431, "fx":[8.19467,8.19461,8.20415,7.87005], "fy":[-3.87581,-4.08897,-4.08896,-4.07954]}, + {"t":1.93329, "x":2.344, "y":5.68217, "heading":3.14098, "vx":-0.88749, "vy":0.44104, "omega":-0.00052, "ax":0.71579, "ay":-0.35572, "alpha":0.00468, "fx":[8.10701,8.10711,8.12684,8.12684], "fy":[-4.0689,-3.91828,-4.07933,-4.06888]}, + {"t":1.98286, "x":2.30089, "y":5.7036, "heading":3.14095, "vx":-0.85201, "vy":0.42341, "omega":-0.00029, "ax":0.71587, "ay":-0.35576, "alpha":0.00475, "fx":[8.18622,8.18617,8.19732,7.90141], "fy":[-3.91644,-4.0772,-4.07719,-4.06615]}, + {"t":2.03243, "x":2.25953, "y":5.72415, "heading":3.14094, "vx":-0.81652, "vy":0.40578, "omega":-0.00005, "ax":0.71592, "ay":-0.35579, "alpha":0.00268, "fx":[8.10747,8.10755,8.12936,8.12935], "fy":[-4.05992,-3.94687,-4.07154,-4.0599]}, + {"t":2.082, "x":2.21994, "y":5.74383, "heading":3.14094, "vx":-0.78103, "vy":0.38814, "omega":0.00008, "ax":0.71597, "ay":-0.35581, "alpha":0.00427, "fx":[8.17687,8.17683,8.189,7.93316], "fy":[-3.94192,-4.06981,-4.06979,-4.05773]}, + {"t":2.13157, "x":2.1821, "y":5.76263, "heading":3.14094, "vx":-0.74554, "vy":0.3705, "omega":0.00029, "ax":0.71601, "ay":-0.35583, "alpha":0.00195, "fx":[8.10773,8.10781,8.13104,8.13103], "fy":[-4.05714,-3.95626,-4.06954,-4.05712]}, + {"t":2.18114, "x":2.14602, "y":5.78056, "heading":3.14095, "vx":-0.71005, "vy":0.35286, "omega":0.00039, "ax":0.71604, "ay":-0.35584, "alpha":0.00252, "fx":[8.16576,8.16572,8.17843,7.96917], "fy":[-3.95042,-4.06767,-4.06765,-4.05503]}, + {"t":2.23071, "x":2.1117, "y":5.79762, "heading":3.14097, "vx":-0.67455, "vy":0.33522, "omega":0.00052, "ax":0.71607, "ay":-0.35586, "alpha":0.00119, "fx":[8.10798,8.10804,8.13216,8.13216], "fy":[-4.05384,-3.96701,-4.0667,-4.05383]}, + {"t":2.28029, "x":2.07914, "y":5.8138, "heading":3.141, "vx":-0.63906, "vy":0.31758, "omega":0.00057, "ax":0.71609, "ay":-0.35587, "alpha":0.00165, "fx":[8.15876,8.15872,8.17178,7.99216], "fy":[-3.95989,-4.065,-4.06499,-4.05201]}, + {"t":2.32986, "x":2.04835, "y":5.8291, "heading":3.14103, "vx":-0.60356, "vy":0.29994, "omega":0.00066, "ax":0.71611, "ay":-0.35588, "alpha":0.00044, "fx":[8.1082,8.10826,8.13295,8.13295], "fy":[-4.05035,-3.97819,-4.06347,-4.05033]}, + {"t":2.37943, "x":2.01931, "y":5.84353, "heading":3.14106, "vx":-0.56806, "vy":0.2823, "omega":0.00068, "ax":0.71613, "ay":-0.35589, "alpha":0.00152, "fx":[8.15449,8.15446,8.16775,8.00648], "fy":[-3.97229,-4.06123,-4.06122,-4.048]}, + {"t":2.429, "x":1.99203, "y":5.85709, "heading":3.14109, "vx":-0.53256, "vy":0.26466, "omega":0.00075, "ax":0.71615, "ay":-0.35589, "alpha":-0.00022, "fx":[8.1084,8.10845,8.13353,8.13353], "fy":[-4.04714,-3.98842,-4.06041,-4.04712]}, + {"t":2.47857, "x":1.96651, "y":5.86977, "heading":3.14113, "vx":-0.49706, "vy":0.24702, "omega":0.00074, "ax":0.71616, "ay":-0.3559, "alpha":0.00078, "fx":[8.14952,8.14949,8.1629,8.02265], "fy":[-3.97672,-4.06002,-4.06001,-4.04666]}, + {"t":2.52814, "x":1.94275, "y":5.88158, "heading":3.14117, "vx":-0.46156, "vy":0.22937, "omega":0.00078, "ax":0.71617, "ay":-0.35591, "alpha":-0.00057, "fx":[8.10855,8.1086,8.13399,8.134], "fy":[-4.04558,-3.99355,-4.05898,-4.04557]}, + {"t":2.57771, "x":1.92075, "y":5.89251, "heading":3.14121, "vx":-0.42606, "vy":0.21173, "omega":0.00075, "ax":0.71619, "ay":-0.35591, "alpha":0.00067, "fx":[8.14654,8.14652,8.16006,8.03255], "fy":[-3.9849,-4.05751,-4.0575,-4.04402]}, + {"t":2.62729, "x":1.90051, "y":5.90257, "heading":3.14124, "vx":-0.39055, "vy":0.19409, "omega":0.00079, "ax":0.7162, "ay":-0.35592, "alpha":-0.00089, "fx":[8.10865,8.10869,8.13441,8.13441], "fy":[-4.04413,-3.99826,-4.05765,-4.04412]}, + {"t":2.67686, "x":1.88203, "y":5.91176, "heading":3.14128, "vx":-0.35505, "vy":0.17644, "omega":0.00074, "ax":0.71621, "ay":-0.35592, "alpha":0.00042, "fx":[8.14397,8.14395,8.15763,8.04104], "fy":[-3.98948,-4.05618,-4.05617,-4.04254]}, + {"t":2.72643, "x":1.86531, "y":5.92007, "heading":3.14132, "vx":-0.31955, "vy":0.1588, "omega":0.00076, "ax":0.71622, "ay":-0.35593, "alpha":-0.00106, "fx":[8.10867,8.10871,8.1348,8.13481], "fy":[-4.04351,-4.00038,-4.05719,-4.04349]}, + {"t":2.776, "x":1.85035, "y":5.9275, "heading":3.14136, "vx":-0.28404, "vy":0.14116, "omega":0.00071, "ax":0.71622, "ay":-0.35593, "alpha":0.00024, "fx":[8.14187,8.14184,8.15571,8.04793], "fy":[-3.99389,-4.0549,-4.05489,-4.04107]}, + {"t":2.82557, "x":1.83714, "y":5.93406, "heading":3.14139, "vx":-0.24854, "vy":0.12351, "omega":0.00072, "ax":0.71623, "ay":-0.35593, "alpha":-0.00126, "fx":[8.10862,8.10865,8.13521,8.13522], "fy":[-4.04275,-4.00275,-4.05666,-4.04274]}, + {"t":2.87514, "x":1.8257, "y":5.93975, "heading":3.14143, "vx":-0.21303, "vy":0.10587, "omega":0.00066, "ax":0.71624, "ay":-0.35594, "alpha":-0.00021, "fx":[8.13958,8.13956,8.15366,8.0552], "fy":[-3.99419,-4.05498,-4.05497,-4.04091]}, + {"t":2.92472, "x":1.81602, "y":5.94456, "heading":3.14146, "vx":-0.17753, "vy":0.08822, "omega":0.00065, "ax":0.71624, "ay":-0.35594, "alpha":-0.00153, "fx":[8.10847,8.1085,8.13566,8.13567], "fy":[-4.04171,-4.0059,-4.0559,-4.04169]}, + {"t":2.97429, "x":1.8081, "y":5.94849, "heading":3.14149, "vx":-0.14202, "vy":0.07058, "omega":0.00057, "ax":0.71625, "ay":-0.35594, "alpha":-0.0002, "fx":[8.13836,8.13834,8.15281,8.05905], "fy":[-3.99891,-4.05362,-4.05361,-4.03918]}, + {"t":3.02386, "x":1.80194, "y":5.95155, "heading":3.14152, "vx":-0.10652, "vy":0.05293, "omega":0.00056, "ax":0.71626, "ay":-0.35595, "alpha":-0.00177, "fx":[8.10821,8.10823,8.13618,8.13619], "fy":[-4.04085,-4.00832,-4.05545,-4.04083]}, + {"t":3.07343, "x":1.79754, "y":5.95374, "heading":3.14155, "vx":-0.07101, "vy":0.03529, "omega":0.00048, "ax":0.71626, "ay":-0.35595, "alpha":-0.00194, "fx":[8.10802,8.10805,8.13648,8.1365], "fy":[-4.04022,-4.01006,-4.05508,-4.0402]}, + {"t":3.123, "x":1.7949, "y":5.95505, "heading":3.14157, "vx":-0.03551, "vy":0.01765, "omega":0.00038, "ax":0.71627, "ay":-0.35595, "alpha":-0.00766, "fx":[8.13406,8.05709,8.14905,8.14906], "fy":[-4.02205,-4.05082,-4.05076,-4.02204]}, + {"t":3.17257, "x":1.79402, "y":5.95549, "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/java/frc/robot/Drive.java b/src/main/java/frc/robot/Drive.java index e75b9fee..f2d6523f 100644 --- a/src/main/java/frc/robot/Drive.java +++ b/src/main/java/frc/robot/Drive.java @@ -9,9 +9,9 @@ public class Drive extends SubsystemBase{ - private final PIDController xController = new PIDController(.01, 0.0, 0.0); - private final PIDController yController = new PIDController(.01, 0.0, 0.0); - private final PIDController headingController = new PIDController(.01, 0.0, 0.0); + private final PIDController xController = new PIDController(10, 0.0, 0.0); + private final PIDController yController = new PIDController(10, 0.0, 0.0); + private final PIDController headingController = new PIDController(5, 0.0, 0.0); private SwerveSubsystem subsystem; public Drive(SwerveSubsystem subsystem) { @@ -29,7 +29,7 @@ public void followTrajectory(SwerveSample sample) { ); ChassisSpeeds robotRelativeSpeeds = - ChassisSpeeds.fromRobotRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation()); + ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation()); subsystem.setChassisSpeeds(robotRelativeSpeeds); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 86ab1c78..2d544b7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,6 +336,16 @@ private void configureBindings() { } public void putShuffleboardCommands() { + SmartDashboard.putData( + "intakedeployer/Deployment State: UP", + new SetDeploymentState(intakeDeployer, DeploymentState.UP)); + SmartDashboard.putData( + "intakedeployer/Deployment State: DOWN", + new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); + SmartDashboard.putData( + "Shooting State: Auto aim", + new SetShootingState(shootState, ShootState.AUTO_AIM)); + if (Constants.DEBUG) { SmartDashboard.putNumber(RunDashboardShotTest.ANGLER_TARGET_POSITION_KEY, 0.0); SmartDashboard.putNumber(RunDashboardShotTest.SHOOTER_TARGET_RPM_KEY, Constants.SHOOTER_SPEED); @@ -508,12 +518,6 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Shooting State: Shuttling", new SetShootingState(shootState, ShootState.SHUTTLING)); - SmartDashboard.putData( - "intakedeployer/Deployment State: UP", - new SetDeploymentState(intakeDeployer, DeploymentState.UP)); - SmartDashboard.putData( - "intakedeployer/Deployment State: DOWN", - new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); SmartDashboard.putData( "intakedeployer/Deployment State: STOPPED", new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 80cc9d06..d8ad47a1 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -44,8 +44,8 @@ public void simReadings() { if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0); Vector stdDevs = truster.calculateTrust(measurement); - double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); - double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); + double readingX = pose.getX(); //+ random.nextGaussian() * stdDevs.get(0); + double readingY = pose.getY(); //+ random.nextGaussian() * stdDevs.get(1); double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index ce806027..4ef777e4 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -8,6 +8,7 @@ public enum AutoAction { DO_NOTHING("Do Nothing"), SHOOT("Shoot"), + FAST_SHOOT("Shoot depot"), //SHOOT_PICKUP("Shoot and Pickup"), //DISTURBANCE("Disturbance"), //NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"), diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index cec590a7..ac253cad 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -11,6 +11,8 @@ import frc.robot.Robot; import frc.robot.commands.auto.neutral.DepotNeutral; import frc.robot.commands.auto.neutral.OutpostNeutral; +import frc.robot.commands.auto.newauto.FastDepotRed; +import frc.robot.commands.auto.newauto.FastDepotBlue; import frc.robot.commands.auto.shoot.ShootBlue; import frc.robot.commands.auto.shoot.ShootRed; import frc.robot.commands.auto.disturbance.DepotDisturbance; @@ -82,9 +84,9 @@ private void populateChoosers() { switch (location) { case INVALID -> {} // Skip the invalid case. case ZERO -> { // Default - locationChooser.addDefaultOption(location.toString(), location); + locationChooser.addDefaultOption(location.getShuffleboardName(), location); } - default -> {locationChooser.addOption(location.toString(), location);} + default -> {locationChooser.addOption(location.getShuffleboardName(), location);} }; } for (AutoAction action : AutoAction.values()) { @@ -125,6 +127,12 @@ private void populateCommandMap() { new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE, Alliance.Blue), new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT, Alliance.Red), + new FastDepotRed(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT, Alliance.Blue), + new FastDepotBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + /* //shoot-pickup @@ -164,6 +172,8 @@ private void populateDescriptionMap() { "shoot from the middle"); descriptionMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE), "shoot from the outpost"); + descriptionMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT), + "depot and shoot from the depot"); /* descriptionMap.put(new AutoEvent(AutoAction.SHOOT_PICKUP, FieldLocation.DEPOT_SIDE), "shoot and pickup from the depot"); @@ -204,7 +214,7 @@ public String getCommandDescription() { return "NO AUTO"; } else { String commandDescription = descriptionMap.get(event.withoutColor()); - return event.getAction() + " at " + event.getLocation() + " → " + commandDescription + "."; + return event.getActionName() + " at " + event.getLocation() + " -> " + commandDescription + "."; } } diff --git a/src/main/java/frc/robot/autochooser/AutoEvent.java b/src/main/java/frc/robot/autochooser/AutoEvent.java index 41d5fc26..c0dec2fe 100644 --- a/src/main/java/frc/robot/autochooser/AutoEvent.java +++ b/src/main/java/frc/robot/autochooser/AutoEvent.java @@ -40,6 +40,9 @@ public AutoEvent(AutoAction action, FieldLocation location, Alliance color) { public AutoAction getAction() { return action; } + public String getActionName() { + return action.name(); + } public FieldLocation getLocation() { return location; diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index 3bdae5c9..18e718a0 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -17,7 +17,8 @@ public enum FieldLocation { INVALID(-1, -1, -1, "INVALID"), DEPOT_SIDE(3.599, 7.33, 180, "Depot side - Driver Station LEFT"), //robot is 24 cm away from the wall MID(3.599, 4.029, 180, "Middle"), - OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"); //robot is 24 cm away from the wall + OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"), //robot is 24 cm away from the wall + FAST_DEPOT(3.596, 5.06, 180, "Depot side - for the fast auto"); //robot's back left corner is next to hopper corner private static final double RED_X_POS = 9.338; // meters public static final double HEIGHT_OF_FIELD = 8.043; diff --git a/src/main/java/frc/robot/commands/auto/AutoShoot.java b/src/main/java/frc/robot/commands/auto/AutoShoot.java index b80f8880..ec0ebfe4 100644 --- a/src/main/java/frc/robot/commands/auto/AutoShoot.java +++ b/src/main/java/frc/robot/commands/auto/AutoShoot.java @@ -1,6 +1,7 @@ package frc.robot.commands.auto; import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.hopper.AutoSpinHopper; import frc.robot.commands.hopper.SpinHopper; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -10,7 +11,7 @@ public class AutoShoot extends LoggableRaceCommandGroup{ public AutoShoot(HopperSubsystem hopperSubsystem, FeederSubsystem feederSubsystem, double time){ super( - new SpinHopper(hopperSubsystem), + new AutoSpinHopper(hopperSubsystem), new SpinFeeder(feederSubsystem), new LoggableWaitCommand(time) ); diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepotBlue.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepotBlue.java new file mode 100644 index 00000000..78d5af16 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepotBlue.java @@ -0,0 +1,47 @@ +package frc.robot.commands.auto.newauto; + +import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.auto.AutoReset; +import frc.robot.commands.auto.AutoShoot; +import frc.robot.commands.drive.DriveSwerve; +import frc.robot.commands.intakeDeployment.ToggleDeployment; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.DriveDirection; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.*; +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; +import frc.robot.utils.logging.commands.LoggableWaitCommand; + +public class FastDepotBlue extends LoggableSequentialCommandGroup { + public FastDepotBlue( + SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, + HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, + ControllerSubsystem controller, IntakeDeployerSubsystem intake) { + super( + new AutoReset(shootstate, turret, angler), + new SetShootingState(shootstate, ShootState.AUTO_AIM), + new LoggableParallelCommandGroup( + LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) + ), + new AutoShoot(hopper, feeder, 1), + new ToggleDeployment(intake, controller), + new LoggableParallelCommandGroup( + new AutoShoot(hopper, feeder, 20), + new LoggableSequentialCommandGroup( + new DriveSwerve(drivetrain, DriveDirection.FORWARD, 6, 0.2), + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 0.5, 0.5), + new DriveSwerve(drivetrain, DriveDirection.FORWARD, 7, 0.2), + new ToggleDeployment(intake, controller) + ) + ) + + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java new file mode 100644 index 00000000..47f9a8d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java @@ -0,0 +1,50 @@ +package frc.robot.commands.auto.newauto; + +import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.auto.AutoReset; +import frc.robot.commands.auto.AutoShoot; +import frc.robot.commands.drive.DriveSwerve; +import frc.robot.commands.intakeDeployment.ToggleDeployment; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.DriveDirection; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.*; +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; +import frc.robot.utils.logging.commands.LoggableWaitCommand; + +public class FastDepotRed extends LoggableSequentialCommandGroup { + public FastDepotRed( + SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, + HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, + ControllerSubsystem controller, IntakeDeployerSubsystem intake) { + super( + + new LoggableParallelCommandGroup( + new LoggableSequentialCommandGroup( + new AutoReset(shootstate, turret, angler), + new SetShootingState(shootstate, ShootState.AUTO_AIM) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) + ), + new AutoShoot(hopper, feeder, 1), + new ToggleDeployment(intake, controller), + new LoggableParallelCommandGroup( + new AutoShoot(hopper, feeder, 20), + new LoggableSequentialCommandGroup( + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 6, 0.2), + new DriveSwerve(drivetrain, DriveDirection.FORWARD, 0.5, 0.5), + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 7, 0.2), + new ToggleDeployment(intake, controller) + ) + ) + + ); + } +} diff --git a/src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java b/src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java new file mode 100644 index 00000000..ed325892 --- /dev/null +++ b/src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java @@ -0,0 +1,47 @@ +package frc.robot.commands.hopper; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class AutoSpinHopper extends LoggableCommand { + + public final HopperSubsystem subsystem; + public final Timer timer; + + + public AutoSpinHopper(HopperSubsystem subsystem){ + timer = new Timer(); + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + subsystem.setSpeed(Constants.HOPPER_AUTO_SPEED); + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + if (timer.hasElapsed(Constants.HOPPER_TIMEOUT)){ + return true; + } + else{ + return false; + } + + } + + +} diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index 4158e718..87dc24e5 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -57,6 +57,9 @@ public void execute() { case SHOOTING_HUB: lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE); break; + case AUTO_AIM: + lightStrip.setPattern(BlinkinPattern.RAINBOW_LAVA_PALETTE); + break; case SHUTTLING: lightStrip.setPattern(BlinkinPattern.GREEN); break; diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 78c19113..4eea8e9e 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick @@ -47,7 +47,8 @@ public enum Mode { //Speeds public static final double INTAKE_SPEED = -0.7; public static final double INTAKE_REVERSE_SPEED = 0.5; - public static final double HOPPER_SPEED = 0.75; + public static final double HOPPER_SPEED = 0.6; + public static final double HOPPER_AUTO_SPEED = 0.35; public static final double CLIMBER_SPEED_UP = 0.1; public static final double CLIMBER_SPEED_DOWN = -0.1; public static final double FEEDER_SPEED = 1; @@ -101,12 +102,12 @@ public enum Mode { public static final double ANGLER_FF = 0.0; public static final double ANGLER_HOME_ROTATIONS = 0.0; public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler - public static final double ANGLER_ENCODER_HIGH = 272; //Highest encoder position of Angler - public static final double ANGLER_ANGLE_LOW = 16; //Lowest angle position of Angler - public static final double ANGLER_ANGLE_HIGH = 37; //Highest angle position of Angler + public static final double ANGLER_ENCODER_HIGH = 265; //Highest encoder position of Angler + public static final double ANGLER_ANGLE_LOW = 17; //Lowest angle position of Angler + public static final double ANGLER_ANGLE_HIGH = 38; //Highest angle position of Angler public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState - public static final double ANGLER_LIMIT_SPEED = 0.3; + public static final double ANGLER_LIMIT_SPEED = 0.6; // turret (pan angle) PID @@ -121,8 +122,8 @@ public enum Mode { public static final double TURRET_ENCODER_MIN = 0; //Lowest encoder position of Turret public static final double TURRET_ENCODER_MAX = 77; //Highest encoder position of Turret public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward - public static final double TURRET_MIN_ANGLE = -92; - public static final double TURRET_MAX_ANGLE = 92; + public static final double TURRET_MIN_ANGLE = -96; + public static final double TURRET_MAX_ANGLE = 96; public static final double TURRET_LIMIT_SPEED = 0.2; public static final double TURRET_OUT_OF_RANGE_FLOP_RPM = -1500.0; public static final double TURRET_PID_DISTANCE_THRESHOLD = 10; diff --git a/src/main/java/frc/robot/constants/enums/ShootingState.java b/src/main/java/frc/robot/constants/enums/ShootingState.java index 18e1141c..b01aec87 100644 --- a/src/main/java/frc/robot/constants/enums/ShootingState.java +++ b/src/main/java/frc/robot/constants/enums/ShootingState.java @@ -8,7 +8,8 @@ public enum ShootState { FIXED, // Shoot from a known location FIXED_2, // Shoot from another known location SHOOTING_HUB, // Auto-aim into the hub - SHUTTLING // Auto-aim into alliance zone + SHUTTLING, // Auto-aim into alliance zone + AUTO_AIM //auto aim without hopper and feeder } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 4f4f7366..a37826d9 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -186,6 +186,15 @@ private void updateTargets(ShootState state, Pose2d robotPose) { useShotTargets(FIXED_TARGETS); } } + case AUTO_AIM ->{ if (Robot.allianceColor().isEmpty()) { + useShotTargets(FIXED_TARGETS); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { + useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose))); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { + useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose))); + } else { + useShotTargets(FIXED_TARGETS); + }} } } @@ -235,7 +244,7 @@ private void useShotTargets(ShotTargets shotTargets) { false, activeTargets.intakeDeploy); - } + } } @@ -249,14 +258,14 @@ private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile double anglerAngleDegrees = calculateAnglerAngleDegrees(state, computedDistanceMeters, profile); double shooterVelocity = calculateShooterVelocity(state, computedDistanceMeters, profile); double turretAngleDegrees = calculateTurretAngleDegrees(state, robotPose, profile); - return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, - true, true); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, state != ShootState.AUTO_AIM, + state != ShootState.AUTO_AIM, true); } private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d targetPose) { double distance = robotPose.getTranslation() .getDistance(targetPose.getTranslation()); - if(state == ShootState.SHOOTING_HUB){ + if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM){ if (distance > Constants.MAX_HUB_DISTANCE) { return Constants.MAX_HUB_DISTANCE; } else if (distance < Constants.MIN_HUB_DISTANCE) { @@ -289,7 +298,7 @@ private double calculateFlightTime(double computedDistanceMeters) { } private double calculateAnglerAngleDegrees(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { - if (state == ShootState.SHOOTING_HUB) { + if (state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM) { double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; return 0.169 * distance * distance - 1.73 * distance @@ -300,10 +309,11 @@ private double calculateAnglerAngleDegrees(ShootState state, double computedDist private double calculateShooterVelocity(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; - if (state == ShootState.SHOOTING_HUB) { - return (8.46 * distance * distance - - 237 * distance - - 1380); + if (state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM) { + return (-0.583675*distance*distance*distance + +20.18291*distance*distance + -261.37309*distance + -1324.19278) * 1.05; }else if(state == ShootState.SHUTTLING){ return (((-distance*distance) - 5 * distance) - 2800); } @@ -311,7 +321,7 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc } private double calculateTurretAngleDegrees(ShootState state, Pose2d robotPose, PoseControlProfile profile) { - if(state == ShootState.SHOOTING_HUB || state == ShootState.SHUTTLING){ + if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM || state == ShootState.SHUTTLING){ return Math.floor( Math.toDegrees(TurretCalculations.calculateTurretAngle(state,robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index ce98ced9..77885f93 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -50,7 +50,7 @@ public static double calculateTurretAngle(ShootState state, double robotPosX, do double hubPosX; double hubPosY; - if(state == ShootState.SHOOTING_HUB) { + if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM) { if (isBlueAlliance) { // hub position determined by which alliance robot is on hubPosX = Constants.BLUE_HUB_X_POSITION; @@ -62,7 +62,7 @@ public static double calculateTurretAngle(ShootState state, double robotPosX, do }else{ //Sets the hubPosX to be either on the blue or red side //Sets the hubPoseY to be lower or upper depending on the Y position of the robot - hubPosX = isBlueAlliance ? Constants.BLUE_HUB_X_POSITION : Constants.RED_HUB_X_POSITION; + hubPosX = isBlueAlliance ? Constants.BLUE_SHUTTLING_TARGET_X_POSITION : Constants.RED_SHUTTLING_TARGET_X_POSITION; hubPosY = robotPosY > (Constants.FIELD_WIDTH / 2) ? Constants.SHUTTLING_TARGET_HIGHER_Y_POSITION : Constants.SHUTTLING_TARGET_LOWER_Y_POSITION; } /* diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json index 322c9e20..48751078 100644 --- a/vendordeps/ChoreoLib2026.json +++ b/vendordeps/ChoreoLib2026.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2026.json", "name": "ChoreoLib", - "version": "2026.0.1", + "version": "2026.0.2", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2026.0.1" + "version": "2026.0.2" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false,