diff --git a/.vscode/settings.json b/.vscode/settings.json index 1ff515e7..dedb8217 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -28,5 +28,6 @@ "java.test.defaultConfig": "WPIlibUnitTests", "java.compile.nullAnalysis.mode": "automatic", "wpilib.autoStartRioLog": true, + "java.debug.settings.onBuildFailureProceed": true, "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/auto1.chor b/auto1.chor new file mode 100644 index 00000000..0586a8bc --- /dev/null +++ b/auto1.chor @@ -0,0 +1,2018 @@ +{ + "version": "v0.2.1", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 1.162295081967213, + "motorMaxVelocity": 4800, + "gearing": 6.75, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "auto1": { + "waypoints": [ + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.2676985263824463, + "y": 4.124765396118164, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 1.3321385383605957, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 2.2287168502807617, + "y": 5.567087173461914, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": -3.1411853071795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 2.2676985263824463, + "y": 7.028899669647217, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 1.332138180732727, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 38 + }, + { + "x": 7.686149597167969, + "y": 7.496679306030273, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 38 + }, + { + "x": 1.3321380615234375, + "y": 5.547595977783203, + "heading": -3.1411853071795863, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": 0, + "angularVelocity": 0, + "velocityX": -3.4724754769815887e-38, + "velocityY": 7.030705743146232e-38, + "timestamp": 0 + }, + { + "x": 1.361410083208431, + "y": 5.5023955382692575, + "heading": 0.056119311202146825, + "angularVelocity": 0.6842584112779871, + "velocityX": 0.35690567728254624, + "velocityY": -0.5511312692222103, + "timestamp": 0.08201479189321614 + }, + { + "x": 1.4202660843784345, + "y": 5.411892088977607, + "heading": 0.16553764559038536, + "angularVelocity": 1.3341292693969398, + "velocityX": 0.7176266599156221, + "velocityY": -1.1035015416424676, + "timestamp": 0.1640295837864323 + }, + { + "x": 1.509321709738084, + "y": 5.2758955282995315, + "heading": 0.32143534479454616, + "angularVelocity": 1.9008485616489859, + "velocityX": 1.0858483366707872, + "velocityY": -1.658195522280223, + "timestamp": 0.24604437567964843 + }, + { + "x": 1.6296766101338032, + "y": 5.093983916449887, + "heading": 0.5046862665772869, + "angularVelocity": 2.2343642842054003, + "velocityX": 1.4674779709545838, + "velocityY": -2.2180341820106597, + "timestamp": 0.3280591675728646 + }, + { + "x": 1.782194657966092, + "y": 4.869104821237669, + "heading": 0.6276214122511846, + "angularVelocity": 1.4989387016181202, + "velocityX": 1.859640734452738, + "velocityY": -2.7419333759331184, + "timestamp": 0.4100739594660807 + }, + { + "x": 1.9333417085303153, + "y": 4.6354174904379, + "heading": 0.6920907561549033, + "angularVelocity": 0.7860697127374073, + "velocityX": 1.8429242710390283, + "velocityY": -2.849331509662689, + "timestamp": 0.49208875135929686 + }, + { + "x": 2.056917521349593, + "y": 4.443493094854259, + "heading": 0.6585466805605577, + "angularVelocity": -0.40900031348028376, + "velocityX": 1.5067503064590375, + "velocityY": -2.340119277916704, + "timestamp": 0.574103543252513 + }, + { + "x": 2.1535962335212764, + "y": 4.293727999952354, + "heading": 0.5265725883978299, + "angularVelocity": -1.609149875482936, + "velocityX": 1.1787960432498548, + "velocityY": -1.8260741927736632, + "timestamp": 0.6561183351457291 + }, + { + "x": 2.2240985983067136, + "y": 4.187111119253089, + "heading": 0.3017485783287344, + "angularVelocity": -2.7412617270530677, + "velocityX": 0.859629868685535, + "velocityY": -1.2999713617280337, + "timestamp": 0.7381331270389453 + }, + { + "x": 2.2676985263824463, + "y": 4.124765396118164, + "heading": -1.6642026159430915e-35, + "angularVelocity": -3.6791970248685537, + "velocityX": 0.5316105432846836, + "velocityY": -0.7601765693205621, + "timestamp": 0.8201479189321614 + }, + { + "x": 2.2826958672458773, + "y": 4.104348331421266, + "heading": -0.17441872129385144, + "angularVelocity": -4.1236397648607985, + "velocityX": 0.35456991481679806, + "velocityY": -0.4827040310886087, + "timestamp": 0.8624451930006055 + }, + { + "x": 2.289905521300629, + "y": 4.095598364764115, + "heading": -0.366838925198976, + "angularVelocity": -4.549234156171776, + "velocityX": 0.17045197860942923, + "velocityY": -0.20686833489535802, + "timestamp": 0.9047424670690496 + }, + { + "x": 2.289262823237601, + "y": 4.0986035079624115, + "heading": -0.5765637041805302, + "angularVelocity": -4.958352130262211, + "velocityX": -0.015194786831599966, + "velocityY": 0.07104815297159969, + "timestamp": 0.9470397411374937 + }, + { + "x": 2.28071834356103, + "y": 4.1136177509731775, + "heading": -0.8009025976234118, + "angularVelocity": -5.303861735388993, + "velocityX": -0.20201017358104575, + "velocityY": 0.3549695185195792, + "timestamp": 0.9893370152059378 + }, + { + "x": 2.264115058699565, + "y": 4.140907151677903, + "heading": -1.0347556438013104, + "angularVelocity": -5.528797099299715, + "velocityX": -0.39253794073343345, + "velocityY": 0.6451810738575218, + "timestamp": 1.0316342892743817 + }, + { + "x": 2.2392828245472374, + "y": 4.180616635647853, + "heading": -1.271268857234422, + "angularVelocity": -5.5916892670292055, + "velocityX": -0.5870882864022154, + "velocityY": 0.9388189864361984, + "timestamp": 1.0739315633428257 + }, + { + "x": 2.206219336908872, + "y": 4.232592488850575, + "heading": -1.500368652073653, + "angularVelocity": -5.416419849385782, + "velocityX": -0.7816931082807688, + "velocityY": 1.228822763344421, + "timestamp": 1.1162288374112697 + }, + { + "x": 2.16560115012261, + "y": 4.295896098840919, + "heading": -1.7081840928578422, + "angularVelocity": -4.913211202403, + "velocityX": -0.9603027069908828, + "velocityY": 1.496635690704529, + "timestamp": 1.1585261114797136 + }, + { + "x": 2.116418529309532, + "y": 4.368321186816809, + "heading": -1.8863054196825848, + "angularVelocity": -4.211177451684291, + "velocityX": -1.162784645022098, + "velocityY": 1.7122873653439958, + "timestamp": 1.2008233855481576 + }, + { + "x": 2.058912682392385, + "y": 4.450563139928545, + "heading": -2.036216963298125, + "angularVelocity": -3.54423652391779, + "velocityX": -1.3595639005977715, + "velocityY": 1.9443795119906613, + "timestamp": 1.2431206596166016 + }, + { + "x": 1.99337005684811, + "y": 4.542961602948437, + "heading": -2.1579434366118924, + "angularVelocity": -2.877879863293171, + "velocityX": -1.549570911785386, + "velocityY": 2.184501603350988, + "timestamp": 1.2854179336850455 + }, + { + "x": 1.9201828437796038, + "y": 4.645652670813241, + "heading": -2.2501769997399386, + "angularVelocity": -2.180603009517744, + "velocityX": -1.7303056681637912, + "velocityY": 2.4278412764527704, + "timestamp": 1.3277152077534895 + }, + { + "x": 1.8398838191629743, + "y": 4.758671823925337, + "heading": -2.3107451299036645, + "angularVelocity": -1.4319629691907945, + "velocityX": -1.8984444360809696, + "velocityY": 2.6720197838095423, + "timestamp": 1.3700124818219335 + }, + { + "x": 1.753396216406846, + "y": 4.882199443808802, + "heading": -2.337574910152261, + "angularVelocity": -0.6343146417705776, + "velocityX": -2.0447559484844615, + "velocityY": 2.920462904620659, + "timestamp": 1.4123097558903774 + }, + { + "x": 1.6751428992292787, + "y": 4.996901240775032, + "heading": -2.400008661282964, + "angularVelocity": -1.4760703261792811, + "velocityX": -1.8500794413119903, + "velocityY": 2.7118011619525055, + "timestamp": 1.4546070299588214 + }, + { + "x": 1.604167639142011, + "y": 5.102445488685979, + "heading": -2.5005028957452873, + "angularVelocity": -2.375903333621606, + "velocityX": -1.6780102654468432, + "velocityY": 2.4952966883908108, + "timestamp": 1.4969043040272654 + }, + { + "x": 1.5418087398499432, + "y": 5.19740105341742, + "heading": -2.6238103064581355, + "angularVelocity": -2.9152566785584497, + "velocityX": -1.4743006651246806, + "velocityY": 2.2449570763776965, + "timestamp": 1.5392015780957093 + }, + { + "x": 1.488225747512813, + "y": 5.280327915096259, + "heading": -2.743877445785595, + "angularVelocity": -2.8386495813694843, + "velocityX": -1.2668190448969345, + "velocityY": 1.960572247389977, + "timestamp": 1.5814988521641533 + }, + { + "x": 1.4429482417442723, + "y": 5.35126150411709, + "heading": -2.8526923539591857, + "angularVelocity": -2.5726222450531995, + "velocityX": -1.0704591907098815, + "velocityY": 1.6770250703638487, + "timestamp": 1.6237961262325973 + }, + { + "x": 1.4056282761179824, + "y": 5.410288137316586, + "heading": -2.94638853961619, + "angularVelocity": -2.215182602675249, + "velocityX": -0.8823255504811011, + "velocityY": 1.3955186120027623, + "timestamp": 1.6660934003010412 + }, + { + "x": 1.3760265384921018, + "y": 5.457458867956814, + "heading": -3.023001318784294, + "angularVelocity": -1.8112935373596752, + "velocityX": -0.6998497723040017, + "velocityY": 1.115219164334259, + "timestamp": 1.7083906743694852 + }, + { + "x": 1.3539833473819511, + "y": 5.492803685860998, + "heading": -3.0814563447777146, + "angularVelocity": -1.3820045683991566, + "velocityX": -0.5211492134098515, + "velocityY": 0.8356287416297835, + "timestamp": 1.7506879484379292 + }, + { + "x": 1.3393859043766252, + "y": 5.516345418500216, + "heading": -3.121051917122202, + "angularVelocity": -0.9361258666554966, + "velocityX": -0.3451154554713123, + "velocityY": 0.5565780102311921, + "timestamp": 1.7929852225063732 + }, + { + "x": 1.3321385383605957, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": -0.47599734263737736, + "velocityX": -0.17134357179382342, + "velocityY": 0.2780283292584069, + "timestamp": 1.8352824965748171 + }, + { + "x": 1.3321385383605957, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": 1.0972891859651771e-35, + "velocityX": 2.010813957988482e-36, + "velocityY": -7.697026441410569e-35, + "timestamp": 1.877579770643261 + }, + { + "x": 1.337370959637994, + "y": 5.5282378526253355, + "heading": -3.1057781238593085, + "angularVelocity": 1.0527517345397082, + "velocityX": 0.1555740970920068, + "velocityY": 0.003942370372304683, + "timestamp": 1.9112127556658494 + }, + { + "x": 1.3479554232342674, + "y": 5.5287097500206475, + "heading": -3.0355501553802027, + "angularVelocity": 2.0880682589410555, + "velocityX": 0.314704852666657, + "velocityY": 0.014030791349476537, + "timestamp": 1.9448457406884376 + }, + { + "x": 1.364027106419543, + "y": 5.529829382282991, + "heading": -2.9313355116530078, + "angularVelocity": 3.0985844300529233, + "velocityX": 0.4778547956561592, + "velocityY": 0.03328970835004707, + "timestamp": 1.978478725711026 + }, + { + "x": 1.3857940218160676, + "y": 5.531991335292171, + "heading": -2.7946612849381784, + "angularVelocity": 4.06369600031153, + "velocityX": 0.6471895189173892, + "velocityY": 0.06428073534739039, + "timestamp": 2.012111710733614 + }, + { + "x": 1.4139333100804088, + "y": 5.535564467259386, + "heading": -2.6300339585531556, + "angularVelocity": 4.89481758084981, + "velocityX": 0.8366574731752962, + "velocityY": 0.10623891887132897, + "timestamp": 2.0457446957562024 + }, + { + "x": 1.4504189067700664, + "y": 5.5389912877920775, + "heading": -2.4501533377113245, + "angularVelocity": 5.348339456667967, + "velocityX": 1.0848158932415233, + "velocityY": 0.10188868250590417, + "timestamp": 2.0793776807787907 + }, + { + "x": 1.4957426020786804, + "y": 5.541383162087915, + "heading": -2.2591276091627437, + "angularVelocity": 5.679713781583368, + "velocityX": 1.3475965715851275, + "velocityY": 0.07111691972124916, + "timestamp": 2.113010665801379 + }, + { + "x": 1.549887277450098, + "y": 5.541604080123591, + "heading": -2.077370178961034, + "angularVelocity": 5.404142096802884, + "velocityX": 1.6098682687562116, + "velocityY": 0.006568493267186675, + "timestamp": 2.1466436508239672 + }, + { + "x": 1.612403659002375, + "y": 5.539682953328213, + "heading": -1.9100118188035742, + "angularVelocity": 4.9760186330490965, + "velocityX": 1.8587818330811392, + "velocityY": -0.05712031786913091, + "timestamp": 2.1802766358465555 + }, + { + "x": 1.6835678333886066, + "y": 5.53654594415181, + "heading": -1.7561585727792173, + "angularVelocity": 4.5744749067330135, + "velocityX": 2.1159042035203575, + "velocityY": -0.0932718036860474, + "timestamp": 2.213909620869144 + }, + { + "x": 1.7632443244228584, + "y": 5.534877661418315, + "heading": -1.616595649842294, + "angularVelocity": 4.149584785388266, + "velocityX": 2.3689984989658393, + "velocityY": -0.04960257712408059, + "timestamp": 2.247542605891732 + }, + { + "x": 1.845517380604842, + "y": 5.542129045770498, + "heading": -1.4828842082324165, + "angularVelocity": 3.9756043515042103, + "velocityX": 2.4462014337035023, + "velocityY": 0.21560335329479804, + "timestamp": 2.2811755909143203 + }, + { + "x": 1.9198078929781421, + "y": 5.552688491598465, + "heading": -1.3358148203184805, + "angularVelocity": 4.372772378519569, + "velocityX": 2.208858723761989, + "velocityY": 0.31396100646060415, + "timestamp": 2.3148085759369086 + }, + { + "x": 1.9854932228404973, + "y": 5.561629411594934, + "heading": -1.1756047034069004, + "angularVelocity": 4.763481945000775, + "velocityX": 1.9530032739657306, + "velocityY": 0.26583783718469556, + "timestamp": 2.348441560959497 + }, + { + "x": 2.0429500865174064, + "y": 5.56779505699511, + "heading": -1.0015843737719388, + "angularVelocity": 5.1740970811270035, + "velocityX": 1.7083486237787309, + "velocityY": 0.18332138512343155, + "timestamp": 2.382074545982085 + }, + { + "x": 2.0926336267477486, + "y": 5.570712245735625, + "heading": -0.8117369336213589, + "angularVelocity": 5.644680066996039, + "velocityX": 1.477226603495769, + "velocityY": 0.08673594504191894, + "timestamp": 2.4157075310046734 + }, + { + "x": 2.1331736827499013, + "y": 5.571318223980459, + "heading": -0.6223345825199257, + "angularVelocity": 5.63144636059598, + "velocityX": 1.2053659814888902, + "velocityY": 0.01801737920159694, + "timestamp": 2.4493405160272617 + }, + { + "x": 2.1650842656051874, + "y": 5.570653927593427, + "heading": -0.4456863837750604, + "angularVelocity": 5.252230767689132, + "velocityX": 0.9487883051074633, + "velocityY": -0.01975133597523799, + "timestamp": 2.48297350104985 + }, + { + "x": 2.1896037980440752, + "y": 5.569969320542384, + "heading": -0.29224766364008553, + "angularVelocity": 4.56214992608965, + "velocityX": 0.7290323003569462, + "velocityY": -0.020355227184960373, + "timestamp": 2.516606486072438 + }, + { + "x": 2.20780773019257, + "y": 5.569409848980837, + "heading": -0.16876882632138132, + "angularVelocity": 3.6713612317127033, + "velocityX": 0.5412523490337908, + "velocityY": -0.016634609184155776, + "timestamp": 2.5502394710950265 + }, + { + "x": 2.220213327560089, + "y": 5.568812984785934, + "heading": -0.07803807473080998, + "angularVelocity": 2.697671691336225, + "velocityX": 0.368852106323223, + "velocityY": -0.017746393741217954, + "timestamp": 2.5838724561176147 + }, + { + "x": 2.227117258661317, + "y": 5.568051315056596, + "heading": -0.021486347391680306, + "angularVelocity": 1.6814364618885036, + "velocityX": 0.2052726243772784, + "velocityY": -0.022646509931434324, + "timestamp": 2.617505441140203 + }, + { + "x": 2.2287168502807617, + "y": 5.567087173461914, + "heading": 6.38149991346766e-37, + "angularVelocity": 0.6388474700431709, + "velocityX": 0.047560203721742585, + "velocityY": -0.02866654845040371, + "timestamp": 2.6511384261627913 + }, + { + "x": 2.2247758271699722, + "y": 5.5659373744789455, + "heading": -0.016384469566950412, + "angularVelocity": -0.4672993433012807, + "velocityX": -0.11240141184197158, + "velocityY": -0.03279326849576676, + "timestamp": 2.686200468945049 + }, + { + "x": 2.215132755191586, + "y": 5.564807940629173, + "heading": -0.07111277017786721, + "angularVelocity": -1.560898803038675, + "velocityX": -0.27502881216225394, + "velocityY": -0.03221243715853021, + "timestamp": 2.7212625117273066 + }, + { + "x": 2.199681422128445, + "y": 5.563984363308005, + "heading": -0.16355266498461823, + "angularVelocity": -2.6364663171744263, + "velocityX": -0.4406854774291637, + "velocityY": -0.023489142554608954, + "timestamp": 2.7563245545095643 + }, + { + "x": 2.178275880729617, + "y": 5.563860339253889, + "heading": -0.2925810137194886, + "angularVelocity": -3.6800008925938044, + "velocityX": -0.6105046854160964, + "velocityY": -0.0035372740511740697, + "timestamp": 2.791386597291822 + }, + { + "x": 2.150434792823077, + "y": 5.56488104971689, + "heading": -0.45485951159806126, + "angularVelocity": -4.628324107820951, + "velocityX": -0.7940520773258658, + "velocityY": 0.029111551467175232, + "timestamp": 2.8264486400740796 + }, + { + "x": 2.1136927979170794, + "y": 5.564806154665508, + "heading": -0.6342616510080993, + "angularVelocity": -5.116705279386068, + "velocityX": -1.047913697846207, + "velocityY": -0.0021360721007366726, + "timestamp": 2.8615106828563373 + }, + { + "x": 2.0677952991091333, + "y": 5.562722960469238, + "heading": -0.8285761603492735, + "angularVelocity": -5.542019058841107, + "velocityX": -1.3090366437853844, + "velocityY": -0.05941451298794179, + "timestamp": 2.896572725638595 + }, + { + "x": 2.0120787764368706, + "y": 5.557776466918295, + "heading": -1.020468287532569, + "angularVelocity": -5.472930609747536, + "velocityX": -1.5890837569925207, + "velocityY": -0.14107830458316797, + "timestamp": 2.9316347684208526 + }, + { + "x": 1.947346732087487, + "y": 5.549990941392819, + "heading": -1.1972983758508349, + "angularVelocity": -5.043348141932757, + "velocityX": -1.8462142879518657, + "velocityY": -0.22204996935933435, + "timestamp": 2.9666968112031102 + }, + { + "x": 1.8731816133340355, + "y": 5.541067205657786, + "heading": -1.3595832736612468, + "angularVelocity": -4.628506639451492, + "velocityX": -2.1152537863818077, + "velocityY": -0.25451271594333, + "timestamp": 3.001758853985368 + }, + { + "x": 1.7903438797008602, + "y": 5.535778355661316, + "heading": -1.5070368489944903, + "angularVelocity": -4.2055044039778195, + "velocityX": -2.362604316799635, + "velocityY": -0.15084260860996357, + "timestamp": 3.0368208967676256 + }, + { + "x": 1.705207783905628, + "y": 5.540259742780199, + "heading": -1.6469795594362229, + "angularVelocity": -3.991287995134942, + "velocityX": -2.4281556075880766, + "velocityY": 0.12781306402233675, + "timestamp": 3.0718829395498832 + }, + { + "x": 1.629345764073995, + "y": 5.546176174189709, + "heading": -1.802120558201536, + "angularVelocity": -4.424756416184017, + "velocityX": -2.1636508831716217, + "velocityY": 0.16874177714778527, + "timestamp": 3.106944982332141 + }, + { + "x": 1.5628215543297233, + "y": 5.550494777199676, + "heading": -1.9719374235468123, + "angularVelocity": -4.843324914063712, + "velocityX": -1.8973284060315727, + "velocityY": 0.12317031944734674, + "timestamp": 3.1420070251143986 + }, + { + "x": 1.5052956910676978, + "y": 5.552468250746859, + "heading": -2.1578472190724005, + "angularVelocity": -5.302309300120524, + "velocityX": -1.6406877265900472, + "velocityY": 0.056285184506758255, + "timestamp": 3.177069067896656 + }, + { + "x": 1.4562451550739939, + "y": 5.551766768072451, + "heading": -2.3624196209535056, + "angularVelocity": -5.834583088941523, + "velocityX": -1.3989640106914938, + "velocityY": -0.02000689688175958, + "timestamp": 3.212131110678914 + }, + { + "x": 1.4173942622657685, + "y": 5.550106751106204, + "heading": -2.5657566721184293, + "angularVelocity": -5.799349810497013, + "velocityX": -1.1080613029165842, + "velocityY": -0.047345129790485885, + "timestamp": 3.2471931534611715 + }, + { + "x": 1.3874992845964569, + "y": 5.548698391964879, + "heading": -2.750369265628105, + "angularVelocity": -5.26531196873374, + "velocityX": -0.852630802345583, + "velocityY": -0.0401676294239809, + "timestamp": 3.282255196243429 + }, + { + "x": 1.3647199881577643, + "y": 5.548009174125998, + "heading": -2.903871904539791, + "angularVelocity": -4.378028966109259, + "velocityX": -0.6496853757254475, + "velocityY": -0.019657093089571077, + "timestamp": 3.317317239025687 + }, + { + "x": 1.3481785543906255, + "y": 5.547743770300603, + "heading": -3.0214740080658498, + "angularVelocity": -3.354114426714698, + "velocityX": -0.4717760990101023, + "velocityY": -0.007569548273095167, + "timestamp": 3.3523792818079445 + }, + { + "x": 1.3374168488281284, + "y": 5.547647789865521, + "heading": -3.10100248389671, + "angularVelocity": -2.268221401837531, + "velocityX": -0.30693321633680876, + "velocityY": -0.0027374456097331258, + "timestamp": 3.387441324590202 + }, + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": -3.1411853071795863, + "angularVelocity": -1.1460491201958698, + "velocityX": -0.15054201206449694, + "velocityY": -0.001464125906141093, + "timestamp": 3.42250336737246 + }, + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": -3.1411853071795863, + "angularVelocity": -1.1877010663188592e-33, + "velocityX": -2.3472123166291743e-34, + "velocityY": -4.828629969149868e-35, + "timestamp": 3.4575654101547175 + }, + { + "x": 1.3353773426968816, + "y": 5.556976212978418, + "heading": -3.1027759330153977, + "angularVelocity": 0.9418411865422114, + "velocityX": 0.07941913622507528, + "velocityY": 0.2300022568362304, + "timestamp": 3.4983465680749837 + }, + { + "x": 1.342768335196551, + "y": 5.576502155287014, + "heading": -3.0330086785474775, + "angularVelocity": 1.7107717883912714, + "velocityX": 0.18123547433645554, + "velocityY": 0.47879813385321396, + "timestamp": 3.53912772599525 + }, + { + "x": 1.357543783583743, + "y": 5.605477021924564, + "heading": -2.939156767804727, + "angularVelocity": 2.301354731669143, + "velocityX": 0.3623106635687124, + "velocityY": 0.7104964183263536, + "timestamp": 3.579908883915516 + }, + { + "x": 1.3800861680237329, + "y": 5.643206466545064, + "heading": -2.8183134070613347, + "angularVelocity": 2.9632155364411483, + "velocityX": 0.5527646979535052, + "velocityY": 0.9251685470595896, + "timestamp": 3.620690041835782 + }, + { + "x": 1.410132685953878, + "y": 5.690126983565226, + "heading": -2.6706561536339386, + "angularVelocity": 3.6207224354956073, + "velocityX": 0.7367745170181611, + "velocityY": 1.1505440113274845, + "timestamp": 3.6614711997560483 + }, + { + "x": 1.447426110042683, + "y": 5.747148126734536, + "heading": -2.4998618512700794, + "angularVelocity": 4.188068977781135, + "velocityX": 0.9144768317201708, + "velocityY": 1.3982227596576835, + "timestamp": 3.7022523576763144 + }, + { + "x": 1.4919514039971702, + "y": 5.815443990853687, + "heading": -2.3159167566388894, + "angularVelocity": 4.510541240413857, + "velocityX": 1.0918104395549988, + "velocityY": 1.6746916370712401, + "timestamp": 3.7430335155965806 + }, + { + "x": 1.543306542439651, + "y": 5.895557258888599, + "heading": -2.140840879075951, + "angularVelocity": 4.293058031977455, + "velocityX": 1.259285931578716, + "velocityY": 1.9644677130440258, + "timestamp": 3.7838146735168467 + }, + { + "x": 1.5991978574253154, + "y": 5.9852989254543125, + "heading": -2.0026162516135204, + "angularVelocity": 3.389423805294663, + "velocityX": 1.3705180979642946, + "velocityY": 2.2005669074226377, + "timestamp": 3.824595831437113 + }, + { + "x": 1.6602838186413746, + "y": 6.084712915583529, + "heading": -1.8987034819220077, + "angularVelocity": 2.5480583433820057, + "velocityX": 1.497896683941456, + "velocityY": 2.4377431931576705, + "timestamp": 3.865376989357379 + }, + { + "x": 1.7272241659282512, + "y": 6.1936323149644315, + "heading": -1.8270894144503607, + "angularVelocity": 1.7560577267488233, + "velocityX": 1.6414528350998836, + "velocityY": 2.670826551660404, + "timestamp": 3.906158147277645 + }, + { + "x": 1.8006198616667228, + "y": 6.311769110233653, + "heading": -1.7863461946081067, + "angularVelocity": 0.9990697155268056, + "velocityX": 1.7997452618185266, + "velocityY": 2.8968474975673533, + "timestamp": 3.9469393051979114 + }, + { + "x": 1.8749454151333007, + "y": 6.423903525675366, + "heading": -1.7351071665224345, + "angularVelocity": 1.2564387746383576, + "velocityX": 1.8225464223428127, + "velocityY": 2.7496623725337597, + "timestamp": 3.9877204631181775 + }, + { + "x": 1.9436310828301862, + "y": 6.525803671980358, + "heading": -1.6558915421182487, + "angularVelocity": 1.9424564785302534, + "velocityX": 1.6842500605592803, + "velocityY": 2.4987065473772168, + "timestamp": 4.028501621038443 + }, + { + "x": 2.0070825887496224, + "y": 6.617181897860731, + "heading": -1.5496488374577189, + "angularVelocity": 2.6051909773688204, + "velocityX": 1.5559025087883662, + "velocityY": 2.240697188123809, + "timestamp": 4.069282778958709 + }, + { + "x": 2.0657895498392027, + "y": 6.697945113539011, + "heading": -1.416445908140072, + "angularVelocity": 3.2662861014903286, + "velocityX": 1.4395609169401762, + "velocityY": 1.9804051625063046, + "timestamp": 4.1100639368789755 + }, + { + "x": 2.1168235029971925, + "y": 6.770262031139991, + "heading": -1.2545233571884964, + "angularVelocity": 3.9705236243698976, + "velocityX": 1.2514101060536296, + "velocityY": 1.7732924048496255, + "timestamp": 4.150845094799242 + }, + { + "x": 2.160098891028687, + "y": 6.834337445285054, + "heading": -1.0613678161788878, + "angularVelocity": 4.736391776497875, + "velocityX": 1.0611613362255508, + "velocityY": 1.571201442350923, + "timestamp": 4.191626252719508 + }, + { + "x": 2.1954531720383796, + "y": 6.890659821255424, + "heading": -0.8342118106482137, + "angularVelocity": 5.570121524621797, + "velocityX": 0.8669268557507981, + "velocityY": 1.3810881996163518, + "timestamp": 4.232407410639774 + }, + { + "x": 2.2216225780243426, + "y": 6.9367359795772305, + "heading": -0.6132278215539244, + "angularVelocity": 5.418776718560821, + "velocityX": 0.6417033581324141, + "velocityY": 1.1298393834695264, + "timestamp": 4.27318856856004 + }, + { + "x": 2.2400584030513464, + "y": 6.973023548052357, + "heading": -0.41519671142857095, + "angularVelocity": 4.855946231652809, + "velocityX": 0.4520672282785309, + "velocityY": 0.889812117303642, + "timestamp": 4.313969726480306 + }, + { + "x": 2.252392360052711, + "y": 6.999798504355172, + "heading": -0.24838728557433673, + "angularVelocity": 4.0903553101747345, + "velocityX": 0.3024425403878878, + "velocityY": 0.6565521350611045, + "timestamp": 4.3547508844005725 + }, + { + "x": 2.2601470415559044, + "y": 7.01732009026677, + "heading": -0.11829622013640562, + "angularVelocity": 3.189979688469885, + "velocityX": 0.19015353900334692, + "velocityY": 0.4296490537580049, + "timestamp": 4.395532042320839 + }, + { + "x": 2.2649306384373356, + "y": 7.026470873344902, + "heading": -0.032599297392582494, + "angularVelocity": 2.101385225779393, + "velocityX": 0.11729919220988534, + "velocityY": 0.2243875246510465, + "timestamp": 4.436313200241105 + }, + { + "x": 2.2676985263824463, + "y": 7.028899669647217, + "heading": 1.4623510884357687e-33, + "angularVelocity": 0.799371549388559, + "velocityX": 0.06787173504299782, + "velocityY": 0.05955682541099079, + "timestamp": 4.477094358161371 + }, + { + "x": 2.268447419698579, + "y": 7.024856278106883, + "heading": -0.027481442707952487, + "angularVelocity": -0.643111881448138, + "velocityX": 0.017525360464520197, + "velocityY": -0.09462214806441192, + "timestamp": 4.5198263360655435 + }, + { + "x": 2.2661447146026696, + "y": 7.012526995839087, + "heading": -0.10890493556394686, + "angularVelocity": -1.9054463858094515, + "velocityX": -0.05388716387229158, + "velocityY": -0.28852589729042577, + "timestamp": 4.562558313969716 + }, + { + "x": 2.259237652200008, + "y": 6.990956553960971, + "heading": -0.23735745507492811, + "angularVelocity": -3.0060045383117964, + "velocityX": -0.16163685233926556, + "velocityY": -0.504784541602271, + "timestamp": 4.605290291873889 + }, + { + "x": 2.2461382818263638, + "y": 6.959794967044901, + "heading": -0.4073842771005642, + "angularVelocity": -3.9789129912714607, + "velocityX": -0.30654725140549266, + "velocityY": -0.7292334323009891, + "timestamp": 4.648022269778061 + }, + { + "x": 2.225216379141952, + "y": 6.918625356851511, + "heading": -0.6110307557476551, + "angularVelocity": -4.765669380054774, + "velocityX": -0.48960763602681573, + "velocityY": -0.9634379734473075, + "timestamp": 4.690754247682234 + }, + { + "x": 2.1950172148844427, + "y": 6.866427702804465, + "heading": -0.8329684676174923, + "angularVelocity": -5.193714935628281, + "velocityX": -0.706711126857547, + "velocityY": -1.2215127079794852, + "timestamp": 4.7334862255864065 + }, + { + "x": 2.1547332634682537, + "y": 6.802939586567042, + "heading": -1.0490897028487878, + "angularVelocity": -5.057599620498584, + "velocityX": -0.9427120716604103, + "velocityY": -1.4857284720074833, + "timestamp": 4.776218203490579 + }, + { + "x": 2.1052130726477585, + "y": 6.731031156240311, + "heading": -1.2322360836735529, + "angularVelocity": -4.285932685715518, + "velocityX": -1.1588555748939446, + "velocityY": -1.6827779535033027, + "timestamp": 4.818950181394752 + }, + { + "x": 2.046678257521749, + "y": 6.650499615743051, + "heading": -1.3842475177981566, + "angularVelocity": -3.557322679177027, + "velocityX": -1.3698129128793415, + "velocityY": -1.8845732036521736, + "timestamp": 4.861682159298924 + }, + { + "x": 1.9824374188049456, + "y": 6.55859622988028, + "heading": -1.505962481707897, + "angularVelocity": -2.848334429608893, + "velocityX": -1.5033434413184743, + "velocityY": -2.150693470563594, + "timestamp": 4.904414137203097 + }, + { + "x": 1.9120980242519563, + "y": 6.455385209029271, + "heading": -1.5975173206853661, + "angularVelocity": -2.1425368884815836, + "velocityX": -1.6460598830863262, + "velocityY": -2.415311106882586, + "timestamp": 4.9471461151072695 + }, + { + "x": 1.835418260391466, + "y": 6.341055354736124, + "heading": -1.6582681711046532, + "angularVelocity": -1.4216718579121885, + "velocityX": -1.7944351659182955, + "velocityY": -2.675510470157384, + "timestamp": 4.989878093011442 + }, + { + "x": 1.7526348309039128, + "y": 6.215560469564215, + "heading": -1.6873132093111056, + "angularVelocity": -0.6797026402940408, + "velocityX": -1.937271185368423, + "velocityY": -2.9367909309822835, + "timestamp": 5.032610070915615 + }, + { + "x": 1.677441366215122, + "y": 6.099853521630283, + "heading": -1.7491066585439166, + "angularVelocity": -1.4460704199413479, + "velocityX": -1.7596532708458867, + "velocityY": -2.70773677252684, + "timestamp": 5.075342048819787 + }, + { + "x": 1.6091252487579695, + "y": 5.994372460119343, + "heading": -1.8457597622108368, + "angularVelocity": -2.2618448386280567, + "velocityX": -1.598711803379506, + "velocityY": -2.4684338681322977, + "timestamp": 5.11807402672396 + }, + { + "x": 1.547030840271468, + "y": 5.899198607270202, + "heading": -1.9803343034636756, + "angularVelocity": -3.149270121655174, + "velocityX": -1.4531133715773699, + "velocityY": -2.2272278868666358, + "timestamp": 5.1608060046281325 + }, + { + "x": 1.4904311214521733, + "y": 5.813988086882659, + "heading": -2.1571135876817413, + "angularVelocity": -4.136931939225903, + "velocityX": -1.3245284116317142, + "velocityY": -1.9940691858127988, + "timestamp": 5.203537982532305 + }, + { + "x": 1.4423633386018753, + "y": 5.740159267538946, + "heading": -2.35427402869071, + "angularVelocity": -4.613885213811227, + "velocityX": -1.1248667908162702, + "velocityY": -1.7277182794879316, + "timestamp": 5.246269960436478 + }, + { + "x": 1.4038900802302197, + "y": 5.6781134856885735, + "heading": -2.5480008926422313, + "angularVelocity": -4.533533748097493, + "velocityX": -0.9003388155337229, + "velocityY": -1.451975426681909, + "timestamp": 5.28900193834065 + }, + { + "x": 1.3744454738206087, + "y": 5.627099122367314, + "heading": -2.7223828634703597, + "angularVelocity": -4.0808307824923435, + "velocityX": -0.6890532068429254, + "velocityY": -1.1938217190802813, + "timestamp": 5.331733916244823 + }, + { + "x": 1.3538849352129523, + "y": 5.586407689469935, + "heading": -2.872592229193493, + "angularVelocity": -3.5151512541727503, + "velocityX": -0.4811511101537121, + "velocityY": -0.9522478221960695, + "timestamp": 5.374465894148996 + }, + { + "x": 1.341639273384249, + "y": 5.556291662758613, + "heading": -2.9981488971519656, + "angularVelocity": -2.9382367518778816, + "velocityX": -0.2865690386755447, + "velocityY": -0.7047655687470963, + "timestamp": 5.417197872053168 + }, + { + "x": 1.3350584223241078, + "y": 5.537231311212578, + "heading": -3.091661499454674, + "angularVelocity": -2.18835183600471, + "velocityX": -0.15400295944407288, + "velocityY": -0.4460442151491003, + "timestamp": 5.459929849957341 + }, + { + "x": 1.332138180732727, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": -1.1589402165275682, + "velocityX": -0.06833855427730316, + "velocityY": -0.21356493938549467, + "timestamp": 5.502661827861513 + }, + { + "x": 1.332138180732727, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": 0, + "velocityX": 4.554923410438002e-36, + "velocityY": -5.3517654956150865e-34, + "timestamp": 5.545393805765686 + }, + { + "x": 1.3511823216382217, + "y": 5.531961266421083, + "heading": -3.038798036456765, + "angularVelocity": 1.6934181625027336, + "velocityX": 0.3149775735885273, + "velocityY": 0.06377582930300447, + "timestamp": 5.60585570052386 + }, + { + "x": 1.3908490268299478, + "y": 5.542189708843604, + "heading": -2.844785530962286, + "angularVelocity": 3.2088393238494826, + "velocityX": 0.6560612324568816, + "velocityY": 0.16917171490292857, + "timestamp": 5.666317595282035 + }, + { + "x": 1.4539728254946123, + "y": 5.56127979063472, + "heading": -2.581938269596155, + "angularVelocity": 4.34732094350375, + "velocityX": 1.0440261410453116, + "velocityY": 0.3157374056415134, + "timestamp": 5.726779490040209 + }, + { + "x": 1.5450944631009007, + "y": 5.588995277628573, + "heading": -2.2890493808444536, + "angularVelocity": 4.8441897152438, + "velocityX": 1.5070919952267785, + "velocityY": 0.45839593854450544, + "timestamp": 5.787241384798383 + }, + { + "x": 1.6638598735645596, + "y": 5.62434991056183, + "heading": -2.0324634607383167, + "angularVelocity": 4.24376247440454, + "velocityX": 1.9643018290888397, + "velocityY": 0.5847423914626325, + "timestamp": 5.8477032795565576 + }, + { + "x": 1.8083021892177116, + "y": 5.6638944259582, + "heading": -1.8350759652142852, + "angularVelocity": 3.264659440685901, + "velocityX": 2.3889809644714033, + "velocityY": 0.6540402935523977, + "timestamp": 5.908165174314732 + }, + { + "x": 1.978235650109889, + "y": 5.709112472816866, + "heading": -1.6974261571036708, + "angularVelocity": 2.276637354174287, + "velocityX": 2.8105877523661738, + "velocityY": 0.7478767749426485, + "timestamp": 5.968627069072906 + }, + { + "x": 2.1727729031197174, + "y": 5.761282618111588, + "heading": -1.6235540862695264, + "angularVelocity": 1.2217954982986527, + "velocityX": 3.217518302856812, + "velocityY": 0.8628599137255631, + "timestamp": 6.0290889638310805 + }, + { + "x": 2.3909229420587215, + "y": 5.821425961949498, + "heading": -1.6168784459669248, + "angularVelocity": 0.1104107029609625, + "velocityX": 3.60805826234069, + "velocityY": 0.994731377150211, + "timestamp": 6.089550858589255 + }, + { + "x": 2.6085958376613507, + "y": 5.891661539060254, + "heading": -1.616878243648269, + "angularVelocity": 0.0000033462175924113523, + "velocityX": 3.6001666251651883, + "velocityY": 1.1616502822426138, + "timestamp": 6.150012753347429 + }, + { + "x": 2.826268683292055, + "y": 5.961897271043331, + "heading": -1.6168780413299741, + "angularVelocity": 0.0000033462116195770168, + "velocityX": 3.6001657986623896, + "velocityY": 1.1616528437290037, + "timestamp": 6.2104746481056035 + }, + { + "x": 3.0439415289220437, + "y": 6.032133003028614, + "heading": -1.6168778390116512, + "angularVelocity": 0.0000033462120828803855, + "velocityX": 3.6001657986505493, + "velocityY": 1.1616528437654758, + "timestamp": 6.270936542863778 + }, + { + "x": 3.2616143745520296, + "y": 6.102368735013889, + "heading": -1.6168776366933004, + "angularVelocity": 0.000003346212545975565, + "velocityX": 3.600165798650516, + "velocityY": 1.1616528437653553, + "timestamp": 6.331398437621952 + }, + { + "x": 3.479287220182014, + "y": 6.172604466999156, + "heading": -1.6168774343749215, + "angularVelocity": 0.0000033462130098357135, + "velocityX": 3.6001657986504836, + "velocityY": 1.1616528437652343, + "timestamp": 6.391860332380126 + }, + { + "x": 3.6969600658119965, + "y": 6.242840198984417, + "heading": -1.6168772320565146, + "angularVelocity": 0.000003346213473053834, + "velocityX": 3.6001657986504503, + "velocityY": 1.1616528437651126, + "timestamp": 6.452322227138301 + }, + { + "x": 3.914632911441977, + "y": 6.313075930969669, + "heading": -1.61687702973808, + "angularVelocity": 0.000003346213936555426, + "velocityX": 3.6001657986504174, + "velocityY": 1.1616528437649918, + "timestamp": 6.512784121896475 + }, + { + "x": 4.132305757071955, + "y": 6.383311662954915, + "heading": -1.616876827419617, + "angularVelocity": 0.0000033462143992467285, + "velocityX": 3.6001657986503846, + "velocityY": 1.161652843764871, + "timestamp": 6.573246016654649 + }, + { + "x": 4.349978602701932, + "y": 6.453547394940153, + "heading": -1.6168766251011262, + "angularVelocity": 0.000003346214863174556, + "velocityX": 3.600165798650352, + "velocityY": 1.1616528437647493, + "timestamp": 6.633707911412824 + }, + { + "x": 4.567651448331906, + "y": 6.523783126925384, + "heading": -1.6168764227826073, + "angularVelocity": 0.000003346215326740712, + "velocityX": 3.6001657986503193, + "velocityY": 1.1616528437646283, + "timestamp": 6.694169806170998 + }, + { + "x": 4.785324293961879, + "y": 6.594018858910608, + "heading": -1.6168762204640603, + "angularVelocity": 0.0000033462157905377784, + "velocityX": 3.6001657986502864, + "velocityY": 1.161652843764507, + "timestamp": 6.754631700929172 + }, + { + "x": 5.002997139591849, + "y": 6.6642545908958235, + "heading": -1.6168760181454853, + "angularVelocity": 0.0000033462162531242337, + "velocityX": 3.600165798650253, + "velocityY": 1.1616528437643858, + "timestamp": 6.815093595687347 + }, + { + "x": 5.220669985221817, + "y": 6.734490322881032, + "heading": -1.6168758158268823, + "angularVelocity": 0.000003346216717473229, + "velocityX": 3.6001657986502207, + "velocityY": 1.1616528437642646, + "timestamp": 6.875555490445521 + }, + { + "x": 5.438342830851783, + "y": 6.804726054866234, + "heading": -1.6168756135082514, + "angularVelocity": 0.000003346217180660181, + "velocityX": 3.600165798650188, + "velocityY": 1.1616528437641434, + "timestamp": 6.936017385203695 + }, + { + "x": 5.656015676481748, + "y": 6.874961786851428, + "heading": -1.6168754111895922, + "angularVelocity": 0.0000033462176438124147, + "velocityX": 3.600165798650156, + "velocityY": 1.1616528437640228, + "timestamp": 6.99647927996187 + }, + { + "x": 5.873688522111711, + "y": 6.945197518836615, + "heading": -1.6168752088709053, + "angularVelocity": 0.0000033462181070326184, + "velocityX": 3.6001657986501225, + "velocityY": 1.161652843763901, + "timestamp": 7.056941174720044 + }, + { + "x": 6.091361367741672, + "y": 7.015433250821794, + "heading": -1.6168750065521902, + "angularVelocity": 0.0000033462185712785595, + "velocityX": 3.6001657986500892, + "velocityY": 1.1616528437637803, + "timestamp": 7.117403069478218 + }, + { + "x": 6.30903421337163, + "y": 7.0856689828069666, + "heading": -1.616874804233447, + "angularVelocity": 0.0000033462190342827524, + "velocityX": 3.600165798650057, + "velocityY": 1.1616528437636586, + "timestamp": 7.1778649642363925 + }, + { + "x": 6.526707059002331, + "y": 7.155904714789823, + "heading": -1.6168746019146758, + "angularVelocity": 0.0000033462194986739715, + "velocityX": 3.6001657986623425, + "velocityY": 1.1616528437253608, + "timestamp": 7.238326858994567 + }, + { + "x": 6.744379956765645, + "y": 7.22614028520157, + "heading": -1.6168743995934785, + "angularVelocity": 0.0000033462596273946, + "velocityX": 3.600166660901485, + "velocityY": 1.1616501714453904, + "timestamp": 7.298788753752741 + }, + { + "x": 6.948380246935694, + "y": 7.286306890939545, + "heading": -1.572684837353693, + "angularVelocity": 0.7308663153301367, + "velocityX": 3.37403071779302, + "velocityY": 0.9951161136881308, + "timestamp": 7.3592506485109155 + }, + { + "x": 7.128326014252639, + "y": 7.34087027160758, + "heading": -1.456360393496805, + "angularVelocity": 1.9239298457672127, + "velocityX": 2.976184719924214, + "velocityY": 0.9024424538177037, + "timestamp": 7.41971254326909 + }, + { + "x": 7.285215733567768, + "y": 7.391288256354559, + "heading": -1.2598604932432955, + "angularVelocity": 3.249979198294029, + "velocityX": 2.5948528398362565, + "velocityY": 0.8338803298942721, + "timestamp": 7.480174438027264 + }, + { + "x": 7.415669802167391, + "y": 7.433594551714448, + "heading": -1.014978314704827, + "angularVelocity": 4.050190281298798, + "velocityX": 2.157624552147962, + "velocityY": 0.6997183189362131, + "timestamp": 7.540636332785438 + }, + { + "x": 7.52162527778786, + "y": 7.463369081831009, + "heading": -0.7214685317692553, + "angularVelocity": 4.854458896954922, + "velocityX": 1.7524339262646644, + "velocityY": 0.49245115846350496, + "timestamp": 7.601098227543613 + }, + { + "x": 7.599835823210802, + "y": 7.482093660895173, + "heading": -0.44961761921846705, + "angularVelocity": 4.4962354163410385, + "velocityX": 1.293551016483298, + "velocityY": 0.30969223076874997, + "timestamp": 7.661560122301787 + }, + { + "x": 7.6517032735458015, + "y": 7.492160163258041, + "heading": -0.22315227789287845, + "angularVelocity": 3.7455878984832975, + "velocityX": 0.8578535380416231, + "velocityY": 0.16649333275330752, + "timestamp": 7.722022017059961 + }, + { + "x": 7.67973874704759, + "y": 7.4966228089126075, + "heading": -0.06798236605855336, + "angularVelocity": 2.5664083544676926, + "velocityX": 0.46368830507082526, + "velocityY": 0.07380922599953843, + "timestamp": 7.782483911818136 + }, + { + "x": 7.686149597167969, + "y": 7.496679306030273, + "heading": 0, + "angularVelocity": 1.1243836523889659, + "velocityX": 0.10603124738348535, + "velocityY": 0.0009344251927952234, + "timestamp": 7.84294580657631 + }, + { + "x": 7.671082178881759, + "y": 7.493118035056542, + "heading": -0.029242529216573936, + "angularVelocity": -0.471080674899979, + "velocityX": -0.24272762190641095, + "velocityY": -0.05737006951015711, + "timestamp": 7.905021223324796 + }, + { + "x": 7.633475390199317, + "y": 7.486720174316825, + "heading": -0.1516714089178372, + "angularVelocity": -1.9722602942371796, + "velocityX": -0.6058241837475781, + "velocityY": -0.1030659329383018, + "timestamp": 7.967096640073281 + }, + { + "x": 7.569735009380553, + "y": 7.475264417263962, + "heading": -0.33932636884787254, + "angularVelocity": -3.023015708301534, + "velocityX": -1.0268216333854858, + "velocityY": -0.1845457936960604, + "timestamp": 8.029172056821766 + }, + { + "x": 7.480376336028035, + "y": 7.452059634472421, + "heading": -0.582197860853168, + "angularVelocity": -3.912522939464948, + "velocityX": -1.4395178966671722, + "velocityY": -0.37381598073777733, + "timestamp": 8.09124747357025 + }, + { + "x": 7.362878563951424, + "y": 7.417380486057705, + "heading": -0.863364111736863, + "angularVelocity": -4.529429935571964, + "velocityX": -1.8928229278376825, + "velocityY": -0.5586615480203228, + "timestamp": 8.153322890318735 + }, + { + "x": 7.218864923659752, + "y": 7.37005336377927, + "heading": -1.0948198175456743, + "angularVelocity": -3.7286210537516657, + "velocityX": -2.319978629788013, + "velocityY": -0.7624132830262488, + "timestamp": 8.21539830706722 + }, + { + "x": 7.047313606455814, + "y": 7.316250565275482, + "heading": -1.2668868462563043, + "angularVelocity": -2.7719029162189006, + "velocityX": -2.7635950943192666, + "velocityY": -0.8667327796087775, + "timestamp": 8.277473723815705 + }, + { + "x": 6.850682482856188, + "y": 7.25802494172709, + "heading": -1.3587604988602537, + "angularVelocity": -1.4800327958521704, + "velocityX": -3.16761664921765, + "velocityY": -0.9379819999325716, + "timestamp": 8.33954914056419 + }, + { + "x": 6.62975279957275, + "y": 7.191687511601611, + "heading": -1.3718894789288036, + "angularVelocity": -0.21150047404023747, + "velocityX": -3.5590527596228974, + "velocityY": -1.0686586349353433, + "timestamp": 8.401624557312674 + }, + { + "x": 6.40649462388328, + "y": 7.118888182289737, + "heading": -1.3718898557853474, + "angularVelocity": -0.000006070946652807348, + "velocityX": -3.596563460766736, + "velocityY": -1.1727561911801452, + "timestamp": 8.463699974061159 + }, + { + "x": 6.183236472249506, + "y": 7.0460887792034885, + "heading": -1.3718902326404057, + "angularVelocity": -0.000006070922729380904, + "velocityX": -3.5965630732430243, + "velocityY": -1.1727573796437725, + "timestamp": 8.525775390809644 + }, + { + "x": 5.959978320616, + "y": 6.973289376116265, + "heading": -1.3718906094952235, + "angularVelocity": -0.000006070918851907899, + "velocityX": -3.5965630732387113, + "velocityY": -1.172757379659484, + "timestamp": 8.587850807558128 + }, + { + "x": 5.736720168982443, + "y": 6.900489973029042, + "heading": -1.3718909863498006, + "angularVelocity": -0.000006070914972563024, + "velocityX": -3.596563073239526, + "velocityY": -1.1727573796594701, + "timestamp": 8.649926224306613 + }, + { + "x": 5.513462017348836, + "y": 6.8276905699418196, + "heading": -1.3718913632041367, + "angularVelocity": -0.000006070911095039567, + "velocityX": -3.5965630732403415, + "velocityY": -1.1727573796594566, + "timestamp": 8.712001641055098 + }, + { + "x": 5.290203865715178, + "y": 6.7548911668545975, + "heading": -1.3718917400582324, + "angularVelocity": -0.0000060709072179749485, + "velocityX": -3.596563073241156, + "velocityY": -1.1727573796594426, + "timestamp": 8.774077057803582 + }, + { + "x": 5.066945714081469, + "y": 6.682091763767377, + "heading": -1.3718921169120872, + "angularVelocity": -0.000006070903338953198, + "velocityX": -3.596563073241971, + "velocityY": -1.1727573796594288, + "timestamp": 8.836152474552067 + }, + { + "x": 4.84368756244771, + "y": 6.609292360680157, + "heading": -1.3718924937657013, + "angularVelocity": -0.000006070899462290803, + "velocityX": -3.596563073242785, + "velocityY": -1.1727573796594153, + "timestamp": 8.898227891300552 + }, + { + "x": 4.620429410813901, + "y": 6.536492957592937, + "heading": -1.371892870619075, + "angularVelocity": -0.000006070895585804876, + "velocityX": -3.5965630732436003, + "velocityY": -1.1727573796594017, + "timestamp": 8.960303308049037 + }, + { + "x": 4.39717125918004, + "y": 6.46369355450572, + "heading": -1.3718932474722079, + "angularVelocity": -0.000006070891709350806, + "velocityX": -3.5965630732444147, + "velocityY": -1.172757379659388, + "timestamp": 9.022378724797521 + }, + { + "x": 4.173913107546129, + "y": 6.390894151418502, + "heading": -1.3718936243251003, + "angularVelocity": -0.000006070887832958865, + "velocityX": -3.5965630732452283, + "velocityY": -1.1727573796593747, + "timestamp": 9.084454141546006 + }, + { + "x": 3.950654955912168, + "y": 6.318094748331286, + "heading": -1.3718940011777518, + "angularVelocity": -0.000006070883956378075, + "velocityX": -3.596563073246043, + "velocityY": -1.1727573796593613, + "timestamp": 9.14652955829449 + }, + { + "x": 3.7273968042781562, + "y": 6.24529534524407, + "heading": -1.3718943780301627, + "angularVelocity": -0.000006070880081532027, + "velocityX": -3.5965630732468568, + "velocityY": -1.1727573796593478, + "timestamp": 9.208604975042975 + }, + { + "x": 3.504138652644094, + "y": 6.172495942156855, + "heading": -1.3718947548823335, + "angularVelocity": -0.000006070876206601658, + "velocityX": -3.5965630732476703, + "velocityY": -1.1727573796593347, + "timestamp": 9.27068039179146 + }, + { + "x": 3.2808805010099813, + "y": 6.099696539069641, + "heading": -1.3718951317342634, + "angularVelocity": -0.000006070872330584258, + "velocityX": -3.596563073248485, + "velocityY": -1.1727573796593216, + "timestamp": 9.332755808539945 + }, + { + "x": 3.057622349375818, + "y": 6.026897135982428, + "heading": -1.371895508585953, + "angularVelocity": -0.000006070868457801708, + "velocityX": -3.5965630732492984, + "velocityY": -1.1727573796593085, + "timestamp": 9.39483122528843 + }, + { + "x": 2.834364197741604, + "y": 5.954097732895216, + "heading": -1.371895885437402, + "angularVelocity": -0.0000060708645841945965, + "velocityX": -3.596563073250112, + "velocityY": -1.1727573796592947, + "timestamp": 9.456906642036914 + }, + { + "x": 2.6111060461064435, + "y": 5.881298329810753, + "heading": -1.3718962622886106, + "angularVelocity": -0.000006070860710280736, + "velocityX": -3.5965630732653646, + "velocityY": -1.1727573796150002, + "timestamp": 9.518982058785399 + }, + { + "x": 2.387847826730136, + "y": 5.808499134475022, + "heading": -1.37189663914262, + "angularVelocity": -0.000006070905830150403, + "velocityX": -3.5965641645370763, + "velocityY": -1.1727540328999504, + "timestamp": 9.581057475533884 + }, + { + "x": 2.167695143748949, + "y": 5.749324083617281, + "heading": -1.3936267357407348, + "angularVelocity": -0.3500596167748615, + "velocityX": -3.546535722397048, + "velocityY": -0.9532767391237054, + "timestamp": 9.643132892282368 + }, + { + "x": 1.9716359535895844, + "y": 5.700085594404985, + "heading": -1.4887110872196767, + "angularVelocity": -1.5317553463104556, + "velocityX": -3.1584031236350607, + "velocityY": -0.7932043277582467, + "timestamp": 9.705208309030853 + }, + { + "x": 1.8021216559438722, + "y": 5.6575856667748505, + "heading": -1.6480423798805228, + "angularVelocity": -2.5667373818273083, + "velocityX": -2.73077985658225, + "velocityY": -0.6846498961470444, + "timestamp": 9.767283725779338 + }, + { + "x": 1.6601304337176481, + "y": 5.619404991825654, + "heading": -1.8664040726402535, + "angularVelocity": -3.5176838786999975, + "velocityX": -2.2873986138754163, + "velocityY": -0.6150691682650391, + "timestamp": 9.829359142527823 + }, + { + "x": 1.5437233022892554, + "y": 5.5885120196672124, + "heading": -2.146539077662866, + "angularVelocity": -4.5128171455965, + "velocityX": -1.8752533212953901, + "velocityY": -0.49766838108574224, + "timestamp": 9.891434559276307 + }, + { + "x": 1.452238647018182, + "y": 5.5675498489404625, + "heading": -2.4956493740995165, + "angularVelocity": -5.623970240121965, + "velocityX": -1.4737662679212806, + "velocityY": -0.33768876351944366, + "timestamp": 9.953509976024792 + }, + { + "x": 1.3894112242957146, + "y": 5.555821056487952, + "heading": -2.802567916985108, + "angularVelocity": -4.94428485481059, + "velocityX": -1.0121143926754297, + "velocityY": -0.188944240197889, + "timestamp": 10.015585392773277 + }, + { + "x": 1.3504462657255363, + "y": 5.550087016097331, + "heading": -3.0243894075867264, + "angularVelocity": -3.573419273210609, + "velocityX": -0.6277035356533314, + "velocityY": -0.09237216100947475, + "timestamp": 10.077660809521761 + }, + { + "x": 1.3321380615234375, + "y": 5.547595977783203, + "heading": -3.1411853071795863, + "angularVelocity": -1.8815161574522963, + "velocityX": -0.2949348576470964, + "velocityY": -0.040129224169711215, + "timestamp": 10.139736226270246 + }, + { + "x": 1.3321380615234375, + "y": 5.547595977783203, + "heading": -3.1411853071795863, + "angularVelocity": 7.133789284871957e-33, + "velocityX": 0, + "velocityY": 1.0797204950028118e-32, + "timestamp": 10.201811643018731 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "54940582-6fbe-4b4f-8291-c0747b2aece5" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "uuid": "13f02721-6063-4019-8ee4-d455a26aa8d3" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint", + "uuid": "6c01d07d-0f28-46b2-a0e7-90f87c695873" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint", + "uuid": "fe0a509c-156e-4805-91a2-25eb8ebb28f9" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint", + "uuid": "45b66dc3-77d0-41da-b7d8-c5d876251180" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + }, + "splitTrajectoriesAtStopPoints": false +} \ No newline at end of file diff --git a/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat new file mode 100644 index 00000000..520f0aea Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat new file mode 100644 index 00000000..9c319613 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat new file mode 100644 index 00000000..7aa8c02b Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat new file mode 100644 index 00000000..0795cb7d Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat new file mode 100644 index 00000000..062dc8c8 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat new file mode 100644 index 00000000..40f1b20d Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat new file mode 100644 index 00000000..88dce4d2 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat new file mode 100644 index 00000000..7eb63603 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat differ diff --git a/deploy/choreo/NewPath.1.traj b/deploy/choreo/NewPath.1.traj new file mode 100644 index 00000000..92058ea3 --- /dev/null +++ b/deploy/choreo/NewPath.1.traj @@ -0,0 +1,1876 @@ +{ + "samples": [ + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.2261748232194093e-37, + "velocityY": 3.7157891353428806e-37, + "timestamp": 0 + }, + { + "x": 1.366635963151808, + "y": 5.509495705296941, + "heading": 0.06192923642991932, + "angularVelocity": 0.7673697851265452, + "velocityX": 0.4274601622031667, + "velocityY": -0.47210922509207903, + "timestamp": 0.08070325106650729 + }, + { + "x": 1.4355761410472572, + "y": 5.43294196238783, + "heading": 0.18410628088281034, + "angularVelocity": 1.5139048655202914, + "velocityX": 0.8542428834575204, + "velocityY": -0.9485831350960487, + "timestamp": 0.16140650213301458 + }, + { + "x": 1.5392732266735116, + "y": 5.317548823896549, + "heading": 0.36164871867370346, + "angularVelocity": 2.1999415815923076, + "velocityX": 1.2849183181083754, + "velocityY": -1.429844981042792, + "timestamp": 0.24210975319952188 + }, + { + "x": 1.67847556185222, + "y": 5.162242328350244, + "heading": 0.5745390252788012, + "angularVelocity": 2.637939659080841, + "velocityX": 1.724866512056522, + "velocityY": -1.9244143636582545, + "timestamp": 0.32281300426602916 + }, + { + "x": 1.8501216668381886, + "y": 4.967122032744555, + "heading": 0.7214821568069908, + "angularVelocity": 1.8207832966616715, + "velocityX": 2.1268796822635494, + "velocityY": -2.417750128119265, + "timestamp": 0.40351625533253643 + }, + { + "x": 1.999355457938739, + "y": 4.729284669626315, + "heading": 0.7742712340167793, + "angularVelocity": 0.6541133908754838, + "velocityX": 1.849167030180327, + "velocityY": -2.9470604960169275, + "timestamp": 0.4842195063990437 + }, + { + "x": 2.1155301056872418, + "y": 4.524468188873081, + "heading": 0.7178805227908414, + "angularVelocity": -0.6987415064538932, + "velocityX": 1.4395287205067313, + "velocityY": -2.537896281085937, + "timestamp": 0.564922757465551 + }, + { + "x": 2.1994300453605806, + "y": 4.354650955915648, + "heading": 0.5603443624430987, + "angularVelocity": -1.952042306423536, + "velocityX": 1.0396104068248448, + "velocityY": -2.104217992624444, + "timestamp": 0.6456260085320583 + }, + { + "x": 2.250899907873132, + "y": 4.221459437616645, + "heading": 0.3158282415144345, + "angularVelocity": -3.029817481914815, + "velocityX": 0.637766903220982, + "velocityY": -1.6503860320229218, + "timestamp": 0.7263292595985656 + }, + { + "x": 2.2676985263824463, + "y": 4.124765396118164, + "heading": 2.2216292217564286e-35, + "angularVelocity": -3.913451284064894, + "velocityX": 0.20815293420422745, + "velocityY": -1.1981430762782603, + "timestamp": 0.807032510665073 + }, + { + "x": 2.2676338555313906, + "y": 4.086690967677836, + "heading": -0.16755058667496922, + "angularVelocity": -4.348312733244185, + "velocityX": -0.0016783533301519722, + "velocityY": -0.9881166355999816, + "timestamp": 0.8455648327296736 + }, + { + "x": 2.259511405376126, + "y": 4.056750075909778, + "heading": -0.352256708092011, + "angularVelocity": -4.793537257042952, + "velocityX": -0.21079576106643969, + "velocityY": -0.7770331546035791, + "timestamp": 0.8840971547942744 + }, + { + "x": 2.2434268935883566, + "y": 4.035188434921252, + "heading": -0.5534507965852989, + "angularVelocity": -5.22143690577431, + "velocityX": -0.41742908098825227, + "velocityY": -0.5595728425704051, + "timestamp": 0.9226294768588752 + }, + { + "x": 2.2194223708397587, + "y": 4.022359555236857, + "heading": -0.7686869662578265, + "angularVelocity": -5.585860341135864, + "velocityX": -0.6229710918629213, + "velocityY": -0.33293814120224735, + "timestamp": 0.961161798923476 + }, + { + "x": 2.1874734502455304, + "y": 4.018617397576101, + "heading": -0.9939561131825075, + "angularVelocity": -5.846238556477593, + "velocityX": -0.8291459969805253, + "velocityY": -0.09711736693372348, + "timestamp": 0.9996941209880768 + }, + { + "x": 2.1476082595993153, + "y": 4.024282627294418, + "heading": -1.2244083666470835, + "angularVelocity": -5.980751771933577, + "velocityX": -1.0345909228979182, + "velocityY": 0.14702539101636564, + "timestamp": 1.0382264430526775 + }, + { + "x": 2.1000522078031225, + "y": 4.039643723439363, + "heading": -1.4537096411283452, + "angularVelocity": -5.950881291213917, + "velocityX": -1.2341859833015967, + "velocityY": 0.3986548259197214, + "timestamp": 1.0767587651172783 + }, + { + "x": 2.045480509029303, + "y": 4.064764963970464, + "heading": -1.6725096563971036, + "angularVelocity": -5.678350110899954, + "velocityX": -1.4162577246792443, + "velocityY": 0.6519524177386574, + "timestamp": 1.115291087181879 + }, + { + "x": 1.9845995907026053, + "y": 4.099193425968209, + "heading": -1.8731412390257827, + "angularVelocity": -5.206838619596132, + "velocityX": -1.5799960932701846, + "velocityY": 0.8934956460714911, + "timestamp": 1.1538234092464799 + }, + { + "x": 1.9178343766904495, + "y": 4.143331162612739, + "heading": -2.0560809001827813, + "angularVelocity": -4.747693659631898, + "velocityX": -1.7327067364437998, + "velocityY": 1.1454730542979317, + "timestamp": 1.1923557313110806 + }, + { + "x": 1.846280610428652, + "y": 4.197802019829692, + "heading": -2.22146373822619, + "angularVelocity": -4.2920548044350495, + "velocityX": -1.856980384982645, + "velocityY": 1.413640660576612, + "timestamp": 1.2308880533756814 + }, + { + "x": 1.770704211681186, + "y": 4.2612245864567395, + "heading": -2.3576115042271706, + "angularVelocity": -3.5333392514659345, + "velocityX": -1.961376701377084, + "velocityY": 1.6459575553406294, + "timestamp": 1.2694203754402822 + }, + { + "x": 1.6915488741102875, + "y": 4.3341463409601095, + "heading": -2.466232613363222, + "angularVelocity": -2.8189608961002843, + "velocityX": -2.0542581741684773, + "velocityY": 1.8924827416607193, + "timestamp": 1.307952697504883 + }, + { + "x": 1.609995812708591, + "y": 4.417230005205154, + "heading": -2.5495370955191317, + "angularVelocity": -2.1619377626982037, + "velocityX": -2.1164844741246154, + "velocityY": 2.156207043680162, + "timestamp": 1.3464850195694837 + }, + { + "x": 1.5308705698098255, + "y": 4.511360824889822, + "heading": -2.6152779027765387, + "angularVelocity": -1.7061210883473432, + "velocityX": -2.053477150069217, + "velocityY": 2.442905452904058, + "timestamp": 1.3850173416340845 + }, + { + "x": 1.4606506153508025, + "y": 4.614058209006809, + "heading": -2.6783194867011537, + "angularVelocity": -1.636070201503133, + "velocityX": -1.822364983384516, + "velocityY": 2.6652269734694745, + "timestamp": 1.4235496636986853 + }, + { + "x": 1.4019701962785118, + "y": 4.720456086702728, + "heading": -2.7505666696039515, + "angularVelocity": -1.874976098810575, + "velocityX": -1.5228882124962768, + "velocityY": 2.761263064227987, + "timestamp": 1.462081985763286 + }, + { + "x": 1.3551627373603712, + "y": 4.8282147910275475, + "heading": -2.8346512790115326, + "angularVelocity": -2.1821838109473566, + "velocityX": -1.2147583226275982, + "velocityY": 2.7965795610282194, + "timestamp": 1.5006143078278869 + }, + { + "x": 1.3203598068196198, + "y": 4.936382222496368, + "heading": -2.9296853375556533, + "angularVelocity": -2.466346522921523, + "velocityX": -0.9032139428919735, + "velocityY": 2.8071869452215745, + "timestamp": 1.5391466298924876 + }, + { + "x": 1.2978728176694776, + "y": 5.0435429055984535, + "heading": -3.0274097940467435, + "angularVelocity": -2.53616837125081, + "velocityX": -0.583587698463697, + "velocityY": 2.7810595718167943, + "timestamp": 1.5776789519570884 + }, + { + "x": 1.286514559723769, + "y": 5.147138897368402, + "heading": -3.1101525298443558, + "angularVelocity": -2.1473591874087323, + "velocityX": -0.2947722155614252, + "velocityY": 2.688547853313019, + "timestamp": 1.6162112740216892 + }, + { + "x": 1.2850446377120623, + "y": 5.2469545287393595, + "heading": -3.168448488726713, + "angularVelocity": -1.5129106100748941, + "velocityX": -0.038147766159596044, + "velocityY": 2.5904390398173645, + "timestamp": 1.65474359608629 + }, + { + "x": 1.2925718642931594, + "y": 5.343394761009087, + "heading": -3.1958165160103977, + "angularVelocity": -0.7102615626901808, + "velocityX": 0.1953483770969554, + "velocityY": 2.5028398783764474, + "timestamp": 1.6932759181508907 + }, + { + "x": 1.3084372785500542, + "y": 5.436937425716649, + "heading": -3.187781062808085, + "angularVelocity": 0.20853799542215204, + "velocityX": 0.4117430096814787, + "velocityY": 2.4276415148491566, + "timestamp": 1.7318082402154915 + }, + { + "x": 1.3321385383605957, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": 1.2092641484304996, + "velocityX": 0.6151007398621232, + "velocityY": 2.366009322567054, + "timestamp": 1.7703405622800923 + }, + { + "x": 1.357521739751378, + "y": 5.604487249946148, + "heading": -3.0708796741945465, + "angularVelocity": 2.1424744850655806, + "velocityX": 0.7735206841904706, + "velocityY": 2.3276437448542358, + "timestamp": 1.8031557175947133 + }, + { + "x": 1.3882591250435519, + "y": 5.675559897172117, + "heading": -2.9834964247525093, + "angularVelocity": 2.6628930628008614, + "velocityX": 0.936682608918773, + "velocityY": 2.1658482656731963, + "timestamp": 1.8359708729093343 + }, + { + "x": 1.423715934589619, + "y": 5.741061467273705, + "heading": -2.8764865415416963, + "angularVelocity": 3.260989691648163, + "velocityX": 1.0805010430735036, + "velocityY": 1.9960767966380097, + "timestamp": 1.8687860282239552 + }, + { + "x": 1.4640545833350669, + "y": 5.800765385011703, + "heading": -2.7509368125913465, + "angularVelocity": 3.8259678415847733, + "velocityX": 1.2292688655194213, + "velocityY": 1.8194007362017863, + "timestamp": 1.9016011835385762 + }, + { + "x": 1.5093560477878591, + "y": 5.854065962524638, + "heading": -2.610542457820649, + "angularVelocity": 4.278338878016634, + "velocityX": 1.3805043437538693, + "velocityY": 1.6242671107878839, + "timestamp": 1.9344163388531972 + }, + { + "x": 1.5593454812172984, + "y": 5.900364561226932, + "heading": -2.4585168251320484, + "angularVelocity": 4.6327872359898405, + "velocityX": 1.5233642184581169, + "velocityY": 1.4108907380872764, + "timestamp": 1.9672314941678182 + }, + { + "x": 1.612840729658989, + "y": 5.938459321827602, + "heading": -2.302873266045166, + "angularVelocity": 4.74303892803867, + "velocityX": 1.6301994590242108, + "velocityY": 1.1608892365563526, + "timestamp": 2.000046649482439 + }, + { + "x": 1.6683469717714092, + "y": 5.967831924392925, + "heading": -2.1497463391084874, + "angularVelocity": 4.666347773415882, + "velocityX": 1.6914819259651337, + "velocityY": 0.89509259620163, + "timestamp": 2.03286180479706 + }, + { + "x": 1.7259927125934769, + "y": 5.988487486256733, + "heading": -1.9957888127587393, + "angularVelocity": 4.691659231036796, + "velocityX": 1.756680420049179, + "velocityY": 0.6294519000677786, + "timestamp": 2.065676960111681 + }, + { + "x": 1.7857676152473871, + "y": 6.000514617426619, + "heading": -1.8372643934887982, + "angularVelocity": 4.830829467362277, + "velocityX": 1.8215639109675918, + "velocityY": 0.36651148088664176, + "timestamp": 2.098492115426302 + }, + { + "x": 1.8473787764602114, + "y": 6.0041716221205474, + "heading": -1.6689363232818417, + "angularVelocity": 5.129583224369399, + "velocityX": 1.877521548263803, + "velocityY": 0.11144255326131712, + "timestamp": 2.131307270740923 + }, + { + "x": 1.9079302163194551, + "y": 5.9989341122190964, + "heading": -1.4983965075420516, + "angularVelocity": 5.196983348233757, + "velocityX": 1.8452278917681775, + "velocityY": -0.15960643340660682, + "timestamp": 2.164122426055544 + }, + { + "x": 1.9644851606047329, + "y": 5.9857778753833975, + "heading": -1.3328127647297614, + "angularVelocity": 5.045953347614136, + "velocityX": 1.7234397869840146, + "velocityY": -0.40091953579256995, + "timestamp": 2.196937581370165 + }, + { + "x": 2.0167542311341298, + "y": 5.964732325175136, + "heading": -1.1669030928960278, + "angularVelocity": 5.055885618795508, + "velocityX": 1.592833251229742, + "velocityY": -0.6413362974053785, + "timestamp": 2.229752736684786 + }, + { + "x": 2.0647189114008015, + "y": 5.935938401754757, + "heading": -0.9958716358792914, + "angularVelocity": 5.211965489023056, + "velocityX": 1.4616624485485903, + "velocityY": -0.8774580874084462, + "timestamp": 2.262567891999407 + }, + { + "x": 2.1085721264603117, + "y": 5.8997079709351175, + "heading": -0.8141851452694338, + "angularVelocity": 5.53666404647202, + "velocityX": 1.3363707908452565, + "velocityY": -1.1040761645731791, + "timestamp": 2.295383047314028 + }, + { + "x": 2.1457870640137746, + "y": 5.857488430688145, + "heading": -0.6354555413511925, + "angularVelocity": 5.446556696277672, + "velocityX": 1.134077751473621, + "velocityY": -1.2865866348089932, + "timestamp": 2.328198202628649 + }, + { + "x": 2.1756614189381724, + "y": 5.811166545923024, + "heading": -0.4679136928264175, + "angularVelocity": 5.105624121490153, + "velocityX": 0.9103828593213182, + "velocityY": -1.41160035114879, + "timestamp": 2.36101335794327 + }, + { + "x": 2.1979477640451273, + "y": 5.762830427028998, + "heading": -0.3159012603830297, + "angularVelocity": 4.632384975354892, + "velocityX": 0.6791479392152971, + "velocityY": -1.4729815669191495, + "timestamp": 2.393828513257891 + }, + { + "x": 2.213866689003856, + "y": 5.714154085995567, + "heading": -0.19000720732011314, + "angularVelocity": 3.8364606797037886, + "velocityX": 0.4851089323242057, + "velocityY": -1.483349402638423, + "timestamp": 2.426643668572512 + }, + { + "x": 2.2241033767575633, + "y": 5.665307000563528, + "heading": -0.09437057287174495, + "angularVelocity": 2.9144044430518514, + "velocityX": 0.31195000162458963, + "velocityY": -1.4885526203886283, + "timestamp": 2.459458823887133 + }, + { + "x": 2.228985536910567, + "y": 5.616279831781338, + "heading": -0.030711851451385126, + "angularVelocity": 1.9399183337704993, + "velocityX": 0.14877760309816687, + "velocityY": -1.4940404307745467, + "timestamp": 2.492273979201754 + }, + { + "x": 2.2287168502807617, + "y": 5.567087173461914, + "heading": -1.4593920868727723e-34, + "angularVelocity": 0.9359044976910779, + "velocityX": -0.008187882313191258, + "velocityY": -1.4990835133273062, + "timestamp": 2.525089134516375 + }, + { + "x": 2.2234617571142565, + "y": 5.5179127265043535, + "heading": -0.002801304121864836, + "angularVelocity": -0.08554590281020508, + "velocityX": -0.16047943019522937, + "velocityY": -1.5016836006283458, + "timestamp": 2.5578353447946145 + }, + { + "x": 2.213100127893157, + "y": 5.468804050064043, + "heading": -0.038527021013621016, + "angularVelocity": -1.090987829987043, + "velocityX": -0.31642224040761213, + "velocityY": -1.499675108143563, + "timestamp": 2.590581555072854 + }, + { + "x": 2.1974774857014916, + "y": 5.420028778568237, + "heading": -0.10642175719814694, + "angularVelocity": -2.0733616381142825, + "velocityX": -0.47708244889780627, + "velocityY": -1.4894936263271208, + "timestamp": 2.623327765351094 + }, + { + "x": 2.176386509447901, + "y": 5.371982692865446, + "heading": -0.2052939647176804, + "angularVelocity": -3.019348091868672, + "velocityX": -0.6440738050108257, + "velocityY": -1.4672258345179665, + "timestamp": 2.6560739756293335 + }, + { + "x": 2.1495039837265555, + "y": 5.325205721708642, + "heading": -0.3329902489476004, + "angularVelocity": -3.8995744284576235, + "velocityX": -0.8209354759811619, + "velocityY": -1.4284697606027426, + "timestamp": 2.688820185907573 + }, + { + "x": 2.1162335968543386, + "y": 5.280389038707846, + "heading": -0.4851442980051073, + "angularVelocity": -4.646462835383888, + "velocityX": -1.0160072444879351, + "velocityY": -1.368606706546959, + "timestamp": 2.7215663961858128 + }, + { + "x": 2.0756862755494496, + "y": 5.238416301489077, + "heading": -0.6539149158793239, + "angularVelocity": -5.153897701144575, + "velocityX": -1.2382294305314772, + "velocityY": -1.2817586176272708, + "timestamp": 2.7543126064640524 + }, + { + "x": 2.0277154786481417, + "y": 5.201197274104196, + "heading": -0.8280730192796384, + "angularVelocity": -5.318420113977116, + "velocityX": -1.4649266737649096, + "velocityY": -1.136590373928381, + "timestamp": 2.787058816742292 + }, + { + "x": 1.9735902732105297, + "y": 5.170441088651969, + "heading": -1.0002810429175644, + "angularVelocity": -5.258868802670591, + "velocityX": -1.6528692932012015, + "velocityY": -0.9392288509386596, + "timestamp": 2.8198050270205317 + }, + { + "x": 1.914416507688317, + "y": 5.147054029448202, + "heading": -1.1711019741635995, + "angularVelocity": -5.216509934877801, + "velocityX": -1.807041639915639, + "velocityY": -0.7141913218369506, + "timestamp": 2.8525512372987714 + }, + { + "x": 1.851848295060789, + "y": 5.131935997009135, + "heading": -1.3411756027953594, + "angularVelocity": -5.193688893666442, + "velocityX": -1.9107008748766678, + "velocityY": -0.46167273435953404, + "timestamp": 2.885297447577011 + }, + { + "x": 1.788148478011349, + "y": 5.125682164538362, + "heading": -1.5101885639025132, + "angularVelocity": -5.161298350895432, + "velocityX": -1.945257680451938, + "velocityY": -0.1909788161022263, + "timestamp": 2.9180436578552507 + }, + { + "x": 1.7253768027949503, + "y": 5.1283168367810115, + "heading": -1.680184610910991, + "angularVelocity": -5.191319714985216, + "velocityX": -1.9169141919946477, + "velocityY": 0.0804573176640312, + "timestamp": 2.9507898681334903 + }, + { + "x": 1.665253742824933, + "y": 5.13946824377609, + "heading": -1.8524223717430155, + "angularVelocity": -5.2597769136808665, + "velocityX": -1.83603108448766, + "velocityY": 0.3405403831565884, + "timestamp": 2.98353607841173 + }, + { + "x": 1.6088715232725108, + "y": 5.158717654167419, + "heading": -2.026839169725991, + "angularVelocity": -5.326320099363601, + "velocityX": -1.7217937304301978, + "velocityY": 0.5878362786951496, + "timestamp": 3.0162822886899696 + }, + { + "x": 1.5568081254735793, + "y": 5.1857922946114, + "heading": -2.2021042643709907, + "angularVelocity": -5.3522252851795065, + "velocityX": -1.5899060488696688, + "velocityY": 0.826802253266321, + "timestamp": 3.0490284989682093 + }, + { + "x": 1.5093538981441093, + "y": 5.2205186818889695, + "heading": -2.3759898286014502, + "angularVelocity": -5.31009734418057, + "velocityX": -1.4491517316434035, + "velocityY": 1.060470417263673, + "timestamp": 3.081774709246449 + }, + { + "x": 1.4666702915238679, + "y": 5.262723781048631, + "heading": -2.545870980641176, + "angularVelocity": -5.187811065655255, + "velocityX": -1.30346706557996, + "velocityY": 1.2888544598306537, + "timestamp": 3.1145209195246886 + }, + { + "x": 1.4288865927350196, + "y": 5.312181622313189, + "heading": -2.7092363373229658, + "angularVelocity": -4.988832457059862, + "velocityX": -1.1538342442623262, + "velocityY": 1.510337863353399, + "timestamp": 3.147267129802928 + }, + { + "x": 1.3968131737185872, + "y": 5.367362011920862, + "heading": -2.8587947946716725, + "angularVelocity": -4.567198954563906, + "velocityX": -0.9794543778931859, + "velocityY": 1.6850923859223195, + "timestamp": 3.180013340081168 + }, + { + "x": 1.3707084315183937, + "y": 5.426032663472122, + "heading": -2.986331521157274, + "angularVelocity": -3.8947018724286298, + "velocityX": -0.7971836123443037, + "velocityY": 1.7916776033850628, + "timestamp": 3.2127595503594075 + }, + { + "x": 1.3494796795233661, + "y": 5.486294372047085, + "heading": -3.0820270140723585, + "angularVelocity": -2.9223379469554978, + "velocityX": -0.6482811847432148, + "velocityY": 1.8402651196253896, + "timestamp": 3.245505760637647 + }, + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": -3.1411853071795863, + "angularVelocity": -1.8065691450878867, + "velocityX": -0.529561772657824, + "velocityY": 1.8720359410264953, + "timestamp": 3.278251970915887 + }, + { + "x": 1.316411922217005, + "y": 5.622158239816111, + "heading": -3.1574444739447625, + "angularVelocity": -0.4153661551974852, + "velocityX": -0.4017613064785548, + "velocityY": 1.9047988429364744, + "timestamp": 3.317396149038551 + }, + { + "x": 1.30673758444171, + "y": 5.698077663390306, + "heading": -3.122874776057638, + "angularVelocity": 0.8831376604407247, + "velocityX": -0.24714627408906595, + "velocityY": 1.9394818646157501, + "timestamp": 3.356540327161215 + }, + { + "x": 1.3044116698031423, + "y": 5.775906190222514, + "heading": -3.0431020650652782, + "angularVelocity": 2.0379201919217973, + "velocityX": -0.05941917163975373, + "velocityY": 1.9882529296776088, + "timestamp": 3.395684505283879 + }, + { + "x": 1.310735543939778, + "y": 5.856467416837523, + "heading": -2.9252067960348933, + "angularVelocity": 3.011821289514447, + "velocityX": 0.16155337625991328, + "velocityY": 2.058064071815707, + "timestamp": 3.434828683406543 + }, + { + "x": 1.3277929402719157, + "y": 5.939930987776361, + "heading": -2.7826616721505166, + "angularVelocity": 3.6415408553908346, + "velocityX": 0.43575819317718334, + "velocityY": 2.132209052321722, + "timestamp": 3.473972861529207 + }, + { + "x": 1.3576022335544815, + "y": 6.023554053808955, + "heading": -2.637362279761064, + "angularVelocity": 3.711903004685308, + "velocityX": 0.7615255885346219, + "velocityY": 2.136283606991328, + "timestamp": 3.513117039651871 + }, + { + "x": 1.4000554524537352, + "y": 6.106306047220255, + "heading": -2.4978098785002194, + "angularVelocity": 3.5650870181393826, + "velocityX": 1.0845346852403, + "velocityY": 2.1140306778695206, + "timestamp": 3.552261217774535 + }, + { + "x": 1.455157655616598, + "y": 6.18915881243997, + "heading": -2.364648055922988, + "angularVelocity": 3.4018295686257636, + "velocityX": 1.407673013090022, + "velocityY": 2.1166050532491445, + "timestamp": 3.591405395897199 + }, + { + "x": 1.5225297792403976, + "y": 6.27399259313729, + "heading": -2.2419731152862643, + "angularVelocity": 3.1339255674829567, + "velocityX": 1.7211275559977186, + "velocityY": 2.16721322980599, + "timestamp": 3.630549574019863 + }, + { + "x": 1.596991898632741, + "y": 6.365541704289421, + "heading": -2.152688244347013, + "angularVelocity": 2.2809233766376438, + "velocityX": 1.9022527222057353, + "velocityY": 2.3387669774352777, + "timestamp": 3.669693752142527 + }, + { + "x": 1.679413821374646, + "y": 6.463137271673749, + "heading": -2.0939697752685182, + "angularVelocity": 1.5000562508808342, + "velocityX": 2.1055985000789685, + "velocityY": 2.4932332741410055, + "timestamp": 3.708837930265191 + }, + { + "x": 1.7662309748150582, + "y": 6.5526721858065455, + "heading": -2.030942574296911, + "angularVelocity": 1.6101296283218078, + "velocityX": 2.2178816264415735, + "velocityY": 2.2873111258646657, + "timestamp": 3.747982108387855 + }, + { + "x": 1.8463290740680989, + "y": 6.633589645928569, + "heading": -1.9424440862784471, + "angularVelocity": 2.260833979989085, + "velocityX": 2.0462327501689237, + "velocityY": 2.0671646207121097, + "timestamp": 3.787126286510519 + }, + { + "x": 1.9201306346515887, + "y": 6.705414092889608, + "heading": -1.8297336528989434, + "angularVelocity": 2.8793664546055737, + "velocityX": 1.88537770169097, + "velocityY": 1.8348692042011108, + "timestamp": 3.826270464633183 + }, + { + "x": 1.9881022552719625, + "y": 6.7678945659852445, + "heading": -1.6930072188425063, + "angularVelocity": 3.4928932120629863, + "velocityX": 1.73644265584974, + "velocityY": 1.596162599195345, + "timestamp": 3.865414642755847 + }, + { + "x": 2.0508860920717975, + "y": 6.821005360416637, + "heading": -1.5311822314903374, + "angularVelocity": 4.134075489976246, + "velocityX": 1.6039125052796561, + "velocityY": 1.3567993244094194, + "timestamp": 3.904558820878511 + }, + { + "x": 2.105765126126845, + "y": 6.86764701678895, + "heading": -1.3406160520225758, + "angularVelocity": 4.868314743270257, + "velocityX": 1.401971804927838, + "velocityY": 1.1915349512807134, + "timestamp": 3.943702999001175 + }, + { + "x": 2.1526116639678397, + "y": 6.908337111426706, + "heading": -1.1177858581420712, + "angularVelocity": 5.692550069188692, + "velocityX": 1.1967689727497768, + "velocityY": 1.039492884746424, + "timestamp": 3.982847177123839 + }, + { + "x": 2.1904354446918113, + "y": 6.943500988064586, + "heading": -0.8664176462137954, + "angularVelocity": 6.421598919271669, + "velocityX": 0.966268358105399, + "velocityY": 0.8983168972839031, + "timestamp": 4.021991355246503 + }, + { + "x": 2.218208118213364, + "y": 6.971474589606098, + "heading": -0.6268162193807357, + "angularVelocity": 6.120997765804011, + "velocityX": 0.7094969125299588, + "velocityY": 0.7146299368925976, + "timestamp": 4.061135533369167 + }, + { + "x": 2.2378592256547205, + "y": 6.992029926313173, + "heading": -0.4114319933826866, + "angularVelocity": 5.502331031784905, + "velocityX": 0.5020186496131576, + "velocityY": 0.5251186177076611, + "timestamp": 4.10027971149183 + }, + { + "x": 2.2511803788337867, + "y": 7.004798124284798, + "heading": -0.22575032024531463, + "angularVelocity": 4.743532296310119, + "velocityX": 0.3403099469178361, + "velocityY": 0.32618383075033625, + "timestamp": 4.139423889614494 + }, + { + "x": 2.261045669673148, + "y": 7.01480850997709, + "heading": -0.09556543349291266, + "angularVelocity": 3.32577903013953, + "velocityX": 0.25202447241187803, + "velocityY": 0.2557311501323063, + "timestamp": 4.1785680677371575 + }, + { + "x": 2.266437621623355, + "y": 7.022862452660783, + "heading": -0.01995585424474321, + "angularVelocity": 1.931566400787266, + "velocityX": 0.1377459486647134, + "velocityY": 0.2057507161972769, + "timestamp": 4.217712245859821 + }, + { + "x": 2.2676985263824463, + "y": 7.028899669647217, + "heading": -4.525227980572114e-34, + "angularVelocity": 0.509803889155846, + "velocityX": 0.03221180823213118, + "velocityY": 0.1542302655458861, + "timestamp": 4.256856423982485 + }, + { + "x": 2.265418134279513, + "y": 7.032677037086849, + "heading": -0.03924023540233394, + "angularVelocity": -0.9804537079645166, + "velocityX": -0.056977713563887346, + "velocityY": 0.09438103198308487, + "timestamp": 4.2968789513935945 + }, + { + "x": 2.260299136724811, + "y": 7.032742565757645, + "heading": -0.13692518133038253, + "angularVelocity": -2.440749054267192, + "velocityX": -0.1279029058340032, + "velocityY": 0.0016372946696696046, + "timestamp": 4.3369014788047044 + }, + { + "x": 2.251431996366295, + "y": 7.026515227476796, + "heading": -0.2837029990611584, + "angularVelocity": -3.667380028829281, + "velocityX": -0.22155373316216304, + "velocityY": -0.15559582774173908, + "timestamp": 4.376924006215814 + }, + { + "x": 2.2367988748775502, + "y": 7.013230050225817, + "heading": -0.4715484199670708, + "angularVelocity": -4.693492216930044, + "velocityX": -0.36562212422103535, + "velocityY": -0.3319424861532188, + "timestamp": 4.416946533626924 + }, + { + "x": 2.214572389947544, + "y": 6.992840585930019, + "heading": -0.6937250351049046, + "angularVelocity": -5.551288974223076, + "velocityX": -0.5553493586673437, + "velocityY": -0.5094496928282007, + "timestamp": 4.456969061038034 + }, + { + "x": 2.182927977382888, + "y": 6.964829235368663, + "heading": -0.9374735446804039, + "angularVelocity": -6.090282781787505, + "velocityX": -0.7906650232157135, + "velocityY": -0.6998895965171087, + "timestamp": 4.496991588449144 + }, + { + "x": 2.1407843072832162, + "y": 6.928562668933627, + "heading": -1.1813002792094054, + "angularVelocity": -6.092237304866431, + "velocityX": -1.0529987191157046, + "velocityY": -0.9061538283803887, + "timestamp": 4.537014115860254 + }, + { + "x": 2.0895949461282366, + "y": 6.88678871775435, + "heading": -1.3936812170017512, + "angularVelocity": -5.3065348824870116, + "velocityX": -1.27901370718457, + "velocityY": -1.0437609486821178, + "timestamp": 4.577036643271364 + }, + { + "x": 2.0327245845451865, + "y": 6.835876323976478, + "heading": -1.5775207555282802, + "angularVelocity": -4.593401527048441, + "velocityX": -1.420958776513053, + "velocityY": -1.2720934201606515, + "timestamp": 4.617059170682474 + }, + { + "x": 1.969383401794422, + "y": 6.775670280789337, + "heading": -1.7350606577956689, + "angularVelocity": -3.9362807013446637, + "velocityX": -1.582638250206598, + "velocityY": -1.5043038778812523, + "timestamp": 4.657081698093584 + }, + { + "x": 1.8991012602387887, + "y": 6.706204931476766, + "heading": -1.8673610734924142, + "angularVelocity": -3.305648699737559, + "velocityX": -1.7560645491899718, + "velocityY": -1.7356562367745094, + "timestamp": 4.6971042255046935 + }, + { + "x": 1.8215077032950175, + "y": 6.627747409048215, + "heading": -1.9742080738130303, + "angularVelocity": -2.6696714883367685, + "velocityX": -1.938747049798569, + "velocityY": -1.960334029448928, + "timestamp": 4.737126752915803 + }, + { + "x": 1.7362669499337922, + "y": 6.540782802917572, + "heading": -2.0543346433472873, + "angularVelocity": -2.002036720749795, + "velocityX": -2.1298193511278303, + "velocityY": -2.172891412812259, + "timestamp": 4.777149280326913 + }, + { + "x": 1.6430739434891832, + "y": 6.445951687040137, + "heading": -2.1059605165739685, + "angularVelocity": -1.2899203665071555, + "velocityX": -2.328513776437315, + "velocityY": -2.3694434612620463, + "timestamp": 4.817171807738023 + }, + { + "x": 1.55728497087458, + "y": 6.34602370365502, + "heading": -2.1651565461839404, + "angularVelocity": -1.4790677510671213, + "velocityX": -2.143517118081594, + "velocityY": -2.496793427328095, + "timestamp": 4.857194335149133 + }, + { + "x": 1.4798885640406532, + "y": 6.2518908184674045, + "heading": -2.25892978627056, + "angularVelocity": -2.3430114526097965, + "velocityX": -1.9338210712910326, + "velocityY": -2.351997519314229, + "timestamp": 4.897216862560243 + }, + { + "x": 1.4136471471462948, + "y": 6.162688939074268, + "heading": -2.372895153669848, + "angularVelocity": -2.847530497727978, + "velocityX": -1.6551032925514553, + "velocityY": -2.2287917621208337, + "timestamp": 4.937239389971353 + }, + { + "x": 1.3600641421628377, + "y": 6.075598413595313, + "heading": -2.5001562416185403, + "angularVelocity": -3.179736417979598, + "velocityX": -1.3388211202419749, + "velocityY": -2.1760376246199, + "timestamp": 4.977261917382463 + }, + { + "x": 1.319394826243984, + "y": 5.989620775668987, + "heading": -2.6390216185129676, + "angularVelocity": -3.4696803494694044, + "velocityX": -1.016160611275252, + "velocityY": -2.148231096031668, + "timestamp": 5.017284444793573 + }, + { + "x": 1.2917069915865356, + "y": 5.904139479101947, + "heading": -2.789119490409166, + "angularVelocity": -3.75033466413552, + "velocityX": -0.6918062513404106, + "velocityY": -2.1358295464196915, + "timestamp": 5.057306972204683 + }, + { + "x": 1.2763632236019438, + "y": 5.822310174000512, + "heading": -2.9261084847899674, + "angularVelocity": -3.422797190533623, + "velocityX": -0.3833782866079771, + "velocityY": -2.0445811495333315, + "timestamp": 5.0973294996157925 + }, + { + "x": 1.2717668608990054, + "y": 5.744491133376307, + "heading": -3.036631348807283, + "angularVelocity": -2.7615163550774677, + "velocityX": -0.11484438890439615, + "velocityY": -1.9443809688691047, + "timestamp": 5.137352027026902 + }, + { + "x": 1.2769543336934923, + "y": 5.67003274843766, + "heading": -3.1136420724595726, + "angularVelocity": -1.9241844189708244, + "velocityX": 0.12961382326511559, + "velocityY": -1.8604118668922167, + "timestamp": 5.177374554438012 + }, + { + "x": 1.290967939745204, + "y": 5.598089181810031, + "heading": -3.1506602139394917, + "angularVelocity": -0.9249326285586663, + "velocityX": 0.35014295593490485, + "velocityY": -1.7975767969031158, + "timestamp": 5.217397081849122 + }, + { + "x": 1.3126477003097534, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": 0.23673934088618437, + "velocityX": 0.5416889428759926, + "velocityY": -1.748613278454628, + "timestamp": 5.257419609260232 + }, + { + "x": 1.3612675355497719, + "y": 5.427793675126224, + "heading": -3.0142432356702726, + "angularVelocity": 2.1028160607727164, + "velocityX": 0.8053954784197191, + "velocityY": -1.6616777008649257, + "timestamp": 5.317787262845048 + }, + { + "x": 1.4285233143505713, + "y": 5.3377057272616115, + "heading": -2.7956250008289785, + "angularVelocity": 3.6214466168399086, + "velocityX": 1.1141029145071253, + "velocityY": -1.4923215085383357, + "timestamp": 5.378154916429864 + }, + { + "x": 1.5158281848971034, + "y": 5.264153119824496, + "heading": -2.514690848185286, + "angularVelocity": 4.653719930475422, + "velocityX": 1.4462193801167051, + "velocityY": -1.218410905001898, + "timestamp": 5.43852257001468 + }, + { + "x": 1.6230211167401283, + "y": 5.213340726984806, + "heading": -2.222036602459031, + "angularVelocity": 4.847865178577475, + "velocityX": 1.7756683501441766, + "velocityY": -0.841715551662108, + "timestamp": 5.498890223599496 + }, + { + "x": 1.749062978630215, + "y": 5.185595086249832, + "heading": -1.9510224888468373, + "angularVelocity": 4.489392870495166, + "velocityX": 2.0879039420175545, + "velocityY": -0.45961105140506303, + "timestamp": 5.5592578771843115 + }, + { + "x": 1.894299674373998, + "y": 5.179254614564922, + "heading": -1.7150026509570102, + "angularVelocity": 3.9097070015852564, + "velocityX": 2.4058694867066146, + "velocityY": -0.10503094469295028, + "timestamp": 5.619625530769127 + }, + { + "x": 2.058119871692227, + "y": 5.193402237247618, + "heading": -1.5227916684845806, + "angularVelocity": 3.1840061863987503, + "velocityX": 2.713708212761054, + "velocityY": 0.2343576707485958, + "timestamp": 5.679993184353943 + }, + { + "x": 2.2389183972423563, + "y": 5.227931703588396, + "heading": -1.382086082537418, + "angularVelocity": 2.3308109159729624, + "velocityX": 2.9949569813262746, + "velocityY": 0.5719862259059945, + "timestamp": 5.740360837938759 + }, + { + "x": 2.4352711849705373, + "y": 5.282232257814921, + "heading": -1.300964814671627, + "angularVelocity": 1.3437869959914348, + "velocityX": 3.252615864095299, + "velocityY": 0.8994975123595437, + "timestamp": 5.800728491523575 + }, + { + "x": 2.6508698274310176, + "y": 5.350520182077314, + "heading": -1.2935554192187855, + "angularVelocity": 0.12273784076155059, + "velocityX": 3.5714265779366063, + "velocityY": 1.1312005719494984, + "timestamp": 5.861096145108391 + }, + { + "x": 2.8627491717000777, + "y": 5.435718232774483, + "heading": -1.2935551586148022, + "angularVelocity": 0.000004316947366860679, + "velocityX": 3.5098157984784524, + "velocityY": 1.4113195666528, + "timestamp": 5.921463798693207 + }, + { + "x": 3.0746283658136546, + "y": 5.520916656893941, + "heading": -1.2935548980122595, + "angularVelocity": 0.000004316923507170679, + "velocityX": 3.5098133111284437, + "velocityY": 1.4113257524537706, + "timestamp": 5.981831452278023 + }, + { + "x": 3.2865075599250586, + "y": 5.606115081018762, + "heading": -1.2935546374096347, + "angularVelocity": 0.000004316924865568706, + "velocityX": 3.509813311092456, + "velocityY": 1.4113257525426077, + "timestamp": 6.042199105862839 + }, + { + "x": 3.4983867540364426, + "y": 5.691313505143594, + "heading": -1.293554376806928, + "angularVelocity": 0.0000043169262224547675, + "velocityX": 3.5098133110921186, + "velocityY": 1.4113257525427836, + "timestamp": 6.1025667594476545 + }, + { + "x": 3.710265948147806, + "y": 5.776511929268437, + "heading": -1.293554116204139, + "angularVelocity": 0.000004316927581395022, + "velocityX": 3.5098133110917824, + "velocityY": 1.4113257525429583, + "timestamp": 6.16293441303247 + }, + { + "x": 3.922145142259149, + "y": 5.861710353393289, + "heading": -1.2935538556012682, + "angularVelocity": 0.000004316928938543054, + "velocityX": 3.509813311091446, + "velocityY": 1.4113257525431333, + "timestamp": 6.223302066617286 + }, + { + "x": 4.134024336370472, + "y": 5.9469087775181535, + "heading": -1.2935535949983157, + "angularVelocity": 0.000004316930294815631, + "velocityX": 3.5098133110911096, + "velocityY": 1.4113257525433083, + "timestamp": 6.283669720202102 + }, + { + "x": 4.345903530481774, + "y": 6.0321072016430275, + "heading": -1.293553334395281, + "angularVelocity": 0.000004316931652676853, + "velocityX": 3.509813311090773, + "velocityY": 1.4113257525434828, + "timestamp": 6.344037373786918 + }, + { + "x": 4.557782724593057, + "y": 6.117305625767913, + "heading": -1.2935530737921643, + "angularVelocity": 0.000004316933011809188, + "velocityX": 3.509813311090437, + "velocityY": 1.4113257525436576, + "timestamp": 6.404405027371734 + }, + { + "x": 4.769661918704319, + "y": 6.202504049892808, + "heading": -1.2935528131889658, + "angularVelocity": 0.000004316934368820573, + "velocityX": 3.5098133110901, + "velocityY": 1.4113257525438323, + "timestamp": 6.46477268095655 + }, + { + "x": 4.9815411128155604, + "y": 6.287702474017714, + "heading": -1.2935525525856855, + "angularVelocity": 0.000004316935725471939, + "velocityX": 3.5098133110897636, + "velocityY": 1.4113257525440075, + "timestamp": 6.525140334541366 + }, + { + "x": 5.193420306926782, + "y": 6.37290089814263, + "heading": -1.293552291982323, + "angularVelocity": 0.000004316937084312971, + "velocityX": 3.5098133110894274, + "velocityY": 1.4113257525441825, + "timestamp": 6.5855079881261815 + }, + { + "x": 5.405299501037983, + "y": 6.458099322267557, + "heading": -1.2935520313788786, + "angularVelocity": 0.000004316938442183061, + "velocityX": 3.509813311089091, + "velocityY": 1.411325752544357, + "timestamp": 6.645875641710997 + }, + { + "x": 5.617178695149163, + "y": 6.543297746392494, + "heading": -1.2935517707753523, + "angularVelocity": 0.000004316939799293205, + "velocityX": 3.509813311088755, + "velocityY": 1.411325752544532, + "timestamp": 6.706243295295813 + }, + { + "x": 5.829057889260324, + "y": 6.628496170517443, + "heading": -1.293551510171744, + "angularVelocity": 0.000004316941156612923, + "velocityX": 3.5098133110884175, + "velocityY": 1.4113257525447067, + "timestamp": 6.766610948880629 + }, + { + "x": 6.040937083371463, + "y": 6.713694594642401, + "heading": -1.293551249568054, + "angularVelocity": 0.0000043169425150049, + "velocityX": 3.5098133110880814, + "velocityY": 1.411325752544882, + "timestamp": 6.826978602465445 + }, + { + "x": 6.252816277482583, + "y": 6.798893018767371, + "heading": -1.2935509889642816, + "angularVelocity": 0.000004316943872175867, + "velocityX": 3.5098133110877447, + "velocityY": 1.4113257525450569, + "timestamp": 6.887346256050261 + }, + { + "x": 6.464695471593406, + "y": 6.884091442893039, + "heading": -1.293550728360427, + "angularVelocity": 0.000004316945230336038, + "velocityX": 3.509813311082821, + "velocityY": 1.4113257525566392, + "timestamp": 6.947713909635077 + }, + { + "x": 6.676574646384632, + "y": 6.969289915063695, + "heading": -1.2935504677556948, + "angularVelocity": 0.000004316959775515561, + "velocityX": 3.5098129910505658, + "velocityY": 1.41132654842967, + "timestamp": 7.008081563219893 + }, + { + "x": 6.8805324076089756, + "y": 7.05464814554522, + "heading": -1.2707475795927152, + "angularVelocity": 0.3777335511465864, + "velocityX": 3.3785934869538337, + "velocityY": 1.4139729708327582, + "timestamp": 7.068449216804709 + }, + { + "x": 7.061472859527864, + "y": 7.1337324563685955, + "heading": -1.1711733781498967, + "angularVelocity": 1.6494628419326967, + "velocityX": 2.997308014708086, + "velocityY": 1.3100444712873218, + "timestamp": 7.1288168703895245 + }, + { + "x": 7.217120419361732, + "y": 7.210021270677617, + "heading": -1.009883648548617, + "angularVelocity": 2.6717906034673087, + "velocityX": 2.5783271436115207, + "velocityY": 1.263736616859493, + "timestamp": 7.18918452397434 + }, + { + "x": 7.347594186911189, + "y": 7.2775930773892314, + "heading": -0.7947029660436775, + "angularVelocity": 3.5645030032948655, + "velocityX": 2.1613191800828018, + "velocityY": 1.119337968249467, + "timestamp": 7.249552177559156 + }, + { + "x": 7.451332936822743, + "y": 7.3349655169215735, + "heading": -0.5434124114773135, + "angularVelocity": 4.162668907005051, + "velocityX": 1.718449264651363, + "velocityY": 0.9503837920706217, + "timestamp": 7.309919831143972 + }, + { + "x": 7.527427510377051, + "y": 7.381739778721793, + "heading": -0.31640528763659864, + "angularVelocity": 3.7604099275081686, + "velocityX": 1.2605189871658125, + "velocityY": 0.7748232542201173, + "timestamp": 7.370287484728788 + }, + { + "x": 7.577573442921129, + "y": 7.419722379939866, + "heading": -0.13731969717190406, + "angularVelocity": 2.966581933039389, + "velocityX": 0.8306755284702909, + "velocityY": 0.6291879667760935, + "timestamp": 7.430655138313604 + }, + { + "x": 7.603931547937356, + "y": 7.45112329376933, + "heading": -0.02759055048880514, + "angularVelocity": 1.8176811614672275, + "velocityX": 0.4366262965512501, + "velocityY": 0.5201612447193243, + "timestamp": 7.49102279189842 + }, + { + "x": 7.608186721801758, + "y": 7.477188587188721, + "heading": 0, + "angularVelocity": 0.4570419562529598, + "velocityX": 0.07048764713743307, + "velocityY": 0.43177582482595267, + "timestamp": 7.551390445483236 + }, + { + "x": 7.590312649337569, + "y": 7.4994357327814924, + "heading": -0.062413166933384966, + "angularVelocity": -1.0141265252388012, + "velocityX": -0.2904286369464457, + "velocityY": 0.3614849488499569, + "timestamp": 7.612934212833851 + }, + { + "x": 7.548542604069605, + "y": 7.5172581696138145, + "heading": -0.20295351245592444, + "angularVelocity": -2.2835837254141333, + "velocityX": -0.6787047180585992, + "velocityY": 0.28958963026732126, + "timestamp": 7.6744779801844665 + }, + { + "x": 7.481084260938195, + "y": 7.5242062844896465, + "heading": -0.3946635775555263, + "angularVelocity": -3.1150199825666713, + "velocityX": -1.0961035704411768, + "velocityY": 0.11289713280385957, + "timestamp": 7.736021747535082 + }, + { + "x": 7.388848586007302, + "y": 7.518355172240059, + "heading": -0.6407957549264521, + "angularVelocity": -3.9993030645769183, + "velocityX": -1.4987005004978822, + "velocityY": -0.09507237696798558, + "timestamp": 7.797565514885697 + }, + { + "x": 7.269389686365513, + "y": 7.4992097780564855, + "heading": -0.9197330786516805, + "angularVelocity": -4.5323407346209255, + "velocityX": -1.941039763803056, + "velocityY": -0.31108583383434507, + "timestamp": 7.859109282236313 + }, + { + "x": 7.1251329311926535, + "y": 7.464894000432003, + "heading": -1.1518882810190125, + "angularVelocity": -3.772196801745026, + "velocityX": -2.343970175745461, + "velocityY": -0.5575833118727538, + "timestamp": 7.920653049586928 + }, + { + "x": 6.953983810333919, + "y": 7.422493842128991, + "heading": -1.327948941925082, + "angularVelocity": -2.8607390883799795, + "velocityX": -2.780933443409385, + "velocityY": -0.6889431721242942, + "timestamp": 7.9821968169375435 + }, + { + "x": 6.759450448912011, + "y": 7.372163465988786, + "heading": -1.4240765574561767, + "angularVelocity": -1.5619390828555313, + "velocityX": -3.1608945925206284, + "velocityY": -0.817798102177794, + "timestamp": 8.043740584288159 + }, + { + "x": 6.542371526128891, + "y": 7.310669881087202, + "heading": -1.4457611917111317, + "angularVelocity": -0.3523449276580275, + "velocityX": -3.5272283795435984, + "velocityY": -0.9991846055060891, + "timestamp": 8.105284351638774 + }, + { + "x": 6.323008319060415, + "y": 7.232674892951184, + "heading": -1.4457615219539912, + "angularVelocity": -0.000005365983815098308, + "velocityX": -3.564344799023447, + "velocityY": -1.2673092905034502, + "timestamp": 8.16682811898939 + }, + { + "x": 6.103645219000666, + "y": 7.154679603847749, + "heading": -1.4457618521936861, + "angularVelocity": -0.000005365932394056072, + "velocityX": -3.5643430602815576, + "velocityY": -1.267314180802329, + "timestamp": 8.228371886340005 + }, + { + "x": 5.884282118942346, + "y": 7.076684314740222, + "heading": -1.4457621824332476, + "angularVelocity": -0.000005365930225246195, + "velocityX": -3.5643430602583215, + "velocityY": -1.2673141808688406, + "timestamp": 8.28991565369062 + }, + { + "x": 5.66491901888401, + "y": 6.998689025632669, + "heading": -1.4457625126726754, + "angularVelocity": -0.000005365928056443698, + "velocityX": -3.564343060258591, + "velocityY": -1.2673141808692443, + "timestamp": 8.351459421041236 + }, + { + "x": 5.445555918825658, + "y": 6.920693736525091, + "heading": -1.44576284291197, + "angularVelocity": -0.000005365925887921031, + "velocityX": -3.5643430602588597, + "velocityY": -1.2673141808696466, + "timestamp": 8.413003188391851 + }, + { + "x": 5.226192818767289, + "y": 6.842698447417488, + "heading": -1.445763173151131, + "angularVelocity": -0.000005365923719151191, + "velocityX": -3.564343060259129, + "velocityY": -1.2673141808700494, + "timestamp": 8.474546955742467 + }, + { + "x": 5.006829718708903, + "y": 6.764703158309861, + "heading": -1.4457635033901584, + "angularVelocity": -0.000005365921549364942, + "velocityX": -3.5643430602593975, + "velocityY": -1.2673141808704518, + "timestamp": 8.536090723093082 + }, + { + "x": 4.787466618650501, + "y": 6.68670786920221, + "heading": -1.4457638336290524, + "angularVelocity": -0.000005365919381086304, + "velocityX": -3.564343060259667, + "velocityY": -1.2673141808708541, + "timestamp": 8.597634490443697 + }, + { + "x": 4.568103518592082, + "y": 6.608712580094533, + "heading": -1.4457641638678127, + "angularVelocity": -0.000005365917211650528, + "velocityX": -3.5643430602599366, + "velocityY": -1.2673141808712571, + "timestamp": 8.659178257794313 + }, + { + "x": 4.348740418533646, + "y": 6.530717290986832, + "heading": -1.4457644941064398, + "angularVelocity": -0.000005365915042310373, + "velocityX": -3.564343060260205, + "velocityY": -1.2673141808716597, + "timestamp": 8.720722025144928 + }, + { + "x": 4.1293773184751945, + "y": 6.452722001879106, + "heading": -1.4457648243449335, + "angularVelocity": -0.000005365912874367381, + "velocityX": -3.564343060260474, + "velocityY": -1.267314180872062, + "timestamp": 8.782265792495544 + }, + { + "x": 3.910014218416726, + "y": 6.374726712771355, + "heading": -1.4457651545832937, + "angularVelocity": -0.000005365910705198878, + "velocityX": -3.5643430602607435, + "velocityY": -1.2673141808724648, + "timestamp": 8.843809559846159 + }, + { + "x": 3.690651118358241, + "y": 6.296731423663579, + "heading": -1.4457654848215202, + "angularVelocity": -0.000005365908536510064, + "velocityX": -3.5643430602610127, + "velocityY": -1.2673141808728674, + "timestamp": 8.905353327196774 + }, + { + "x": 3.47128801829974, + "y": 6.218736134555778, + "heading": -1.4457658150596133, + "angularVelocity": -0.0000053659063672733, + "velocityX": -3.5643430602612813, + "velocityY": -1.2673141808732702, + "timestamp": 8.96689709454739 + }, + { + "x": 3.2519249182412215, + "y": 6.140740845447953, + "heading": -1.445766145297573, + "angularVelocity": -0.000005365904199203489, + "velocityX": -3.564343060261551, + "velocityY": -1.2673141808736725, + "timestamp": 9.028440861898005 + }, + { + "x": 3.0325618181826868, + "y": 6.062745556340103, + "heading": -1.4457664755353992, + "angularVelocity": -0.000005365902029653181, + "velocityX": -3.5643430602618196, + "velocityY": -1.267314180874075, + "timestamp": 9.08998462924862 + }, + { + "x": 2.8131987181241356, + "y": 5.984750267232228, + "heading": -1.445766805773092, + "angularVelocity": -0.000005365899860579176, + "velocityX": -3.5643430602620887, + "velocityY": -1.2673141808744774, + "timestamp": 9.151528396599236 + }, + { + "x": 2.5938356180647952, + "y": 5.906754978126503, + "heading": -1.4457671360106512, + "angularVelocity": -0.000005365897692675495, + "velocityX": -3.564343060274917, + "velocityY": -1.2673141808395556, + "timestamp": 9.213072163949851 + }, + { + "x": 2.374472460827342, + "y": 5.8287598498370325, + "heading": -1.4457674662499087, + "angularVelocity": -0.000005365925286950734, + "velocityX": -3.5643439893391378, + "velocityY": -1.2673115678006912, + "timestamp": 9.274615931300467 + }, + { + "x": 2.1579642030031834, + "y": 5.762355072468581, + "heading": -1.4644178853402519, + "angularVelocity": -0.30304318200235486, + "velocityX": -3.5179558734308305, + "velocityY": -1.0789846027810281, + "timestamp": 9.336159698651082 + }, + { + "x": 1.9655559601169903, + "y": 5.705313391080233, + "heading": -1.5528631085528386, + "angularVelocity": -1.4371109702906752, + "velocityX": -3.1263643934899394, + "velocityY": -0.9268474102890126, + "timestamp": 9.397703466001698 + }, + { + "x": 1.799063271019786, + "y": 5.655117829615961, + "heading": -1.7042345036546405, + "angularVelocity": -2.459573107369865, + "velocityX": -2.7052729506904143, + "velocityY": -0.8156075525618497, + "timestamp": 9.459247233352313 + }, + { + "x": 1.6591648707007773, + "y": 5.609591955542646, + "heading": -1.9150308855459262, + "angularVelocity": -3.425145891547008, + "velocityX": -2.273153015186835, + "velocityY": -0.739731674435107, + "timestamp": 9.520791000702928 + }, + { + "x": 1.5431948510849691, + "y": 5.574795624095963, + "heading": -2.189608491806892, + "angularVelocity": -4.461501433552055, + "velocityX": -1.884350351110707, + "velocityY": -0.5653916382539252, + "timestamp": 9.582334768053544 + }, + { + "x": 1.4525455234391338, + "y": 5.551032580854212, + "heading": -2.523374227659994, + "angularVelocity": -5.423225620096252, + "velocityX": -1.4729245795013663, + "velocityY": -0.3861161619563059, + "timestamp": 9.64387853540416 + }, + { + "x": 1.3896696664094041, + "y": 5.537613504449024, + "heading": -2.816143624187386, + "angularVelocity": -4.757092539679631, + "velocityX": -1.0216445911009162, + "velocityY": -0.21804119219317228, + "timestamp": 9.705422302754775 + }, + { + "x": 1.3505170646137672, + "y": 5.530954725925202, + "heading": -3.0286369343399735, + "angularVelocity": -3.4527185984896, + "velocityX": -0.6361749285282489, + "velocityY": -0.1081958224280782, + "timestamp": 9.76696607010539 + }, + { + "x": 1.332138180732727, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": -1.828753384537267, + "velocityX": -0.2986311152571343, + "velocityY": -0.046299846535530764, + "timestamp": 9.828509837456005 + }, + { + "x": 1.332138180732727, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": -8.15091951330742e-35, + "velocityX": 0, + "velocityY": 2.0839850321711294e-35, + "timestamp": 9.89005360480662 + } + ] +} \ No newline at end of file diff --git a/deploy/choreo/NewPath.traj b/deploy/choreo/NewPath.traj new file mode 100644 index 00000000..92058ea3 --- /dev/null +++ b/deploy/choreo/NewPath.traj @@ -0,0 +1,1876 @@ +{ + "samples": [ + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": 0, + "angularVelocity": 0, + "velocityX": 2.2261748232194093e-37, + "velocityY": 3.7157891353428806e-37, + "timestamp": 0 + }, + { + "x": 1.366635963151808, + "y": 5.509495705296941, + "heading": 0.06192923642991932, + "angularVelocity": 0.7673697851265452, + "velocityX": 0.4274601622031667, + "velocityY": -0.47210922509207903, + "timestamp": 0.08070325106650729 + }, + { + "x": 1.4355761410472572, + "y": 5.43294196238783, + "heading": 0.18410628088281034, + "angularVelocity": 1.5139048655202914, + "velocityX": 0.8542428834575204, + "velocityY": -0.9485831350960487, + "timestamp": 0.16140650213301458 + }, + { + "x": 1.5392732266735116, + "y": 5.317548823896549, + "heading": 0.36164871867370346, + "angularVelocity": 2.1999415815923076, + "velocityX": 1.2849183181083754, + "velocityY": -1.429844981042792, + "timestamp": 0.24210975319952188 + }, + { + "x": 1.67847556185222, + "y": 5.162242328350244, + "heading": 0.5745390252788012, + "angularVelocity": 2.637939659080841, + "velocityX": 1.724866512056522, + "velocityY": -1.9244143636582545, + "timestamp": 0.32281300426602916 + }, + { + "x": 1.8501216668381886, + "y": 4.967122032744555, + "heading": 0.7214821568069908, + "angularVelocity": 1.8207832966616715, + "velocityX": 2.1268796822635494, + "velocityY": -2.417750128119265, + "timestamp": 0.40351625533253643 + }, + { + "x": 1.999355457938739, + "y": 4.729284669626315, + "heading": 0.7742712340167793, + "angularVelocity": 0.6541133908754838, + "velocityX": 1.849167030180327, + "velocityY": -2.9470604960169275, + "timestamp": 0.4842195063990437 + }, + { + "x": 2.1155301056872418, + "y": 4.524468188873081, + "heading": 0.7178805227908414, + "angularVelocity": -0.6987415064538932, + "velocityX": 1.4395287205067313, + "velocityY": -2.537896281085937, + "timestamp": 0.564922757465551 + }, + { + "x": 2.1994300453605806, + "y": 4.354650955915648, + "heading": 0.5603443624430987, + "angularVelocity": -1.952042306423536, + "velocityX": 1.0396104068248448, + "velocityY": -2.104217992624444, + "timestamp": 0.6456260085320583 + }, + { + "x": 2.250899907873132, + "y": 4.221459437616645, + "heading": 0.3158282415144345, + "angularVelocity": -3.029817481914815, + "velocityX": 0.637766903220982, + "velocityY": -1.6503860320229218, + "timestamp": 0.7263292595985656 + }, + { + "x": 2.2676985263824463, + "y": 4.124765396118164, + "heading": 2.2216292217564286e-35, + "angularVelocity": -3.913451284064894, + "velocityX": 0.20815293420422745, + "velocityY": -1.1981430762782603, + "timestamp": 0.807032510665073 + }, + { + "x": 2.2676338555313906, + "y": 4.086690967677836, + "heading": -0.16755058667496922, + "angularVelocity": -4.348312733244185, + "velocityX": -0.0016783533301519722, + "velocityY": -0.9881166355999816, + "timestamp": 0.8455648327296736 + }, + { + "x": 2.259511405376126, + "y": 4.056750075909778, + "heading": -0.352256708092011, + "angularVelocity": -4.793537257042952, + "velocityX": -0.21079576106643969, + "velocityY": -0.7770331546035791, + "timestamp": 0.8840971547942744 + }, + { + "x": 2.2434268935883566, + "y": 4.035188434921252, + "heading": -0.5534507965852989, + "angularVelocity": -5.22143690577431, + "velocityX": -0.41742908098825227, + "velocityY": -0.5595728425704051, + "timestamp": 0.9226294768588752 + }, + { + "x": 2.2194223708397587, + "y": 4.022359555236857, + "heading": -0.7686869662578265, + "angularVelocity": -5.585860341135864, + "velocityX": -0.6229710918629213, + "velocityY": -0.33293814120224735, + "timestamp": 0.961161798923476 + }, + { + "x": 2.1874734502455304, + "y": 4.018617397576101, + "heading": -0.9939561131825075, + "angularVelocity": -5.846238556477593, + "velocityX": -0.8291459969805253, + "velocityY": -0.09711736693372348, + "timestamp": 0.9996941209880768 + }, + { + "x": 2.1476082595993153, + "y": 4.024282627294418, + "heading": -1.2244083666470835, + "angularVelocity": -5.980751771933577, + "velocityX": -1.0345909228979182, + "velocityY": 0.14702539101636564, + "timestamp": 1.0382264430526775 + }, + { + "x": 2.1000522078031225, + "y": 4.039643723439363, + "heading": -1.4537096411283452, + "angularVelocity": -5.950881291213917, + "velocityX": -1.2341859833015967, + "velocityY": 0.3986548259197214, + "timestamp": 1.0767587651172783 + }, + { + "x": 2.045480509029303, + "y": 4.064764963970464, + "heading": -1.6725096563971036, + "angularVelocity": -5.678350110899954, + "velocityX": -1.4162577246792443, + "velocityY": 0.6519524177386574, + "timestamp": 1.115291087181879 + }, + { + "x": 1.9845995907026053, + "y": 4.099193425968209, + "heading": -1.8731412390257827, + "angularVelocity": -5.206838619596132, + "velocityX": -1.5799960932701846, + "velocityY": 0.8934956460714911, + "timestamp": 1.1538234092464799 + }, + { + "x": 1.9178343766904495, + "y": 4.143331162612739, + "heading": -2.0560809001827813, + "angularVelocity": -4.747693659631898, + "velocityX": -1.7327067364437998, + "velocityY": 1.1454730542979317, + "timestamp": 1.1923557313110806 + }, + { + "x": 1.846280610428652, + "y": 4.197802019829692, + "heading": -2.22146373822619, + "angularVelocity": -4.2920548044350495, + "velocityX": -1.856980384982645, + "velocityY": 1.413640660576612, + "timestamp": 1.2308880533756814 + }, + { + "x": 1.770704211681186, + "y": 4.2612245864567395, + "heading": -2.3576115042271706, + "angularVelocity": -3.5333392514659345, + "velocityX": -1.961376701377084, + "velocityY": 1.6459575553406294, + "timestamp": 1.2694203754402822 + }, + { + "x": 1.6915488741102875, + "y": 4.3341463409601095, + "heading": -2.466232613363222, + "angularVelocity": -2.8189608961002843, + "velocityX": -2.0542581741684773, + "velocityY": 1.8924827416607193, + "timestamp": 1.307952697504883 + }, + { + "x": 1.609995812708591, + "y": 4.417230005205154, + "heading": -2.5495370955191317, + "angularVelocity": -2.1619377626982037, + "velocityX": -2.1164844741246154, + "velocityY": 2.156207043680162, + "timestamp": 1.3464850195694837 + }, + { + "x": 1.5308705698098255, + "y": 4.511360824889822, + "heading": -2.6152779027765387, + "angularVelocity": -1.7061210883473432, + "velocityX": -2.053477150069217, + "velocityY": 2.442905452904058, + "timestamp": 1.3850173416340845 + }, + { + "x": 1.4606506153508025, + "y": 4.614058209006809, + "heading": -2.6783194867011537, + "angularVelocity": -1.636070201503133, + "velocityX": -1.822364983384516, + "velocityY": 2.6652269734694745, + "timestamp": 1.4235496636986853 + }, + { + "x": 1.4019701962785118, + "y": 4.720456086702728, + "heading": -2.7505666696039515, + "angularVelocity": -1.874976098810575, + "velocityX": -1.5228882124962768, + "velocityY": 2.761263064227987, + "timestamp": 1.462081985763286 + }, + { + "x": 1.3551627373603712, + "y": 4.8282147910275475, + "heading": -2.8346512790115326, + "angularVelocity": -2.1821838109473566, + "velocityX": -1.2147583226275982, + "velocityY": 2.7965795610282194, + "timestamp": 1.5006143078278869 + }, + { + "x": 1.3203598068196198, + "y": 4.936382222496368, + "heading": -2.9296853375556533, + "angularVelocity": -2.466346522921523, + "velocityX": -0.9032139428919735, + "velocityY": 2.8071869452215745, + "timestamp": 1.5391466298924876 + }, + { + "x": 1.2978728176694776, + "y": 5.0435429055984535, + "heading": -3.0274097940467435, + "angularVelocity": -2.53616837125081, + "velocityX": -0.583587698463697, + "velocityY": 2.7810595718167943, + "timestamp": 1.5776789519570884 + }, + { + "x": 1.286514559723769, + "y": 5.147138897368402, + "heading": -3.1101525298443558, + "angularVelocity": -2.1473591874087323, + "velocityX": -0.2947722155614252, + "velocityY": 2.688547853313019, + "timestamp": 1.6162112740216892 + }, + { + "x": 1.2850446377120623, + "y": 5.2469545287393595, + "heading": -3.168448488726713, + "angularVelocity": -1.5129106100748941, + "velocityX": -0.038147766159596044, + "velocityY": 2.5904390398173645, + "timestamp": 1.65474359608629 + }, + { + "x": 1.2925718642931594, + "y": 5.343394761009087, + "heading": -3.1958165160103977, + "angularVelocity": -0.7102615626901808, + "velocityX": 0.1953483770969554, + "velocityY": 2.5028398783764474, + "timestamp": 1.6932759181508907 + }, + { + "x": 1.3084372785500542, + "y": 5.436937425716649, + "heading": -3.187781062808085, + "angularVelocity": 0.20853799542215204, + "velocityX": 0.4117430096814787, + "velocityY": 2.4276415148491566, + "timestamp": 1.7318082402154915 + }, + { + "x": 1.3321385383605957, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": 1.2092641484304996, + "velocityX": 0.6151007398621232, + "velocityY": 2.366009322567054, + "timestamp": 1.7703405622800923 + }, + { + "x": 1.357521739751378, + "y": 5.604487249946148, + "heading": -3.0708796741945465, + "angularVelocity": 2.1424744850655806, + "velocityX": 0.7735206841904706, + "velocityY": 2.3276437448542358, + "timestamp": 1.8031557175947133 + }, + { + "x": 1.3882591250435519, + "y": 5.675559897172117, + "heading": -2.9834964247525093, + "angularVelocity": 2.6628930628008614, + "velocityX": 0.936682608918773, + "velocityY": 2.1658482656731963, + "timestamp": 1.8359708729093343 + }, + { + "x": 1.423715934589619, + "y": 5.741061467273705, + "heading": -2.8764865415416963, + "angularVelocity": 3.260989691648163, + "velocityX": 1.0805010430735036, + "velocityY": 1.9960767966380097, + "timestamp": 1.8687860282239552 + }, + { + "x": 1.4640545833350669, + "y": 5.800765385011703, + "heading": -2.7509368125913465, + "angularVelocity": 3.8259678415847733, + "velocityX": 1.2292688655194213, + "velocityY": 1.8194007362017863, + "timestamp": 1.9016011835385762 + }, + { + "x": 1.5093560477878591, + "y": 5.854065962524638, + "heading": -2.610542457820649, + "angularVelocity": 4.278338878016634, + "velocityX": 1.3805043437538693, + "velocityY": 1.6242671107878839, + "timestamp": 1.9344163388531972 + }, + { + "x": 1.5593454812172984, + "y": 5.900364561226932, + "heading": -2.4585168251320484, + "angularVelocity": 4.6327872359898405, + "velocityX": 1.5233642184581169, + "velocityY": 1.4108907380872764, + "timestamp": 1.9672314941678182 + }, + { + "x": 1.612840729658989, + "y": 5.938459321827602, + "heading": -2.302873266045166, + "angularVelocity": 4.74303892803867, + "velocityX": 1.6301994590242108, + "velocityY": 1.1608892365563526, + "timestamp": 2.000046649482439 + }, + { + "x": 1.6683469717714092, + "y": 5.967831924392925, + "heading": -2.1497463391084874, + "angularVelocity": 4.666347773415882, + "velocityX": 1.6914819259651337, + "velocityY": 0.89509259620163, + "timestamp": 2.03286180479706 + }, + { + "x": 1.7259927125934769, + "y": 5.988487486256733, + "heading": -1.9957888127587393, + "angularVelocity": 4.691659231036796, + "velocityX": 1.756680420049179, + "velocityY": 0.6294519000677786, + "timestamp": 2.065676960111681 + }, + { + "x": 1.7857676152473871, + "y": 6.000514617426619, + "heading": -1.8372643934887982, + "angularVelocity": 4.830829467362277, + "velocityX": 1.8215639109675918, + "velocityY": 0.36651148088664176, + "timestamp": 2.098492115426302 + }, + { + "x": 1.8473787764602114, + "y": 6.0041716221205474, + "heading": -1.6689363232818417, + "angularVelocity": 5.129583224369399, + "velocityX": 1.877521548263803, + "velocityY": 0.11144255326131712, + "timestamp": 2.131307270740923 + }, + { + "x": 1.9079302163194551, + "y": 5.9989341122190964, + "heading": -1.4983965075420516, + "angularVelocity": 5.196983348233757, + "velocityX": 1.8452278917681775, + "velocityY": -0.15960643340660682, + "timestamp": 2.164122426055544 + }, + { + "x": 1.9644851606047329, + "y": 5.9857778753833975, + "heading": -1.3328127647297614, + "angularVelocity": 5.045953347614136, + "velocityX": 1.7234397869840146, + "velocityY": -0.40091953579256995, + "timestamp": 2.196937581370165 + }, + { + "x": 2.0167542311341298, + "y": 5.964732325175136, + "heading": -1.1669030928960278, + "angularVelocity": 5.055885618795508, + "velocityX": 1.592833251229742, + "velocityY": -0.6413362974053785, + "timestamp": 2.229752736684786 + }, + { + "x": 2.0647189114008015, + "y": 5.935938401754757, + "heading": -0.9958716358792914, + "angularVelocity": 5.211965489023056, + "velocityX": 1.4616624485485903, + "velocityY": -0.8774580874084462, + "timestamp": 2.262567891999407 + }, + { + "x": 2.1085721264603117, + "y": 5.8997079709351175, + "heading": -0.8141851452694338, + "angularVelocity": 5.53666404647202, + "velocityX": 1.3363707908452565, + "velocityY": -1.1040761645731791, + "timestamp": 2.295383047314028 + }, + { + "x": 2.1457870640137746, + "y": 5.857488430688145, + "heading": -0.6354555413511925, + "angularVelocity": 5.446556696277672, + "velocityX": 1.134077751473621, + "velocityY": -1.2865866348089932, + "timestamp": 2.328198202628649 + }, + { + "x": 2.1756614189381724, + "y": 5.811166545923024, + "heading": -0.4679136928264175, + "angularVelocity": 5.105624121490153, + "velocityX": 0.9103828593213182, + "velocityY": -1.41160035114879, + "timestamp": 2.36101335794327 + }, + { + "x": 2.1979477640451273, + "y": 5.762830427028998, + "heading": -0.3159012603830297, + "angularVelocity": 4.632384975354892, + "velocityX": 0.6791479392152971, + "velocityY": -1.4729815669191495, + "timestamp": 2.393828513257891 + }, + { + "x": 2.213866689003856, + "y": 5.714154085995567, + "heading": -0.19000720732011314, + "angularVelocity": 3.8364606797037886, + "velocityX": 0.4851089323242057, + "velocityY": -1.483349402638423, + "timestamp": 2.426643668572512 + }, + { + "x": 2.2241033767575633, + "y": 5.665307000563528, + "heading": -0.09437057287174495, + "angularVelocity": 2.9144044430518514, + "velocityX": 0.31195000162458963, + "velocityY": -1.4885526203886283, + "timestamp": 2.459458823887133 + }, + { + "x": 2.228985536910567, + "y": 5.616279831781338, + "heading": -0.030711851451385126, + "angularVelocity": 1.9399183337704993, + "velocityX": 0.14877760309816687, + "velocityY": -1.4940404307745467, + "timestamp": 2.492273979201754 + }, + { + "x": 2.2287168502807617, + "y": 5.567087173461914, + "heading": -1.4593920868727723e-34, + "angularVelocity": 0.9359044976910779, + "velocityX": -0.008187882313191258, + "velocityY": -1.4990835133273062, + "timestamp": 2.525089134516375 + }, + { + "x": 2.2234617571142565, + "y": 5.5179127265043535, + "heading": -0.002801304121864836, + "angularVelocity": -0.08554590281020508, + "velocityX": -0.16047943019522937, + "velocityY": -1.5016836006283458, + "timestamp": 2.5578353447946145 + }, + { + "x": 2.213100127893157, + "y": 5.468804050064043, + "heading": -0.038527021013621016, + "angularVelocity": -1.090987829987043, + "velocityX": -0.31642224040761213, + "velocityY": -1.499675108143563, + "timestamp": 2.590581555072854 + }, + { + "x": 2.1974774857014916, + "y": 5.420028778568237, + "heading": -0.10642175719814694, + "angularVelocity": -2.0733616381142825, + "velocityX": -0.47708244889780627, + "velocityY": -1.4894936263271208, + "timestamp": 2.623327765351094 + }, + { + "x": 2.176386509447901, + "y": 5.371982692865446, + "heading": -0.2052939647176804, + "angularVelocity": -3.019348091868672, + "velocityX": -0.6440738050108257, + "velocityY": -1.4672258345179665, + "timestamp": 2.6560739756293335 + }, + { + "x": 2.1495039837265555, + "y": 5.325205721708642, + "heading": -0.3329902489476004, + "angularVelocity": -3.8995744284576235, + "velocityX": -0.8209354759811619, + "velocityY": -1.4284697606027426, + "timestamp": 2.688820185907573 + }, + { + "x": 2.1162335968543386, + "y": 5.280389038707846, + "heading": -0.4851442980051073, + "angularVelocity": -4.646462835383888, + "velocityX": -1.0160072444879351, + "velocityY": -1.368606706546959, + "timestamp": 2.7215663961858128 + }, + { + "x": 2.0756862755494496, + "y": 5.238416301489077, + "heading": -0.6539149158793239, + "angularVelocity": -5.153897701144575, + "velocityX": -1.2382294305314772, + "velocityY": -1.2817586176272708, + "timestamp": 2.7543126064640524 + }, + { + "x": 2.0277154786481417, + "y": 5.201197274104196, + "heading": -0.8280730192796384, + "angularVelocity": -5.318420113977116, + "velocityX": -1.4649266737649096, + "velocityY": -1.136590373928381, + "timestamp": 2.787058816742292 + }, + { + "x": 1.9735902732105297, + "y": 5.170441088651969, + "heading": -1.0002810429175644, + "angularVelocity": -5.258868802670591, + "velocityX": -1.6528692932012015, + "velocityY": -0.9392288509386596, + "timestamp": 2.8198050270205317 + }, + { + "x": 1.914416507688317, + "y": 5.147054029448202, + "heading": -1.1711019741635995, + "angularVelocity": -5.216509934877801, + "velocityX": -1.807041639915639, + "velocityY": -0.7141913218369506, + "timestamp": 2.8525512372987714 + }, + { + "x": 1.851848295060789, + "y": 5.131935997009135, + "heading": -1.3411756027953594, + "angularVelocity": -5.193688893666442, + "velocityX": -1.9107008748766678, + "velocityY": -0.46167273435953404, + "timestamp": 2.885297447577011 + }, + { + "x": 1.788148478011349, + "y": 5.125682164538362, + "heading": -1.5101885639025132, + "angularVelocity": -5.161298350895432, + "velocityX": -1.945257680451938, + "velocityY": -0.1909788161022263, + "timestamp": 2.9180436578552507 + }, + { + "x": 1.7253768027949503, + "y": 5.1283168367810115, + "heading": -1.680184610910991, + "angularVelocity": -5.191319714985216, + "velocityX": -1.9169141919946477, + "velocityY": 0.0804573176640312, + "timestamp": 2.9507898681334903 + }, + { + "x": 1.665253742824933, + "y": 5.13946824377609, + "heading": -1.8524223717430155, + "angularVelocity": -5.2597769136808665, + "velocityX": -1.83603108448766, + "velocityY": 0.3405403831565884, + "timestamp": 2.98353607841173 + }, + { + "x": 1.6088715232725108, + "y": 5.158717654167419, + "heading": -2.026839169725991, + "angularVelocity": -5.326320099363601, + "velocityX": -1.7217937304301978, + "velocityY": 0.5878362786951496, + "timestamp": 3.0162822886899696 + }, + { + "x": 1.5568081254735793, + "y": 5.1857922946114, + "heading": -2.2021042643709907, + "angularVelocity": -5.3522252851795065, + "velocityX": -1.5899060488696688, + "velocityY": 0.826802253266321, + "timestamp": 3.0490284989682093 + }, + { + "x": 1.5093538981441093, + "y": 5.2205186818889695, + "heading": -2.3759898286014502, + "angularVelocity": -5.31009734418057, + "velocityX": -1.4491517316434035, + "velocityY": 1.060470417263673, + "timestamp": 3.081774709246449 + }, + { + "x": 1.4666702915238679, + "y": 5.262723781048631, + "heading": -2.545870980641176, + "angularVelocity": -5.187811065655255, + "velocityX": -1.30346706557996, + "velocityY": 1.2888544598306537, + "timestamp": 3.1145209195246886 + }, + { + "x": 1.4288865927350196, + "y": 5.312181622313189, + "heading": -2.7092363373229658, + "angularVelocity": -4.988832457059862, + "velocityX": -1.1538342442623262, + "velocityY": 1.510337863353399, + "timestamp": 3.147267129802928 + }, + { + "x": 1.3968131737185872, + "y": 5.367362011920862, + "heading": -2.8587947946716725, + "angularVelocity": -4.567198954563906, + "velocityX": -0.9794543778931859, + "velocityY": 1.6850923859223195, + "timestamp": 3.180013340081168 + }, + { + "x": 1.3707084315183937, + "y": 5.426032663472122, + "heading": -2.986331521157274, + "angularVelocity": -3.8947018724286298, + "velocityX": -0.7971836123443037, + "velocityY": 1.7916776033850628, + "timestamp": 3.2127595503594075 + }, + { + "x": 1.3494796795233661, + "y": 5.486294372047085, + "heading": -3.0820270140723585, + "angularVelocity": -2.9223379469554978, + "velocityX": -0.6482811847432148, + "velocityY": 1.8402651196253896, + "timestamp": 3.245505760637647 + }, + { + "x": 1.3321385383605957, + "y": 5.547596454620361, + "heading": -3.1411853071795863, + "angularVelocity": -1.8065691450878867, + "velocityX": -0.529561772657824, + "velocityY": 1.8720359410264953, + "timestamp": 3.278251970915887 + }, + { + "x": 1.316411922217005, + "y": 5.622158239816111, + "heading": -3.1574444739447625, + "angularVelocity": -0.4153661551974852, + "velocityX": -0.4017613064785548, + "velocityY": 1.9047988429364744, + "timestamp": 3.317396149038551 + }, + { + "x": 1.30673758444171, + "y": 5.698077663390306, + "heading": -3.122874776057638, + "angularVelocity": 0.8831376604407247, + "velocityX": -0.24714627408906595, + "velocityY": 1.9394818646157501, + "timestamp": 3.356540327161215 + }, + { + "x": 1.3044116698031423, + "y": 5.775906190222514, + "heading": -3.0431020650652782, + "angularVelocity": 2.0379201919217973, + "velocityX": -0.05941917163975373, + "velocityY": 1.9882529296776088, + "timestamp": 3.395684505283879 + }, + { + "x": 1.310735543939778, + "y": 5.856467416837523, + "heading": -2.9252067960348933, + "angularVelocity": 3.011821289514447, + "velocityX": 0.16155337625991328, + "velocityY": 2.058064071815707, + "timestamp": 3.434828683406543 + }, + { + "x": 1.3277929402719157, + "y": 5.939930987776361, + "heading": -2.7826616721505166, + "angularVelocity": 3.6415408553908346, + "velocityX": 0.43575819317718334, + "velocityY": 2.132209052321722, + "timestamp": 3.473972861529207 + }, + { + "x": 1.3576022335544815, + "y": 6.023554053808955, + "heading": -2.637362279761064, + "angularVelocity": 3.711903004685308, + "velocityX": 0.7615255885346219, + "velocityY": 2.136283606991328, + "timestamp": 3.513117039651871 + }, + { + "x": 1.4000554524537352, + "y": 6.106306047220255, + "heading": -2.4978098785002194, + "angularVelocity": 3.5650870181393826, + "velocityX": 1.0845346852403, + "velocityY": 2.1140306778695206, + "timestamp": 3.552261217774535 + }, + { + "x": 1.455157655616598, + "y": 6.18915881243997, + "heading": -2.364648055922988, + "angularVelocity": 3.4018295686257636, + "velocityX": 1.407673013090022, + "velocityY": 2.1166050532491445, + "timestamp": 3.591405395897199 + }, + { + "x": 1.5225297792403976, + "y": 6.27399259313729, + "heading": -2.2419731152862643, + "angularVelocity": 3.1339255674829567, + "velocityX": 1.7211275559977186, + "velocityY": 2.16721322980599, + "timestamp": 3.630549574019863 + }, + { + "x": 1.596991898632741, + "y": 6.365541704289421, + "heading": -2.152688244347013, + "angularVelocity": 2.2809233766376438, + "velocityX": 1.9022527222057353, + "velocityY": 2.3387669774352777, + "timestamp": 3.669693752142527 + }, + { + "x": 1.679413821374646, + "y": 6.463137271673749, + "heading": -2.0939697752685182, + "angularVelocity": 1.5000562508808342, + "velocityX": 2.1055985000789685, + "velocityY": 2.4932332741410055, + "timestamp": 3.708837930265191 + }, + { + "x": 1.7662309748150582, + "y": 6.5526721858065455, + "heading": -2.030942574296911, + "angularVelocity": 1.6101296283218078, + "velocityX": 2.2178816264415735, + "velocityY": 2.2873111258646657, + "timestamp": 3.747982108387855 + }, + { + "x": 1.8463290740680989, + "y": 6.633589645928569, + "heading": -1.9424440862784471, + "angularVelocity": 2.260833979989085, + "velocityX": 2.0462327501689237, + "velocityY": 2.0671646207121097, + "timestamp": 3.787126286510519 + }, + { + "x": 1.9201306346515887, + "y": 6.705414092889608, + "heading": -1.8297336528989434, + "angularVelocity": 2.8793664546055737, + "velocityX": 1.88537770169097, + "velocityY": 1.8348692042011108, + "timestamp": 3.826270464633183 + }, + { + "x": 1.9881022552719625, + "y": 6.7678945659852445, + "heading": -1.6930072188425063, + "angularVelocity": 3.4928932120629863, + "velocityX": 1.73644265584974, + "velocityY": 1.596162599195345, + "timestamp": 3.865414642755847 + }, + { + "x": 2.0508860920717975, + "y": 6.821005360416637, + "heading": -1.5311822314903374, + "angularVelocity": 4.134075489976246, + "velocityX": 1.6039125052796561, + "velocityY": 1.3567993244094194, + "timestamp": 3.904558820878511 + }, + { + "x": 2.105765126126845, + "y": 6.86764701678895, + "heading": -1.3406160520225758, + "angularVelocity": 4.868314743270257, + "velocityX": 1.401971804927838, + "velocityY": 1.1915349512807134, + "timestamp": 3.943702999001175 + }, + { + "x": 2.1526116639678397, + "y": 6.908337111426706, + "heading": -1.1177858581420712, + "angularVelocity": 5.692550069188692, + "velocityX": 1.1967689727497768, + "velocityY": 1.039492884746424, + "timestamp": 3.982847177123839 + }, + { + "x": 2.1904354446918113, + "y": 6.943500988064586, + "heading": -0.8664176462137954, + "angularVelocity": 6.421598919271669, + "velocityX": 0.966268358105399, + "velocityY": 0.8983168972839031, + "timestamp": 4.021991355246503 + }, + { + "x": 2.218208118213364, + "y": 6.971474589606098, + "heading": -0.6268162193807357, + "angularVelocity": 6.120997765804011, + "velocityX": 0.7094969125299588, + "velocityY": 0.7146299368925976, + "timestamp": 4.061135533369167 + }, + { + "x": 2.2378592256547205, + "y": 6.992029926313173, + "heading": -0.4114319933826866, + "angularVelocity": 5.502331031784905, + "velocityX": 0.5020186496131576, + "velocityY": 0.5251186177076611, + "timestamp": 4.10027971149183 + }, + { + "x": 2.2511803788337867, + "y": 7.004798124284798, + "heading": -0.22575032024531463, + "angularVelocity": 4.743532296310119, + "velocityX": 0.3403099469178361, + "velocityY": 0.32618383075033625, + "timestamp": 4.139423889614494 + }, + { + "x": 2.261045669673148, + "y": 7.01480850997709, + "heading": -0.09556543349291266, + "angularVelocity": 3.32577903013953, + "velocityX": 0.25202447241187803, + "velocityY": 0.2557311501323063, + "timestamp": 4.1785680677371575 + }, + { + "x": 2.266437621623355, + "y": 7.022862452660783, + "heading": -0.01995585424474321, + "angularVelocity": 1.931566400787266, + "velocityX": 0.1377459486647134, + "velocityY": 0.2057507161972769, + "timestamp": 4.217712245859821 + }, + { + "x": 2.2676985263824463, + "y": 7.028899669647217, + "heading": -4.525227980572114e-34, + "angularVelocity": 0.509803889155846, + "velocityX": 0.03221180823213118, + "velocityY": 0.1542302655458861, + "timestamp": 4.256856423982485 + }, + { + "x": 2.265418134279513, + "y": 7.032677037086849, + "heading": -0.03924023540233394, + "angularVelocity": -0.9804537079645166, + "velocityX": -0.056977713563887346, + "velocityY": 0.09438103198308487, + "timestamp": 4.2968789513935945 + }, + { + "x": 2.260299136724811, + "y": 7.032742565757645, + "heading": -0.13692518133038253, + "angularVelocity": -2.440749054267192, + "velocityX": -0.1279029058340032, + "velocityY": 0.0016372946696696046, + "timestamp": 4.3369014788047044 + }, + { + "x": 2.251431996366295, + "y": 7.026515227476796, + "heading": -0.2837029990611584, + "angularVelocity": -3.667380028829281, + "velocityX": -0.22155373316216304, + "velocityY": -0.15559582774173908, + "timestamp": 4.376924006215814 + }, + { + "x": 2.2367988748775502, + "y": 7.013230050225817, + "heading": -0.4715484199670708, + "angularVelocity": -4.693492216930044, + "velocityX": -0.36562212422103535, + "velocityY": -0.3319424861532188, + "timestamp": 4.416946533626924 + }, + { + "x": 2.214572389947544, + "y": 6.992840585930019, + "heading": -0.6937250351049046, + "angularVelocity": -5.551288974223076, + "velocityX": -0.5553493586673437, + "velocityY": -0.5094496928282007, + "timestamp": 4.456969061038034 + }, + { + "x": 2.182927977382888, + "y": 6.964829235368663, + "heading": -0.9374735446804039, + "angularVelocity": -6.090282781787505, + "velocityX": -0.7906650232157135, + "velocityY": -0.6998895965171087, + "timestamp": 4.496991588449144 + }, + { + "x": 2.1407843072832162, + "y": 6.928562668933627, + "heading": -1.1813002792094054, + "angularVelocity": -6.092237304866431, + "velocityX": -1.0529987191157046, + "velocityY": -0.9061538283803887, + "timestamp": 4.537014115860254 + }, + { + "x": 2.0895949461282366, + "y": 6.88678871775435, + "heading": -1.3936812170017512, + "angularVelocity": -5.3065348824870116, + "velocityX": -1.27901370718457, + "velocityY": -1.0437609486821178, + "timestamp": 4.577036643271364 + }, + { + "x": 2.0327245845451865, + "y": 6.835876323976478, + "heading": -1.5775207555282802, + "angularVelocity": -4.593401527048441, + "velocityX": -1.420958776513053, + "velocityY": -1.2720934201606515, + "timestamp": 4.617059170682474 + }, + { + "x": 1.969383401794422, + "y": 6.775670280789337, + "heading": -1.7350606577956689, + "angularVelocity": -3.9362807013446637, + "velocityX": -1.582638250206598, + "velocityY": -1.5043038778812523, + "timestamp": 4.657081698093584 + }, + { + "x": 1.8991012602387887, + "y": 6.706204931476766, + "heading": -1.8673610734924142, + "angularVelocity": -3.305648699737559, + "velocityX": -1.7560645491899718, + "velocityY": -1.7356562367745094, + "timestamp": 4.6971042255046935 + }, + { + "x": 1.8215077032950175, + "y": 6.627747409048215, + "heading": -1.9742080738130303, + "angularVelocity": -2.6696714883367685, + "velocityX": -1.938747049798569, + "velocityY": -1.960334029448928, + "timestamp": 4.737126752915803 + }, + { + "x": 1.7362669499337922, + "y": 6.540782802917572, + "heading": -2.0543346433472873, + "angularVelocity": -2.002036720749795, + "velocityX": -2.1298193511278303, + "velocityY": -2.172891412812259, + "timestamp": 4.777149280326913 + }, + { + "x": 1.6430739434891832, + "y": 6.445951687040137, + "heading": -2.1059605165739685, + "angularVelocity": -1.2899203665071555, + "velocityX": -2.328513776437315, + "velocityY": -2.3694434612620463, + "timestamp": 4.817171807738023 + }, + { + "x": 1.55728497087458, + "y": 6.34602370365502, + "heading": -2.1651565461839404, + "angularVelocity": -1.4790677510671213, + "velocityX": -2.143517118081594, + "velocityY": -2.496793427328095, + "timestamp": 4.857194335149133 + }, + { + "x": 1.4798885640406532, + "y": 6.2518908184674045, + "heading": -2.25892978627056, + "angularVelocity": -2.3430114526097965, + "velocityX": -1.9338210712910326, + "velocityY": -2.351997519314229, + "timestamp": 4.897216862560243 + }, + { + "x": 1.4136471471462948, + "y": 6.162688939074268, + "heading": -2.372895153669848, + "angularVelocity": -2.847530497727978, + "velocityX": -1.6551032925514553, + "velocityY": -2.2287917621208337, + "timestamp": 4.937239389971353 + }, + { + "x": 1.3600641421628377, + "y": 6.075598413595313, + "heading": -2.5001562416185403, + "angularVelocity": -3.179736417979598, + "velocityX": -1.3388211202419749, + "velocityY": -2.1760376246199, + "timestamp": 4.977261917382463 + }, + { + "x": 1.319394826243984, + "y": 5.989620775668987, + "heading": -2.6390216185129676, + "angularVelocity": -3.4696803494694044, + "velocityX": -1.016160611275252, + "velocityY": -2.148231096031668, + "timestamp": 5.017284444793573 + }, + { + "x": 1.2917069915865356, + "y": 5.904139479101947, + "heading": -2.789119490409166, + "angularVelocity": -3.75033466413552, + "velocityX": -0.6918062513404106, + "velocityY": -2.1358295464196915, + "timestamp": 5.057306972204683 + }, + { + "x": 1.2763632236019438, + "y": 5.822310174000512, + "heading": -2.9261084847899674, + "angularVelocity": -3.422797190533623, + "velocityX": -0.3833782866079771, + "velocityY": -2.0445811495333315, + "timestamp": 5.0973294996157925 + }, + { + "x": 1.2717668608990054, + "y": 5.744491133376307, + "heading": -3.036631348807283, + "angularVelocity": -2.7615163550774677, + "velocityX": -0.11484438890439615, + "velocityY": -1.9443809688691047, + "timestamp": 5.137352027026902 + }, + { + "x": 1.2769543336934923, + "y": 5.67003274843766, + "heading": -3.1136420724595726, + "angularVelocity": -1.9241844189708244, + "velocityX": 0.12961382326511559, + "velocityY": -1.8604118668922167, + "timestamp": 5.177374554438012 + }, + { + "x": 1.290967939745204, + "y": 5.598089181810031, + "heading": -3.1506602139394917, + "angularVelocity": -0.9249326285586663, + "velocityX": 0.35014295593490485, + "velocityY": -1.7975767969031158, + "timestamp": 5.217397081849122 + }, + { + "x": 1.3126477003097534, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": 0.23673934088618437, + "velocityX": 0.5416889428759926, + "velocityY": -1.748613278454628, + "timestamp": 5.257419609260232 + }, + { + "x": 1.3612675355497719, + "y": 5.427793675126224, + "heading": -3.0142432356702726, + "angularVelocity": 2.1028160607727164, + "velocityX": 0.8053954784197191, + "velocityY": -1.6616777008649257, + "timestamp": 5.317787262845048 + }, + { + "x": 1.4285233143505713, + "y": 5.3377057272616115, + "heading": -2.7956250008289785, + "angularVelocity": 3.6214466168399086, + "velocityX": 1.1141029145071253, + "velocityY": -1.4923215085383357, + "timestamp": 5.378154916429864 + }, + { + "x": 1.5158281848971034, + "y": 5.264153119824496, + "heading": -2.514690848185286, + "angularVelocity": 4.653719930475422, + "velocityX": 1.4462193801167051, + "velocityY": -1.218410905001898, + "timestamp": 5.43852257001468 + }, + { + "x": 1.6230211167401283, + "y": 5.213340726984806, + "heading": -2.222036602459031, + "angularVelocity": 4.847865178577475, + "velocityX": 1.7756683501441766, + "velocityY": -0.841715551662108, + "timestamp": 5.498890223599496 + }, + { + "x": 1.749062978630215, + "y": 5.185595086249832, + "heading": -1.9510224888468373, + "angularVelocity": 4.489392870495166, + "velocityX": 2.0879039420175545, + "velocityY": -0.45961105140506303, + "timestamp": 5.5592578771843115 + }, + { + "x": 1.894299674373998, + "y": 5.179254614564922, + "heading": -1.7150026509570102, + "angularVelocity": 3.9097070015852564, + "velocityX": 2.4058694867066146, + "velocityY": -0.10503094469295028, + "timestamp": 5.619625530769127 + }, + { + "x": 2.058119871692227, + "y": 5.193402237247618, + "heading": -1.5227916684845806, + "angularVelocity": 3.1840061863987503, + "velocityX": 2.713708212761054, + "velocityY": 0.2343576707485958, + "timestamp": 5.679993184353943 + }, + { + "x": 2.2389183972423563, + "y": 5.227931703588396, + "heading": -1.382086082537418, + "angularVelocity": 2.3308109159729624, + "velocityX": 2.9949569813262746, + "velocityY": 0.5719862259059945, + "timestamp": 5.740360837938759 + }, + { + "x": 2.4352711849705373, + "y": 5.282232257814921, + "heading": -1.300964814671627, + "angularVelocity": 1.3437869959914348, + "velocityX": 3.252615864095299, + "velocityY": 0.8994975123595437, + "timestamp": 5.800728491523575 + }, + { + "x": 2.6508698274310176, + "y": 5.350520182077314, + "heading": -1.2935554192187855, + "angularVelocity": 0.12273784076155059, + "velocityX": 3.5714265779366063, + "velocityY": 1.1312005719494984, + "timestamp": 5.861096145108391 + }, + { + "x": 2.8627491717000777, + "y": 5.435718232774483, + "heading": -1.2935551586148022, + "angularVelocity": 0.000004316947366860679, + "velocityX": 3.5098157984784524, + "velocityY": 1.4113195666528, + "timestamp": 5.921463798693207 + }, + { + "x": 3.0746283658136546, + "y": 5.520916656893941, + "heading": -1.2935548980122595, + "angularVelocity": 0.000004316923507170679, + "velocityX": 3.5098133111284437, + "velocityY": 1.4113257524537706, + "timestamp": 5.981831452278023 + }, + { + "x": 3.2865075599250586, + "y": 5.606115081018762, + "heading": -1.2935546374096347, + "angularVelocity": 0.000004316924865568706, + "velocityX": 3.509813311092456, + "velocityY": 1.4113257525426077, + "timestamp": 6.042199105862839 + }, + { + "x": 3.4983867540364426, + "y": 5.691313505143594, + "heading": -1.293554376806928, + "angularVelocity": 0.0000043169262224547675, + "velocityX": 3.5098133110921186, + "velocityY": 1.4113257525427836, + "timestamp": 6.1025667594476545 + }, + { + "x": 3.710265948147806, + "y": 5.776511929268437, + "heading": -1.293554116204139, + "angularVelocity": 0.000004316927581395022, + "velocityX": 3.5098133110917824, + "velocityY": 1.4113257525429583, + "timestamp": 6.16293441303247 + }, + { + "x": 3.922145142259149, + "y": 5.861710353393289, + "heading": -1.2935538556012682, + "angularVelocity": 0.000004316928938543054, + "velocityX": 3.509813311091446, + "velocityY": 1.4113257525431333, + "timestamp": 6.223302066617286 + }, + { + "x": 4.134024336370472, + "y": 5.9469087775181535, + "heading": -1.2935535949983157, + "angularVelocity": 0.000004316930294815631, + "velocityX": 3.5098133110911096, + "velocityY": 1.4113257525433083, + "timestamp": 6.283669720202102 + }, + { + "x": 4.345903530481774, + "y": 6.0321072016430275, + "heading": -1.293553334395281, + "angularVelocity": 0.000004316931652676853, + "velocityX": 3.509813311090773, + "velocityY": 1.4113257525434828, + "timestamp": 6.344037373786918 + }, + { + "x": 4.557782724593057, + "y": 6.117305625767913, + "heading": -1.2935530737921643, + "angularVelocity": 0.000004316933011809188, + "velocityX": 3.509813311090437, + "velocityY": 1.4113257525436576, + "timestamp": 6.404405027371734 + }, + { + "x": 4.769661918704319, + "y": 6.202504049892808, + "heading": -1.2935528131889658, + "angularVelocity": 0.000004316934368820573, + "velocityX": 3.5098133110901, + "velocityY": 1.4113257525438323, + "timestamp": 6.46477268095655 + }, + { + "x": 4.9815411128155604, + "y": 6.287702474017714, + "heading": -1.2935525525856855, + "angularVelocity": 0.000004316935725471939, + "velocityX": 3.5098133110897636, + "velocityY": 1.4113257525440075, + "timestamp": 6.525140334541366 + }, + { + "x": 5.193420306926782, + "y": 6.37290089814263, + "heading": -1.293552291982323, + "angularVelocity": 0.000004316937084312971, + "velocityX": 3.5098133110894274, + "velocityY": 1.4113257525441825, + "timestamp": 6.5855079881261815 + }, + { + "x": 5.405299501037983, + "y": 6.458099322267557, + "heading": -1.2935520313788786, + "angularVelocity": 0.000004316938442183061, + "velocityX": 3.509813311089091, + "velocityY": 1.411325752544357, + "timestamp": 6.645875641710997 + }, + { + "x": 5.617178695149163, + "y": 6.543297746392494, + "heading": -1.2935517707753523, + "angularVelocity": 0.000004316939799293205, + "velocityX": 3.509813311088755, + "velocityY": 1.411325752544532, + "timestamp": 6.706243295295813 + }, + { + "x": 5.829057889260324, + "y": 6.628496170517443, + "heading": -1.293551510171744, + "angularVelocity": 0.000004316941156612923, + "velocityX": 3.5098133110884175, + "velocityY": 1.4113257525447067, + "timestamp": 6.766610948880629 + }, + { + "x": 6.040937083371463, + "y": 6.713694594642401, + "heading": -1.293551249568054, + "angularVelocity": 0.0000043169425150049, + "velocityX": 3.5098133110880814, + "velocityY": 1.411325752544882, + "timestamp": 6.826978602465445 + }, + { + "x": 6.252816277482583, + "y": 6.798893018767371, + "heading": -1.2935509889642816, + "angularVelocity": 0.000004316943872175867, + "velocityX": 3.5098133110877447, + "velocityY": 1.4113257525450569, + "timestamp": 6.887346256050261 + }, + { + "x": 6.464695471593406, + "y": 6.884091442893039, + "heading": -1.293550728360427, + "angularVelocity": 0.000004316945230336038, + "velocityX": 3.509813311082821, + "velocityY": 1.4113257525566392, + "timestamp": 6.947713909635077 + }, + { + "x": 6.676574646384632, + "y": 6.969289915063695, + "heading": -1.2935504677556948, + "angularVelocity": 0.000004316959775515561, + "velocityX": 3.5098129910505658, + "velocityY": 1.41132654842967, + "timestamp": 7.008081563219893 + }, + { + "x": 6.8805324076089756, + "y": 7.05464814554522, + "heading": -1.2707475795927152, + "angularVelocity": 0.3777335511465864, + "velocityX": 3.3785934869538337, + "velocityY": 1.4139729708327582, + "timestamp": 7.068449216804709 + }, + { + "x": 7.061472859527864, + "y": 7.1337324563685955, + "heading": -1.1711733781498967, + "angularVelocity": 1.6494628419326967, + "velocityX": 2.997308014708086, + "velocityY": 1.3100444712873218, + "timestamp": 7.1288168703895245 + }, + { + "x": 7.217120419361732, + "y": 7.210021270677617, + "heading": -1.009883648548617, + "angularVelocity": 2.6717906034673087, + "velocityX": 2.5783271436115207, + "velocityY": 1.263736616859493, + "timestamp": 7.18918452397434 + }, + { + "x": 7.347594186911189, + "y": 7.2775930773892314, + "heading": -0.7947029660436775, + "angularVelocity": 3.5645030032948655, + "velocityX": 2.1613191800828018, + "velocityY": 1.119337968249467, + "timestamp": 7.249552177559156 + }, + { + "x": 7.451332936822743, + "y": 7.3349655169215735, + "heading": -0.5434124114773135, + "angularVelocity": 4.162668907005051, + "velocityX": 1.718449264651363, + "velocityY": 0.9503837920706217, + "timestamp": 7.309919831143972 + }, + { + "x": 7.527427510377051, + "y": 7.381739778721793, + "heading": -0.31640528763659864, + "angularVelocity": 3.7604099275081686, + "velocityX": 1.2605189871658125, + "velocityY": 0.7748232542201173, + "timestamp": 7.370287484728788 + }, + { + "x": 7.577573442921129, + "y": 7.419722379939866, + "heading": -0.13731969717190406, + "angularVelocity": 2.966581933039389, + "velocityX": 0.8306755284702909, + "velocityY": 0.6291879667760935, + "timestamp": 7.430655138313604 + }, + { + "x": 7.603931547937356, + "y": 7.45112329376933, + "heading": -0.02759055048880514, + "angularVelocity": 1.8176811614672275, + "velocityX": 0.4366262965512501, + "velocityY": 0.5201612447193243, + "timestamp": 7.49102279189842 + }, + { + "x": 7.608186721801758, + "y": 7.477188587188721, + "heading": 0, + "angularVelocity": 0.4570419562529598, + "velocityX": 0.07048764713743307, + "velocityY": 0.43177582482595267, + "timestamp": 7.551390445483236 + }, + { + "x": 7.590312649337569, + "y": 7.4994357327814924, + "heading": -0.062413166933384966, + "angularVelocity": -1.0141265252388012, + "velocityX": -0.2904286369464457, + "velocityY": 0.3614849488499569, + "timestamp": 7.612934212833851 + }, + { + "x": 7.548542604069605, + "y": 7.5172581696138145, + "heading": -0.20295351245592444, + "angularVelocity": -2.2835837254141333, + "velocityX": -0.6787047180585992, + "velocityY": 0.28958963026732126, + "timestamp": 7.6744779801844665 + }, + { + "x": 7.481084260938195, + "y": 7.5242062844896465, + "heading": -0.3946635775555263, + "angularVelocity": -3.1150199825666713, + "velocityX": -1.0961035704411768, + "velocityY": 0.11289713280385957, + "timestamp": 7.736021747535082 + }, + { + "x": 7.388848586007302, + "y": 7.518355172240059, + "heading": -0.6407957549264521, + "angularVelocity": -3.9993030645769183, + "velocityX": -1.4987005004978822, + "velocityY": -0.09507237696798558, + "timestamp": 7.797565514885697 + }, + { + "x": 7.269389686365513, + "y": 7.4992097780564855, + "heading": -0.9197330786516805, + "angularVelocity": -4.5323407346209255, + "velocityX": -1.941039763803056, + "velocityY": -0.31108583383434507, + "timestamp": 7.859109282236313 + }, + { + "x": 7.1251329311926535, + "y": 7.464894000432003, + "heading": -1.1518882810190125, + "angularVelocity": -3.772196801745026, + "velocityX": -2.343970175745461, + "velocityY": -0.5575833118727538, + "timestamp": 7.920653049586928 + }, + { + "x": 6.953983810333919, + "y": 7.422493842128991, + "heading": -1.327948941925082, + "angularVelocity": -2.8607390883799795, + "velocityX": -2.780933443409385, + "velocityY": -0.6889431721242942, + "timestamp": 7.9821968169375435 + }, + { + "x": 6.759450448912011, + "y": 7.372163465988786, + "heading": -1.4240765574561767, + "angularVelocity": -1.5619390828555313, + "velocityX": -3.1608945925206284, + "velocityY": -0.817798102177794, + "timestamp": 8.043740584288159 + }, + { + "x": 6.542371526128891, + "y": 7.310669881087202, + "heading": -1.4457611917111317, + "angularVelocity": -0.3523449276580275, + "velocityX": -3.5272283795435984, + "velocityY": -0.9991846055060891, + "timestamp": 8.105284351638774 + }, + { + "x": 6.323008319060415, + "y": 7.232674892951184, + "heading": -1.4457615219539912, + "angularVelocity": -0.000005365983815098308, + "velocityX": -3.564344799023447, + "velocityY": -1.2673092905034502, + "timestamp": 8.16682811898939 + }, + { + "x": 6.103645219000666, + "y": 7.154679603847749, + "heading": -1.4457618521936861, + "angularVelocity": -0.000005365932394056072, + "velocityX": -3.5643430602815576, + "velocityY": -1.267314180802329, + "timestamp": 8.228371886340005 + }, + { + "x": 5.884282118942346, + "y": 7.076684314740222, + "heading": -1.4457621824332476, + "angularVelocity": -0.000005365930225246195, + "velocityX": -3.5643430602583215, + "velocityY": -1.2673141808688406, + "timestamp": 8.28991565369062 + }, + { + "x": 5.66491901888401, + "y": 6.998689025632669, + "heading": -1.4457625126726754, + "angularVelocity": -0.000005365928056443698, + "velocityX": -3.564343060258591, + "velocityY": -1.2673141808692443, + "timestamp": 8.351459421041236 + }, + { + "x": 5.445555918825658, + "y": 6.920693736525091, + "heading": -1.44576284291197, + "angularVelocity": -0.000005365925887921031, + "velocityX": -3.5643430602588597, + "velocityY": -1.2673141808696466, + "timestamp": 8.413003188391851 + }, + { + "x": 5.226192818767289, + "y": 6.842698447417488, + "heading": -1.445763173151131, + "angularVelocity": -0.000005365923719151191, + "velocityX": -3.564343060259129, + "velocityY": -1.2673141808700494, + "timestamp": 8.474546955742467 + }, + { + "x": 5.006829718708903, + "y": 6.764703158309861, + "heading": -1.4457635033901584, + "angularVelocity": -0.000005365921549364942, + "velocityX": -3.5643430602593975, + "velocityY": -1.2673141808704518, + "timestamp": 8.536090723093082 + }, + { + "x": 4.787466618650501, + "y": 6.68670786920221, + "heading": -1.4457638336290524, + "angularVelocity": -0.000005365919381086304, + "velocityX": -3.564343060259667, + "velocityY": -1.2673141808708541, + "timestamp": 8.597634490443697 + }, + { + "x": 4.568103518592082, + "y": 6.608712580094533, + "heading": -1.4457641638678127, + "angularVelocity": -0.000005365917211650528, + "velocityX": -3.5643430602599366, + "velocityY": -1.2673141808712571, + "timestamp": 8.659178257794313 + }, + { + "x": 4.348740418533646, + "y": 6.530717290986832, + "heading": -1.4457644941064398, + "angularVelocity": -0.000005365915042310373, + "velocityX": -3.564343060260205, + "velocityY": -1.2673141808716597, + "timestamp": 8.720722025144928 + }, + { + "x": 4.1293773184751945, + "y": 6.452722001879106, + "heading": -1.4457648243449335, + "angularVelocity": -0.000005365912874367381, + "velocityX": -3.564343060260474, + "velocityY": -1.267314180872062, + "timestamp": 8.782265792495544 + }, + { + "x": 3.910014218416726, + "y": 6.374726712771355, + "heading": -1.4457651545832937, + "angularVelocity": -0.000005365910705198878, + "velocityX": -3.5643430602607435, + "velocityY": -1.2673141808724648, + "timestamp": 8.843809559846159 + }, + { + "x": 3.690651118358241, + "y": 6.296731423663579, + "heading": -1.4457654848215202, + "angularVelocity": -0.000005365908536510064, + "velocityX": -3.5643430602610127, + "velocityY": -1.2673141808728674, + "timestamp": 8.905353327196774 + }, + { + "x": 3.47128801829974, + "y": 6.218736134555778, + "heading": -1.4457658150596133, + "angularVelocity": -0.0000053659063672733, + "velocityX": -3.5643430602612813, + "velocityY": -1.2673141808732702, + "timestamp": 8.96689709454739 + }, + { + "x": 3.2519249182412215, + "y": 6.140740845447953, + "heading": -1.445766145297573, + "angularVelocity": -0.000005365904199203489, + "velocityX": -3.564343060261551, + "velocityY": -1.2673141808736725, + "timestamp": 9.028440861898005 + }, + { + "x": 3.0325618181826868, + "y": 6.062745556340103, + "heading": -1.4457664755353992, + "angularVelocity": -0.000005365902029653181, + "velocityX": -3.5643430602618196, + "velocityY": -1.267314180874075, + "timestamp": 9.08998462924862 + }, + { + "x": 2.8131987181241356, + "y": 5.984750267232228, + "heading": -1.445766805773092, + "angularVelocity": -0.000005365899860579176, + "velocityX": -3.5643430602620887, + "velocityY": -1.2673141808744774, + "timestamp": 9.151528396599236 + }, + { + "x": 2.5938356180647952, + "y": 5.906754978126503, + "heading": -1.4457671360106512, + "angularVelocity": -0.000005365897692675495, + "velocityX": -3.564343060274917, + "velocityY": -1.2673141808395556, + "timestamp": 9.213072163949851 + }, + { + "x": 2.374472460827342, + "y": 5.8287598498370325, + "heading": -1.4457674662499087, + "angularVelocity": -0.000005365925286950734, + "velocityX": -3.5643439893391378, + "velocityY": -1.2673115678006912, + "timestamp": 9.274615931300467 + }, + { + "x": 2.1579642030031834, + "y": 5.762355072468581, + "heading": -1.4644178853402519, + "angularVelocity": -0.30304318200235486, + "velocityX": -3.5179558734308305, + "velocityY": -1.0789846027810281, + "timestamp": 9.336159698651082 + }, + { + "x": 1.9655559601169903, + "y": 5.705313391080233, + "heading": -1.5528631085528386, + "angularVelocity": -1.4371109702906752, + "velocityX": -3.1263643934899394, + "velocityY": -0.9268474102890126, + "timestamp": 9.397703466001698 + }, + { + "x": 1.799063271019786, + "y": 5.655117829615961, + "heading": -1.7042345036546405, + "angularVelocity": -2.459573107369865, + "velocityX": -2.7052729506904143, + "velocityY": -0.8156075525618497, + "timestamp": 9.459247233352313 + }, + { + "x": 1.6591648707007773, + "y": 5.609591955542646, + "heading": -1.9150308855459262, + "angularVelocity": -3.425145891547008, + "velocityX": -2.273153015186835, + "velocityY": -0.739731674435107, + "timestamp": 9.520791000702928 + }, + { + "x": 1.5431948510849691, + "y": 5.574795624095963, + "heading": -2.189608491806892, + "angularVelocity": -4.461501433552055, + "velocityX": -1.884350351110707, + "velocityY": -0.5653916382539252, + "timestamp": 9.582334768053544 + }, + { + "x": 1.4525455234391338, + "y": 5.551032580854212, + "heading": -2.523374227659994, + "angularVelocity": -5.423225620096252, + "velocityX": -1.4729245795013663, + "velocityY": -0.3861161619563059, + "timestamp": 9.64387853540416 + }, + { + "x": 1.3896696664094041, + "y": 5.537613504449024, + "heading": -2.816143624187386, + "angularVelocity": -4.757092539679631, + "velocityX": -1.0216445911009162, + "velocityY": -0.21804119219317228, + "timestamp": 9.705422302754775 + }, + { + "x": 1.3505170646137672, + "y": 5.530954725925202, + "heading": -3.0286369343399735, + "angularVelocity": -3.4527185984896, + "velocityX": -0.6361749285282489, + "velocityY": -0.1081958224280782, + "timestamp": 9.76696607010539 + }, + { + "x": 1.332138180732727, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": -1.828753384537267, + "velocityX": -0.2986311152571343, + "velocityY": -0.046299846535530764, + "timestamp": 9.828509837456005 + }, + { + "x": 1.332138180732727, + "y": 5.52810525894165, + "heading": -3.1411853071795863, + "angularVelocity": -8.15091951330742e-35, + "velocityX": 0, + "velocityY": 2.0839850321711294e-35, + "timestamp": 9.89005360480662 + } + ] +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd3..67354b11 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -92,7 +92,11 @@ "robotJoysticks": [ { "guid": "78696e70757401000000000000000000", + "name": "Keyboard", "useGamepad": true + }, + { + "guid": "Keyboard1" } ] } diff --git a/simgui.json b/simgui.json index 1d91652d..f43cbda2 100644 --- a/simgui.json +++ b/simgui.json @@ -1,10 +1,50 @@ { + "HALProvider": { + "Other Devices": { + "Talon FX (v6)[20]/Rotor Sensor": { + "header": { + "open": true + } + }, + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d", "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d", "/FMSInfo": "FMSInfo", - "/SmartDashboard/Shooter shoot": "Command" + "/SmartDashboard/Auto Chooser": "String Chooser", + "/SmartDashboard/Blacklist All": "Command", + "/SmartDashboard/Blacklist Note": "Command", + "/SmartDashboard/Dynamic Auto": "Command", + "/SmartDashboard/Dynamic Demo": "Command", + "/SmartDashboard/Note Picker": "String Chooser", + "/SmartDashboard/Note To Note": "Command", + "/SmartDashboard/Note To Shooting": "Command", + "/SmartDashboard/Run Elevator Sysid": "Command", + "/SmartDashboard/Run Flywheel Sysid": "Command", + "/SmartDashboard/Run Pivot Sysid": "Command", + "/SmartDashboard/Run Swerve Azimuth Sysid": "Command", + "/SmartDashboard/Run Swerve Drive Sysid": "Command", + "/SmartDashboard/SendableChooser[0]": "String Chooser", + "/SmartDashboard/Shoot To Note": "Command", + "/SmartDashboard/Shooter shoot": "Command", + "/SmartDashboard/Start To Note": "Command", + "/SmartDashboard/Update Note": "Command", + "/SmartDashboard/Whitelist All": "Command", + "/SmartDashboard/Whitelist Note": "Command", + "/SmartDashboard/gleeb": "Command", + "/SmartDashboard/gloob": "Command" + }, + "windows": { + "/SmartDashboard/Auto Chooser": { + "window": { + "visible": true + } + } } }, "NetworkTables": { diff --git a/src/main/deploy/choreo/amp 4 local sgmt 1.traj b/src/main/deploy/choreo/amp 4 local sgmt 1.traj deleted file mode 100644 index 1dbcc825..00000000 --- a/src/main/deploy/choreo/amp 4 local sgmt 1.traj +++ /dev/null @@ -1,337 +0,0 @@ -{ - "samples": [ - { - "x": 0.7470832465334709, - "y": 6.625458718233556, - "heading": 1.0564954979067924, - "angularVelocity": -5.684301329861994e-12, - "velocityX": -3.602456269610706e-10, - "velocityY": 4.1137821262407583e-10, - "timestamp": 0 - }, - { - "x": 0.7589569772775385, - "y": 6.629936126338657, - "heading": 1.0434265097431823, - "angularVelocity": -0.12778392744309977, - "velocityX": 0.11609712676013798, - "velocityY": 0.04377849867759475, - "timestamp": 0.10227411521599863 - }, - { - "x": 0.7822022838432713, - "y": 6.638629979602176, - "heading": 1.0183346599429852, - "angularVelocity": -0.2453392014361959, - "velocityX": 0.2272843592055021, - "velocityY": 0.08500540295845989, - "timestamp": 0.20454823043199727 - }, - { - "x": 0.816345318524229, - "y": 6.6512661070932495, - "heading": 0.9822355978698938, - "angularVelocity": -0.35296381641965074, - "velocityX": 0.33383847695279084, - "velocityY": 0.12355156118457596, - "timestamp": 0.30682234564799593 - }, - { - "x": 0.8609432271767217, - "y": 6.667555740683275, - "heading": 0.9361120278121537, - "angularVelocity": -0.45097989764859636, - "velocityX": 0.4360625240585109, - "velocityY": 0.15927424713723962, - "timestamp": 0.40909646086399454 - }, - { - "x": 0.9155883048461597, - "y": 6.687191502411657, - "heading": 0.8809291072748399, - "angularVelocity": -0.5395590113414345, - "velocityX": 0.5343001768187209, - "velocityY": 0.19199150048411856, - "timestamp": 0.5113705760799931 - }, - { - "x": 0.9799143577111046, - "y": 6.709840447255909, - "heading": 0.817662056557047, - "angularVelocity": -0.6186027670698295, - "velocityX": 0.6289573163604619, - "velocityY": 0.22145333556813593, - "timestamp": 0.6136446912959918 - }, - { - "x": 1.0536065024635548, - "y": 6.735133328172977, - "heading": 0.7473295527982347, - "angularVelocity": -0.687686259272288, - "velocityX": 0.7205356386732681, - "velocityY": 0.24730480411634378, - "timestamp": 0.7159188065119904 - }, - { - "x": 1.1364169197016813, - "y": 6.762648211314195, - "heading": 0.6710254557943266, - "angularVelocity": -0.7460743786173882, - "velocityX": 0.8096908708297301, - "velocityY": 0.2690307532427302, - "timestamp": 0.818192921727989 - }, - { - "x": 1.2281932228229642, - "y": 6.7918845460908575, - "heading": 0.5899378478266369, - "angularVelocity": -0.7928458510838684, - "velocityX": 0.8973561208709863, - "velocityY": 0.28586249636547506, - "timestamp": 0.9204670369439876 - }, - { - "x": 1.3289409008958384, - "y": 6.822217824239773, - "heading": 0.5053285615301966, - "angularVelocity": -0.8272795718879136, - "velocityX": 0.985075041075245, - "velocityY": 0.296588019337818, - "timestamp": 1.0227411521599863 - }, - { - "x": 1.4390031913745853, - "y": 6.852792823991799, - "heading": 0.4183893727045359, - "angularVelocity": -0.8500605319021087, - "velocityX": 1.076150015287458, - "velocityY": 0.29895148738224403, - "timestamp": 1.125015267375985 - }, - { - "x": 1.5597885498551032, - "y": 6.882059112916124, - "heading": 0.32979940511385675, - "angularVelocity": -0.8662012607776253, - "velocityX": 1.1809963684348201, - "velocityY": 0.28615537738935176, - "timestamp": 1.2272893825919837 - }, - { - "x": 1.700662064049805, - "y": 6.9025972281907375, - "heading": 0.2406449492427404, - "angularVelocity": -0.8717206268168672, - "velocityX": 1.3774112254273, - "velocityY": 0.20081439319892427, - "timestamp": 1.3295634978079824 - }, - { - "x": 1.8510164004103928, - "y": 6.913603640101383, - "heading": 0.18435642050200896, - "angularVelocity": -0.5503692556839603, - "velocityX": 1.4701113396826178, - "velocityY": 0.10761678064700267, - "timestamp": 1.4318376130239812 - }, - { - "x": 2.0039488544462833, - "y": 6.918843013673937, - "heading": 0.15617120081925578, - "angularVelocity": -0.2755850738942481, - "velocityX": 1.4953192592999736, - "velocityY": 0.051228726583383746, - "timestamp": 1.5341117282399799 - }, - { - "x": 2.1557445524327394, - "y": 6.919295783765447, - "heading": 0.15617120073466711, - "angularVelocity": -8.01993499988118e-11, - "velocityX": 1.484204462099971, - "velocityY": 0.0044270165813428694, - "timestamp": 1.6363858434559786 - }, - { - "x": 2.232501376452394, - "y": 6.918644981823818, - "heading": 0.15617120066422163, - "angularVelocity": -2.7881719949913897e-12, - "velocityX": 1.4631774376072004, - "velocityY": -0.012405855819009865, - "timestamp": 1.6888448428916174 - }, - { - "x": 2.3078780299226054, - "y": 6.916920762093343, - "heading": 0.1561712005937278, - "angularVelocity": -3.698975819730093e-12, - "velocityX": 1.436867928003674, - "velocityY": -0.032867889033463694, - "timestamp": 1.7413038423272562 - }, - { - "x": 2.381659462849091, - "y": 6.91403766609857, - "heading": 0.15617120052323402, - "angularVelocity": -3.687857737825465e-12, - "velocityX": 1.4064590265589882, - "velocityY": -0.05495897496281437, - "timestamp": 1.793762841762895 - }, - { - "x": 2.453627858185915, - "y": 6.909923503320222, - "heading": 0.15617120045273983, - "angularVelocity": -3.68806627753873e-12, - "velocityX": 1.3718979863339387, - "velocityY": -0.07842619311071317, - "timestamp": 1.8462218411985338 - }, - { - "x": 2.5235476514141553, - "y": 6.90450568335544, - "heading": 0.15617120038224525, - "angularVelocity": -3.688143550349055e-12, - "velocityX": 1.3328464960153679, - "velocityY": -0.10327716626043287, - "timestamp": 1.8986808406341726 - }, - { - "x": 2.5911718313337544, - "y": 6.897715977103231, - "heading": 0.1561712003117504, - "angularVelocity": -3.687560329325201e-12, - "velocityX": 1.2890863531735766, - "velocityY": -0.12942875706821344, - "timestamp": 1.9511398400698114 - }, - { - "x": 2.65624736130467, - "y": 6.889492702059127, - "heading": 0.15617120024125528, - "angularVelocity": -3.68944362324301e-12, - "velocityX": 1.240502697821755, - "velocityY": -0.1567561709974853, - "timestamp": 2.0035988395054503 - }, - { - "x": 2.7185182573768354, - "y": 6.879780772610566, - "heading": 0.1561712001707086, - "angularVelocity": -4.669569996251489e-12, - "velocityX": 1.187039346844362, - "velocityY": -0.1851336547556834, - "timestamp": 2.056057838941089 - }, - { - "x": 2.844727229024539, - "y": 6.826831262995891, - "heading": 0.36522912798339696, - "angularVelocity": 1.2815424227266763, - "velocityX": 0.7736714562084378, - "velocityY": -0.32458488264739044, - "timestamp": 2.219187770584854 - }, - { - "x": 2.905891887894138, - "y": 6.746880408353911, - "heading": 0.5636352419280429, - "angularVelocity": 1.2162459209838357, - "velocityX": 0.3749444284785087, - "velocityY": -0.490105365340026, - "timestamp": 2.382317702228619 - }, - { - "x": 2.909090532344001, - "y": 6.6442122814929245, - "heading": 0.7377210107974022, - "angularVelocity": 1.0671601901772823, - "velocityX": 0.0196079562833389, - "velocityY": -0.6293641266849203, - "timestamp": 2.5454476338723837 - }, - { - "x": 2.8528015320306115, - "y": 6.520167648469734, - "heading": 0.8617887991289785, - "angularVelocity": 0.7605458243522792, - "velocityX": -0.34505623601923857, - "velocityY": -0.7604038809886424, - "timestamp": 2.7085775655161486 - }, - { - "x": 2.6673775887021587, - "y": 6.358333521535233, - "heading": 0.8824380102910534, - "angularVelocity": 0.1265813757217392, - "velocityX": -1.1366641384675562, - "velocityY": -0.9920566103835737, - "timestamp": 2.8717074971599135 - }, - { - "x": 2.433334548523251, - "y": 6.191208743378355, - "heading": 0.7641593769894643, - "angularVelocity": -0.7250578241718747, - "velocityX": -1.4347032313583656, - "velocityY": -1.0244887408273557, - "timestamp": 3.0348374288036783 - }, - { - "x": 2.190845809692594, - "y": 6.024313186516238, - "heading": 0.6297541443819642, - "angularVelocity": -0.823915214372992, - "velocityX": -1.486476064011715, - "velocityY": -1.0230835952941921, - "timestamp": 3.1979673604474432 - }, - { - "x": 1.9580404786548902, - "y": 5.870547470854263, - "heading": 0.47066977289213696, - "angularVelocity": -0.9752003809508628, - "velocityX": -1.4271159717504125, - "velocityY": -0.9425965806313563, - "timestamp": 3.361097292091208 - }, - { - "x": 1.7498273467354928, - "y": 5.73486661370809, - "heading": 0.32338313207816166, - "angularVelocity": -0.9028793141986351, - "velocityX": -1.2763637531323493, - "velocityY": -0.831734899187721, - "timestamp": 3.524227223734973 - }, - { - "x": 1.577014249967444, - "y": 5.624033816048722, - "heading": 0.19882585300104294, - "angularVelocity": -0.7635464431083702, - "velocityX": -1.0593586039568081, - "velocityY": -0.6794142353068856, - "timestamp": 3.687357155378738 - }, - { - "x": 1.4506486184747536, - "y": 5.5441849844920235, - "heading": 0.1067515452077215, - "angularVelocity": -0.5644231371867715, - "velocityX": -0.7746317925903302, - "velocityY": -0.4894799553714506, - "timestamp": 3.8504870870225028 - }, - { - "x": 1.3818649056319274, - "y": 5.50129938101995, - "heading": 0.05628563946839357, - "angularVelocity": -0.30936018439404245, - "velocityX": -0.4216498592955241, - "velocityY": -0.2628923039688523, - "timestamp": 4.013617018666268 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 2.traj b/src/main/deploy/choreo/amp 4 local sgmt 2.traj deleted file mode 100644 index ad2ef04b..00000000 --- a/src/main/deploy/choreo/amp 4 local sgmt 2.traj +++ /dev/null @@ -1,193 +0,0 @@ -{ - "samples": [ - { - "x": 1.3818649054659777, - "y": 5.501299381110093, - "heading": 0.05628563946121549, - "angularVelocity": -8.20711890061584e-12, - "velocityX": -2.918186949691226e-10, - "velocityY": -8.516677331367793e-11, - "timestamp": 0 - }, - { - "x": 1.4254749911225055, - "y": 5.4996771535618185, - "heading": 0.052286234874463405, - "angularVelocity": -0.03818204740477268, - "velocityX": 0.41634256351427257, - "velocityY": -0.015487297221703413, - "timestamp": 0.10474568177260579 - }, - { - "x": 1.5054277602383004, - "y": 5.496460245033646, - "heading": 0.04534031368940726, - "angularVelocity": -0.06631224378557163, - "velocityX": 0.763303725485265, - "velocityY": -0.030711609603001202, - "timestamp": 0.20949136354521158 - }, - { - "x": 1.614706352195534, - "y": 5.491742221647794, - "heading": 0.036622661203774276, - "angularVelocity": -0.08322684370600701, - "velocityX": 1.0432753896642522, - "velocityY": -0.045042652489690874, - "timestamp": 0.3142370453178174 - }, - { - "x": 1.7468754648697162, - "y": 5.485736649446459, - "heading": 0.02746720464328704, - "angularVelocity": -0.08740652985101428, - "velocityX": 1.2618096559898304, - "velocityY": -0.05733479469253905, - "timestamp": 0.41898272709042317 - }, - { - "x": 1.8974421226741423, - "y": 5.478814465512338, - "heading": 0.0192537076414664, - "angularVelocity": -0.07841370510897432, - "velocityX": 1.43744978567254, - "velocityY": -0.06608562544064354, - "timestamp": 0.523728408863029 - }, - { - "x": 2.0478771487383725, - "y": 5.472018862260595, - "heading": 0.011842428389913316, - "angularVelocity": -0.07075498604111852, - "velocityX": 1.436193106275662, - "velocityY": -0.06487716815402857, - "timestamp": 0.6284740906356348 - }, - { - "x": 2.1991998312667134, - "y": 5.465297323381209, - "heading": 0.007319920190768939, - "angularVelocity": -0.04317608251475354, - "velocityX": 1.4446675030153622, - "velocityY": -0.06417008055763802, - "timestamp": 0.7332197724082405 - }, - { - "x": 2.337086103631991, - "y": 5.4645313180146555, - "heading": 0.0021217678335590726, - "angularVelocity": -0.04962641198543019, - "velocityX": 1.316390996187784, - "velocityY": -0.007313001468104138, - "timestamp": 0.8379654541808463 - }, - { - "x": 2.45341531313919, - "y": 5.464492524135499, - "heading": -0.0044756440905307565, - "angularVelocity": -0.06298504924092899, - "velocityX": 1.1105871626237722, - "velocityY": -0.0003703621622037101, - "timestamp": 0.9427111359534521 - }, - { - "x": 2.541111145262716, - "y": 5.463618987693384, - "heading": -0.012133903650681083, - "angularVelocity": -0.07311289048562652, - "velocityX": 0.8372262287970834, - "velocityY": -0.008339593435811936, - "timestamp": 1.047456817726058 - }, - { - "x": 2.592916727068705, - "y": 5.461223602405139, - "heading": -0.020267736871702392, - "angularVelocity": -0.07765316033563659, - "velocityX": 0.4945844156915881, - "velocityY": -0.022868582319897673, - "timestamp": 1.1522024994986637 - }, - { - "x": 2.573983182200651, - "y": 5.456183859404588, - "heading": -0.02941680850548689, - "angularVelocity": -0.07029420270393813, - "velocityX": -0.14547032678251723, - "velocityY": -0.03872138453206129, - "timestamp": 1.2823564991476335 - }, - { - "x": 2.4879155076147748, - "y": 5.449739339031148, - "heading": -0.03668510065770142, - "angularVelocity": -0.05584378637679435, - "velocityX": -0.6612756796636089, - "velocityY": -0.04951457856894505, - "timestamp": 1.4125104987966033 - }, - { - "x": 2.35046961428428, - "y": 5.4436438239165845, - "heading": -0.03977633600135786, - "angularVelocity": -0.02375059815495546, - "velocityX": -1.056025121733672, - "velocityY": -0.046833099245860266, - "timestamp": 1.542664498445573 - }, - { - "x": 2.1764781877651598, - "y": 5.443212327064482, - "heading": -0.032264347817817784, - "angularVelocity": 0.05771615319957162, - "velocityX": -1.336811984121226, - "velocityY": -0.0033152797571240995, - "timestamp": 1.6728184980945429 - }, - { - "x": 1.9752868983122995, - "y": 5.4534701667830054, - "heading": -0.009494098721117665, - "angularVelocity": 0.174948516049147, - "velocityX": -1.5457941360881122, - "velocityY": 0.07881309585526655, - "timestamp": 1.8029724977435126 - }, - { - "x": 1.7725080662079153, - "y": 5.4634789711909475, - "heading": 0.004869525328448035, - "angularVelocity": 0.11035868346800264, - "velocityX": -1.5579915534229911, - "velocityY": 0.07689970624561784, - "timestamp": 1.9331264973924824 - }, - { - "x": 1.5970859275969194, - "y": 5.473760403676058, - "heading": 0.019510408429697427, - "angularVelocity": 0.11248892189634652, - "velocityX": -1.347804439948399, - "velocityY": 0.07899436394267238, - "timestamp": 2.063280497041452 - }, - { - "x": 1.463998994938607, - "y": 5.481958295434183, - "heading": 0.031093440957313423, - "angularVelocity": 0.0889948258110792, - "velocityX": -1.0225343286428261, - "velocityY": 0.06298609118419185, - "timestamp": 2.193434496690422 - }, - { - "x": 1.3892531402609951, - "y": 5.486522674152253, - "heading": 0.03752685288213878, - "angularVelocity": 0.049429229542960515, - "velocityX": -0.5742878042328909, - "velocityY": 0.03506906174326123, - "timestamp": 2.323588496339392 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 3.traj b/src/main/deploy/choreo/amp 4 local sgmt 3.traj deleted file mode 100644 index 104394ca..00000000 --- a/src/main/deploy/choreo/amp 4 local sgmt 3.traj +++ /dev/null @@ -1,229 +0,0 @@ -{ - "samples": [ - { - "x": 1.3892531401370203, - "y": 5.486522674374907, - "heading": 0.03752685288891443, - "angularVelocity": 5.018347137854837e-11, - "velocityX": -8.981333628450582e-10, - "velocityY": 1.1863234183511445e-9, - "timestamp": 0 - }, - { - "x": 1.410785208000178, - "y": 5.466798705442517, - "heading": 0.02538794622123859, - "angularVelocity": -0.12074981514388294, - "velocityX": 0.21418676347263918, - "velocityY": -0.1962009963268651, - "timestamp": 0.1005294019105829 - }, - { - "x": 1.4512665581597533, - "y": 5.429718184658836, - "heading": 0.002445244265967334, - "angularVelocity": -0.2282188250282345, - "velocityX": 0.4026816906907998, - "velocityY": -0.36885249450974994, - "timestamp": 0.2010588038211658 - }, - { - "x": 1.5081514675148215, - "y": 5.3776444456377, - "heading": -0.029986935931297812, - "angularVelocity": -0.3226138782330449, - "velocityX": 0.5658534482437384, - "velocityY": -0.5179951130124598, - "timestamp": 0.3015882057317487 - }, - { - "x": 1.578941487701644, - "y": 5.312948064530918, - "heading": -0.07059739560458454, - "angularVelocity": -0.4039659933419245, - "velocityX": 0.7041722939494359, - "velocityY": -0.6435568078574709, - "timestamp": 0.4021176076423316 - }, - { - "x": 1.6611946188865518, - "y": 5.238029568056684, - "heading": -0.11800108662062794, - "angularVelocity": -0.4715405656932785, - "velocityX": 0.8181997404499256, - "velocityY": -0.7452396506196601, - "timestamp": 0.5026470095529145 - }, - { - "x": 1.7524893345353523, - "y": 5.155358397604862, - "heading": -0.17044488274189246, - "angularVelocity": -0.521676197753198, - "velocityX": 0.9081394425873396, - "velocityY": -0.8223581237255294, - "timestamp": 0.6031764114634974 - }, - { - "x": 1.8499500532222615, - "y": 5.067431436588914, - "heading": -0.22499900927916092, - "angularVelocity": -0.5426683689609221, - "velocityX": 0.9694747626122975, - "velocityY": -0.8746392520744162, - "timestamp": 0.7037058133740803 - }, - { - "x": 1.9473638091809122, - "y": 4.979075974351683, - "heading": -0.27982409115325835, - "angularVelocity": -0.5453636534436577, - "velocityX": 0.9690076084526664, - "velocityY": -0.8789016988127862, - "timestamp": 0.8042352152846632 - }, - { - "x": 2.05818821772957, - "y": 4.854821291737014, - "heading": -0.34544281347225186, - "angularVelocity": -0.6527316496725523, - "velocityX": 1.1024079106043232, - "velocityY": -1.2360033993617228, - "timestamp": 0.9047646171952461 - }, - { - "x": 2.1664425710584037, - "y": 4.732231232720307, - "heading": -0.4041393246178968, - "angularVelocity": -0.5838740711290391, - "velocityX": 1.076842701113489, - "velocityY": -1.2194448247922516, - "timestamp": 1.005294019105829 - }, - { - "x": 2.2666201121554423, - "y": 4.61627032113389, - "heading": -0.4602072656334145, - "angularVelocity": -0.557726794034664, - "velocityX": 0.996499915039965, - "velocityY": -1.153502450677793, - "timestamp": 1.105823421016412 - }, - { - "x": 2.356480498307365, - "y": 4.5087945921361055, - "heading": -0.5124957847583047, - "angularVelocity": -0.5201316047807618, - "velocityX": 0.8938716814183492, - "velocityY": -1.0690974663909212, - "timestamp": 1.2063528229269949 - }, - { - "x": 2.433798199341425, - "y": 4.411691494217003, - "heading": -0.5602485683833155, - "angularVelocity": -0.47501310796660834, - "velocityX": 0.7691053456052521, - "velocityY": -0.9659173928845837, - "timestamp": 1.3068822248375778 - }, - { - "x": 2.4961951429135376, - "y": 4.327016530758609, - "heading": -0.6027268742617791, - "angularVelocity": -0.4225460918867537, - "velocityX": 0.620683518971687, - "velocityY": -0.8422905312450144, - "timestamp": 1.4074116267481598 - }, - { - "x": 2.541198492257727, - "y": 4.256927967764492, - "heading": -0.6391760227839379, - "angularVelocity": -0.3625720221944257, - "velocityX": 0.4476635477259144, - "velocityY": -0.6971946668803632, - "timestamp": 1.5079410286587427 - }, - { - "x": 2.5331758624705114, - "y": 4.192320334785442, - "heading": -0.6763224470307224, - "angularVelocity": -0.2449990301925222, - "velocityX": -0.052913214713167914, - "velocityY": -0.426119279886695, - "timestamp": 1.6595596871052694 - }, - { - "x": 2.46411262676954, - "y": 4.160473976285577, - "heading": -0.7018758155970488, - "angularVelocity": -0.16853709734646205, - "velocityX": -0.45550618406914944, - "velocityY": -0.21004248027074415, - "timestamp": 1.811178345551796 - }, - { - "x": 2.348779581051416, - "y": 4.153307846262027, - "heading": -0.7202693675207003, - "angularVelocity": -0.121314567375086, - "velocityX": -0.7606784530447123, - "velocityY": -0.04726417363495295, - "timestamp": 1.9627970039983227 - }, - { - "x": 2.201596779448434, - "y": 4.163036990055964, - "heading": -0.7333684069992377, - "angularVelocity": -0.0863946406365267, - "velocityX": -0.9707433369527995, - "velocityY": 0.06416850879734587, - "timestamp": 2.1144156624448494 - }, - { - "x": 2.03723470076713, - "y": 4.1831659239379135, - "heading": -0.7500897294060416, - "angularVelocity": -0.11028538710126731, - "velocityX": -1.0840491612778367, - "velocityY": 0.13276026521150996, - "timestamp": 2.266034320891376 - }, - { - "x": 1.8800877783569956, - "y": 4.195672966501764, - "heading": -0.7289968148061016, - "angularVelocity": 0.13911819814698503, - "velocityX": -1.0364616391016628, - "velocityY": 0.08249012434493122, - "timestamp": 2.4176529793379027 - }, - { - "x": 1.7046442157621393, - "y": 4.233321160679635, - "heading": -0.7238359462014516, - "angularVelocity": 0.03403847932720532, - "velocityX": -1.1571370254440554, - "velocityY": 0.24830844691554524, - "timestamp": 2.5692716377844294 - }, - { - "x": 1.572815192920781, - "y": 4.2658136358994225, - "heading": -0.7241682787394389, - "angularVelocity": -0.002191897597681109, - "velocityX": -0.8694775764003041, - "velocityY": 0.21430393183203963, - "timestamp": 2.720890296230956 - }, - { - "x": 1.4994455554717883, - "y": 4.286480902556393, - "heading": -0.7254698893818832, - "angularVelocity": -0.00858476582372283, - "velocityX": -0.4839090323218549, - "velocityY": 0.13631083596916982, - "timestamp": 2.8725089546774827 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local.traj b/src/main/deploy/choreo/amp 4 local.traj deleted file mode 100644 index 6dfc86f9..00000000 --- a/src/main/deploy/choreo/amp 4 local.traj +++ /dev/null @@ -1,760 +0,0 @@ -{ - "samples": [ - { - "x": 0.7470832465334709, - "y": 6.625458718233556, - "heading": 1.0564954979067924, - "angularVelocity": -5.684301329861994e-12, - "velocityX": -3.602456269610706e-10, - "velocityY": 4.1137821262407583e-10, - "timestamp": 0 - }, - { - "x": 0.7589569772775385, - "y": 6.629936126338657, - "heading": 1.0434265097431823, - "angularVelocity": -0.12778392744309977, - "velocityX": 0.11609712676013798, - "velocityY": 0.04377849867759475, - "timestamp": 0.10227411521599863 - }, - { - "x": 0.7822022838432713, - "y": 6.638629979602176, - "heading": 1.0183346599429852, - "angularVelocity": -0.2453392014361959, - "velocityX": 0.2272843592055021, - "velocityY": 0.08500540295845989, - "timestamp": 0.20454823043199727 - }, - { - "x": 0.816345318524229, - "y": 6.6512661070932495, - "heading": 0.9822355978698938, - "angularVelocity": -0.35296381641965074, - "velocityX": 0.33383847695279084, - "velocityY": 0.12355156118457596, - "timestamp": 0.30682234564799593 - }, - { - "x": 0.8609432271767217, - "y": 6.667555740683275, - "heading": 0.9361120278121537, - "angularVelocity": -0.45097989764859636, - "velocityX": 0.4360625240585109, - "velocityY": 0.15927424713723962, - "timestamp": 0.40909646086399454 - }, - { - "x": 0.9155883048461597, - "y": 6.687191502411657, - "heading": 0.8809291072748399, - "angularVelocity": -0.5395590113414345, - "velocityX": 0.5343001768187209, - "velocityY": 0.19199150048411856, - "timestamp": 0.5113705760799931 - }, - { - "x": 0.9799143577111046, - "y": 6.709840447255909, - "heading": 0.817662056557047, - "angularVelocity": -0.6186027670698295, - "velocityX": 0.6289573163604619, - "velocityY": 0.22145333556813593, - "timestamp": 0.6136446912959918 - }, - { - "x": 1.0536065024635548, - "y": 6.735133328172977, - "heading": 0.7473295527982347, - "angularVelocity": -0.687686259272288, - "velocityX": 0.7205356386732681, - "velocityY": 0.24730480411634378, - "timestamp": 0.7159188065119904 - }, - { - "x": 1.1364169197016813, - "y": 6.762648211314195, - "heading": 0.6710254557943266, - "angularVelocity": -0.7460743786173882, - "velocityX": 0.8096908708297301, - "velocityY": 0.2690307532427302, - "timestamp": 0.818192921727989 - }, - { - "x": 1.2281932228229642, - "y": 6.7918845460908575, - "heading": 0.5899378478266369, - "angularVelocity": -0.7928458510838684, - "velocityX": 0.8973561208709863, - "velocityY": 0.28586249636547506, - "timestamp": 0.9204670369439876 - }, - { - "x": 1.3289409008958384, - "y": 6.822217824239773, - "heading": 0.5053285615301966, - "angularVelocity": -0.8272795718879136, - "velocityX": 0.985075041075245, - "velocityY": 0.296588019337818, - "timestamp": 1.0227411521599863 - }, - { - "x": 1.4390031913745853, - "y": 6.852792823991799, - "heading": 0.4183893727045359, - "angularVelocity": -0.8500605319021087, - "velocityX": 1.076150015287458, - "velocityY": 0.29895148738224403, - "timestamp": 1.125015267375985 - }, - { - "x": 1.5597885498551032, - "y": 6.882059112916124, - "heading": 0.32979940511385675, - "angularVelocity": -0.8662012607776253, - "velocityX": 1.1809963684348201, - "velocityY": 0.28615537738935176, - "timestamp": 1.2272893825919837 - }, - { - "x": 1.700662064049805, - "y": 6.9025972281907375, - "heading": 0.2406449492427404, - "angularVelocity": -0.8717206268168672, - "velocityX": 1.3774112254273, - "velocityY": 0.20081439319892427, - "timestamp": 1.3295634978079824 - }, - { - "x": 1.8510164004103928, - "y": 6.913603640101383, - "heading": 0.18435642050200896, - "angularVelocity": -0.5503692556839603, - "velocityX": 1.4701113396826178, - "velocityY": 0.10761678064700267, - "timestamp": 1.4318376130239812 - }, - { - "x": 2.0039488544462833, - "y": 6.918843013673937, - "heading": 0.15617120081925578, - "angularVelocity": -0.2755850738942481, - "velocityX": 1.4953192592999736, - "velocityY": 0.051228726583383746, - "timestamp": 1.5341117282399799 - }, - { - "x": 2.1557445524327394, - "y": 6.919295783765447, - "heading": 0.15617120073466711, - "angularVelocity": -8.01993499988118e-11, - "velocityX": 1.484204462099971, - "velocityY": 0.0044270165813428694, - "timestamp": 1.6363858434559786 - }, - { - "x": 2.232501376452394, - "y": 6.918644981823818, - "heading": 0.15617120066422163, - "angularVelocity": -2.7881719949913897e-12, - "velocityX": 1.4631774376072004, - "velocityY": -0.012405855819009865, - "timestamp": 1.6888448428916174 - }, - { - "x": 2.3078780299226054, - "y": 6.916920762093343, - "heading": 0.1561712005937278, - "angularVelocity": -3.698975819730093e-12, - "velocityX": 1.436867928003674, - "velocityY": -0.032867889033463694, - "timestamp": 1.7413038423272562 - }, - { - "x": 2.381659462849091, - "y": 6.91403766609857, - "heading": 0.15617120052323402, - "angularVelocity": -3.687857737825465e-12, - "velocityX": 1.4064590265589882, - "velocityY": -0.05495897496281437, - "timestamp": 1.793762841762895 - }, - { - "x": 2.453627858185915, - "y": 6.909923503320222, - "heading": 0.15617120045273983, - "angularVelocity": -3.68806627753873e-12, - "velocityX": 1.3718979863339387, - "velocityY": -0.07842619311071317, - "timestamp": 1.8462218411985338 - }, - { - "x": 2.5235476514141553, - "y": 6.90450568335544, - "heading": 0.15617120038224525, - "angularVelocity": -3.688143550349055e-12, - "velocityX": 1.3328464960153679, - "velocityY": -0.10327716626043287, - "timestamp": 1.8986808406341726 - }, - { - "x": 2.5911718313337544, - "y": 6.897715977103231, - "heading": 0.1561712003117504, - "angularVelocity": -3.687560329325201e-12, - "velocityX": 1.2890863531735766, - "velocityY": -0.12942875706821344, - "timestamp": 1.9511398400698114 - }, - { - "x": 2.65624736130467, - "y": 6.889492702059127, - "heading": 0.15617120024125528, - "angularVelocity": -3.68944362324301e-12, - "velocityX": 1.240502697821755, - "velocityY": -0.1567561709974853, - "timestamp": 2.0035988395054503 - }, - { - "x": 2.7185182573768354, - "y": 6.879780772610566, - "heading": 0.1561712001707086, - "angularVelocity": -4.669569996251489e-12, - "velocityX": 1.187039346844362, - "velocityY": -0.1851336547556834, - "timestamp": 2.056057838941089 - }, - { - "x": 2.844727229024539, - "y": 6.826831262995891, - "heading": 0.36522912798339696, - "angularVelocity": 1.2815424227266763, - "velocityX": 0.7736714562084378, - "velocityY": -0.32458488264739044, - "timestamp": 2.219187770584854 - }, - { - "x": 2.905891887894138, - "y": 6.746880408353911, - "heading": 0.5636352419280429, - "angularVelocity": 1.2162459209838357, - "velocityX": 0.3749444284785087, - "velocityY": -0.490105365340026, - "timestamp": 2.382317702228619 - }, - { - "x": 2.909090532344001, - "y": 6.6442122814929245, - "heading": 0.7377210107974022, - "angularVelocity": 1.0671601901772823, - "velocityX": 0.0196079562833389, - "velocityY": -0.6293641266849203, - "timestamp": 2.5454476338723837 - }, - { - "x": 2.8528015320306115, - "y": 6.520167648469734, - "heading": 0.8617887991289785, - "angularVelocity": 0.7605458243522792, - "velocityX": -0.34505623601923857, - "velocityY": -0.7604038809886424, - "timestamp": 2.7085775655161486 - }, - { - "x": 2.6673775887021587, - "y": 6.358333521535233, - "heading": 0.8824380102910534, - "angularVelocity": 0.1265813757217392, - "velocityX": -1.1366641384675562, - "velocityY": -0.9920566103835737, - "timestamp": 2.8717074971599135 - }, - { - "x": 2.433334548523251, - "y": 6.191208743378355, - "heading": 0.7641593769894643, - "angularVelocity": -0.7250578241718747, - "velocityX": -1.4347032313583656, - "velocityY": -1.0244887408273557, - "timestamp": 3.0348374288036783 - }, - { - "x": 2.190845809692594, - "y": 6.024313186516238, - "heading": 0.6297541443819642, - "angularVelocity": -0.823915214372992, - "velocityX": -1.486476064011715, - "velocityY": -1.0230835952941921, - "timestamp": 3.1979673604474432 - }, - { - "x": 1.9580404786548902, - "y": 5.870547470854263, - "heading": 0.47066977289213696, - "angularVelocity": -0.9752003809508628, - "velocityX": -1.4271159717504125, - "velocityY": -0.9425965806313563, - "timestamp": 3.361097292091208 - }, - { - "x": 1.7498273467354928, - "y": 5.73486661370809, - "heading": 0.32338313207816166, - "angularVelocity": -0.9028793141986351, - "velocityX": -1.2763637531323493, - "velocityY": -0.831734899187721, - "timestamp": 3.524227223734973 - }, - { - "x": 1.577014249967444, - "y": 5.624033816048722, - "heading": 0.19882585300104294, - "angularVelocity": -0.7635464431083702, - "velocityX": -1.0593586039568081, - "velocityY": -0.6794142353068856, - "timestamp": 3.687357155378738 - }, - { - "x": 1.4506486184747536, - "y": 5.5441849844920235, - "heading": 0.1067515452077215, - "angularVelocity": -0.5644231371867715, - "velocityX": -0.7746317925903302, - "velocityY": -0.4894799553714506, - "timestamp": 3.8504870870225028 - }, - { - "x": 1.3818649056319274, - "y": 5.50129938101995, - "heading": 0.05628563946839357, - "angularVelocity": -0.30936018439404245, - "velocityX": -0.4216498592955241, - "velocityY": -0.2628923039688523, - "timestamp": 4.013617018666268 - }, - { - "x": 1.3818649054659777, - "y": 5.501299381110093, - "heading": 0.05628563946121549, - "angularVelocity": -8.20711890061584e-12, - "velocityX": -2.918186949691226e-10, - "velocityY": -8.516677331367793e-11, - "timestamp": 4.176746950310032 - }, - { - "x": 1.4254749911225055, - "y": 5.4996771535618185, - "heading": 0.052286234874463405, - "angularVelocity": -0.03818204740477268, - "velocityX": 0.41634256351427257, - "velocityY": -0.015487297221703413, - "timestamp": 4.281492632082638 - }, - { - "x": 1.5054277602383004, - "y": 5.496460245033646, - "heading": 0.04534031368940726, - "angularVelocity": -0.06631224378557163, - "velocityX": 0.763303725485265, - "velocityY": -0.030711609603001202, - "timestamp": 4.386238313855244 - }, - { - "x": 1.614706352195534, - "y": 5.491742221647794, - "heading": 0.036622661203774276, - "angularVelocity": -0.08322684370600701, - "velocityX": 1.0432753896642522, - "velocityY": -0.045042652489690874, - "timestamp": 4.4909839956278494 - }, - { - "x": 1.7468754648697162, - "y": 5.485736649446459, - "heading": 0.02746720464328704, - "angularVelocity": -0.08740652985101428, - "velocityX": 1.2618096559898304, - "velocityY": -0.05733479469253905, - "timestamp": 4.595729677400455 - }, - { - "x": 1.8974421226741423, - "y": 5.478814465512338, - "heading": 0.0192537076414664, - "angularVelocity": -0.07841370510897432, - "velocityX": 1.43744978567254, - "velocityY": -0.06608562544064354, - "timestamp": 4.700475359173061 - }, - { - "x": 2.0478771487383725, - "y": 5.472018862260595, - "heading": 0.011842428389913316, - "angularVelocity": -0.07075498604111852, - "velocityX": 1.436193106275662, - "velocityY": -0.06487716815402857, - "timestamp": 4.805221040945667 - }, - { - "x": 2.1991998312667134, - "y": 5.465297323381209, - "heading": 0.007319920190768939, - "angularVelocity": -0.04317608251475354, - "velocityX": 1.4446675030153622, - "velocityY": -0.06417008055763802, - "timestamp": 4.909966722718273 - }, - { - "x": 2.337086103631991, - "y": 5.4645313180146555, - "heading": 0.0021217678335590726, - "angularVelocity": -0.04962641198543019, - "velocityX": 1.316390996187784, - "velocityY": -0.007313001468104138, - "timestamp": 5.014712404490878 - }, - { - "x": 2.45341531313919, - "y": 5.464492524135499, - "heading": -0.0044756440905307565, - "angularVelocity": -0.06298504924092899, - "velocityX": 1.1105871626237722, - "velocityY": -0.0003703621622037101, - "timestamp": 5.119458086263484 - }, - { - "x": 2.541111145262716, - "y": 5.463618987693384, - "heading": -0.012133903650681083, - "angularVelocity": -0.07311289048562652, - "velocityX": 0.8372262287970834, - "velocityY": -0.008339593435811936, - "timestamp": 5.22420376803609 - }, - { - "x": 2.592916727068705, - "y": 5.461223602405139, - "heading": -0.020267736871702392, - "angularVelocity": -0.07765316033563659, - "velocityX": 0.4945844156915881, - "velocityY": -0.022868582319897673, - "timestamp": 5.328949449808696 - }, - { - "x": 2.573983182200651, - "y": 5.456183859404588, - "heading": -0.02941680850548689, - "angularVelocity": -0.07029420270393813, - "velocityX": -0.14547032678251723, - "velocityY": -0.03872138453206129, - "timestamp": 5.459103449457666 - }, - { - "x": 2.4879155076147748, - "y": 5.449739339031148, - "heading": -0.03668510065770142, - "angularVelocity": -0.05584378637679435, - "velocityX": -0.6612756796636089, - "velocityY": -0.04951457856894505, - "timestamp": 5.589257449106635 - }, - { - "x": 2.35046961428428, - "y": 5.4436438239165845, - "heading": -0.03977633600135786, - "angularVelocity": -0.02375059815495546, - "velocityX": -1.056025121733672, - "velocityY": -0.046833099245860266, - "timestamp": 5.719411448755605 - }, - { - "x": 2.1764781877651598, - "y": 5.443212327064482, - "heading": -0.032264347817817784, - "angularVelocity": 0.05771615319957162, - "velocityX": -1.336811984121226, - "velocityY": -0.0033152797571240995, - "timestamp": 5.849565448404575 - }, - { - "x": 1.9752868983122995, - "y": 5.4534701667830054, - "heading": -0.009494098721117665, - "angularVelocity": 0.174948516049147, - "velocityX": -1.5457941360881122, - "velocityY": 0.07881309585526655, - "timestamp": 5.979719448053545 - }, - { - "x": 1.7725080662079153, - "y": 5.4634789711909475, - "heading": 0.004869525328448035, - "angularVelocity": 0.11035868346800264, - "velocityX": -1.5579915534229911, - "velocityY": 0.07689970624561784, - "timestamp": 6.1098734477025145 - }, - { - "x": 1.5970859275969194, - "y": 5.473760403676058, - "heading": 0.019510408429697427, - "angularVelocity": 0.11248892189634652, - "velocityX": -1.347804439948399, - "velocityY": 0.07899436394267238, - "timestamp": 6.240027447351484 - }, - { - "x": 1.463998994938607, - "y": 5.481958295434183, - "heading": 0.031093440957313423, - "angularVelocity": 0.0889948258110792, - "velocityX": -1.0225343286428261, - "velocityY": 0.06298609118419185, - "timestamp": 6.370181447000454 - }, - { - "x": 1.3892531402609951, - "y": 5.486522674152253, - "heading": 0.03752685288213878, - "angularVelocity": 0.049429229542960515, - "velocityX": -0.5742878042328909, - "velocityY": 0.03506906174326123, - "timestamp": 6.500335446649424 - }, - { - "x": 1.3892531401370203, - "y": 5.486522674374907, - "heading": 0.03752685288891443, - "angularVelocity": 5.018347137854837e-11, - "velocityX": -8.981333628450582e-10, - "velocityY": 1.1863234183511445e-9, - "timestamp": 6.630489446298394 - }, - { - "x": 1.410785208000178, - "y": 5.466798705442517, - "heading": 0.02538794622123859, - "angularVelocity": -0.12074981514388294, - "velocityX": 0.21418676347263918, - "velocityY": -0.1962009963268651, - "timestamp": 6.731018848208977 - }, - { - "x": 1.4512665581597533, - "y": 5.429718184658836, - "heading": 0.002445244265967334, - "angularVelocity": -0.2282188250282345, - "velocityX": 0.4026816906907998, - "velocityY": -0.36885249450974994, - "timestamp": 6.8315482501195595 - }, - { - "x": 1.5081514675148215, - "y": 5.3776444456377, - "heading": -0.029986935931297812, - "angularVelocity": -0.3226138782330449, - "velocityX": 0.5658534482437384, - "velocityY": -0.5179951130124598, - "timestamp": 6.932077652030142 - }, - { - "x": 1.578941487701644, - "y": 5.312948064530918, - "heading": -0.07059739560458454, - "angularVelocity": -0.4039659933419245, - "velocityX": 0.7041722939494359, - "velocityY": -0.6435568078574709, - "timestamp": 7.032607053940725 - }, - { - "x": 1.6611946188865518, - "y": 5.238029568056684, - "heading": -0.11800108662062794, - "angularVelocity": -0.4715405656932785, - "velocityX": 0.8181997404499256, - "velocityY": -0.7452396506196601, - "timestamp": 7.133136455851308 - }, - { - "x": 1.7524893345353523, - "y": 5.155358397604862, - "heading": -0.17044488274189246, - "angularVelocity": -0.521676197753198, - "velocityX": 0.9081394425873396, - "velocityY": -0.8223581237255294, - "timestamp": 7.233665857761891 - }, - { - "x": 1.8499500532222615, - "y": 5.067431436588914, - "heading": -0.22499900927916092, - "angularVelocity": -0.5426683689609221, - "velocityX": 0.9694747626122975, - "velocityY": -0.8746392520744162, - "timestamp": 7.334195259672474 - }, - { - "x": 1.9473638091809122, - "y": 4.979075974351683, - "heading": -0.27982409115325835, - "angularVelocity": -0.5453636534436577, - "velocityX": 0.9690076084526664, - "velocityY": -0.8789016988127862, - "timestamp": 7.434724661583057 - }, - { - "x": 2.05818821772957, - "y": 4.854821291737014, - "heading": -0.34544281347225186, - "angularVelocity": -0.6527316496725523, - "velocityX": 1.1024079106043232, - "velocityY": -1.2360033993617228, - "timestamp": 7.53525406349364 - }, - { - "x": 2.1664425710584037, - "y": 4.732231232720307, - "heading": -0.4041393246178968, - "angularVelocity": -0.5838740711290391, - "velocityX": 1.076842701113489, - "velocityY": -1.2194448247922516, - "timestamp": 7.635783465404223 - }, - { - "x": 2.2666201121554423, - "y": 4.61627032113389, - "heading": -0.4602072656334145, - "angularVelocity": -0.557726794034664, - "velocityX": 0.996499915039965, - "velocityY": -1.153502450677793, - "timestamp": 7.736312867314806 - }, - { - "x": 2.356480498307365, - "y": 4.5087945921361055, - "heading": -0.5124957847583047, - "angularVelocity": -0.5201316047807618, - "velocityX": 0.8938716814183492, - "velocityY": -1.0690974663909212, - "timestamp": 7.8368422692253885 - }, - { - "x": 2.433798199341425, - "y": 4.411691494217003, - "heading": -0.5602485683833155, - "angularVelocity": -0.47501310796660834, - "velocityX": 0.7691053456052521, - "velocityY": -0.9659173928845837, - "timestamp": 7.937371671135971 - }, - { - "x": 2.4961951429135376, - "y": 4.327016530758609, - "heading": -0.6027268742617791, - "angularVelocity": -0.4225460918867537, - "velocityX": 0.620683518971687, - "velocityY": -0.8422905312450144, - "timestamp": 8.037901073046553 - }, - { - "x": 2.541198492257727, - "y": 4.256927967764492, - "heading": -0.6391760227839379, - "angularVelocity": -0.3625720221944257, - "velocityX": 0.4476635477259144, - "velocityY": -0.6971946668803632, - "timestamp": 8.138430474957136 - }, - { - "x": 2.5331758624705114, - "y": 4.192320334785442, - "heading": -0.6763224470307224, - "angularVelocity": -0.2449990301925222, - "velocityX": -0.052913214713167914, - "velocityY": -0.426119279886695, - "timestamp": 8.290049133403663 - }, - { - "x": 2.46411262676954, - "y": 4.160473976285577, - "heading": -0.7018758155970488, - "angularVelocity": -0.16853709734646205, - "velocityX": -0.45550618406914944, - "velocityY": -0.21004248027074415, - "timestamp": 8.44166779185019 - }, - { - "x": 2.348779581051416, - "y": 4.153307846262027, - "heading": -0.7202693675207003, - "angularVelocity": -0.121314567375086, - "velocityX": -0.7606784530447123, - "velocityY": -0.04726417363495295, - "timestamp": 8.593286450296716 - }, - { - "x": 2.201596779448434, - "y": 4.163036990055964, - "heading": -0.7333684069992377, - "angularVelocity": -0.0863946406365267, - "velocityX": -0.9707433369527995, - "velocityY": 0.06416850879734587, - "timestamp": 8.744905108743243 - }, - { - "x": 2.03723470076713, - "y": 4.1831659239379135, - "heading": -0.7500897294060416, - "angularVelocity": -0.11028538710126731, - "velocityX": -1.0840491612778367, - "velocityY": 0.13276026521150996, - "timestamp": 8.89652376718977 - }, - { - "x": 1.8800877783569956, - "y": 4.195672966501764, - "heading": -0.7289968148061016, - "angularVelocity": 0.13911819814698503, - "velocityX": -1.0364616391016628, - "velocityY": 0.08249012434493122, - "timestamp": 9.048142425636296 - }, - { - "x": 1.7046442157621393, - "y": 4.233321160679635, - "heading": -0.7238359462014516, - "angularVelocity": 0.03403847932720532, - "velocityX": -1.1571370254440554, - "velocityY": 0.24830844691554524, - "timestamp": 9.199761084082823 - }, - { - "x": 1.572815192920781, - "y": 4.2658136358994225, - "heading": -0.7241682787394389, - "angularVelocity": -0.002191897597681109, - "velocityX": -0.8694775764003041, - "velocityY": 0.21430393183203963, - "timestamp": 9.35137974252935 - }, - { - "x": 1.4994455554717883, - "y": 4.286480902556393, - "heading": -0.7254698893818832, - "angularVelocity": -0.00858476582372283, - "velocityX": -0.4839090323218549, - "velocityY": 0.13631083596916982, - "timestamp": 9.502998400975876 - }, - { - "x": 1.4994455567524874, - "y": 4.286480903049883, - "heading": -0.7254698893413497, - "angularVelocity": 3.8000232610425283e-11, - "velocityX": 2.8946950656209713e-9, - "velocityY": -5.415769760526697e-10, - "timestamp": 9.654617059422403 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj deleted file mode 100644 index 9ee66c72..00000000 --- a/src/main/deploy/choreo/test.traj +++ /dev/null @@ -1,1084 +0,0 @@ -{ - "samples": [ - { - "x": 2.1199185848236084, - "y": 5.957952976226807, - "heading": -6.25322258455275e-30, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 2.157285981558088, - "y": 5.958401136866338, - "heading": 0.034313098562848664, - "angularVelocity": 0.4715592535645141, - "velocityX": 0.5135339695272734, - "velocityY": 0.006158997744474669, - "timestamp": 0.07276518974757916 - }, - { - "x": 2.2137097634050407, - "y": 5.959057625247641, - "heading": 0.03431310711765102, - "angularVelocity": 1.1756723768181273e-7, - "velocityX": 0.7754227267555416, - "velocityY": 0.009022011535733282, - "timestamp": 0.14553037949515832 - }, - { - "x": 2.270133545252147, - "y": 5.959714113615942, - "heading": 0.03431311567244465, - "angularVelocity": 1.1756711762076828e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058456, - "timestamp": 0.21829556924273746 - }, - { - "x": 2.3265573270992537, - "y": 5.960370601984243, - "heading": 0.03431312422723829, - "angularVelocity": 1.1756711788910916e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058367, - "timestamp": 0.29106075899031664 - }, - { - "x": 2.38298110894636, - "y": 5.961027090352544, - "heading": 0.0343131327820319, - "angularVelocity": 1.175671175035012e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058381, - "timestamp": 0.3638259487378958 - }, - { - "x": 2.4394048907934667, - "y": 5.961683578720844, - "heading": 0.034313141336825545, - "angularVelocity": 1.1756711790299388e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058397, - "timestamp": 0.436591138485475 - }, - { - "x": 2.495828672640573, - "y": 5.962340067089145, - "heading": 0.03431314989161917, - "angularVelocity": 1.1756711758579864e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.0090220113570584, - "timestamp": 0.5093563282330541 - }, - { - "x": 2.5522524544876797, - "y": 5.962996555457446, - "heading": 0.0343131584464128, - "angularVelocity": 1.175671177574276e-7, - "velocityX": 0.7754227267576639, - "velocityY": 0.009022011357058414, - "timestamp": 0.5821215179806333 - }, - { - "x": 2.6086762363347864, - "y": 5.963653043825747, - "heading": 0.034313167001206415, - "angularVelocity": 1.1756711751242314e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058426, - "timestamp": 0.6548867077282124 - }, - { - "x": 2.6651000181818927, - "y": 5.964309532194048, - "heading": 0.03431317555600002, - "angularVelocity": 1.1756711746464553e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058426, - "timestamp": 0.7276518974757916 - }, - { - "x": 2.7215238000289994, - "y": 5.964966020562349, - "heading": 0.03431318411079364, - "angularVelocity": 1.1756711761494635e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058443, - "timestamp": 0.8004170872233708 - }, - { - "x": 2.7779475818761057, - "y": 5.965622508930649, - "heading": 0.034313192665587286, - "angularVelocity": 1.1756711789914241e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058452, - "timestamp": 0.87318227697095 - }, - { - "x": 2.8343713637232124, - "y": 5.96627899729895, - "heading": 0.03431320122038093, - "angularVelocity": 1.1756711786542428e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058466, - "timestamp": 0.9459474667185291 - }, - { - "x": 2.8907951455703187, - "y": 5.966935485667251, - "heading": 0.03431320977517455, - "angularVelocity": 1.1756711758478168e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058475, - "timestamp": 1.0187126564661082 - }, - { - "x": 2.9472189274174254, - "y": 5.967591974035552, - "heading": 0.03431321832996816, - "angularVelocity": 1.1756711747039204e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058483, - "timestamp": 1.0914778462136874 - }, - { - "x": 3.0036427092645317, - "y": 5.968248462403853, - "heading": 0.034313226884761766, - "angularVelocity": 1.1756711737664598e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058489, - "timestamp": 1.1642430359612665 - }, - { - "x": 3.0600664911116384, - "y": 5.968904950772154, - "heading": 0.03431323543955538, - "angularVelocity": 1.1756711759331021e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058496, - "timestamp": 1.2370082257088457 - }, - { - "x": 3.116490272958745, - "y": 5.969561439140454, - "heading": 0.034313243994349, - "angularVelocity": 1.1756711756317827e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058518, - "timestamp": 1.309773415456425 - }, - { - "x": 3.1729140548058514, - "y": 5.970217927508755, - "heading": 0.03431325254914262, - "angularVelocity": 1.1756711755305748e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058523, - "timestamp": 1.382538605204004 - }, - { - "x": 3.229337836652958, - "y": 5.970874415877057, - "heading": 0.03431326110393624, - "angularVelocity": 1.175671175226974e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.00902201135705854, - "timestamp": 1.4553037949515832 - }, - { - "x": 3.2857616185000644, - "y": 5.971530904245357, - "heading": 0.03431326965872984, - "angularVelocity": 1.1756711738766866e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058546, - "timestamp": 1.5280689846991624 - }, - { - "x": 3.342185400347171, - "y": 5.972187392613658, - "heading": 0.03431327821352346, - "angularVelocity": 1.1756711754575316e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058565, - "timestamp": 1.6008341744467416 - }, - { - "x": 3.3986091821942774, - "y": 5.972843880981959, - "heading": 0.03431328676831708, - "angularVelocity": 1.1756711769845501e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058572, - "timestamp": 1.6735993641943208 - }, - { - "x": 3.455032964041384, - "y": 5.973500369350259, - "heading": 0.034313295323110705, - "angularVelocity": 1.175671175699485e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058579, - "timestamp": 1.7463645539419 - }, - { - "x": 3.5114567458884904, - "y": 5.97415685771856, - "heading": 0.03431330387790433, - "angularVelocity": 1.1756711766849268e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058603, - "timestamp": 1.819129743689479 - }, - { - "x": 3.567880527735597, - "y": 5.9748133460868615, - "heading": 0.03431331243269794, - "angularVelocity": 1.1756711756878859e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058601, - "timestamp": 1.8918949334370583 - }, - { - "x": 3.624304309582704, - "y": 5.975469834455162, - "heading": 0.03431332098749156, - "angularVelocity": 1.1756711758891969e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058617, - "timestamp": 1.9646601231846375 - }, - { - "x": 3.68072809142981, - "y": 5.976126322823463, - "heading": 0.03431332954228517, - "angularVelocity": 1.1756711749627294e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058617, - "timestamp": 2.0374253129322164 - }, - { - "x": 3.737151873276917, - "y": 5.976782811191764, - "heading": 0.03431333809707879, - "angularVelocity": 1.1756711754302844e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058624, - "timestamp": 2.1101905026797954 - }, - { - "x": 3.793575655124023, - "y": 5.977439299560064, - "heading": 0.03431334665187242, - "angularVelocity": 1.175671176839478e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058646, - "timestamp": 2.1829556924273743 - }, - { - "x": 3.84999943697113, - "y": 5.978095787928365, - "heading": 0.034313355206666014, - "angularVelocity": 1.1756711730438892e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.00902201135705865, - "timestamp": 2.2557208821749533 - }, - { - "x": 3.906423218818236, - "y": 5.9787522762966665, - "heading": 0.03431336376145962, - "angularVelocity": 1.1756711741363398e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.009022011357058672, - "timestamp": 2.328486071922532 - }, - { - "x": 3.962847000665343, - "y": 5.979408764664967, - "heading": 0.03431337231625322, - "angularVelocity": 1.1756711736937194e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058674, - "timestamp": 2.401251261670111 - }, - { - "x": 4.019270782512449, - "y": 5.980065253033268, - "heading": 0.034313380871046816, - "angularVelocity": 1.17567117272878e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.00902201135705869, - "timestamp": 2.47401645141769 - }, - { - "x": 4.075694564359556, - "y": 5.9807217414015685, - "heading": 0.034313389425840425, - "angularVelocity": 1.1756711748988721e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058704, - "timestamp": 2.546781641165269 - }, - { - "x": 4.132118346206663, - "y": 5.981378229769869, - "heading": 0.034313397980634035, - "angularVelocity": 1.1756711734690936e-7, - "velocityX": 0.7754227267576638, - "velocityY": 0.00902201135705871, - "timestamp": 2.619546830912848 - }, - { - "x": 4.188542128053769, - "y": 5.98203471813817, - "heading": 0.03431340653542763, - "angularVelocity": 1.1756711733140912e-7, - "velocityX": 0.7754227267576637, - "velocityY": 0.009022011357058714, - "timestamp": 2.692312020660427 - }, - { - "x": 4.244965909900876, - "y": 5.982691206506428, - "heading": 0.03431341509022124, - "angularVelocity": 1.1756711743485013e-7, - "velocityX": 0.7754227267576708, - "velocityY": 0.00902201135645878, - "timestamp": 2.765077210408006 - }, - { - "x": 4.30138969265204, - "y": 5.983347617167627, - "heading": 0.03431342364501558, - "angularVelocity": 1.1756712748463029e-7, - "velocityX": 0.7754227391819724, - "velocityY": 0.009020943441183957, - "timestamp": 2.837842400155585 - }, - { - "x": 4.337240695953369, - "y": 5.957952976226807, - "heading": -3.4992233566514893e-26, - "angularVelocity": -0.47156372111511136, - "velocityX": 0.4926944247063125, - "velocityY": -0.3489943615747365, - "timestamp": 2.910607589903164 - }, - { - "x": 4.337075774441293, - "y": 5.930386954424772, - "heading": -0.09015124251095946, - "angularVelocity": -1.224439774519884, - "velocityX": -0.002239974219267059, - "velocityY": -0.37440342007034494, - "timestamp": 2.9842341123234344 - }, - { - "x": 4.333264434450346, - "y": 5.912061346555676, - "heading": -0.20505051130417343, - "angularVelocity": -1.5605690044322955, - "velocityX": -0.05176585645578975, - "velocityY": -0.24889954416819945, - "timestamp": 3.057860634743705 - }, - { - "x": 4.325553986362593, - "y": 5.887325303672255, - "heading": -0.3021610086195407, - "angularVelocity": -1.31896080546962, - "velocityX": -0.10472378477610225, - "velocityY": -0.33596647064523955, - "timestamp": 3.1314871571639755 - }, - { - "x": 4.313628651056446, - "y": 5.857761645119009, - "heading": -0.3834128542416627, - "angularVelocity": -1.1035676132892005, - "velocityX": -0.16197064473690057, - "velocityY": -0.40153544648615047, - "timestamp": 3.205113679584246 - }, - { - "x": 4.297606686426737, - "y": 5.824666184248557, - "heading": -0.4508559142750475, - "angularVelocity": -0.9160158298447155, - "velocityX": -0.2176113186258289, - "velocityY": -0.4495046048968324, - "timestamp": 3.2787402020045167 - }, - { - "x": 4.277844157832715, - "y": 5.789044520818464, - "heading": -0.5065111263049437, - "angularVelocity": -0.7559125461910066, - "velocityX": -0.26841589069241406, - "velocityY": -0.4838156449486921, - "timestamp": 3.3523667244247872 - }, - { - "x": 4.254799811398479, - "y": 5.751649129927804, - "heading": -0.5522697074374715, - "angularVelocity": -0.6214958907244229, - "velocityX": -0.3129897444116032, - "velocityY": -0.5079065214733656, - "timestamp": 3.425993246845058 - }, - { - "x": 4.228951375108858, - "y": 5.713030269053256, - "heading": -0.5898366283888508, - "angularVelocity": -0.5102362534107197, - "velocityX": -0.35107506697211693, - "velocityY": -0.524523766776681, - "timestamp": 3.4996197692653284 - }, - { - "x": 4.2007493706625425, - "y": 5.673585962715614, - "heading": -0.6207093446974378, - "angularVelocity": -0.419315150216672, - "velocityX": -0.3830413758419039, - "velocityY": -0.5357350183197291, - "timestamp": 3.573246291685599 - }, - { - "x": 4.1705955025497214, - "y": 5.633604594294146, - "heading": -0.6461809126237779, - "angularVelocity": -0.34595641745707684, - "velocityX": -0.4095517093785753, - "velocityY": -0.5430294288958701, - "timestamp": 3.6468728141058695 - }, - { - "x": 4.138835739465679, - "y": 5.593298519659669, - "heading": -0.6673577962188777, - "angularVelocity": -0.28762574815389413, - "velocityX": -0.43136307460989803, - "velocityY": -0.5474396088464458, - "timestamp": 3.72049933652614 - }, - { - "x": 4.1057614035434415, - "y": 5.552829496334061, - "heading": -0.6851851845597893, - "angularVelocity": -0.2421326955950787, - "velocityX": -0.44921768453825217, - "velocityY": -0.549652788089118, - "timestamp": 3.7941258589464106 - }, - { - "x": 4.071614137002915, - "y": 5.512327581345269, - "heading": -0.700475200245641, - "angularVelocity": -0.20766994261353336, - "velocityX": -0.4637902948289247, - "velocityY": -0.5500995247011782, - "timestamp": 3.867752381366681 - }, - { - "x": 4.036592388923184, - "y": 5.471905240898815, - "heading": -0.7139354213530316, - "angularVelocity": -0.18281755901165134, - "velocityX": -0.47566755740304917, - "velocityY": -0.5490187383252648, - "timestamp": 3.9413789037869518 - }, - { - "x": 4.0008581978077, - "y": 5.431668194433558, - "heading": -0.7261965659955223, - "angularVelocity": -0.16653162799816065, - "velocityX": -0.4853440029600765, - "velocityY": -0.5465020639651851, - "timestamp": 4.015005426207223 - }, - { - "x": 3.964543737766948, - "y": 5.391724222217225, - "heading": -0.7378391147106159, - "angularVelocity": -0.15812981969508613, - "velocityX": -0.49322525153997593, - "velocityY": -0.5425215113152707, - "timestamp": 4.088631948627493 - }, - { - "x": 3.9277575195733885, - "y": 5.352190902296246, - "heading": -0.7494192294944804, - "angularVelocity": -0.15728183816374608, - "velocityX": -0.4996327000693894, - "velocityY": -0.536944006336704, - "timestamp": 4.162258471047764 - }, - { - "x": 3.89059042061129, - "y": 5.3132030416510085, - "heading": -0.761494708226674, - "angularVelocity": -0.16400990207394403, - "velocityX": -0.5048058463218383, - "velocityY": -0.5295355445784652, - "timestamp": 4.2358849934680345 - }, - { - "x": 3.8531219539867014, - "y": 5.274920426877835, - "heading": -0.7746519957947015, - "angularVelocity": -0.1787030968666938, - "velocityX": -0.5088990406298591, - "velocityY": -0.5199568513456381, - "timestamp": 4.309511515888305 - }, - { - "x": 3.8154274539330624, - "y": 5.23753642375436, - "heading": -0.7895355361070671, - "angularVelocity": -0.2021491688471734, - "velocityX": -0.5119690407007602, - "velocityY": -0.507751852112243, - "timestamp": 4.383138038308576 - }, - { - "x": 3.7775872299458775, - "y": 5.201287866945812, - "heading": -0.8068810412551409, - "angularVelocity": -0.23558772814317216, - "velocityX": -0.5139482722161929, - "velocityY": -0.49233014974734224, - "timestamp": 4.456764560728846 - }, - { - "x": 3.7396993230404707, - "y": 5.166466510716887, - "heading": -0.8275545372921488, - "angularVelocity": -0.2807887070776005, - "velocityX": -0.5145959045727775, - "velocityY": -0.4729458228402956, - "timestamp": 4.530391083149117 - }, - { - "x": 3.701898417706247, - "y": 5.133430805056498, - "heading": -0.8525964909440766, - "angularVelocity": -0.3401213697013267, - "velocityX": -0.5134142438298316, - "velocityY": -0.44869300592275085, - "timestamp": 4.604017605569387 - }, - { - "x": 3.664343129524347, - "y": 5.101026571041832, - "heading": -0.8792757408403825, - "angularVelocity": -0.36235922897480916, - "velocityX": -0.5100782564132129, - "velocityY": -0.44011631881374225, - "timestamp": 4.677644127989658 - }, - { - "x": 3.6270751479601078, - "y": 5.068774406382283, - "heading": -0.9064492634455257, - "angularVelocity": -0.3690724716024585, - "velocityX": -0.5061760400893075, - "velocityY": -0.4380509033884428, - "timestamp": 4.7512706504099285 - }, - { - "x": 3.590111595107405, - "y": 5.036682857394856, - "heading": -0.9341662862028731, - "angularVelocity": -0.37645432442313, - "velocityX": -0.5020412704230309, - "velocityY": -0.43586941135483154, - "timestamp": 4.824897172830199 - }, - { - "x": 3.55347222485786, - "y": 5.004762522170154, - "heading": -0.9624850330743058, - "angularVelocity": -0.38462697870999835, - "velocityX": -0.4976382021739686, - "velocityY": -0.4335439753964831, - "timestamp": 4.89852369525047 - }, - { - "x": 3.517180102788644, - "y": 4.973026350131823, - "heading": -0.9914744586380927, - "angularVelocity": -0.3937361783612594, - "velocityX": -0.49292185582330617, - "velocityY": -0.4310426595619462, - "timestamp": 4.97215021767074 - }, - { - "x": 3.4812625207112933, - "y": 4.941490315870616, - "heading": -1.0212172545129166, - "angularVelocity": -0.40396850071293, - "velocityX": -0.48783483039342795, - "velocityY": -0.42832437584372535, - "timestamp": 5.045776740091011 - }, - { - "x": 3.445752274995712, - "y": 4.9101744053392995, - "heading": -1.051814154831974, - "angularVelocity": -0.41556900031765764, - "velocityX": -0.4823023626307331, - "velocityY": -0.4253346416738305, - "timestamp": 5.119403262511281 - }, - { - "x": 3.4106895099559638, - "y": 4.879104067704057, - "heading": -1.0833902078498774, - "angularVelocity": -0.4288679130824993, - "velocityX": -0.4762246523013149, - "velocityY": -0.4219992553483426, - "timestamp": 5.193029784931552 - }, - { - "x": 3.376124474151927, - "y": 4.848312423976759, - "heading": -1.1161042305606605, - "angularVelocity": -0.44432388812345175, - "velocityX": -0.4694644629109969, - "velocityY": -0.4182140173826824, - "timestamp": 5.266656307351822 - }, - { - "x": 3.3421218274128583, - "y": 4.817843776695172, - "heading": -1.1501637008410697, - "angularVelocity": -0.4625978405715368, - "velocityX": -0.4618260597040901, - "velocityY": -0.4138270595977395, - "timestamp": 5.340282829772093 - }, - { - "x": 3.3087677521953345, - "y": 4.787759503770135, - "heading": -1.1858495459870186, - "angularVelocity": -0.48468736499938486, - "velocityX": -0.45301712101970637, - "velocityY": -0.40860646321595784, - "timestamp": 5.413909352192364 - }, - { - "x": 3.2761825599174124, - "y": 4.758148688152823, - "heading": -1.2235604524642407, - "angularVelocity": -0.5121918737647652, - "velocityX": -0.44257410518347007, - "velocityY": -0.4021759366589302, - "timestamp": 5.487535874612634 - }, - { - "x": 3.244545279648596, - "y": 4.729149218235016, - "heading": -1.2639000345369864, - "angularVelocity": -0.547894708953951, - "velocityX": -0.4296995054068451, - "velocityY": -0.3938726014013467, - "timestamp": 5.561162397032905 - }, - { - "x": 3.2141486367937797, - "y": 4.700995814255907, - "heading": -1.3078734515357762, - "angularVelocity": -0.5972496805944914, - "velocityX": -0.4128490910015826, - "velocityY": -0.382381281278044, - "timestamp": 5.634788919453175 - }, - { - "x": 3.185551521789466, - "y": 4.674155611334741, - "heading": -1.357437524753386, - "angularVelocity": -0.6731823205595795, - "velocityX": -0.38840779197850694, - "velocityY": -0.36454530295424753, - "timestamp": 5.708415441873446 - }, - { - "x": 3.161389111124608, - "y": 4.65130116728667, - "heading": -1.4227635315487523, - "angularVelocity": -0.8872618812888667, - "velocityX": -0.3281753622279693, - "velocityY": -0.3104104784090629, - "timestamp": 5.782041964293716 - }, - { - "x": 3.1613881587982178, - "y": 4.647716999053955, - "heading": -1.5707963267948966, - "angularVelocity": -2.010590618434374, - "velocityX": -0.00001293455617664302, - "velocityY": -0.048680395527252375, - "timestamp": 5.855668486713987 - }, - { - "x": 3.1590242751538056, - "y": 4.648068652143942, - "heading": -1.71822885135636, - "angularVelocity": -2.0416470093627317, - "velocityX": -0.032735083303029204, - "velocityY": 0.0048696953514198, - "timestamp": 5.927881030746015 - }, - { - "x": 3.147722843506569, - "y": 4.651261007949863, - "heading": -1.8460861775851984, - "angularVelocity": -1.7705694757427723, - "velocityX": -0.15650233347580403, - "velocityY": 0.04420777371456313, - "timestamp": 6.000093574778043 - }, - { - "x": 3.1294383579479246, - "y": 4.658664086881976, - "heading": -1.9555006668792347, - "angularVelocity": -1.5151728935835482, - "velocityX": -0.25320373078858216, - "velocityY": 0.10251790781432463, - "timestamp": 6.07230611881007 - }, - { - "x": 3.105950392035797, - "y": 4.670811298853881, - "heading": -2.048080530965875, - "angularVelocity": -1.2820468427975569, - "velocityX": -0.325261576461146, - "velocityY": 0.16821470749621126, - "timestamp": 6.144518662842098 - }, - { - "x": 3.078734489629141, - "y": 4.687681461713096, - "heading": -2.125691691683944, - "angularVelocity": -1.0747600954710448, - "velocityX": -0.37688607667090807, - "velocityY": 0.23361817652806605, - "timestamp": 6.216731206874126 - }, - { - "x": 3.048937913872785, - "y": 4.708930614799334, - "heading": -2.190285310241282, - "angularVelocity": -0.8944930472009076, - "velocityX": -0.41262326588495246, - "velocityY": 0.2942584750485111, - "timestamp": 6.288943750906154 - }, - { - "x": 3.0174119547024816, - "y": 4.734060139937329, - "heading": -2.243768219004051, - "angularVelocity": -0.7406318317638603, - "velocityX": -0.4365717839316291, - "velocityY": 0.347993904311833, - "timestamp": 6.361156294938182 - }, - { - "x": 2.9847672349131935, - "y": 4.762525988198673, - "heading": -2.2879169282753247, - "angularVelocity": -0.6113717479845765, - "velocityX": -0.4520643916770092, - "velocityY": 0.3941953388142518, - "timestamp": 6.4333688389702095 - }, - { - "x": 2.9514318737909195, - "y": 4.793802004717416, - "heading": -2.3243308683279778, - "angularVelocity": -0.5042605899122328, - "velocityX": -0.4616283994577112, - "velocityY": 0.4331105757037482, - "timestamp": 6.505581383002237 - }, - { - "x": 2.9177026495619596, - "y": 4.827411588267587, - "heading": -2.3544162803840454, - "angularVelocity": -0.4166230737242065, - "velocityX": -0.46708261952383673, - "velocityY": 0.46542583426037293, - "timestamp": 6.577793927034265 - }, - { - "x": 2.88378589769779, - "y": 4.862939604927141, - "heading": -2.379391061920221, - "angularVelocity": -0.3458510134347111, - "velocityX": -0.4696795040087044, - "velocityY": 0.4919923142964852, - "timestamp": 6.650006471066293 - }, - { - "x": 2.8498283989976017, - "y": 4.900033117168976, - "heading": -2.4003021659557313, - "angularVelocity": -0.289577168562792, - "velocityX": -0.47024376658337114, - "velocityY": 0.5136713120837215, - "timestamp": 6.722219015098321 - }, - { - "x": 2.8159399738054236, - "y": 4.938396448572123, - "heading": -2.4180494138871094, - "angularVelocity": -0.2457640589910084, - "velocityX": -0.46928723598420935, - "velocityY": 0.5312557799671519, - "timestamp": 6.794431559130349 - }, - { - "x": 2.7822098245267197, - "y": 4.977783840614619, - "heading": -2.4334117988726813, - "angularVelocity": -0.2127384541217516, - "velocityX": -0.4670954296215342, - "velocityY": 0.5454369814893605, - "timestamp": 6.866644103162376 - }, - { - "x": 2.7487184813842567, - "y": 5.0179914540575155, - "heading": -2.4470741047795617, - "angularVelocity": -0.18919574278978962, - "velocityX": -0.46378843996424124, - "velocityY": 0.5567954152821849, - "timestamp": 6.938856647194404 - }, - { - "x": 2.7155468563385656, - "y": 5.0588495367779895, - "heading": -2.459652889491748, - "angularVelocity": -0.17419113092882704, - "velocityX": -0.4593609807041143, - "velocityY": 0.5658031200556053, - "timestamp": 7.011069191226432 - }, - { - "x": 2.682783562068564, - "y": 5.100215023933873, - "heading": -2.471721683170927, - "angularVelocity": -0.16712877022898326, - "velocityX": -0.45370641221933317, - "velocityY": 0.5728296615271857, - "timestamp": 7.08328173525846 - }, - { - "x": 2.650531366839192, - "y": 5.141964497495229, - "heading": -2.4838357664761026, - "angularVelocity": -0.16775594140268155, - "velocityX": -0.44662870781934594, - "velocityY": 0.5781471089405039, - "timestamp": 7.155494279290488 - }, - { - "x": 2.618913440143494, - "y": 5.183987197473736, - "heading": -2.496557238457018, - "angularVelocity": -0.17616706559006176, - "velocityX": -0.4378453511023647, - "velocityY": 0.5819307509768665, - "timestamp": 7.2277068233225155 - }, - { - "x": 2.5880798824866296, - "y": 5.226177564560792, - "heading": -2.5104813496102354, - "angularVelocity": -0.1928212243435465, - "velocityX": -0.4269834011551906, - "velocityY": 0.5842526066986774, - "timestamp": 7.299919367354543 - }, - { - "x": 2.5582148956282778, - "y": 5.268426535906725, - "heading": -2.5262653286045555, - "angularVelocity": -0.21857669198470553, - "velocityX": -0.41357062347929907, - "velocityY": 0.585064159035808, - "timestamp": 7.372131911386571 - }, - { - "x": 2.5295447873325214, - "y": 5.31061044475372, - "heading": -2.5446612115646436, - "angularVelocity": -0.2547463630685689, - "velocityX": -0.39702393372321826, - "velocityY": 0.5841631729285931, - "timestamp": 7.444344455418599 - }, - { - "x": 2.5023467091367184, - "y": 5.352575812308809, - "heading": -2.566554449178874, - "angularVelocity": -0.3031777637486395, - "velocityX": -0.3766392468286359, - "velocityY": 0.5811368110293486, - "timestamp": 7.516556999450627 - }, - { - "x": 2.476956466389222, - "y": 5.394117632986223, - "heading": -2.593007817119188, - "angularVelocity": -0.36632649209230705, - "velocityX": -0.3516043242602833, - "velocityY": 0.5752715298188288, - "timestamp": 7.5887695434826545 - }, - { - "x": 2.4521989817386887, - "y": 5.435268589005595, - "heading": -2.621259955334882, - "angularVelocity": -0.39123588005934157, - "velocityX": -0.3428418840852991, - "velocityY": 0.5698588322981821, - "timestamp": 7.660982087514682 - }, - { - "x": 2.427559065081996, - "y": 5.476070836186979, - "heading": -2.6500802362174434, - "angularVelocity": -0.39910352514071706, - "velocityX": -0.34121380138282903, - "velocityY": 0.5650299089765801, - "timestamp": 7.73319463154671 - }, - { - "x": 2.403044029416898, - "y": 5.516501717159452, - "heading": -2.6795300674706377, - "angularVelocity": -0.407821544690809, - "velocityX": -0.3394844482175417, - "velocityY": 0.559887226165873, - "timestamp": 7.805407175578738 - }, - { - "x": 2.378663293884139, - "y": 5.556534603961008, - "heading": -2.7096829003029756, - "angularVelocity": -0.41755671727843513, - "velocityX": -0.33762465870120245, - "velocityY": 0.5543757990828, - "timestamp": 7.877619719610766 - }, - { - "x": 2.354428772799968, - "y": 5.59613781059984, - "heading": -2.74062708945702, - "angularVelocity": -0.42851542718561086, - "velocityX": -0.33559987961957327, - "velocityY": 0.5484255840822709, - "timestamp": 7.949832263642794 - }, - { - "x": 2.330355697409695, - "y": 5.635273029678329, - "heading": -2.77247058331099, - "angularVelocity": -0.4409690072662031, - "velocityX": -0.3333641781072918, - "velocityY": 0.5419448878733852, - "timestamp": 8.02204480767482 - }, - { - "x": 2.306463861338364, - "y": 5.673893057942382, - "heading": -2.8053478457859313, - "angularVelocity": -0.45528464501069815, - "velocityX": -0.33085437428619124, - "velocityY": 0.534810520550616, - "timestamp": 8.094257351706847 - }, - { - "x": 2.282779542094817, - "y": 5.711938365696433, - "heading": -2.839430372846894, - "angularVelocity": -0.47197516051846805, - "velocityX": -0.3279806792714956, - "velocityY": 0.5268517854346452, - "timestamp": 8.166469895738874 - }, - { - "x": 2.259338596376033, - "y": 5.749331681355029, - "heading": -2.8749434001407064, - "angularVelocity": -0.4917847414164131, - "velocityX": -0.3246104403742845, - "velocityY": 0.5178229926647014, - "timestamp": 8.238682439770901 - }, - { - "x": 2.2361917393720554, - "y": 5.785968937848207, - "heading": -2.9121940133772717, - "angularVelocity": -0.5158468481603996, - "velocityX": -0.3205378970405998, - "velocityY": 0.5073530781151929, - "timestamp": 8.310894983802928 - }, - { - "x": 2.2134142469672544, - "y": 5.821702966064213, - "heading": -2.9516221084722227, - "angularVelocity": -0.5460006377488126, - "velocityX": -0.3154229325406202, - "velocityY": 0.4948451642993865, - "timestamp": 8.383107527834955 - }, - { - "x": 2.191125686501144, - "y": 5.856311001786043, - "heading": -2.9939026154296866, - "angularVelocity": -0.5855008644856994, - "velocityX": -0.3086521983801695, - "velocityY": 0.4792524094772386, - "timestamp": 8.455320071866982 - }, - { - "x": 2.169536365545648, - "y": 5.8894197426408175, - "heading": -3.0401828782490323, - "angularVelocity": -0.6408895218927577, - "velocityX": -0.29896912295350214, - "velocityY": 0.4584901598272232, - "timestamp": 8.527532615899009 - }, - { - "x": 2.1490856813219974, - "y": 5.9202865649088325, - "heading": -3.092780352693255, - "angularVelocity": -0.7283703288571992, - "velocityX": -0.28320127060722483, - "velocityY": 0.42744404980837086, - "timestamp": 8.599745159931036 - }, - { - "x": 2.131117343902588, - "y": 5.946754455566406, - "heading": -3.158540067224243, - "angularVelocity": -0.9106411553901581, - "velocityX": -0.24882570833455084, - "velocityY": 0.3665276028197312, - "timestamp": 8.671957703963063 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/auto1.auto b/src/main/deploy/pathplanner/autos/auto1.auto new file mode 100644 index 00000000..3c131b0f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/auto1.auto @@ -0,0 +1,135 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3321385383605957, + "y": 5.547596454620361 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "auto1.1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "fender" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "fender" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "fender" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.1 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.1 + } + }, + { + "type": "named", + "data": { + "name": "fender" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto index c5819b58..6279b00d 100644 --- a/src/main/deploy/pathplanner/autos/local 4.auto +++ b/src/main/deploy/pathplanner/autos/local 4.auto @@ -18,7 +18,7 @@ } }, { - "type": "race", + "type": "deadline", "data": { "commands": [ { @@ -28,22 +28,9 @@ } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intake" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] + "name": "intake" } } ] @@ -56,7 +43,7 @@ } }, { - "type": "race", + "type": "deadline", "data": { "commands": [ { @@ -66,22 +53,9 @@ } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intake" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.5 - } - } - ] + "name": "intake" } } ] @@ -94,7 +68,7 @@ } }, { - "type": "race", + "type": "deadline", "data": { "commands": [ { @@ -104,22 +78,9 @@ } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intake" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.5 - } - } - ] + "name": "intake" } } ] diff --git a/src/main/java/frc/robot/AutoManager.java b/src/main/java/frc/robot/AutoManager.java new file mode 100644 index 00000000..df87d69f --- /dev/null +++ b/src/main/java/frc/robot/AutoManager.java @@ -0,0 +1,67 @@ +package frc.robot; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.feeder.FeederSubsystem; +import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.ShooterSubystem; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + +public final class AutoManager { + LoggedDashboardChooser chooser = new LoggedDashboardChooser("Auto Chooser"); + + SwerveSubsystem swerveSubsystem; + IntakeSubsystem intakeSubsystem; + ElevatorSubsystem elevatorSubsystem; + ShooterSubystem shooterSubsystem; + FeederSubsystem feederSubsystem; + + private void setUpChooser(String defaultAutoName) { + List autoNames = AutoBuilder.getAllAutoNames(); + + List options = new ArrayList<>(); + + for (String autoName : autoNames) { + try { + options.add(new PathPlannerAuto(autoName)); + } catch (Exception e) { + System.out.println("Error loading auto: " + autoName); + e.printStackTrace(); + } + } + + if (!defaultAutoName.isEmpty()) { + chooser.addDefaultOption(defaultAutoName, new PathPlannerAuto(defaultAutoName)); + } + + options.forEach(auto -> chooser.addOption(auto.getName(), auto)); + } + + public AutoManager( + SwerveSubsystem swerveSubsystem, + IntakeSubsystem intakeSubsystem, + ElevatorSubsystem elevatorSubsystem, + ShooterSubystem shooterSubsystem, + FeederSubsystem feederSubsystem) { + this.swerveSubsystem = swerveSubsystem; + this.intakeSubsystem = intakeSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + this.shooterSubsystem = shooterSubsystem; + this.feederSubsystem = feederSubsystem; + + registerNamedCommands(); + setUpChooser("single"); + } + + public void registerNamedCommands() { + NamedCommands.registerCommand("intake", intakeSubsystem.getDefaultCommand()); + NamedCommands.registerCommand("fender", shooterSubsystem.getDefaultCommand()); + NamedCommands.registerCommand("stop", swerveSubsystem.stopWithXCmd().asProxy()); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8d50c41d..ee700088 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,32 +5,37 @@ package frc.robot; import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.carriage.CarriageIOReal; import frc.robot.subsystems.carriage.CarriageSubsystem; +import frc.robot.subsystems.elevator.ElevatorIOReal; import frc.robot.subsystems.elevator.ElevatorIOSim; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.feeder.FeederIOReal; import frc.robot.subsystems.feeder.FeederSubsystem; import frc.robot.subsystems.intake.IntakeIOReal; import frc.robot.subsystems.intake.IntakeSubsystem; -import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseIOReal; -import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseSubsystem; +import frc.robot.subsystems.leds.LEDIOReal; +import frc.robot.subsystems.leds.LEDIOSim; +import frc.robot.subsystems.leds.LEDSubsystem; import frc.robot.subsystems.shooter.ShooterIOReal; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterSubystem; @@ -40,10 +45,16 @@ import frc.robot.subsystems.swerve.SwerveSubsystem.AutoAimStates; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.dynamicauto.DynamicAuto; +import frc.robot.utils.dynamicauto.LocalADStarAK; +import frc.robot.utils.dynamicauto.Note; +import java.util.Map; import java.util.function.Supplier; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; @@ -61,6 +72,15 @@ public static enum Target { SPEAKER } + private static enum CommandSelector { + START_TO_NOTE, + NOTE_TO_SHOOT, + NOTE_TO_NOTE, + SHOOT_TO_NOTE, + DYNAMIC_TO_NOTE, + DYNAMIC_TO_SHOOT + } + public static final RobotMode mode = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM; private Command autonomousCommand; @@ -68,7 +88,12 @@ public static enum Target { private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); private Target currentTarget = Target.SPEAKER; - private double flywheelIdleSpeed = 1.0; + private double flywheelIdleSpeed = -0.1; + + private int dynamicAutoCounter = 0; + private boolean atShootingLocation = false; + private ChoreoTrajectory curTrajectory = Choreo.getTrajectory("Amp Side To C1"); + private Pose2d[] forwardLookingTrajectory = curTrajectory.getPoses(); private final SwerveSubsystem swerve = new SwerveSubsystem( @@ -87,18 +112,29 @@ public void setYaw(Rotation2d yaw) {} : SwerveSubsystem.createSimModules()); private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOReal()); private final FeederSubsystem feeder = new FeederSubsystem(new FeederIOReal()); - private final ElevatorSubsystem elevator = new ElevatorSubsystem(new ElevatorIOSim()); + private final ElevatorSubsystem elevator = + new ElevatorSubsystem(mode == RobotMode.REAL ? new ElevatorIOReal() : new ElevatorIOSim()); private final ShooterSubystem shooter = new ShooterSubystem(mode == RobotMode.REAL ? new ShooterIOReal() : new ShooterIOSim()); private final CarriageSubsystem carriage = new CarriageSubsystem(new CarriageIOReal()); - private final ReactionBarReleaseSubsystem reactionBarRelease = - new ReactionBarReleaseSubsystem(new ReactionBarReleaseIOReal()); + // private final ReactionBarReleaseSubsystem reactionBarRelease = + // new ReactionBarReleaseSubsystem(new ReactionBarReleaseIOReal()); + private final LEDSubsystem leds = + new LEDSubsystem(mode == RobotMode.REAL ? new LEDIOReal() : new LEDIOSim()); + + private AutoManager autoManager = new AutoManager(swerve, intake, elevator, shooter, feeder); + ; + + private LoggedDashboardChooser noteDropdown = + new LoggedDashboardChooser("Note Picker"); @Override public void robotInit() { + Pathfinding.setPathfinder(new LocalADStarAK()); // Metadata about the current code running on the robot Logger.recordMetadata("Codebase", "Comp2024"); Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); + Logger.recordMetadata("Robot Mode", mode.toString()); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); @@ -116,7 +152,7 @@ public void robotInit() { switch (mode) { case REAL: - // Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick + Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging break; @@ -137,6 +173,11 @@ public void robotInit() { Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may // be added. + SignalLogger.setPath("/media/sda1/"); + + for (Note note : DynamicAuto.notes) { + noteDropdown.addOption(note.getName(), note); + } // Default Commands here swerve.setDefaultCommand( @@ -148,14 +189,35 @@ public void robotInit() { -teleopAxisAdjustment(controller.getRightX()) * SwerveSubsystem.MAX_ANGULAR_SPEED))); elevator.setDefaultCommand(elevator.setExtensionCmd(() -> 0.0)); - feeder.setDefaultCommand(feeder.runVoltageCmd(0.0)); - carriage.setDefaultCommand(carriage.runVoltageCmd(0.0)); - intake.setDefaultCommand(intake.runVoltageCmd(10.0)); + feeder.setDefaultCommand( + Commands.repeatingSequence( + feeder.indexCmd().until(() -> currentTarget == Target.AMP), + Commands.sequence( + feeder + .runVoltageCmd(-FeederSubsystem.INDEXING_VOLTAGE) + .until(() -> carriage.getBeambreak()), + feeder.runVoltageCmd(-FeederSubsystem.INDEXING_VOLTAGE).withTimeout(0.5), + feeder.runVoltageCmd(0.0)) + .until(() -> currentTarget == Target.SPEAKER))); + carriage.setDefaultCommand( + Commands.repeatingSequence( + carriage.indexBackwardsCmd().until(() -> currentTarget == Target.AMP), + Commands.sequence( + carriage + .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE) + .until(() -> feeder.getFirstBeambreak()), + carriage.runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE).withTimeout(0.5), + carriage.runVoltageCmd(0.0)) + .until(() -> currentTarget == Target.SPEAKER))); + intake.setDefaultCommand(intake.runVoltageCmd(0.0, 0.0)); shooter.setDefaultCommand( shooter.runStateCmd( - () -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed)); - reactionBarRelease.setDefaultCommand( - reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0))); + () -> Rotation2d.fromDegrees(5.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed)); + // reactionBarRelease.setDefaultCommand( + // reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0))); + leds.setDefaultCommand( + leds.defaultStateDisplayCmd( + () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER)); controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); @@ -165,29 +227,23 @@ public void robotInit() { .debounce(0.25) .whileTrue( Commands.parallel( - intake.runVoltageCmd(0.0).withInterruptBehavior(InterruptionBehavior.kCancelSelf), - controller.rumbleCmd(1.0, 1.0).withTimeout(0.25))); - new Trigger(() -> currentTarget == Target.SPEAKER) - .whileTrue(Commands.parallel(carriage.runVoltageCmd(5.0), feeder.indexCmd())); - new Trigger(() -> currentTarget == Target.AMP) - .whileTrue( - Commands.either( - Commands.parallel(carriage.indexBackwardsCmd(), feeder.runVoltageCmd(-5.0)), - Commands.parallel(carriage.indexForwardsCmd(), feeder.runVoltageCmd(0.0)), - () -> feeder.getFirstBeambreak())); + controller.rumbleCmd(1.0, 1.0), + leds.setBlinkingCmd(new Color("#ff8000"), new Color("#000000"), 25.0)) + .withTimeout(0.5)); // ---- Controller bindings here ---- - controller.leftTrigger().whileTrue(intake.runVoltageCmd(10.0)); + controller.leftTrigger().whileTrue(intake.runVelocityCmd(80.0, 30.0)); controller .rightTrigger() .and(() -> currentTarget == Target.SPEAKER) + .and(() -> false) .whileTrue( Commands.parallel( teleopAutoAim( () -> { double vx = swerve.getVelocity().vxMetersPerSecond; double vy = swerve.getVelocity().vyMetersPerSecond; - double vTheta = swerve.getVelocity().omegaRadiansPerSecond; + double omega = swerve.getVelocity().omegaRadiansPerSecond; double polarVelocity = MathUtil.clamp( @@ -199,11 +255,21 @@ public void robotInit() { new ChassisSpeeds( polarVelocity * Math.cos(polarRadians), polarVelocity * Math.sin(polarRadians), - vTheta); + omega); Logger.recordOutput("AutoAim/Polar Speeds", polarSpeeds); return polarSpeeds; }), - Commands.waitSeconds(0.5).andThen(feeder.runVoltageCmd(3.0)))); + Commands.waitSeconds(0.5) + .andThen(feeder.runVoltageCmd(FeederSubsystem.INDEXING_VOLTAGE)))); + controller + .rightTrigger() + .and(() -> currentTarget == Target.SPEAKER) + .and(() -> true) + .whileTrue( + Commands.parallel( + shooter.runStateCmd(Rotation2d.fromDegrees(60.0), 80.0, 60.0), + Commands.waitSeconds(1.0) + .andThen(feeder.runVoltageCmd(FeederSubsystem.INDEXING_VOLTAGE)))); controller .rightTrigger() .and(() -> currentTarget == Target.AMP) @@ -225,81 +291,94 @@ public void robotInit() { swerve.teleopPointTowardsTranslationCmd( () -> -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED, () -> -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED)); + // Test binding for elevator - controller.b().whileTrue(elevator.setExtensionCmd(() -> 0.5)); - controller.x().whileTrue(elevator.setExtensionCmd(() -> Units.inchesToMeters(30.0))); + controller.b().whileTrue(elevator.setExtensionCmd(() -> 1.0)); controller - .y() - .and(() -> elevator.getExtensionMeters() > 0.9 * ElevatorSubsystem.CLIMB_EXTENSION_METERS) - .onTrue( - Commands.sequence( - elevator - .setExtensionCmd(() -> 0.0) - .until(() -> elevator.getExtensionMeters() < 0.05), - Commands.waitUntil(() -> controller.y().getAsBoolean()), - Commands.parallel( - carriage - .runVoltageCmd(-CarriageSubsystem.INDEXING_VOLTAGE) - .withTimeout(0.25) - .andThen( - Commands.waitUntil( - () -> - elevator.getExtensionMeters() - > 0.95 * ElevatorSubsystem.TRAP_EXTENSION_METERS), - carriage.runVoltageCmd(-CarriageSubsystem.INDEXING_VOLTAGE)), - elevator.setExtensionCmd(() -> ElevatorSubsystem.TRAP_EXTENSION_METERS)))); - - // Prep climb - operator - .rightTrigger(0.5) - .and(operator.rightBumper()) - .toggleOnFalse( - Commands.parallel( - elevator.setExtensionCmd(() -> ElevatorSubsystem.CLIMB_EXTENSION_METERS))); - operator.leftTrigger().onTrue(Commands.runOnce(() -> currentTarget = Target.SPEAKER)); - operator.leftBumper().onTrue(Commands.runOnce(() -> currentTarget = Target.AMP)); - operator.a().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 1.0)); - operator.b().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0)); - operator.x().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0)); - operator.y().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 80.0)); + .x() + .toggleOnTrue(swerve.runChoreoTraj(() -> DynamicAuto.makeNoteToNote(swerve::getPose))); + controller + .leftBumper() + .toggleOnTrue( + Commands.runOnce( + () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(), swerve)); + controller + .rightBumper() + .toggleOnTrue( + Commands.runOnce( + () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve)); - SmartDashboard.putData("Shooter shoot", shootWithDashboard()); + SmartDashboard.putData( + "Whitelist Note", + Commands.runOnce( + () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve)); + SmartDashboard.putData( + "Blacklist All", + Commands.runOnce( + () -> { + for (Note note : DynamicAuto.notes) { + note.blacklist(); + } + }, + swerve)); + SmartDashboard.putData( + "Whitelist All", + Commands.runOnce( + () -> { + for (Note note : DynamicAuto.notes) { + note.whitelist(); + } + }, + swerve)); + SmartDashboard.putData("Dynamic Demo", dynamicAutoDemo()); + SmartDashboard.putData( + "gloob", + Commands.sequence( + Commands.runOnce( + () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve), + startToNote(), + blacklistNote(), + noteToShoot(), + noteToNote())); + SmartDashboard.putData( + "gleeb", + Commands.sequence( + Commands.runOnce( + () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve), + startToNote(), + blacklistNote(), + noteToNote())); - NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); NamedCommands.registerCommand( "auto aim amp 4 local sgmt 1", autonomousAutoAim("amp 4 local sgmt 1")); - controller - .leftBumper() - .whileTrue( - teleopAutoAim( - () -> { - double vx = swerve.getVelocity().vxMetersPerSecond; - double vy = swerve.getVelocity().vyMetersPerSecond; - double vTheta = swerve.getVelocity().omegaRadiansPerSecond; - - double polarVelocity = - MathUtil.clamp( - Math.sqrt(Math.pow(vx, 2) + Math.pow(vy, 2)), - -SwerveSubsystem.MAX_LINEAR_SPEED / 2, - SwerveSubsystem.MAX_LINEAR_SPEED / 2); - double polarRadians = Math.atan2(vy, vx); - ChassisSpeeds polarSpeeds = - new ChassisSpeeds( - polarVelocity * Math.cos(polarRadians), - polarVelocity * Math.sin(polarRadians), - vTheta); - Logger.recordOutput("AutoAim/Polar Sppeeds", polarSpeeds); - return polarSpeeds; - })); - // Dashboard command buttons + SmartDashboard.putData("Shooter shoot", shootWithDashboard()); SmartDashboard.putData("Run Swerve Azimuth Sysid", swerve.runModuleSteerCharacterizationCmd()); SmartDashboard.putData("Run Swerve Drive Sysid", swerve.runDriveCharacterizationCmd()); SmartDashboard.putData("Run Elevator Sysid", elevator.runSysidCmd()); SmartDashboard.putData("Run Pivot Sysid", shooter.runPivotSysidCmd()); SmartDashboard.putData("Run Flywheel Sysid", shooter.runFlywheelSysidCmd()); + SmartDashboard.putData("Update Note", updateNote()); + + SmartDashboard.putData("Dynamic Auto", dynamicAuto()); + } + + public Command dynamicAutoDemo() { + return Commands.sequence( + Commands.runOnce( + () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve), + startToNote(), + blacklistNote(), + noteToShoot(), + shootToNote(), + blacklistNote(), + noteToShoot(), + shootToNote(), + blacklistNote(), + noteToNote()) + .asProxy(); } @Override @@ -311,6 +390,34 @@ public void robotPeriodic() { new Pose3d[] { shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose() }); + Logger.recordOutput( + "DynamicAuto/Closest Note Within Distance", + swerve + .getPose() + .minus(DynamicAuto.getClosestNote(swerve::getPose).getPoseAllianceSpicific()) + .getTranslation() + .getNorm() + > 1.5); + Logger.recordOutput("DynamicAuto/Current Path Ending Pose", curTrajectory.getFinalPose()); + Logger.recordOutput("DynamicAuto/Current Path Initial Pose", curTrajectory.getInitialPose()); + Logger.recordOutput( + "DynamicAuto/Closest Note Exists", + DynamicAuto.getClosestNote(swerve::getPose).getExistence()); + Logger.recordOutput( + "DynamicAuto/Closest Note", + DynamicAuto.getClosestNote(swerve::getPose).getPoseAllianceSpicific()); + + Logger.recordOutput( + "DynamicAuto/Absolute Closest Note", + DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific()); + + Logger.recordOutput( + "DynamicAuto/Closest Shooting Location", + DynamicAuto.closestShootingLocation(() -> swerve.getPose(), DynamicAuto.shootingLocations) + .getPoseAllianceSpicific()); + Logger.recordOutput("DynamicAuto/Curent Trajectory Followed", curTrajectory.getPoses()); + Logger.recordOutput("DynamicAuto/Forward Trajectory Followed", forwardLookingTrajectory); + // Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization); } private LoggedDashboardNumber rotation = new LoggedDashboardNumber("Rotation (Rotations)"); @@ -419,23 +526,227 @@ public Command autonomousAutoAim(String pathName) { } + private LoggedDashboardNumber priority = new LoggedDashboardNumber("Note Priority"); + private LoggedDashboardBoolean blacklist = new LoggedDashboardBoolean("Note Blacklisted"); + + public Command updateNote() { + return Commands.runOnce( + () -> + DynamicAuto.updateNote( + noteDropdown.get(), () -> blacklist.get(), () -> (int) priority.get())); + } + + public Command startToNote() { + return swerve + .runChoreoTraj( + () -> { + curTrajectory = DynamicAuto.makeStartToNote(swerve::getPose); + forwardLookingTrajectory = + DynamicAuto.addTwoTrajectories( + curTrajectory, DynamicAuto.makeNoteToShooting(curTrajectory::getFinalPose)); + return curTrajectory; + }) + .onlyIf( + () -> { + if (DynamicAuto.whitelistCount > 0) { + return true; + } else { + System.out.println("No more avalible notes!!!!! >:("); + return false; + } + }); + } + + public Command noteToNote() { + return swerve + .runChoreoTraj( + () -> { + System.out.println(DynamicAuto.whitelistCount); + curTrajectory = DynamicAuto.makeNoteToNote(swerve::getPose); + forwardLookingTrajectory = + DynamicAuto.addTwoTrajectories( + curTrajectory, DynamicAuto.makeNoteToShooting(curTrajectory::getFinalPose)); + return DynamicAuto.makeNoteToNote(swerve::getPose); + }) + .onlyIf( + () -> { + if (DynamicAuto.whitelistCount > 0) { + return true; + } else { + System.out.println("No more avalible notes!!!!! >:("); + return false; + } + }); + } + + public Command shootToNote() { + return swerve + .runChoreoTraj( + () -> { + curTrajectory = DynamicAuto.makeShootingToNote(swerve::getPose); + forwardLookingTrajectory = + DynamicAuto.addTwoTrajectories( + curTrajectory, DynamicAuto.makeNoteToShooting(curTrajectory::getFinalPose)); + return DynamicAuto.makeShootingToNote(swerve::getPose); + }) + .onlyIf( + () -> { + if (DynamicAuto.whitelistCount > 0) { + return true; + } else { + System.out.println("No more avalible notes!!!!! >:("); + return false; + } + }); + } + + public Command noteToShoot() { + return swerve + .runChoreoTraj( + () -> { + curTrajectory = DynamicAuto.makeNoteToShooting(swerve::getPose); + return DynamicAuto.makeNoteToShooting(swerve::getPose); + }) + .onlyIf( + () -> { + if (DynamicAuto.whitelistCount > 0) { + return true; + } else { + System.out.println("No more avalible notes!!!!! >:("); + return false; + } + }); + } + + public Command blacklistNote() { + return Commands.runOnce(() -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist()); + } + + public CommandSelector selectAuto() { + if (dynamicAutoCounter == 0) { + // swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))); + System.out.println("start to note"); + return CommandSelector.START_TO_NOTE; + + } else if (atShootingLocation) { + atShootingLocation = false; + System.out.println("shoot to note"); + return CommandSelector.SHOOT_TO_NOTE; + } else if ((carriage.getBeambreak() || feeder.getFirstBeambreak()) + || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence() + && swerve + .getPose() + .minus( + DynamicAuto.getAbsoluteClosestNote(swerve::getPose) + .getPoseAllianceSpicific()) + .getTranslation() + .getNorm() + < 1.5)) { + System.out.println("note to shoot"); + atShootingLocation = true; + DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(); + return CommandSelector.NOTE_TO_SHOOT; + } else if ((!(carriage.getBeambreak() || feeder.getFirstBeambreak()) && Robot.isReal()) + || (!DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence() + && swerve + .getPose() + .minus( + DynamicAuto.getAbsoluteClosestNote(swerve::getPose) + .getPoseAllianceSpicific()) + .getTranslation() + .getNorm() + < 1.5)) { + + System.out.println("note to note 1"); + DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(); + return CommandSelector.NOTE_TO_NOTE; + } else if ((carriage.getBeambreak() || feeder.getFirstBeambreak()) + || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence() + && swerve + .getPose() + .minus( + DynamicAuto.getAbsoluteClosestNote(swerve::getPose) + .getPoseAllianceSpicific()) + .getTranslation() + .getNorm() + > 1.5)) { + + System.out.println("dynamic to note"); + return CommandSelector.DYNAMIC_TO_NOTE; + } else { + atShootingLocation = true; + System.out.println("dynamoci to shoot"); + return CommandSelector.DYNAMIC_TO_SHOOT; + } + } + + double distance = 0; + + public Command dynamicAuto() { + + return Commands.repeatingSequence( + Commands.select( + Map.ofEntries( + Map.entry( + CommandSelector.NOTE_TO_NOTE, + Commands.race(noteToNote(), intake.runVelocityCmd(80.0, 30.0))), + Map.entry( + CommandSelector.NOTE_TO_SHOOT, + Commands.race( + noteToShoot(), + shooter.runStateCmd( + () -> AutoAim.shotMap.get(distance).getRotation(), + () -> AutoAim.shotMap.get(distance).getLeftRPS(), + () -> AutoAim.shotMap.get(distance).getRightRPS()))), + Map.entry( + CommandSelector.START_TO_NOTE, + Commands.race(startToNote(), intake.runVelocityCmd(80.0, 30.0))), + Map.entry( + CommandSelector.SHOOT_TO_NOTE, + Commands.race(shootToNote(), intake.runVelocityCmd(80.0, 30.0))), + Map.entry( + CommandSelector.DYNAMIC_TO_NOTE, + Commands.race( + DynamicAuto.DynamicToNote(swerve::getPose), + intake.runVelocityCmd(80.0, 30.0))), + Map.entry( + CommandSelector.DYNAMIC_TO_SHOOT, + Commands.race( + DynamicAuto.DynamicToShoot(swerve::getPose), + shooter.runStateCmd( + () -> AutoAim.shotMap.get(distance).getRotation(), + () -> AutoAim.shotMap.get(distance).getLeftRPS(), + () -> AutoAim.shotMap.get(distance).getRightRPS())))), + this::selectAuto), + Commands.runOnce( + () -> { + dynamicAutoCounter++; + System.out.println(dynamicAutoCounter); + distance = + swerve + .getPose() + .minus(FieldConstants.getSpeaker()) + .getTranslation() + .getNorm(); + })) + .asProxy(); + } + @Override public void disabledPeriodic() {} @Override public void autonomousInit() { - autonomousCommand = new PathPlannerAuto("New Auto"); - - if (autonomousCommand != null) { - autonomousCommand.schedule(); + dynamicAutoCounter = 0; + for (Note note : DynamicAuto.notes) { + note.whitelist(); } + dynamicAuto().schedule(); } @Override public void teleopInit() { - if (autonomousCommand != null) { - autonomousCommand.cancel(); - } + dynamicAuto().cancel(); } @Override diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java index 7b73aa1e..0a5fc5c4 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java @@ -10,13 +10,13 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; -import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.utils.components.InvertedDigitalInput; /** Create a CarriageIO that uses a real TalonFX. */ public class CarriageIOReal implements CarriageIO { - final TalonFX motor = new TalonFX(18); + final TalonFX motor = new TalonFX(18, "canivore"); - final DigitalInput beambreak = new DigitalInput(2); + final InvertedDigitalInput beambreak = new InvertedDigitalInput(2); final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); @@ -28,7 +28,7 @@ public class CarriageIOReal implements CarriageIO { public CarriageIOReal() { var config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; motor.getConfigurator().apply(config); BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, amperage, temp); diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java index 1b031de4..b0b1c3db 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java @@ -24,7 +24,7 @@ public CarriageSubsystem(CarriageIO io) { @Override public void periodic() { io.updateInputs(inputs); - Logger.processInputs("Intake", inputs); + Logger.processInputs("Carriage", inputs); } /** Run the carriage roller at the specified voltage */ @@ -39,7 +39,7 @@ public Command runVoltageCmd(double voltage) { public Command indexForwardsCmd() { return runVoltageCmd(INDEXING_VOLTAGE) .until(() -> inputs.beambreak) - .andThen(runVoltageCmd(INDEXING_VOLTAGE).withTimeout(0.25)); + .andThen(runVoltageCmd(INDEXING_VOLTAGE / 2).withTimeout(0.12), runVoltageCmd(0.0)); } /** Run the amp mech backwards until the beambreak cycles, then forward index. */ @@ -48,6 +48,7 @@ public Command indexBackwardsCmd() { runVoltageCmd(-INDEXING_VOLTAGE).until(() -> inputs.beambreak), runVoltageCmd(-INDEXING_VOLTAGE).withTimeout(0.25), runVoltageCmd(-INDEXING_VOLTAGE).until(() -> !inputs.beambreak), + runVoltageCmd(-INDEXING_VOLTAGE).withTimeout(0.25), indexForwardsCmd()); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 2c87f3de..ff5f69d2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -16,8 +16,8 @@ /** Elevator IO using TalonFXs. */ public class ElevatorIOReal implements ElevatorIO { - private final TalonFX motor = new TalonFX(16); - private final TalonFX follower = new TalonFX(17); + private final TalonFX motor = new TalonFX(16, "canivore"); + private final TalonFX follower = new TalonFX(17, "canivore"); private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private final MotionMagicVoltage positionVoltage = @@ -32,15 +32,15 @@ public class ElevatorIOReal implements ElevatorIO { public ElevatorIOReal() { var config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; - config.Slot0.kG = 0.0; - config.Slot0.kS = 0.0; - config.Slot0.kV = 0.0; - config.Slot0.kA = 0.0; - config.Slot0.kP = 0.0; - config.Slot0.kD = 0.0; + config.Slot0.kG = 0.11591; + config.Slot0.kS = 0.16898; + config.Slot0.kV = 10.911; + config.Slot0.kA = 0.28688; + config.Slot0.kP = 69.785; + config.Slot0.kD = 17.53; config.CurrentLimits.StatorCurrentLimit = 60.0; config.CurrentLimits.StatorCurrentLimitEnable = true; @@ -52,7 +52,7 @@ public ElevatorIOReal() { // Carriage position meters in direction of elevator config.Feedback.SensorToMechanismRatio = - ElevatorSubsystem.GEAR_RATIO * 2 * Math.PI * ElevatorSubsystem.DRUM_RADIUS_METERS; + ElevatorSubsystem.GEAR_RATIO / (2 * Math.PI * ElevatorSubsystem.DRUM_RADIUS_METERS); motor.getConfigurator().apply(config); motor.setPosition(0.0); // Assume we boot 0ed diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index e24b6678..511a3dbd 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -32,10 +32,10 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(1.751 / 2.0); public static final Rotation2d ELEVATOR_ANGLE = Rotation2d.fromDegrees(65.0); - public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(31.3); + public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(30); - public static final double CLIMB_EXTENSION_METERS = 0.6; - public static final double TRAP_EXTENSION_METERS = 0.9; + public static final double CLIMB_EXTENSION_METERS = 0.65; + public static final double TRAP_EXTENSION_METERS = Units.inchesToMeters(29); public static final double AMP_EXTENSION_METERS = 0.6; private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java index c8afed61..8170b741 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java @@ -11,12 +11,13 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.utils.components.InvertedDigitalInput; /** Feeder IO using a TalonFX. */ public class FeederIOReal implements FeederIO { - private final TalonFX motor = new TalonFX(13); + private final TalonFX motor = new TalonFX(13, "canivore"); - DigitalInput firstBeambreak = new DigitalInput(0); + InvertedDigitalInput firstBeambreak = new InvertedDigitalInput(0); DigitalInput lastBeambreak = new DigitalInput(1); private final StatusSignal velocity = motor.getVelocity(); @@ -32,7 +33,7 @@ public FeederIOReal() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.StatorCurrentLimit = 20.0; + config.CurrentLimits.StatorCurrentLimit = 40.0; motor.getConfigurator().apply(config); diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java index ea77b741..89b8a99e 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java @@ -10,6 +10,8 @@ /** Feeder motor for shooter and associated beambreaks for indexing */ public class FeederSubsystem extends SubsystemBase { + public static final double INDEXING_VOLTAGE = 4.0; + private final FeederIO io; private final FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); @@ -34,11 +36,11 @@ public Command indexCmd() { return this.run( () -> { if (inputs.lastBeambreak) { - io.setVoltage(-3.0); + io.setVoltage(-INDEXING_VOLTAGE); } else if (inputs.firstBeambreak) { io.setVoltage(0.0); } else { - io.setVoltage(3.0); + io.setVoltage(INDEXING_VOLTAGE); } }); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index a709888c..ea4e01ad 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -26,6 +26,10 @@ public static class IntakeIOInputs { /** Run the intake at a specified voltage */ public void setIntakeVoltage(final double volts); + public void setIntakeSpeed(final double rps); + /** Run the centering wheels at a specified voltage */ public void setCenteringVoltage(final double volts); + + public void setCenteringSpeed(final double rps); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index 297503a4..cf0c143b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -7,17 +7,22 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; /** Intake IO implementation for TalonFX motors. */ public class IntakeIOReal implements IntakeIO { - private final TalonFX intakeMotor = new TalonFX(14); - private final TalonFX centeringMotor = new TalonFX(15); + private final TalonFX intakeMotor = new TalonFX(14, "canivore"); + private final TalonFX centeringMotor = new TalonFX(15, "canivore"); private final VoltageOut intakeVoltageOut = new VoltageOut(0.0).withEnableFOC(true); + private final VelocityVoltage intakeVelocityVoltage = + new VelocityVoltage(0.0).withEnableFOC(true); private final VoltageOut centeringVoltageOut = new VoltageOut(0.0).withEnableFOC(true); + private final VelocityVoltage centeringVelocityVoltage = + new VelocityVoltage(0.0).withEnableFOC(true); private final StatusSignal intakeVelocity = intakeMotor.getVelocity(); private final StatusSignal intakeVoltage = intakeMotor.getMotorVoltage(); @@ -31,11 +36,23 @@ public class IntakeIOReal implements IntakeIO { public IntakeIOReal() { var intakeConfig = new TalonFXConfiguration(); + intakeConfig.CurrentLimits.SupplyCurrentLimit = 20.0; + intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + intakeConfig.Slot0.kV = (12.0 * 60.0) / 5800; + intakeConfig.Slot0.kP = 1.0; + intakeMotor.getConfigurator().apply(intakeConfig); var centeringConfig = new TalonFXConfiguration(); - centeringConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + centeringConfig.CurrentLimits.SupplyCurrentLimit = 20.0; + centeringConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + centeringConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + centeringConfig.Slot0.kV = (12.0 * 60.0) / 5800; + centeringConfig.Slot0.kP = 0.1; + centeringMotor.getConfigurator().apply(centeringConfig); BaseStatusSignal.setUpdateFrequencyForAll( @@ -78,4 +95,14 @@ public void setIntakeVoltage(final double volts) { public void setCenteringVoltage(final double volts) { centeringMotor.setControl(centeringVoltageOut.withOutput(volts)); } + + @Override + public void setIntakeSpeed(final double rps) { + intakeMotor.setControl(intakeVelocityVoltage.withVelocity(rps)); + } + + @Override + public void setCenteringSpeed(final double rps) { + centeringMotor.setControl(centeringVelocityVoltage.withVelocity(rps)); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 1e3ce8c1..8c90b0f2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -25,11 +25,19 @@ public void periodic() { } /** Run the intake and centering motors at the specified voltage */ - public Command runVoltageCmd(double voltage) { + public Command runVoltageCmd(double intakeVoltage, double centeringVoltage) { return this.run( () -> { - io.setIntakeVoltage(voltage); - io.setCenteringVoltage(voltage); + io.setIntakeVoltage(intakeVoltage); + io.setCenteringVoltage(centeringVoltage); + }); + } + + public Command runVelocityCmd(double intakeVelocity, double centeringVelocity) { + return this.run( + () -> { + io.setIntakeSpeed(intakeVelocity); + io.setCenteringSpeed(centeringVelocity); }); } } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIO.java b/src/main/java/frc/robot/subsystems/leds/LEDIO.java new file mode 100644 index 00000000..7049e6d6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LEDIO.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.util.Color; +import org.littletonrobotics.junction.AutoLog; + +public interface LEDIO { + @AutoLog + public static class LEDIOInputs { + // No sensors so no inputs + } + + public void updateInputs(LEDIOInputs inputs); + + public void set(int i, Color color); + + public default void solid(Color color) { + for (int i = 0; i < LEDSubsystem.LED_LENGTH; i++) { + set(i, color); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java new file mode 100644 index 00000000..df5d365c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.util.Color; + +public class LEDIOReal implements LEDIO { + private final AddressableLED led; + private final AddressableLEDBuffer buffer; + + public LEDIOReal() { + led = new AddressableLED(3); + buffer = new AddressableLEDBuffer(LEDSubsystem.LED_LENGTH); + led.setLength(buffer.getLength()); + led.start(); + } + + /** Write data from buffer to leds */ + @Override + public void updateInputs(LEDIOInputs inputs) { + led.setData(buffer); + } + + @Override + public void set(int i, Color color) { + buffer.setLED(i, color); + } +} diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java new file mode 100644 index 00000000..94bf0127 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.util.Color; + +// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w +// LEDIOReal +public class LEDIOSim implements LEDIO { + private final AddressableLED led; + private final AddressableLEDBuffer buffer; + + public LEDIOSim() { + led = new AddressableLED(3); + buffer = new AddressableLEDBuffer(LEDSubsystem.LED_LENGTH); + led.setLength(buffer.getLength()); + led.start(); + } + + /** Write data from buffer to leds */ + @Override + public void updateInputs(LEDIOInputs inputs) { + led.setData(buffer); + } + + @Override + public void set(int i, Color color) { + buffer.setLED(i, color); + } +} diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java new file mode 100644 index 00000000..f17d2633 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java @@ -0,0 +1,123 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.leds; + +import com.google.common.base.Supplier; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + +public class LEDSubsystem extends SubsystemBase { + public static final int LED_LENGTH = 36; + + private final LEDIO io; + private final LEDIOInputsAutoLogged inputs = new LEDIOInputsAutoLogged(); + + private double rainbowStart = 0; + private double dashStart = 0; + + /** Creates a new LEDSubsystem. */ + public LEDSubsystem(LEDIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("LED", inputs); + } + + private void setIndex(int i, Color color) { + io.set(i, color); + } + + private void setSolid(Color color) { + io.solid(color); + } + + public Command setSolidCmd(Color color) { + return this.run(() -> setSolid(color)); + } + + public Command setBlinkingCmd(Color onColor, Color offColor, double frequency) { + return Commands.repeatingSequence( + setSolidCmd(onColor).withTimeout(1.0 / frequency), + setSolidCmd(offColor).withTimeout(1.0 / frequency)); + } + + /** Sets the first portion of the leds to a color, and the rest off */ + public Command setProgressCmd(Color color, DoubleSupplier progress) { + return this.run( + () -> { + for (int i = 0; i < LED_LENGTH; i++) { + if (i < progress.getAsDouble() * LED_LENGTH) { + setIndex(i, color); + } else { + setIndex(i, Color.kBlack); + } + } + }); + } + + public Command setRainbowCmd() { + return this.run( + () -> { + for (int i = 0; i < LED_LENGTH; i++) { + setIndex(i, Color.fromHSV((int) rainbowStart % 180 + i, 255, 255)); + } + rainbowStart += 6; + }); + } + + public Command setRunAlongCmd( + Supplier colorDash, Supplier colorBg, int dashLength, double frequency) { + return this.run( + () -> { + setSolid(colorBg.get()); + for (int i = (int) dashStart; i < dashStart + dashLength; i++) { + setIndex(i % LED_LENGTH, colorDash.get()); + } + + dashStart += LED_LENGTH * frequency * 0.020; + dashStart %= LED_LENGTH; + }); + } + + public Command defaultStateDisplayCmd(BooleanSupplier enabled, BooleanSupplier targetIsSpeaker) { + return Commands.either( + Commands.either( + this.setBlinkingCmd(new Color("#ffff00"), new Color(), 10.0) + .until(() -> !targetIsSpeaker.getAsBoolean() || !enabled.getAsBoolean()), + this.setBlinkingCmd(new Color("#ff7777"), new Color(), 10.0) + .until(() -> targetIsSpeaker.getAsBoolean() || !enabled.getAsBoolean()), + targetIsSpeaker), + this.setRunAlongCmd( + // Set color to be purple with a moving dash corresponding to alliance color + () -> { + if (DriverStation.getAlliance().isEmpty()) { + return new Color("#b59aff"); + } else if (DriverStation.getAlliance().get() == Alliance.Red) { + return new Color("#ff0000"); + } else { // Blue + return new Color("#0000ff"); + } + }, + () -> new Color("#350868"), + 10, + 1.0) + .until(enabled), + enabled) + .ignoringDisable(true) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf) + .repeatedly(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 4ed9283f..60cf31b2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -12,9 +12,9 @@ import edu.wpi.first.math.geometry.Rotation2d; public class ShooterIOReal implements ShooterIO { - private final TalonFX pivotMotor = new TalonFX(10); - private final TalonFX flywheelLeftMotor = new TalonFX(11); - private final TalonFX flywheelRightMotor = new TalonFX(12); + private final TalonFX pivotMotor = new TalonFX(10, "canivore"); + private final TalonFX flywheelLeftMotor = new TalonFX(11, "canivore"); + private final TalonFX flywheelRightMotor = new TalonFX(12, "canivore"); private final StatusSignal pivotVelocity = pivotMotor.getVelocity(); private final StatusSignal pivotVoltage = pivotMotor.getMotorVoltage(); @@ -55,25 +55,26 @@ public ShooterIOReal() { pivotConfig.CurrentLimits.StatorCurrentLimit = 40.0; pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - pivotConfig.Slot0.kG = 0.0; // TODO: Find using sysid or hand tuning - pivotConfig.Slot0.kA = 0.0; + pivotConfig.Slot0.kG = 0.47195; // TODO: Find using sysid or hand tuning + pivotConfig.Slot0.kV = 7.8333; + pivotConfig.Slot0.kA = 0.098325; pivotConfig.Slot0.kS = 0.0; - pivotConfig.Slot0.kP = 0.0; - pivotConfig.Slot0.kD = 0.0; + pivotConfig.Slot0.kP = 69.562; + pivotConfig.Slot0.kD = 12.212; - pivotConfig.MotionMagic.MotionMagicAcceleration = 1.0; + pivotConfig.MotionMagic.MotionMagicAcceleration = 10.0; pivotConfig.MotionMagic.MotionMagicCruiseVelocity = 1.0; pivotMotor.getConfigurator().apply(pivotConfig); pivotMotor.setPosition( ShooterSubystem.PIVOT_MIN_ANGLE.getRotations()); // Assume we boot at hard stop BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, pivotVelocity, pivotVoltage, pivotAmps, pivotTempC, pivotRotations); + 250.0, pivotVelocity, pivotVoltage, pivotAmps, pivotTempC, pivotRotations); pivotMotor.optimizeBusUtilization(); var flywheelConfig = new TalonFXConfiguration(); - flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + flywheelConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; flywheelConfig.Feedback.SensorToMechanismRatio = ShooterSubystem.FLYWHEEL_RATIO; // TODO add in once cad is done @@ -81,15 +82,14 @@ public ShooterIOReal() { flywheelConfig.CurrentLimits.StatorCurrentLimitEnable = true; flywheelConfig.CurrentLimits.StatorCurrentLimit = 40.0; - flywheelConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - flywheelConfig.Slot0.kG = 0.0; // TODO: Find using sysid or hand tuning - flywheelConfig.Slot0.kA = 0.0; - flywheelConfig.Slot0.kS = 0.0; - flywheelConfig.Slot0.kP = 0.0; + flywheelConfig.Slot0.kA = 0.0051316; + flywheelConfig.Slot0.kV = 0.096766; + flywheelConfig.Slot0.kS = 0.23777; + flywheelConfig.Slot0.kP = 0.057995; flywheelConfig.Slot0.kD = 0.0; flywheelLeftMotor.getConfigurator().apply(flywheelConfig); - flywheelConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; flywheelRightMotor.getConfigurator().apply(flywheelConfig); BaseStatusSignal.setUpdateFrequencyForAll( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java index 07feb35e..493c88fb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.SignalLogger; import com.google.common.base.Supplier; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -90,7 +91,21 @@ public Command runStateCmd( () -> { Logger.recordOutput("Shooter/Left Velocity Setpoint", left.getAsDouble()); Logger.recordOutput("Shooter/Right Velocity Setpoint", right.getAsDouble()); + Logger.recordOutput( + "Shooter/Left At Target", + MathUtil.isNear( + left.getAsDouble(), inputs.flywheelLeftVelocityRotationsPerSecond, 1.0)); + Logger.recordOutput( + "Shooter/Right At Target", + MathUtil.isNear( + right.getAsDouble(), inputs.flywheelRightVelocityRotationsPerSecond, 1.0)); Logger.recordOutput("Shooter/Rotation Setpoint", rotation.get()); + Logger.recordOutput( + "Shooter/Pivot At Target", + MathUtil.isNear( + right.getAsDouble(), + inputs.flywheelRightVelocityRotationsPerSecond, + Units.degreesToRotations(0.5))); io.setFlywheelVelocity(left.getAsDouble(), right.getAsDouble()); io.setPivotSetpoint(rotation.get()); }); @@ -139,19 +154,19 @@ public Command runPivotSysidCmd() { .quasistatic(Direction.kForward) .until(() -> inputs.pivotRotation.getDegrees() > 80.0), this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)), - Commands.waitSeconds(1.0), + Commands.waitSeconds(0.25), // Stop when near horizontal so we avoid hard stop pivotRoutine .quasistatic(Direction.kReverse) .until(() -> inputs.pivotRotation.getDegrees() < 10.0), this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)), - Commands.waitSeconds(1.0), + Commands.waitSeconds(0.25), // Stop when we get close to vertical so it falls back pivotRoutine .dynamic(Direction.kForward) .until(() -> inputs.pivotRotation.getDegrees() > 80.0), this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)), - Commands.waitSeconds(1.0), + Commands.waitSeconds(0.25), // Stop when near horizontal so we avoid hard stop pivotRoutine .dynamic(Direction.kReverse) diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java index 1d91209e..27b8d877 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java @@ -24,7 +24,7 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(SwerveSubsystem.PIGEON_ID); + private final Pigeon2 pigeon = new Pigeon2(SwerveSubsystem.PIGEON_ID, "canivore"); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 7be37170..e8505977 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -34,8 +34,10 @@ public record ModuleConstants( public static final double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0); public static final double TURN_GEAR_RATIO = 150.0 / 7.0; - public static final double DRIVE_STATOR_CURRENT_LIMIT = 50.0; // TODO bump as needed - public static final double TURN_STATOR_CURRENT_LIMIT = 40.0; + public static final double DRIVE_STATOR_CURRENT_LIMIT = 60.0; + public static final double DRIVE_SUPPLY_TIME_CURRENT_LIMIT = 40.0; + public static final double DRIVE_SUPPLY_TIME_CUTOFF = 0.5; + public static final double TURN_STATOR_CURRENT_LIMIT = 20.0; private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); @@ -76,7 +78,9 @@ public SwerveModuleState runSetpoint(SwerveModuleState state) { final var optimizedState = SwerveModuleState.optimize(state, getAngle()); io.setTurnSetpoint(optimizedState.angle); - io.setDriveSetpoint(optimizedState.speedMetersPerSecond); + io.setDriveSetpoint( + optimizedState.speedMetersPerSecond + * Math.cos(optimizedState.angle.minus(inputs.turnPosition).getRadians())); return optimizedState; } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index a5f7c88e..1dfdde9c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -17,14 +17,15 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import frc.robot.subsystems.swerve.Module.ModuleConstants; @@ -44,7 +45,7 @@ */ public class ModuleIOReal implements ModuleIO { // Constants - private static final boolean IS_TURN_MOTOR_INVERTED = false; + private static final boolean IS_TURN_MOTOR_INVERTED = true; private final String name; @@ -70,22 +71,24 @@ public class ModuleIOReal implements ModuleIO { // Control modes private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); private final VoltageOut turnVoltage = new VoltageOut(0.0).withEnableFOC(true); - private final MotionMagicVelocityVoltage drivePIDF = - new MotionMagicVelocityVoltage(0.0).withEnableFOC(true); + private final VelocityVoltage drivePIDF = new VelocityVoltage(0.0).withEnableFOC(true); private final MotionMagicVoltage turnPID = new MotionMagicVoltage(0.0).withEnableFOC(true); public ModuleIOReal(ModuleConstants constants) { name = constants.prefix(); - driveTalon = new TalonFX(constants.driveID()); - turnTalon = new TalonFX(constants.turnID()); - cancoder = new CANcoder(constants.cancoderID()); + driveTalon = new TalonFX(constants.driveID(), "canivore"); + turnTalon = new TalonFX(constants.turnID(), "canivore"); + cancoder = new CANcoder(constants.cancoderID(), "canivore"); var driveConfig = new TalonFXConfiguration(); // Current limits // TODO: Do we want to limit supply current? driveConfig.CurrentLimits.StatorCurrentLimit = Module.DRIVE_STATOR_CURRENT_LIMIT; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.CurrentLimits.SupplyTimeThreshold = Module.DRIVE_SUPPLY_TIME_CUTOFF; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.CurrentLimits.SupplyCurrentThreshold = Module.DRIVE_SUPPLY_TIME_CURRENT_LIMIT; // Inverts driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -94,14 +97,15 @@ public ModuleIOReal(ModuleConstants constants) { driveConfig.Feedback.SensorToMechanismRatio = (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI)); // Controls Gains - driveConfig.Slot0.kV = 2.5; - // (5800.0 * (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI))) - // / 12.0; // Hypothetical based on free speed - driveConfig.Slot0.kA = 0.0; // TODO: Find using sysid or hand tuning - driveConfig.Slot0.kS = 0.0; - driveConfig.Slot0.kP = 0.25; // Guess + driveConfig.Slot0.kV = 2.0733; + driveConfig.Slot0.kA = 0.4; + driveConfig.Slot0.kS = 0.04; + driveConfig.Slot0.kP = 1.9855; driveConfig.Slot0.kD = 0.0; + driveConfig.MotionMagic.MotionMagicCruiseVelocity = SwerveSubsystem.MAX_LINEAR_SPEED; + driveConfig.MotionMagic.MotionMagicAcceleration = SwerveSubsystem.MAX_LINEAR_SPEED / 0.75; + driveTalon.getConfigurator().apply(driveConfig); var turnConfig = new TalonFXConfiguration(); @@ -122,25 +126,23 @@ public ModuleIOReal(ModuleConstants constants) { turnConfig.Feedback.FeedbackRotorOffset = 0.0; // Is this correct? Cancoder config should handle it // Controls Gains - turnConfig.Slot0.kV = 0.0; - // (5800.0 / Module.TURN_GEAR_RATIO) - // / 12.0; // Free speed over voltage, should find empirically - turnConfig.Slot0.kA = 0.0; - // Module.TURN_GEAR_RATIO - // * (9.37 / 483.0) - // / (0.004 * (12.0 / 483.0)); // Based on motor dynamics math, should find in real life - // gearing * Kt (torque per amp) / (intertia * resistance (nominal voltage / stall current)) - // Yeah its messy and should be found using sysid later but its worth trying as a first guess - // If this works we can use a similar technique on future mechanisms - turnConfig.Slot0.kS = 0.0; // TODO: Find empirically - turnConfig.Slot0.kP = 50.0; - turnConfig.Slot0.kD = 0.0; + turnConfig.Slot0.kV = 2.7935; + turnConfig.Slot0.kA = 0.031543; + turnConfig.Slot0.kS = 0.28; + turnConfig.Slot0.kP = 28.579; + turnConfig.Slot0.kD = 0.68275; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 5500 / Module.TURN_GEAR_RATIO; + turnConfig.MotionMagic.MotionMagicAcceleration = (5500 * 0.1) / Module.TURN_GEAR_RATIO; turnConfig.ClosedLoopGeneral.ContinuousWrap = true; turnTalon.getConfigurator().apply(turnConfig); var cancoderConfig = new CANcoderConfiguration(); cancoderConfig.MagnetSensor.MagnetOffset = constants.cancoderOffset().getRotations(); + cancoderConfig.MagnetSensor.SensorDirection = + IS_TURN_MOTOR_INVERTED + ? SensorDirectionValue.CounterClockwise_Positive + : SensorDirectionValue.Clockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); drivePosition = driveTalon.getPosition(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 51993813..b377f99b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -17,6 +17,7 @@ import static edu.wpi.first.units.Units.Volts; import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; import com.choreo.lib.ChoreoTrajectoryState; import com.ctre.phoenix6.SignalLogger; import com.google.common.collect.Streams; @@ -25,6 +26,7 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -42,6 +44,7 @@ import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -53,6 +56,8 @@ import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.ShotData; import java.util.Arrays; +import java.util.Optional; +import java.util.Set; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; @@ -80,7 +85,7 @@ public class AutoAimStates { } // Drivebase constants - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(12.5); + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18.9); public static final double TRACK_WIDTH_X = Units.inchesToMeters(21.75); public static final double TRACK_WIDTH_Y = Units.inchesToMeters(21.25); public static final double DRIVE_BASE_RADIUS = @@ -90,13 +95,13 @@ public class AutoAimStates { public static final int PIGEON_ID = 0; public static final ModuleConstants frontLeft = - new ModuleConstants("Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Front Left", 0, 1, 0, Rotation2d.fromRotations(0.374023)); public static final ModuleConstants frontRight = - new ModuleConstants("Front Right", 2, 3, 1, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Front Right", 2, 3, 1, Rotation2d.fromRotations(-0.077148)); public static final ModuleConstants backLeft = - new ModuleConstants("Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Back Left", 4, 5, 2, Rotation2d.fromRotations(0.134277)); public static final ModuleConstants backRight = - new ModuleConstants("Back Right", 6, 7, 3, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Back Right", 6, 7, 3, Rotation2d.fromRotations(0.511963)); public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -151,7 +156,6 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) { PathPlannerLogging.setLogActivePathCallback( (path) -> Logger.recordOutput("PathPlanner/Active Path", path.toArray(Pose2d[]::new))); Logger.recordOutput("PathPlanner/Target", new Pose2d()); - Logger.recordOutput("PathPlanner/Absolute Translation Error", 0.0); odometry = new SwerveDriveOdometry(kinematics, getRotation(), getModulePositions()); @@ -171,8 +175,8 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) { new SysIdRoutine( new SysIdRoutine.Config( null, // Default ramp rate is acceptable - Volts.of(4), // Reduce dynamic voltage to 4 to prevent motor brownout - Seconds.of(5), + Volts.of(3.5), + Seconds.of(3.5), // Log state with Phoenix SignalLogger class (state) -> SignalLogger.writeString("state", state.toString())), new SysIdRoutine.Mechanism( @@ -307,7 +311,7 @@ public Command stopCmd() { public Command runVelocityFieldRelative(Supplier speeds) { return this.runVelocityCmd( - () -> ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), getRotation())); + () -> ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), gyroInputs.yawPosition)); } /** @@ -450,7 +454,6 @@ public Pose2d getVirtualTarget(ChassisSpeeds speedsRobotRelative) { .inverse()); } - @AutoLogOutput(key = "AutoAim/Virtual Target") public Pose2d getVirtualTarget() { return getVirtualTarget(getVelocity()); } @@ -663,6 +666,26 @@ public Command teleopPointTowardsTranslationCmd( xMetersPerSecond, yMetersPerSecond, AutoAim.LOOKAHEAD_TIME_SECONDS); } + public Command runChoreoTraj(Supplier traj) { + + return Commands.defer( + () -> + Choreo.choreoSwerveCommand( + traj.get(), + this::getPose, + new PIDController(6.0, 0.0, 0.0), + new PIDController(6.0, 0.0, 0.0), + new PIDController(1.0, 0.0, 0.0), + (ChassisSpeeds speeds) -> // + this.runVelocity(speeds), + () -> { + Optional alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == Alliance.Red; + }, + this), + Set.of(this)); + } + public Command runModuleSteerCharacterizationCmd() { return Commands.sequence( this.runOnce(() -> SignalLogger.start()), diff --git a/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java b/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java index 1e3e8ff3..72c52ab3 100644 --- a/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java +++ b/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java @@ -4,14 +4,10 @@ /** A digital input (ie a beambreak or limit switch) whose normal state is reversed */ public class InvertedDigitalInput extends DigitalInput { - private InvertedDigitalInput(int channel) { + public InvertedDigitalInput(int channel) { super(channel); } - public static InvertedDigitalInput from(final DigitalInput input) { - return new InvertedDigitalInput(input.getChannel()); - } - public boolean get() { return !super.get(); } diff --git a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py new file mode 100644 index 00000000..324be529 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py @@ -0,0 +1,117 @@ +import json + +# Assuming locations is a list of dictionaries where each dictionary represents a location +# L,M, and R were change to S1, S2, and S4 with an S3 being added. +locations = [ + {"name": "Amp Side", "x": 0.71, "y": 6.72, "angle": 1.04, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "Center", "x": 1.35, "y": 5.56, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "Stage Side", "x": 0.71, "y": 4.36, "angle": -1.04, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "W1", "x": 2.3, "y": 6.757, "angle": 0.39, "allowed_destinations": [ "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "W2", "x": 2.25, "y": 5.56, "angle": 0, "allowed_destinations": ["W1", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "W3", "x": 2.3, "y": 4.36, "angle": -0.39, "allowed_destinations": ["W1", "W2", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "S1", "x": 5.176, "y": 6.63, "angle": 0.26, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "S2", "x": 4.263, "y": 5.56, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "S3", "x": 4.263, "y": 3, "angle": -0.544, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]}, + {"name": "S4", "x": 5.176, "y": 1.62, "angle": -2.89, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]}, + + {"name": "C1", "x": 7.68, "y": 7.467, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C2", "C3", "C4", "C5"]}, + {"name": "C2", "x": 7.68, "y": 5.797, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C3", "C4", "C5"]}, + {"name": "C3", "x": 7.68, "y": 4.127, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C4", "C5"]}, + {"name": "C4", "x": 7.68, "y": 2.457, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C5"]}, + {"name": "C5", "x": 7.68, "y": 0.787, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4"]}, +] + +obastacles = [ + {"x": 3.42, "y": 4.04, "radius": 0.50}, + {"x": 5.61, "y": 2.81, "radius": 0.50}, + {"x": 5.61, "y": 5.44, "radius": 0.50} +] + +def create_waypoint(point): + return { + "x": point["x"], + "y": point["y"], + "heading": point["angle"], + "isInitialGuess": False, + "translationConstrained": True, + "headingConstrained": True, + "controlIntervalCount": 40 + } + +def create_path(allPoints): + return { + "waypoints": [create_waypoint(point) for point in allPoints], + "trajectory": [], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": True, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": True, + "circleObstacles": obastacles + } + + +paths = {} +def find_path(start, end_name, newPath, path_name): + end = next((loc for loc in locations if loc["name"] == end_name), None) + if end: + newPath.append(end) + path_name += f" To {end['name']}" + paths[path_name] = create_path(newPath) + return + #else: + # midPoint = next((mid for mid in midPoints if mid["name"] == end_name), None) + # if midPoint: + # newPath.append(midPoint) + # path_name += f" To {midPoint['name']}" + # for end_name in midPoint["allowed_destinations"]: + # find_path(midPoint, end_name, newPath.copy(), path_name) + # else: + # break +# +for start in locations: + newPath = [start] + for end_name in start["allowed_destinations"]: + path_name = start['name'] + find_path(start, end_name, newPath.copy(), path_name) + +# Wrap the paths in another dictionary with the specified format +data = { + "version": "v0.2.1", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 1.162295081967213, + "motorMaxVelocity": 4800, + "gearing": 6.75, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, + "wheelRadius": 0.050799972568014815 + }, + "paths": paths, + "splitTrajectoriesAtStopPoints": False +} + +#Prints names of all paths +#for i in paths: + + +# Write the data to a JSON file +# C:\\Users\\Robotics\\Desktop\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\data.chor +with open('C:\\Users\\stoopi_poopy\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\data.chor', 'w') as f: + json.dump(data, f, indent=4) + diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java new file mode 100644 index 00000000..2408512f --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java @@ -0,0 +1,194 @@ +package frc.robot.utils.dynamicauto; + +import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; +import com.google.common.base.Supplier; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.BooleanSupplier; +import java.util.function.IntSupplier; +import org.littletonrobotics.junction.Logger; + +public class DynamicAuto { + + public static final Note[] notes = { + new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1", true), // w1 + new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2", true), // w2 + new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3", true), // w3 + new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 3, "C1", true), // c1 + new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2", true), // c2 + new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3", true), // c3 + new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4", true), // c4 + new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C5", true) // c5 + }; + + public static final ShootingLocation[] shootingLocations = { + new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(-3.04)), "S1"), // Left + new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(-Math.PI)), "S2"), + new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(-0.566)), "S3"), // Middle + new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(2.961)), "S4") // Right + }; + + public static final ShootingLocation[] startingLocations = { + new ShootingLocation( + new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04)), "Amp Side"), // Speaker (Amp Side) + new ShootingLocation( + new Pose2d(1.35, 5.56, Rotation2d.fromRadians(0)), "Center"), // Speaker (Center) + new ShootingLocation( + new Pose2d(0.71, 4.36, Rotation2d.fromRadians(-1.04)), "Stage Side") // Speaker (Stage Side) + }; + + public static int whitelistCount = notes.length; + + public static void updateWhitelistCount() { + whitelistCount = 0; + for (Note note : notes) { + whitelistCount += note.getBlacklist() ? 1 : 0; + } + } + + public static void updateNote(Note note, BooleanSupplier blacklist, IntSupplier priority) { + if (blacklist.getAsBoolean()) { + note.blacklist(); + } else { + note.whitelist(); + } + note.setPriority(priority.getAsInt()); + } + + public static Command DynamicToNote(Supplier curPose) { + return AutoBuilder.pathfindToPose( + getClosestNote(curPose).getPoseAllianceSpicific(), + new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720))); + } + + public static Command DynamicToShoot(Supplier curPose) { + return AutoBuilder.pathfindToPose( + closestShootingLocation(curPose, shootingLocations).getPoseAllianceSpicific(), + new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720))); + } + + public static ShootingLocation closestShootingLocation( + Supplier curPose, ShootingLocation[] shootingLocations) { + + ShootingLocation closestLocation = new ShootingLocation("shooting shooting"); + double shortestDistance = 9999999; + for (ShootingLocation location : shootingLocations) { + if (shortestDistance + > curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm()) { + closestLocation = location; + shortestDistance = + curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm(); + } + } + return closestLocation; + } + + public static ChoreoTrajectory makeStartToNote(Supplier startingPose) { + ShootingLocation startingLocation = closestShootingLocation(startingPose, startingLocations); + startingLocation.setPose(startingLocation.getPoseAllianceSpicific()); + Note closestNote = getClosestNote(startingPose); + if ("Uninitialized" == closestNote.getName()) { + closestNote = getAbsoluteClosestNote(startingPose); + System.out.println("No more avaliable notes!"); + } + return Choreo.getTrajectory(startingLocation.getName() + " To " + closestNote.getName()); + } + + public static ChoreoTrajectory makeNoteToShooting(Supplier startingPose) { + Note closestNote = getAbsoluteClosestNote(startingPose); + ShootingLocation startingLocation = closestShootingLocation(startingPose, shootingLocations); + if ("Uninitialized" == closestNote.getName()) { + closestNote = getAbsoluteClosestNote(startingPose); + System.out.println("No more avaliable notes!"); + } + return Choreo.getTrajectory(closestNote.getName() + " To " + startingLocation.getName()); + } + + public static ChoreoTrajectory makeShootingToNote(Supplier startingPose) { + Note closestNote = getClosestNote(startingPose); + ShootingLocation startingLocation = closestShootingLocation(startingPose, shootingLocations); + if ("Uninitialized" == closestNote.getName()) { + closestNote = getAbsoluteClosestNote(startingPose); + System.out.println("No more avaliable notes!"); + } + return Choreo.getTrajectory(startingLocation.getName() + " To " + closestNote.getName()); + } + + public static ChoreoTrajectory makeNoteToNote(Supplier startingPose) { + Note closestNote = getAbsoluteClosestNote(startingPose); + Note nextClosest = new Note(); + if (!closestNote.getBlacklist()) { + closestNote.blacklist(); + nextClosest = getClosestNote(startingPose); + closestNote.whitelist(); + } else { + nextClosest = getClosestNote(startingPose); + } + + Logger.recordOutput( + "DynamicAuto/Path Name", closestNote.getName() + " To " + nextClosest.getName()); + return Choreo.getTrajectory(closestNote.getName() + " To " + nextClosest.getName()); + } + + public static Note getAbsoluteClosestNote(Supplier curPose) { + Note closestNote = new Note(); + double shortestDistance = 99999999; + for (Note note : notes) { + if (shortestDistance + > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) { + shortestDistance = + curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm(); + closestNote = note; + } + } + + return closestNote; + } + + public static Note getClosestNote(Supplier curPose) { + Note closestNote = new Note(); + double shortestDistance = Double.POSITIVE_INFINITY; + for (Note note : notes) { + if (note.getBlacklist()) { + + continue; + } else if (note.getPriority() > closestNote.getPriority()) { + shortestDistance = + curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm(); + closestNote = note; + if (shortestDistance + > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) { + shortestDistance = + curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm(); + closestNote = note; + } + } else if (note.getPriority() == closestNote.getPriority()) { + if (shortestDistance + > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) { + shortestDistance = + curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm(); + closestNote = note; + } + } + } + + return closestNote; + } + + public static Pose2d[] addTwoTrajectories( + ChoreoTrajectory trajectory1, ChoreoTrajectory trajectory2) { + Pose2d[] poses = new Pose2d[trajectory1.getPoses().length + trajectory2.getPoses().length]; + for (int i = 0; i < trajectory1.getPoses().length; i++) { + poses[i] = trajectory1.getPoses()[i]; + } + for (int i = 0; i < trajectory2.getPoses().length; i++) { + poses[i + trajectory1.getPoses().length] = trajectory2.getPoses()[i]; + } + return poses; + } +} diff --git a/src/main/java/frc/robot/utils/dynamicauto/FindMissingPath.py b/src/main/java/frc/robot/utils/dynamicauto/FindMissingPath.py new file mode 100644 index 00000000..f3b2a456 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/FindMissingPath.py @@ -0,0 +1,45 @@ +import json +import os + +# we know that the file missing must be missing from the deploy/choreo folder + +# open file in read mode + +paths = {} + +with open('src/main/java/frc/robot/utils/dynamicauto/data.chor', 'r') as file: + data = json.load(file) + + paths = data["paths"] + +print("Number of paths in data.chor:") +print(len(paths)) + +folder_path = "src/main/java/frc/robot/utils/dynamicauto/deploy/choreo" +files_in_folder = os.listdir(folder_path) + +print("Files in deploy/choreo folder:") +print(len(files_in_folder) / 2) + +originalFiles = [] + +for file in files_in_folder: + if not file.endswith(".1.traj"): + originalFiles.append(file[:-5]) + +print("Files that end with '.1.traj':") +print(len(originalFiles)) + +print("Finding missing files...") + +pathNames = [value for value in paths.keys()] + +missingFiles = [] + +print(originalFiles) + +print(pathNames) + +print("Missing files:") + +print(list(set(originalFiles) ^ set(pathNames))) \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/dynamicauto/LocalADStarAK.java b/src/main/java/frc/robot/utils/dynamicauto/LocalADStarAK.java new file mode 100644 index 00000000..a813443b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/LocalADStarAK.java @@ -0,0 +1,150 @@ +package frc.robot.utils.dynamicauto; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java new file mode 100644 index 00000000..71e10f80 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java @@ -0,0 +1,102 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils.dynamicauto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/** Add your docs here. */ +public class Note { + + private boolean blacklisted; + private int priority; + private Pose2d pose; + private final String name; + private boolean isThere; + + public Note(Pose2d pose, boolean blacklisted, int priority, String name, boolean isThere) { + this.pose = pose; + this.blacklisted = blacklisted; + this.priority = priority; + this.name = name; + this.isThere = isThere; + } + + public Note(Pose2d pose, boolean blacklisted, int priority, String name) { + this.pose = pose; + this.blacklisted = blacklisted; + this.priority = priority; + this.name = name; + this.isThere = false; + } + + public Note(String name) { + this.pose = new Pose2d(); + this.blacklisted = true; + this.priority = -1; + this.name = name; + this.isThere = false; + } + + public Note() { + this.pose = new Pose2d(); + this.blacklisted = true; + this.priority = -1; + this.name = "Uninitialized"; + this.isThere = false; + } + + public void blacklist() { + System.out.println("Blacklisted " + name); + this.blacklisted = true; + } + + public void whitelist() { + System.out.println("Whitelisted " + name); + this.blacklisted = false; + } + + public void setPriority(Integer priority) { + this.priority = priority; + } + + public void setPose(Pose2d pose) { + this.pose = pose; + } + + public Pose2d getPoseAllianceSpicific() { + if (DriverStation.getAlliance().isPresent()) { + if (DriverStation.getAlliance().get() == Alliance.Red) { + return new Pose2d( + 16.5410515 - this.pose.getX(), + this.pose.getY(), + Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians())); + } + } + return this.pose; + } + + public boolean getBlacklist() { + return blacklisted; + } + + public boolean getExistence() { + return isThere; + } + + public int getPriority() { + return priority; + } + + public Pose2d getPose() { + return pose; + } + + public String getName() { + return name; + } +} diff --git a/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py b/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py new file mode 100644 index 00000000..878f1c16 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py @@ -0,0 +1,22 @@ +import json +import os +import urllib.parse + +folder_path = "C:\\Users\\Robotics\\Desktop\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\deploy\\choreo" +files_in_folder = os.listdir(folder_path) + +print("Files in deploy/choreo folder:") +print(len(files_in_folder) / 2) + +repeatFiles = [] + +for file in files_in_folder: + if file.endswith(".1.traj"): + repeatFiles.append(file) + +print(f"Removing {len(repeatFiles)} files...") + +for file in repeatFiles: + os.remove(os.path.join(folder_path, file)) + +print("Files removed.") \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java new file mode 100644 index 00000000..9e081011 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils.dynamicauto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/** Add your docs here. */ +public class ShootingLocation { + private Pose2d pose; + private String name; + + public ShootingLocation(Pose2d pose, String name) { + this.pose = pose; + this.name = name; + } + + public ShootingLocation() { + this.pose = new Pose2d(); + this.name = "Uninitialized"; + } + + public ShootingLocation(String name) { + this.name = name; + } + + public void setPose(Pose2d pose) { + this.pose = pose; + } + + public Pose2d getPose() { + return pose; + } + + public Pose2d getPoseAllianceSpicific() { + if (DriverStation.getAlliance().isPresent()) { + if (DriverStation.getAlliance().get() == Alliance.Red) { + return new Pose2d( + 16.5410515 - this.pose.getX(), + this.pose.getY(), + Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians())); + } + } + return this.pose; + } + + public String getName() { + return name; + } +} diff --git a/src/main/java/frc/robot/utils/dynamicauto/SwapHeadings.py b/src/main/java/frc/robot/utils/dynamicauto/SwapHeadings.py new file mode 100644 index 00000000..990a148d --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/SwapHeadings.py @@ -0,0 +1,26 @@ +import json + +# open file in write mode +with open('src/main/java/frc/robot/utils/dynamicauto/data.chor', 'r+') as file: + data = json.load(file) + + """ + Things to Change for each path + 1. Clear Trajectory array + 2. Reverse headings for each waypoint + """ + + for path in data["paths"]: + path = data["paths"][path] + + waypoints = path["waypoints"] + path["trajectory"] = [] + + for waypoint in waypoints: + if waypoint["heading"] > 0: + waypoint["heading"] = round(waypoint["heading"] - 3.14, 3) + else: + waypoint["heading"] = round(waypoint["heading"] + 3.14, 3) + + with open('src/main/java/frc/robot/utils/dynamicauto/data.chor', 'w') as file: + json.dump(data, file, indent=4) \ No newline at end of file diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json index bf5ee6a5..387b2de5 100644 --- a/vendordeps/ChoreoLib.json +++ b/vendordeps/ChoreoLib.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib.json", "name": "ChoreoLib", - "version": "2024.1.2", + "version": "2024.1.3", "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.choreo.lib", "artifactId": "ChoreoLib-java", - "version": "2024.1.2" + "version": "2024.1.3" }, { "groupId": "com.google.code.gson", @@ -25,7 +25,7 @@ { "groupId": "com.choreo.lib", "artifactId": "ChoreoLib-cpp", - "version": "2024.1.2", + "version": "2024.1.3", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false,