diff --git a/.vscode/settings.json b/.vscode/settings.json index 8be11f28..8e786f6c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,6 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.debug.settings.onBuildFailureProceed": true } diff --git a/build.gradle b/build.gradle index c7504ce6..75d04022 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { diff --git a/networktables.json b/networktables.json new file mode 100644 index 00000000..fe51488c --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/src/main/Untitled.chor b/src/main/Untitled.chor new file mode 100644 index 00000000..a7ff72cf --- /dev/null +++ b/src/main/Untitled.chor @@ -0,0 +1,681 @@ +{ + "version": "v0.3.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": { + "Circle": { + "waypoints": [ + { + "x": 1.5, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 4.353217601776123, + "y": 7.126353740692139, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 4.450671672821045, + "y": 4.339164733886719, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 1.5, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.5, + "y": 5.5, + "heading": -3.3219213067501535e-28, + "angularVelocity": -1.3665051940262503e-28, + "velocityX": -2.805794210798643e-23, + "velocityY": 1.2484693727650662e-22, + "timestamp": 0 + }, + { + "x": 1.5362282626284263, + "y": 5.5432844591906445, + "heading": -9.472390498067437e-18, + "angularVelocity": -1.1512814988371418e-16, + "velocityX": 0.44032104155440455, + "velocityY": 0.5260825877691788, + "timestamp": 0.08227692798993784 + }, + { + "x": 1.6086847866611431, + "y": 5.629853376109373, + "heading": -3.486400420928457e-17, + "angularVelocity": -3.086115917488839e-16, + "velocityX": 0.8806420682305758, + "velocityY": 1.0521651577622833, + "timestamp": 0.16455385597987568 + }, + { + "x": 1.7173695700579255, + "y": 5.759706748318585, + "heading": -7.301754283392471e-17, + "angularVelocity": -4.637209914966371e-16, + "velocityX": 1.3209630701096913, + "velocityY": 1.5782476981285976, + "timestamp": 0.2468307839698135 + }, + { + "x": 1.862282608738322, + "y": 5.932844570943078, + "heading": -1.192226659960793e-16, + "angularVelocity": -5.615805583767775e-16, + "velocityX": 1.7612840223946997, + "velocityY": 2.104330179241336, + "timestamp": 0.32910771195975136 + }, + { + "x": 2.043423890460986, + "y": 6.14926682935725, + "heading": -1.8781998178842608e-16, + "angularVelocity": -8.33736959658091e-16, + "velocityX": 2.201604825897445, + "velocityY": 2.6304124825934165, + "timestamp": 0.4113846399496892 + }, + { + "x": 2.2431937917892997, + "y": 6.3879460127932965, + "heading": -2.0530355513809777e-16, + "angularVelocity": -2.1249667162436542e-16, + "velocityX": 2.428018476245788, + "velocityY": 2.900924831141438, + "timestamp": 0.49366156793962707 + }, + { + "x": 2.4429638847633592, + "y": 6.626625035825442, + "heading": -2.0383025572099072e-16, + "angularVelocity": 1.7906592436584598e-17, + "velocityX": 2.4280208055226695, + "velocityY": 2.900922881580298, + "timestamp": 0.5759384959295649 + }, + { + "x": 2.6621600398809373, + "y": 6.847597458073065, + "heading": -1.9812607044504066e-16, + "angularVelocity": 6.932909886183889e-17, + "velocityX": 2.6641266327345763, + "velocityY": 2.6857155176557943, + "timestamp": 0.6582154239195027 + }, + { + "x": 2.9176597571753207, + "y": 7.025349134953072, + "heading": -2.0290881342506434e-16, + "angularVelocity": -5.812981960056516e-17, + "velocityX": 3.1053628706899454, + "velocityY": 2.1604073125061927, + "timestamp": 0.7404923519094405 + }, + { + "x": 3.2010604808925094, + "y": 7.154033982149684, + "heading": -1.9347228589546108e-16, + "angularVelocity": 1.1469226864191852e-16, + "velocityX": 3.444473811076744, + "velocityY": 1.5640453568265125, + "timestamp": 0.8227692798993783 + }, + { + "x": 3.5030417885342837, + "y": 7.229419779766539, + "heading": -1.8536764185493148e-16, + "angularVelocity": 9.850445607453505e-17, + "velocityX": 3.6703036321276663, + "velocityY": 0.9162446807211185, + "timestamp": 0.9050462078893161 + }, + { + "x": 3.804727969097522, + "y": 7.248361426815143, + "heading": -1.2128971877681688e-16, + "angularVelocity": 7.788079190947775e-16, + "velocityX": 3.6667166353140046, + "velocityY": 0.23021820954374964, + "timestamp": 0.9873231358792539 + }, + { + "x": 4.090299967419989, + "y": 7.2132072004642325, + "heading": -5.322461627386045e-17, + "angularVelocity": 8.272683991127659e-16, + "velocityX": 3.47086364670048, + "velocityY": -0.42726712347852686, + "timestamp": 1.0696000638691918 + }, + { + "x": 4.353217601776123, + "y": 7.126353740692139, + "heading": -1.7247782060093015e-27, + "angularVelocity": 6.468960080495983e-16, + "velocityX": 3.1955207951892257, + "velocityY": -1.0556235131034137, + "timestamp": 1.1518769918591296 + }, + { + "x": 4.58144387602119, + "y": 6.996965486159656, + "heading": 5.199201984564153e-17, + "angularVelocity": 6.587448099688971e-16, + "velocityX": 2.8916528747838215, + "velocityY": -1.639363913816324, + "timestamp": 1.230802881005533 + }, + { + "x": 4.782026897902942, + "y": 6.823603519584229, + "heading": 9.551997300692058e-17, + "angularVelocity": 5.515041215496502e-16, + "velocityX": 2.5414097205757624, + "velocityY": -2.196515851140449, + "timestamp": 1.3097287701519362 + }, + { + "x": 4.950456239944392, + "y": 6.6094496730444945, + "heading": 1.2540926885038495e-16, + "angularVelocity": 3.787007807807601e-16, + "velocityX": 2.134018936790482, + "velocityY": -2.713353613824885, + "timestamp": 1.3886546592983395 + }, + { + "x": 5.081311669842177, + "y": 6.35943446092387, + "heading": 1.3960950290458758e-16, + "angularVelocity": 1.7991858195137434e-16, + "velocityX": 1.6579531927103452, + "velocityY": -3.1677211980071323, + "timestamp": 1.4675805484447428 + }, + { + "x": 5.168573540307643, + "y": 6.08118030776853, + "heading": 1.3851795279191138e-16, + "angularVelocity": -1.3830064177977227e-17, + "velocityX": 1.1056178322375505, + "velocityY": -3.525511795492021, + "timestamp": 1.5465064375911461 + }, + { + "x": 5.206787103011429, + "y": 5.785835234655995, + "heading": 1.1611809267335485e-16, + "angularVelocity": -2.8380877758076653e-16, + "velocityX": 0.48417018949132823, + "velocityY": -3.742055696891635, + "timestamp": 1.6254323267375494 + }, + { + "x": 5.193141803403618, + "y": 5.487575652887037, + "heading": 8.296484960384974e-17, + "angularVelocity": -4.2005536367974825e-16, + "velocityX": -0.17288749934129347, + "velocityY": -3.778982853341056, + "timestamp": 1.7043582158839528 + }, + { + "x": 5.128449250262065, + "y": 5.1989084597897195, + "heading": 5.766159875503332e-17, + "angularVelocity": -3.2059506863832877e-16, + "velocityX": -0.8196620125692016, + "velocityY": -3.657446196923542, + "timestamp": 1.783284105030356 + }, + { + "x": 5.016690466802075, + "y": 4.9322095650866205, + "heading": 3.327908681576341e-17, + "angularVelocity": -3.0892920184187437e-16, + "velocityX": -1.4159965084800308, + "velocityY": -3.3791053555112245, + "timestamp": 1.8622099941767594 + }, + { + "x": 4.863164798870976, + "y": 4.6963872590942435, + "heading": 9.698495363712997e-18, + "angularVelocity": -2.987687779794426e-16, + "velocityX": -1.945187689255118, + "velocityY": -2.9878954617152007, + "timestamp": 1.9411358833231627 + }, + { + "x": 4.673014997916854, + "y": 4.4973957749437625, + "heading": 3.081671052827974e-18, + "angularVelocity": -8.383591723601681e-17, + "velocityX": -2.4092196237587498, + "velocityY": -2.5212447563481035, + "timestamp": 2.0200617724695658 + }, + { + "x": 4.450671672821045, + "y": 4.339164733886719, + "heading": 8.278658336073892e-28, + "angularVelocity": -3.904512302530694e-17, + "velocityX": -2.8171152393782313, + "velocityY": -2.0048053023962087, + "timestamp": 2.098987661615969 + }, + { + "x": 4.153965896620894, + "y": 4.213937531814997, + "heading": 8.290067362617875e-18, + "angularVelocity": 9.044327654116548e-17, + "velocityX": -3.2370114015394744, + "velocityY": -1.3662082554658637, + "timestamp": 2.1906480666608945 + }, + { + "x": 3.8249997993536913, + "y": 4.150894110176715, + "heading": 2.3888082893966074e-17, + "angularVelocity": 1.701717936252325e-16, + "velocityX": -3.588966218357502, + "velocityY": -0.6877934000769791, + "timestamp": 2.28230847170582 + }, + { + "x": 3.478286252707822, + "y": 4.155619248980717, + "heading": 3.558873113658147e-17, + "angularVelocity": 1.2765215510930902e-16, + "velocityX": -3.782587983065696, + "velocityY": 0.051550490112787714, + "timestamp": 2.373968876750746 + }, + { + "x": 3.1395982702697465, + "y": 4.229937042598315, + "heading": 4.921578190575193e-17, + "angularVelocity": 1.486688910228552e-16, + "velocityX": -3.695030392589622, + "velocityY": 0.8107949510060795, + "timestamp": 2.4656292817956715 + }, + { + "x": 2.8227601851385815, + "y": 4.370814002416527, + "heading": 5.890626112127518e-17, + "angularVelocity": 1.0572154039974704e-16, + "velocityX": -3.456651593192001, + "velocityY": 1.5369445481848356, + "timestamp": 2.557289686840597 + }, + { + "x": 2.54070424472652, + "y": 4.572499745217142, + "heading": 7.805444228664704e-17, + "angularVelocity": 2.0890351898005458e-16, + "velocityX": -3.0771840935441808, + "velocityY": 2.2003584066834767, + "timestamp": 2.648950091885523 + }, + { + "x": 2.2818433514518452, + "y": 4.803202434252147, + "heading": 7.067779216783429e-17, + "angularVelocity": -8.047804409499006e-17, + "velocityX": -2.8241299299059275, + "velocityY": 2.516928535521215, + "timestamp": 2.7406104969304486 + }, + { + "x": 2.022982633974282, + "y": 5.033905320540323, + "heading": 3.011799781222237e-17, + "angularVelocity": -4.4250071048418785e-16, + "velocityX": -2.8241280119882584, + "velocityY": 2.516930687520981, + "timestamp": 2.8322709019753742 + }, + { + "x": 1.8137895934579804, + "y": 5.220343180672855, + "heading": 1.710283920370377e-17, + "angularVelocity": -1.4199324781572106e-16, + "velocityX": -2.2822617946513546, + "velocityY": 2.0340065052205936, + "timestamp": 2.9239313070203 + }, + { + "x": 1.6568948001100603, + "y": 5.360171587323137, + "heading": 1.0302381022210203e-17, + "angularVelocity": -7.419188446827068e-17, + "velocityX": -1.7116964873875544, + "velocityY": 1.5255050049336634, + "timestamp": 3.0155917120652256 + }, + { + "x": 1.5522982674547041, + "y": 5.453390528438091, + "heading": 4.4011411079174696e-18, + "angularVelocity": -6.438156051477007e-17, + "velocityX": -1.1411310325771633, + "velocityY": 1.0170033731496684, + "timestamp": 3.1072521171101513 + }, + { + "x": 1.5, + "y": 5.5, + "heading": 1.7365292199524757e-28, + "angularVelocity": -4.801572833443273e-17, + "velocityX": -0.5705655285842443, + "velocityY": 0.508501697533024, + "timestamp": 3.198912522155077 + }, + { + "x": 1.5, + "y": 5.5, + "heading": 8.504010006620182e-29, + "angularVelocity": -3.897781011434181e-29, + "velocityX": -6.009517149045476e-23, + "velocityY": -7.563147799258595e-23, + "timestamp": 3.2905729272000026 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.5, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 1.1518769918591296, + "isStopPoint": false, + "x": 4.353217601776123, + "y": 7.126353740692139, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 2.098987661615969, + "isStopPoint": false, + "x": 4.450671672821045, + "y": 4.339164733886719, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 3.2905729272000026, + "isStopPoint": true, + "x": 1.5, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "Line": { + "waypoints": [ + { + "x": 1.5, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.55, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.5, + "y": 5.5, + "heading": -1.2874479772672074e-29, + "angularVelocity": -5.012654343497732e-30, + "velocityX": 2.633710619062863e-19, + "velocityY": -7.373916538898162e-46, + "timestamp": 0 + }, + { + "x": 1.5789016505791718, + "y": 5.5, + "heading": -6.693322095338404e-19, + "angularVelocity": -6.880708036173696e-18, + "velocityX": 0.8111057760332284, + "velocityY": 6.608245262710766e-34, + "timestamp": 0.09727664739985706 + }, + { + "x": 1.7367049486758532, + "y": 5.5, + "heading": -2.0081784525315156e-18, + "angularVelocity": -1.3763285215143903e-17, + "velocityX": 1.6222115206411716, + "velocityY": 9.05293138093418e-34, + "timestamp": 0.19455329479971412 + }, + { + "x": 1.9734098881938018, + "y": 5.5, + "heading": -4.016923229987951e-18, + "angularVelocity": -2.0649815049430785e-17, + "velocityX": 2.433317202398612, + "velocityY": 8.038324084205884e-34, + "timestamp": 0.29182994219957115 + }, + { + "x": 2.289016450766005, + "y": 5.5, + "heading": -6.696777202113547e-18, + "angularVelocity": -2.7548790422010714e-17, + "velocityX": 3.2444226956047455, + "velocityY": 8.752223368923682e-34, + "timestamp": 0.38910658959942823 + }, + { + "x": 2.6570082253839193, + "y": 5.5, + "heading": -5.1008819244864145e-18, + "angularVelocity": 1.640573889311827e-17, + "velocityX": 3.7829405561642684, + "velocityY": -7.164097231420146e-34, + "timestamp": 0.4863832369992853 + }, + { + "x": 3.0249999999998676, + "y": 5.5, + "heading": 1.4749694041473506e-17, + "angularVelocity": 2.0406311788662626e-16, + "velocityX": 3.7829405561646503, + "velocityY": 3.457561705895053e-33, + "timestamp": 0.5836598843991424 + }, + { + "x": 3.3929917746173133, + "y": 5.5, + "heading": 1.3703190360166544e-17, + "angularVelocity": -1.075801550800218e-17, + "velocityX": 3.7829405561646503, + "velocityY": -8.436403566578749e-34, + "timestamp": 0.6809365317989995 + }, + { + "x": 3.7609835492331904, + "y": 5.5, + "heading": 6.6978603967447e-18, + "angularVelocity": -7.201450862727049e-17, + "velocityX": 3.7829405561642684, + "velocityY": -1.6207126856714834e-33, + "timestamp": 0.7782131791988566 + }, + { + "x": 4.076590111811133, + "y": 5.5, + "heading": 4.0174424481780384e-18, + "angularVelocity": -2.7554588077011827e-17, + "velocityX": 3.2444226956047326, + "velocityY": -1.5328681912688769e-33, + "timestamp": 0.8754898265987137 + }, + { + "x": 4.3132950513190655, + "y": 5.5, + "heading": 2.0084039923467558e-18, + "angularVelocity": -2.0652834051193523e-17, + "velocityX": 2.433317202398599, + "velocityY": -1.0912710639486781e-33, + "timestamp": 0.9727664739985707 + }, + { + "x": 4.471098349419339, + "y": 5.5, + "heading": 6.69399083833827e-19, + "angularVelocity": -1.376491629039911e-17, + "velocityX": 1.6222115206411583, + "velocityY": -8.707726946074912e-34, + "timestamp": 1.0700431213984278 + }, + { + "x": 4.55, + "y": 5.5, + "heading": 2.6358429152716765e-29, + "angularVelocity": -6.881395501116776e-18, + "velocityX": 0.8111057760332147, + "velocityY": -5.515722786564321e-35, + "timestamp": 1.1673197687982848 + }, + { + "x": 4.55, + "y": 5.5, + "heading": 1.287447299074069e-29, + "angularVelocity": -6.265462343923493e-30, + "velocityX": 1.2222778711369996e-19, + "velocityY": -9.16410582597125e-46, + "timestamp": 1.2645964161981418 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.5, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 1.2645964161981418, + "isStopPoint": true, + "x": 4.55, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + } + }, + "splitTrajectoriesAtStopPoints": false, + "usesObstacles": false +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Choreo Test.1.traj b/src/main/deploy/choreo/Choreo Test.1.traj new file mode 100644 index 00000000..2437192e --- /dev/null +++ b/src/main/deploy/choreo/Choreo Test.1.traj @@ -0,0 +1,1390 @@ +{ + "samples": [ + { + "x": 1.4585455656051636, + "y": 4.074051856994629, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": 2.0171394626419585e-36, + "velocityY": -3.1684110491546193e-35, + "timestamp": 0 + }, + { + "x": 1.4905205741903755, + "y": 4.0671846353687675, + "heading": 3.1068407756328744, + "angularVelocity": -0.5198162463824533, + "velocityX": 0.501252042472078, + "velocityY": -0.1076531021687804, + "timestamp": 0.06379028088846767 + }, + { + "x": 1.5549156419284027, + "y": 4.054610420448303, + "heading": 3.0435644184480135, + "angularVelocity": -0.9919435422379561, + "velocityX": 1.0094808619923894, + "velocityY": -0.19711803656186136, + "timestamp": 0.12758056177693533 + }, + { + "x": 1.6523135702857736, + "y": 4.038202658793452, + "heading": 2.9552733306856536, + "angularVelocity": -1.3840836963350238, + "velocityX": 1.526845892521838, + "velocityY": -0.25721413084133044, + "timestamp": 0.191370842665403 + }, + { + "x": 1.783322512462502, + "y": 4.02131537659787, + "heading": 2.8504247025188674, + "angularVelocity": -1.6436458141657342, + "velocityX": 2.053744557196534, + "velocityY": -0.26473127191755363, + "timestamp": 0.25516112355387066 + }, + { + "x": 1.9476819873344473, + "y": 4.010657961922336, + "heading": 2.7442259551324777, + "angularVelocity": -1.664810781630976, + "velocityX": 2.576559823577434, + "velocityY": -0.16706956807680673, + "timestamp": 0.3189514044423383 + }, + { + "x": 2.1378010179584184, + "y": 4.0200538425068055, + "heading": 2.67135854139101, + "angularVelocity": -1.1422964866524252, + "velocityX": 2.9803761321631335, + "velocityY": 0.14729329379969647, + "timestamp": 0.382741685330806 + }, + { + "x": 2.3327531815626203, + "y": 4.054874437218412, + "heading": 2.6867523157379845, + "angularVelocity": 0.2413184913527656, + "velocityX": 3.0561421095646404, + "velocityY": 0.5458605014216376, + "timestamp": 0.44653196621927366 + }, + { + "x": 2.510732792290265, + "y": 4.113199213721838, + "heading": 2.7651162027533043, + "angularVelocity": 1.2284612314583365, + "velocityX": 2.79007410296292, + "velocityY": 0.9143207349314517, + "timestamp": 0.5103222471077413 + }, + { + "x": 2.666890859603882, + "y": 4.191041946411133, + "heading": 2.906047416060082, + "angularVelocity": 2.209289743576844, + "velocityX": 2.4479915300364854, + "velocityY": 1.220291423789085, + "timestamp": 0.5741125279962089 + }, + { + "x": 2.756813611394881, + "y": 4.2481449117517185, + "heading": 3.019078744416041, + "angularVelocity": 2.7742492473081195, + "velocityX": 2.207070642277403, + "velocityY": 1.401539386640599, + "timestamp": 0.6148555608962393 + }, + { + "x": 2.8357643538183357, + "y": 4.312490934812126, + "heading": 3.1475106055134408, + "angularVelocity": 3.152241057079095, + "velocityX": 1.937772836331876, + "velocityY": 1.5793135287274918, + "timestamp": 0.6555985937962696 + }, + { + "x": 2.9029140041003427, + "y": 4.3834707929163566, + "heading": 3.286187250098307, + "angularVelocity": 3.403689777467768, + "velocityX": 1.648125961726257, + "velocityY": 1.7421348645887855, + "timestamp": 0.6963416266963 + }, + { + "x": 2.957867862311061, + "y": 4.460664789436284, + "heading": 3.4321788864396923, + "angularVelocity": 3.583229473848938, + "velocityX": 1.3487915429751285, + "velocityY": 1.8946551355009582, + "timestamp": 0.7370846595963303 + }, + { + "x": 3.0002910660148934, + "y": 4.543635590242566, + "heading": 3.582061930766736, + "angularVelocity": 3.6787404780298614, + "velocityX": 1.0412382359439127, + "velocityY": 2.0364414453353255, + "timestamp": 0.7778276924963606 + }, + { + "x": 3.0299626612502326, + "y": 4.631989358642389, + "heading": 3.7319206979896067, + "angularVelocity": 3.6781446189971536, + "velocityX": 0.7282618186069594, + "velocityY": 2.168561398377315, + "timestamp": 0.818570725396391 + }, + { + "x": 3.0468078340815077, + "y": 4.7254089445273975, + "heading": 3.877554919455978, + "angularVelocity": 3.5744570568349348, + "velocityX": 0.4134491625257187, + "velocityY": 2.2928971958034863, + "timestamp": 0.8593137582964213 + }, + { + "x": 3.0508603788207704, + "y": 4.823595123995901, + "heading": 4.014992213293226, + "angularVelocity": 3.373271061446823, + "velocityX": 0.09946595652823349, + "velocityY": 2.4098888197503454, + "timestamp": 0.9000567911964517 + }, + { + "x": 3.0418255519972037, + "y": 4.925400743620523, + "heading": 4.142750839635198, + "angularVelocity": 3.1357171336618257, + "velocityX": -0.22175145492322643, + "velocityY": 2.4987246254941016, + "timestamp": 0.940799824096482 + }, + { + "x": 3.021206090160081, + "y": 5.020464579226095, + "heading": 4.282785145406025, + "angularVelocity": 3.4370123136002837, + "velocityX": -0.5060855898409946, + "velocityY": 2.3332537820349804, + "timestamp": 0.9815428569965123 + }, + { + "x": 2.9996081410347006, + "y": 5.103938301937717, + "heading": 4.452456839415369, + "angularVelocity": 4.164434553158123, + "velocityX": -0.5301016539042277, + "velocityY": 2.048785197617431, + "timestamp": 1.0222858898965426 + }, + { + "x": 2.9785438954530656, + "y": 5.173710724146211, + "heading": 4.628924606077659, + "angularVelocity": 4.331237860845608, + "velocityX": -0.5170023948221859, + "velocityY": 1.7124994690427882, + "timestamp": 1.0630289227965728 + }, + { + "x": 2.9587317784828078, + "y": 5.234319209722237, + "heading": 4.760996474258561, + "angularVelocity": 3.241581659003185, + "velocityX": -0.4862700579721273, + "velocityY": 1.487579133461626, + "timestamp": 1.103771955696603 + }, + { + "x": 2.940244887270436, + "y": 5.287568324744329, + "heading": 4.840301578000097, + "angularVelocity": 1.9464703066196472, + "velocityX": -0.45374361937493524, + "velocityY": 1.3069502006084648, + "timestamp": 1.1445149885966333 + }, + { + "x": 2.9231014391412486, + "y": 5.334086773278073, + "heading": 4.864342694978022, + "angularVelocity": 0.590066945603065, + "velocityX": -0.42077005340401513, + "velocityY": 1.1417522266416396, + "timestamp": 1.1852580214966635 + }, + { + "x": 2.9071312956135524, + "y": 5.374187557857939, + "heading": 4.831856104171122, + "angularVelocity": -0.7973532772244076, + "velocityX": -0.39197237885755587, + "velocityY": 0.9842366099318068, + "timestamp": 1.2260010543966937 + }, + { + "x": 2.8919732570648193, + "y": 5.408133029937744, + "heading": 4.741792239549658, + "angularVelocity": -2.210534125980521, + "velocityX": -0.3720400144467828, + "velocityY": 0.8331601666252006, + "timestamp": 1.266744087296724 + }, + { + "x": 2.882026462257207, + "y": 5.428283750424018, + "heading": 4.654897403574336, + "angularVelocity": -3.1758469572855184, + "velocityX": -0.3635371155251437, + "velocityY": 0.7364718930089131, + "timestamp": 1.2941052407603635 + }, + { + "x": 2.8722493985664026, + "y": 5.446002750654774, + "heading": 4.541017712722759, + "angularVelocity": -4.162093933755802, + "velocityX": -0.3573337543608044, + "velocityY": 0.6475969755552201, + "timestamp": 1.321466394224003 + }, + { + "x": 2.862908284982934, + "y": 5.461788364835415, + "heading": 4.399049387011055, + "angularVelocity": -5.188682045161824, + "velocityX": -0.34140057713143623, + "velocityY": 0.5769352597513185, + "timestamp": 1.3488275476876426 + }, + { + "x": 2.855341721346824, + "y": 5.475808186129583, + "heading": 4.230496751378239, + "angularVelocity": -6.160289837809847, + "velocityX": -0.27654402970127984, + "velocityY": 0.5123987668429125, + "timestamp": 1.3761887011512821 + }, + { + "x": 2.850499346368405, + "y": 5.487175210010101, + "heading": 4.040136228473314, + "angularVelocity": -6.957328138884789, + "velocityX": -0.17697992830800321, + "velocityY": 0.4154438845432298, + "timestamp": 1.4035498546149217 + }, + { + "x": 2.850511331304342, + "y": 5.495665667507396, + "heading": 3.8398105873897572, + "angularVelocity": -7.321534940029888, + "velocityX": 0.0004380274374277367, + "velocityY": 0.3103106566241056, + "timestamp": 1.4309110080785612 + }, + { + "x": 2.856241467196838, + "y": 5.505496640264063, + "heading": 3.647301887529118, + "angularVelocity": -7.03584006852873, + "velocityX": 0.2094259622537722, + "velocityY": 0.35930403189076365, + "timestamp": 1.4582721615422007 + }, + { + "x": 2.864610492964689, + "y": 5.5169142669590086, + "heading": 3.4794236810579164, + "angularVelocity": -6.1356406883319226, + "velocityX": 0.30587254952437387, + "velocityY": 0.41729332464432606, + "timestamp": 1.4856333150058403 + }, + { + "x": 2.87348931347585, + "y": 5.530255151802143, + "heading": 3.33971041673408, + "angularVelocity": -5.106263685465693, + "velocityX": 0.32450461282488824, + "velocityY": 0.48758488419952456, + "timestamp": 1.5129944684694798 + }, + { + "x": 2.8826084077382985, + "y": 5.545984706383625, + "heading": 3.227220485531993, + "angularVelocity": -4.111300766306338, + "velocityX": 0.333286177959096, + "velocityY": 0.5748863841718318, + "timestamp": 1.5403556219331194 + }, + { + "x": 2.8919732570648193, + "y": 5.564249038696289, + "heading": 3.141592653589793, + "angularVelocity": -3.1295402825758374, + "velocityX": 0.3422680750271005, + "velocityY": 0.6675278634336637, + "timestamp": 1.567716775396759 + }, + { + "x": 2.907609226294196, + "y": 5.599859473084939, + "heading": 3.0730087116624225, + "angularVelocity": -1.5751077296069407, + "velocityX": 0.35909770277085135, + "velocityY": 0.8178338672866402, + "timestamp": 1.6112591570626056 + }, + { + "x": 2.923994290533328, + "y": 5.642177598480751, + "heading": 3.0715517727837245, + "angularVelocity": -0.033460247762260184, + "velocityX": 0.37630151618426566, + "velocityY": 0.9718835712885416, + "timestamp": 1.6548015387284523 + }, + { + "x": 2.9408764364223456, + "y": 5.691439774506376, + "heading": 3.1364313706480664, + "angularVelocity": 1.4900332821075657, + "velocityX": 0.3877175580007244, + "velocityY": 1.1313615411227034, + "timestamp": 1.698343920394299 + }, + { + "x": 2.957708600687666, + "y": 5.747992956441908, + "heading": 3.2662643836700145, + "angularVelocity": 2.981761861772134, + "velocityX": 0.38656967353080685, + "velocityY": 1.2988077310408146, + "timestamp": 1.7418863020601456 + }, + { + "x": 2.9735616677098964, + "y": 5.813004702648798, + "heading": 3.4555164896398725, + "angularVelocity": 4.346388477833355, + "velocityX": 0.3640835989149611, + "velocityY": 1.493068218128355, + "timestamp": 1.7854286837259923 + }, + { + "x": 2.9912888649896243, + "y": 5.893296485362626, + "heading": 3.6575441846858756, + "angularVelocity": 4.639794318014249, + "velocityX": 0.40712511813822955, + "velocityY": 1.8439915237068092, + "timestamp": 1.828971065391839 + }, + { + "x": 3.013279885181438, + "y": 5.986062802451694, + "heading": 3.8282299654345575, + "angularVelocity": 3.9199918382637, + "velocityX": 0.5050486296449646, + "velocityY": 2.1304833024747283, + "timestamp": 1.8725134470576856 + }, + { + "x": 3.038931226373425, + "y": 6.091819637602481, + "heading": 3.9686109305861716, + "angularVelocity": 3.224007502137277, + "velocityX": 0.5891120377576196, + "velocityY": 2.4288252296897244, + "timestamp": 1.9160558287235323 + }, + { + "x": 3.0561859989868845, + "y": 6.209311599123153, + "heading": 4.084927810945441, + "angularVelocity": 2.6713486012756342, + "velocityX": 0.39627535181415346, + "velocityY": 2.6983356680470427, + "timestamp": 1.959598210389379 + }, + { + "x": 3.060479174814897, + "y": 6.319934118129855, + "heading": 4.222181718483572, + "angularVelocity": 3.1521910903139445, + "velocityX": 0.09859763439122565, + "velocityY": 2.540571157904074, + "timestamp": 2.003140592055226 + }, + { + "x": 3.0516520414546364, + "y": 6.423402292036398, + "heading": 4.3775834596079815, + "angularVelocity": 3.5689765965719165, + "velocityX": -0.202725092715463, + "velocityY": 2.376263537915287, + "timestamp": 2.0466829737210728 + }, + { + "x": 3.0294751087244585, + "y": 6.519092659204333, + "heading": 4.544867397627524, + "angularVelocity": 3.8418646757385675, + "velocityX": -0.5093183211788506, + "velocityY": 2.1976374168570505, + "timestamp": 2.0902253553869197 + }, + { + "x": 3.0006004811835547, + "y": 6.602636985626806, + "heading": 4.684421864644313, + "angularVelocity": 3.205026038487255, + "velocityX": -0.6631384512334111, + "velocityY": 1.918689865511037, + "timestamp": 2.1337677370527666 + }, + { + "x": 2.969939574786974, + "y": 6.676232256444707, + "heading": 4.772267122240108, + "angularVelocity": 2.0174656101712003, + "velocityX": -0.7041623637374467, + "velocityY": 1.690198560627341, + "timestamp": 2.1773101187186135 + }, + { + "x": 2.9384435097066675, + "y": 6.741492258657738, + "heading": 4.799993644312657, + "angularVelocity": 0.6367709117367123, + "velocityX": -0.7233427266798164, + "velocityY": 1.4987696978509293, + "timestamp": 2.2208525003844604 + }, + { + "x": 2.906165599822998, + "y": 6.798983573913574, + "heading": 4.76497199558932, + "angularVelocity": -0.8043117391257965, + "velocityX": -0.7412986761123197, + "velocityY": 1.3203530228786522, + "timestamp": 2.2643948820503073 + }, + { + "x": 2.8841461143685847, + "y": 6.834127530524869, + "heading": 4.712816195825582, + "angularVelocity": -1.7885707636427255, + "velocityX": -0.7551108044095549, + "velocityY": 1.205186261133561, + "timestamp": 2.2935554838134236 + }, + { + "x": 2.861839419847924, + "y": 6.865714456702858, + "heading": 4.63253644617705, + "angularVelocity": -2.753021021331392, + "velocityX": -0.7649600204367242, + "velocityY": 1.0832055673810859, + "timestamp": 2.32271608557654 + }, + { + "x": 2.8394465658695167, + "y": 6.8935981434744615, + "heading": 4.5249783872818, + "angularVelocity": -3.6884718555874985, + "velocityX": -0.7679146733772294, + "velocityY": 0.9562109519588466, + "timestamp": 2.351876687339656 + }, + { + "x": 2.820938490444669, + "y": 6.917249808372328, + "heading": 4.402250710323785, + "angularVelocity": -4.208681218411876, + "velocityX": -0.6346945640970328, + "velocityY": 0.8110828812793317, + "timestamp": 2.3810372891027725 + }, + { + "x": 2.8074623766708267, + "y": 6.937003163491629, + "heading": 4.267613712646861, + "angularVelocity": -4.617085709363492, + "velocityX": -0.46213428252662064, + "velocityY": 0.67739874779557, + "timestamp": 2.4101978908658888 + }, + { + "x": 2.800319635523422, + "y": 6.953877630904262, + "heading": 4.128017954879309, + "angularVelocity": -4.787135701161031, + "velocityX": -0.24494491593240095, + "velocityY": 0.5786734975399592, + "timestamp": 2.439358492629005 + }, + { + "x": 2.8000620488247647, + "y": 6.969065578302858, + "heading": 3.987893358621342, + "angularVelocity": -4.805271077608657, + "velocityX": -0.008833380763189107, + "velocityY": 0.5208379278992338, + "timestamp": 2.4685190943921214 + }, + { + "x": 2.806819200515747, + "y": 6.983483791351318, + "heading": 3.850218629262867, + "angularVelocity": -4.721258171448737, + "velocityX": 0.23172195642166132, + "velocityY": 0.49444154704302007, + "timestamp": 2.4976796961552377 + }, + { + "x": 2.8787618126815926, + "y": 7.022620957055028, + "heading": 3.50526697162818, + "angularVelocity": -4.279157773416301, + "velocityX": 0.8924548738228634, + "velocityY": 0.48550022341933585, + "timestamp": 2.5782917408378743 + }, + { + "x": 3.0036413022355246, + "y": 7.060885439295975, + "heading": 3.2022892030921404, + "angularVelocity": -3.7584677293429207, + "velocityX": 1.5491418192600497, + "velocityY": 0.4746745029429695, + "timestamp": 2.658903785520511 + }, + { + "x": 3.1809084432206887, + "y": 7.097904157350605, + "heading": 2.949774131290394, + "angularVelocity": -3.132473227740082, + "velocityX": 2.1990155650194754, + "velocityY": 0.45922068098346275, + "timestamp": 2.7395158302031475 + }, + { + "x": 3.409173750773556, + "y": 7.1327227997437035, + "heading": 2.7639734419701147, + "angularVelocity": -2.3048750351360234, + "velocityX": 2.831652620294277, + "velocityY": 0.4319285353718156, + "timestamp": 2.820127874885784 + }, + { + "x": 3.6828401651873204, + "y": 7.161792378430392, + "heading": 2.689447557134054, + "angularVelocity": -0.9245006143866398, + "velocityX": 3.394857623214588, + "velocityY": 0.36061085909845547, + "timestamp": 2.9007399195684207 + }, + { + "x": 3.98778430036543, + "y": 7.159811819402102, + "heading": 2.689447566809108, + "angularVelocity": 1.2001995625948416e-7, + "velocityX": 3.78286069257579, + "velocityY": -0.024569021119451855, + "timestamp": 2.9813519642510573 + }, + { + "x": 4.292728427914202, + "y": 7.1578300860217094, + "heading": 2.689447576484348, + "angularVelocity": 1.2002226411898335e-7, + "velocityX": 3.7828605979331416, + "velocityY": -0.024583589067793413, + "timestamp": 3.061964008933694 + }, + { + "x": 4.597672555462928, + "y": 7.155848352634247, + "heading": 2.6894475861595883, + "angularVelocity": 1.2002226360503321e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.024583589155504765, + "timestamp": 3.1425760536163305 + }, + { + "x": 4.902616683011654, + "y": 7.1538666192467835, + "heading": 2.6894475958348285, + "angularVelocity": 1.200222635014855e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.02458358915550535, + "timestamp": 3.223188098298967 + }, + { + "x": 5.207560810560381, + "y": 7.151884885859321, + "heading": 2.689447605510069, + "angularVelocity": 1.2002226399006615e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.024583589155505726, + "timestamp": 3.3038001429816037 + }, + { + "x": 5.512504938109107, + "y": 7.1499031524718575, + "heading": 2.6894476151853093, + "angularVelocity": 1.2002226414752478e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.02458358915550553, + "timestamp": 3.3844121876642403 + }, + { + "x": 5.817449065657833, + "y": 7.147921419084395, + "heading": 2.6894476248605494, + "angularVelocity": 1.2002226378664505e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.024583589155505473, + "timestamp": 3.465024232346877 + }, + { + "x": 6.122393193206559, + "y": 7.1459396856969315, + "heading": 2.6894476345357896, + "angularVelocity": 1.2002226381671276e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.02458358915550571, + "timestamp": 3.5456362770295136 + }, + { + "x": 6.427337320755285, + "y": 7.143957952309468, + "heading": 2.68944764421103, + "angularVelocity": 1.2002226400357947e-7, + "velocityX": 3.782860597932571, + "velocityY": -0.024583589155503634, + "timestamp": 3.62624832171215 + }, + { + "x": 6.732281448304175, + "y": 7.141976218947104, + "heading": 2.6894476538862704, + "angularVelocity": 1.2002226442354249e-7, + "velocityX": 3.7828605979345946, + "velocityY": -0.02458358884415627, + "timestamp": 3.7068603663947868 + }, + { + "x": 7.037225602914722, + "y": 7.13999865415615, + "heading": 2.689447663561551, + "angularVelocity": 1.2002276876636888e-7, + "velocityX": 3.78286093363701, + "velocityY": -0.024531877323536525, + "timestamp": 3.7874724110774234 + }, + { + "x": 7.337445623088021, + "y": 7.1893453075077804, + "heading": 2.6911915610396586, + "angularVelocity": 0.021633212319241236, + "velocityX": 3.7242576013949478, + "velocityY": 0.6121498784195986, + "timestamp": 3.86808445576006 + }, + { + "x": 7.586806915545534, + "y": 7.242263721634424, + "heading": 2.760497046001778, + "angularVelocity": 0.8597410627030903, + "velocityX": 3.0933502981995935, + "velocityY": 0.6564579069415494, + "timestamp": 3.9486965004426966 + }, + { + "x": 7.782285469357728, + "y": 7.296718330297362, + "heading": 2.849465104670445, + "angularVelocity": 1.1036571398099098, + "velocityX": 2.4249298548595077, + "velocityY": 0.6755145447224652, + "timestamp": 4.029308545125334 + }, + { + "x": 7.923630857992861, + "y": 7.352282841018828, + "heading": 2.9457862165386364, + "angularVelocity": 1.1948724566831164, + "velocityX": 1.7534028468276504, + "velocityY": 0.6892829842019053, + "timestamp": 4.10992058980797 + }, + { + "x": 8.010802097992581, + "y": 7.4087569649443, + "heading": 3.043737345702012, + "angularVelocity": 1.2150929746169037, + "velocityX": 1.0813674351383629, + "velocityY": 0.7005668215935522, + "timestamp": 4.190532634490607 + }, + { + "x": 8.043797492980957, + "y": 7.466023921966553, + "heading": 3.14, + "angularVelocity": 1.1941472850238053, + "velocityX": 0.40931097974595354, + "velocityY": 0.7104019907658747, + "timestamp": 4.2711446791732435 + }, + { + "x": 8.020274058203087, + "y": 7.5258000313901805, + "heading": 3.235103120292391, + "angularVelocity": 1.1448120726893416, + "velocityX": -0.2831653897583131, + "velocityY": 0.71956011029055, + "timestamp": 4.354217807559267 + }, + { + "x": 7.939231949152505, + "y": 7.586227133184176, + "heading": 3.3247503092396378, + "angularVelocity": 1.0791358251331902, + "velocityX": -0.97555142830298, + "velocityY": 0.7273964875043922, + "timestamp": 4.43729093594529 + }, + { + "x": 7.800688081182825, + "y": 7.647135861779641, + "heading": 3.406847460215054, + "angularVelocity": 0.9882515871308956, + "velocityX": -1.6677338468089835, + "velocityY": 0.7331941119688657, + "timestamp": 4.520364064331313 + }, + { + "x": 7.604687466154285, + "y": 7.7082307652906845, + "heading": 3.4777415630125312, + "angularVelocity": 0.8533939214140001, + "velocityX": -2.3593744311369353, + "velocityY": 0.7354352086892426, + "timestamp": 4.603437192717337 + }, + { + "x": 7.3513972375822965, + "y": 7.768865175468256, + "heading": 3.529440478672651, + "angularVelocity": 0.622330188648799, + "velocityX": -3.0490031312532504, + "velocityY": 0.7298919801818097, + "timestamp": 4.68651032110336 + }, + { + "x": 7.042446476619965, + "y": 7.826390272767311, + "heading": 3.5294407359210145, + "angularVelocity": 0.000003096649523888497, + "velocityX": -3.7190216254611523, + "velocityY": 0.6924633562822848, + "timestamp": 4.769583449489383 + }, + { + "x": 6.7282629808547805, + "y": 7.833355832469298, + "heading": 3.529440737430345, + "angularVelocity": 1.816869994863009e-8, + "velocityX": -3.7820111252490736, + "velocityY": 0.08384853005198241, + "timestamp": 4.852656577875407 + }, + { + "x": 6.416545562348172, + "y": 7.793455476408919, + "heading": 3.5294407390176192, + "angularVelocity": 1.910695113703758e-8, + "velocityX": -3.7523255060062675, + "velocityY": -0.4803040024563794, + "timestamp": 4.93572970626143 + }, + { + "x": 6.117328879898073, + "y": 7.697386963559072, + "heading": 3.529440736387819, + "angularVelocity": -3.165645417876012e-8, + "velocityX": -3.6018468097132645, + "velocityY": -1.1564330694690592, + "timestamp": 5.018802834647453 + }, + { + "x": 5.848961075575228, + "y": 7.552831079331328, + "heading": 3.519045211837445, + "angularVelocity": -0.12513702989572795, + "velocityX": -3.2305007592322372, + "velocityY": -1.7401040148148046, + "timestamp": 5.101875963033477 + }, + { + "x": 5.634955917705784, + "y": 7.389476234986078, + "heading": 3.5030562857096847, + "angularVelocity": -0.19246808731534845, + "velocityX": -2.576105679745284, + "velocityY": -1.9663981304059515, + "timestamp": 5.1849490914195 + }, + { + "x": 5.477332577410991, + "y": 7.214683999741706, + "heading": 3.4827939160779, + "angularVelocity": -0.24391003475431908, + "velocityX": -1.8974046524689734, + "velocityY": -2.104076716987816, + "timestamp": 5.268022219805523 + }, + { + "x": 5.376626668066582, + "y": 7.031496552279893, + "heading": 3.458815082623469, + "angularVelocity": -0.28864729089058233, + "velocityX": -1.2122561326503705, + "velocityY": -2.2051348133969344, + "timestamp": 5.351095348191547 + }, + { + "x": 5.3330583572387695, + "y": 6.8415608406066895, + "heading": 3.431423980439692, + "angularVelocity": -0.32972277216401125, + "velocityX": -0.5244573266262534, + "velocityY": -2.2863676301031086, + "timestamp": 5.43416847657757 + }, + { + "x": 5.34693872876846, + "y": 6.645346154308901, + "heading": 3.400717544814328, + "angularVelocity": -0.36860502765209313, + "velocityX": 0.16662222844571778, + "velocityY": -2.355392880859189, + "timestamp": 5.517472917162537 + }, + { + "x": 5.418495574903921, + "y": 6.444510062585301, + "heading": 3.3681952548408147, + "angularVelocity": -0.3904028374134781, + "velocityX": 0.858979973132117, + "velocityY": -2.4108689802526824, + "timestamp": 5.6007773577475035 + }, + { + "x": 5.547849243844485, + "y": 6.240899298718153, + "heading": 3.336181210288504, + "angularVelocity": -0.3843017770422251, + "velocityX": 1.552782397099573, + "velocityY": -2.4441765941573466, + "timestamp": 5.68408179833247 + }, + { + "x": 5.7350445619956005, + "y": 6.038062186003621, + "heading": 3.3091328137988687, + "angularVelocity": -0.32469333326891314, + "velocityX": 2.2471229244998536, + "velocityY": -2.434889560390793, + "timestamp": 5.767386238917437 + }, + { + "x": 5.979026091079565, + "y": 5.8453030071489485, + "heading": 3.298853791542748, + "angularVelocity": -0.12339104835157319, + "velocityX": 2.928793799834883, + "velocityY": -2.313912409724021, + "timestamp": 5.850690679502404 + }, + { + "x": 6.257928216078006, + "y": 5.698591940467709, + "heading": 3.298853780719763, + "angularVelocity": -1.299208718165422e-7, + "velocityX": 3.3479862902863196, + "velocityY": -1.7611434114559776, + "timestamp": 5.933995120087371 + }, + { + "x": 6.558953297615358, + "y": 5.605347982037956, + "heading": 3.2988537749203877, + "angularVelocity": -6.96166383865064e-8, + "velocityX": 3.61355384447146, + "velocityY": -1.1193155823986038, + "timestamp": 6.017299560672337 + }, + { + "x": 6.868955008416185, + "y": 5.548695751369894, + "heading": 3.298853769327315, + "angularVelocity": -6.714014967276945e-8, + "velocityX": 3.721310756353238, + "velocityY": -0.680062554531874, + "timestamp": 6.100604001257304 + }, + { + "x": 7.184039775897806, + "y": 5.543028549631772, + "heading": 3.2988537480257967, + "angularVelocity": -2.5570687177244914e-7, + "velocityX": 3.782328592198499, + "velocityY": -0.06803000774420709, + "timestamp": 6.183908441842271 + }, + { + "x": 7.453135749501623, + "y": 5.570966234514223, + "heading": 3.2626249853587836, + "angularVelocity": -0.43489593607031335, + "velocityX": 3.2302716603606547, + "velocityY": 0.3353684951999138, + "timestamp": 6.267212882427238 + }, + { + "x": 7.666594614732487, + "y": 5.614797465909912, + "heading": 3.2252844849339093, + "angularVelocity": -0.4482414162158435, + "velocityX": 2.5623947983078446, + "velocityY": 0.5261572022800323, + "timestamp": 6.3505173230122045 + }, + { + "x": 7.8237895341689345, + "y": 5.672106049462931, + "heading": 3.191367480553881, + "angularVelocity": -0.40714521509131607, + "velocityX": 1.8869932783008683, + "velocityY": 0.6879415208912717, + "timestamp": 6.433821763597171 + }, + { + "x": 7.924513457831174, + "y": 5.741949093717095, + "heading": 3.1626288175157167, + "angularVelocity": -0.34498356673858915, + "velocityX": 1.2091062967946495, + "velocityY": 0.838407217715205, + "timestamp": 6.517126204182138 + }, + { + "x": 7.968664646148682, + "y": 5.823824882507324, + "heading": 3.14, + "angularVelocity": -0.2716399912995765, + "velocityX": 0.5299980170021705, + "velocityY": 0.9828502324161114, + "timestamp": 6.600430644767105 + }, + { + "x": 7.953629359811841, + "y": 5.921217077492859, + "heading": 3.1237350830688406, + "angularVelocity": -0.18848196569172904, + "velocityX": -0.17423269577704348, + "velocityY": 1.1286053554161422, + "timestamp": 6.6867249322415985 + }, + { + "x": 7.877765712492472, + "y": 6.030911295018945, + "heading": 3.1145546861487188, + "angularVelocity": -0.10638475835188216, + "velocityX": -0.8791271072467286, + "velocityY": 1.271164299937107, + "timestamp": 6.773019219716092 + }, + { + "x": 7.740982038726218, + "y": 6.152452285533978, + "heading": 3.112307173835679, + "angularVelocity": -0.02604474037408179, + "velocityX": -1.5850837612708069, + "velocityY": 1.4084476976643152, + "timestamp": 6.859313507190586 + }, + { + "x": 7.543108693142391, + "y": 6.2849461115263425, + "heading": 3.1166940705371036, + "angularVelocity": 0.050836467045642465, + "velocityX": -2.293006308700471, + "velocityY": 1.5353719217106456, + "timestamp": 6.945607794665079 + }, + { + "x": 7.283736061719511, + "y": 6.425841206700315, + "heading": 3.126860221171615, + "angularVelocity": 0.11780792138200498, + "velocityX": -3.00567556687392, + "velocityY": 1.6327279510317223, + "timestamp": 7.031902082139573 + }, + { + "x": 6.974363478123194, + "y": 6.530031875169815, + "heading": 3.126860227088612, + "angularVelocity": 6.856765823922045e-8, + "velocityX": -3.5850876419572795, + "velocityY": 1.2073877833488817, + "timestamp": 7.118196369614067 + }, + { + "x": 6.654515721034659, + "y": 6.595335054105491, + "heading": 3.1268602314216754, + "angularVelocity": 5.0212631563865974e-8, + "velocityX": -3.7064765982693104, + "velocityY": 0.7567497321879761, + "timestamp": 7.20449065708856 + }, + { + "x": 6.328089013605824, + "y": 6.598897905700907, + "heading": 3.126860245581997, + "angularVelocity": 1.6409338766070953e-7, + "velocityX": -3.782715136564709, + "velocityY": 0.04128722421480937, + "timestamp": 7.290784944563054 + }, + { + "x": 6.025018317514451, + "y": 6.546337315021749, + "heading": 3.1724115352008173, + "angularVelocity": 0.527859849729727, + "velocityX": -3.512059777780222, + "velocityY": -0.6090854008695971, + "timestamp": 7.377079232037548 + }, + { + "x": 5.779136888025266, + "y": 6.469611949486163, + "heading": 3.2211889094820707, + "angularVelocity": 0.5652445336624501, + "velocityX": -2.8493361111749387, + "velocityY": -0.8891129155943783, + "timestamp": 7.463373519512041 + }, + { + "x": 5.5922330688018125, + "y": 6.373475998792455, + "heading": 3.2682230815265036, + "angularVelocity": 0.5450438658333501, + "velocityX": -2.165888666485556, + "velocityY": -1.1140476792525056, + "timestamp": 7.549667806986535 + }, + { + "x": 5.464860063299637, + "y": 6.259720552678007, + "heading": 3.3116817508923533, + "angularVelocity": 0.5036100376713217, + "velocityX": -1.476030560421781, + "velocityY": -1.3182268426293073, + "timestamp": 7.6359620944610285 + }, + { + "x": 5.397282123565674, + "y": 6.129282474517822, + "heading": 3.350614829351744, + "angularVelocity": 0.4511663471454952, + "velocityX": -0.7831102348917176, + "velocityY": -1.5115493965778382, + "timestamp": 7.722256381935522 + }, + { + "x": 5.389062072099223, + "y": 5.984597411179855, + "heading": 3.3841120445321926, + "angularVelocity": 0.3926696956274206, + "velocityX": -0.09635920747397948, + "velocityY": -1.6960645676573736, + "timestamp": 7.807562723508242 + }, + { + "x": 5.439577737943889, + "y": 5.8247498327009, + "heading": 3.4125162997310934, + "angularVelocity": 0.33296768651938463, + "velocityX": 0.5921677675229385, + "velocityY": -1.873806513466449, + "timestamp": 7.892869065080961 + }, + { + "x": 5.549063488192787, + "y": 5.650683264733526, + "heading": 3.43566101949846, + "angularVelocity": 0.2713130037071968, + "velocityX": 1.2834421009083707, + "velocityY": -2.0404880195101427, + "timestamp": 7.978175406653681 + }, + { + "x": 5.717925305048545, + "y": 5.464212491314424, + "heading": 3.4532309731984996, + "angularVelocity": 0.20596304302960114, + "velocityX": 1.9794755435832592, + "velocityY": -2.185895796036977, + "timestamp": 8.0634817482264 + }, + { + "x": 5.946971580653903, + "y": 5.270228748236446, + "heading": 3.464410638066347, + "angularVelocity": 0.13105315105228252, + "velocityX": 2.684985329140002, + "velocityY": -2.2739662667706324, + "timestamp": 8.148788089799119 + }, + { + "x": 6.226744570884399, + "y": 5.1093932756817635, + "heading": 3.464410636874865, + "angularVelocity": -1.3967096544667774e-8, + "velocityX": 3.279627107112591, + "velocityY": -1.8853870602055813, + "timestamp": 8.234094431371838 + }, + { + "x": 6.531679081607643, + "y": 5.003771669462237, + "heading": 3.464410634209207, + "angularVelocity": -3.124806173053443e-8, + "velocityX": 3.5745819724703742, + "velocityY": -1.2381448350998507, + "timestamp": 8.319400772944558 + }, + { + "x": 6.839623513036132, + "y": 4.907277074993315, + "heading": 3.4644106315608414, + "angularVelocity": -3.104535902985177e-8, + "velocityX": 3.609865641301503, + "velocityY": -1.1311538238533516, + "timestamp": 8.404707114517278 + }, + { + "x": 7.147567913336122, + "y": 4.81078238118367, + "heading": 3.464410628912475, + "angularVelocity": -3.104536108601854e-8, + "velocityX": 3.6098652763989647, + "velocityY": -1.1311549883707996, + "timestamp": 8.490013456089997 + }, + { + "x": 7.447305692554561, + "y": 4.691207128277643, + "heading": 3.4644106212813455, + "angularVelocity": -8.945560032337055e-8, + "velocityX": 3.5136635060469357, + "velocityY": -1.4017158713117939, + "timestamp": 8.575319797662717 + }, + { + "x": 7.693247872040493, + "y": 4.5498334622955285, + "heading": 3.399037241317186, + "angularVelocity": -0.7663366961813945, + "velocityX": 2.883046851520149, + "velocityY": -1.657246851473523, + "timestamp": 8.660626139235436 + }, + { + "x": 7.88024564060197, + "y": 4.422851480577848, + "heading": 3.3318555601258444, + "angularVelocity": -0.7875344312365365, + "velocityX": 2.1920734744212487, + "velocityY": -1.4885409381837746, + "timestamp": 8.745932480808156 + }, + { + "x": 8.008768514882867, + "y": 4.3120678627120395, + "heading": 3.265924152728589, + "angularVelocity": -0.772878149288027, + "velocityX": 1.5066039864262268, + "velocityY": -1.2986562994425228, + "timestamp": 8.831238822380875 + }, + { + "x": 8.07899777943964, + "y": 4.218114377697436, + "heading": 3.202319904856858, + "angularVelocity": -0.7455981196604378, + "velocityX": 0.8232595990170849, + "velocityY": -1.101365775186992, + "timestamp": 8.916545163953595 + }, + { + "x": 8.091029167175293, + "y": 4.141312599182129, + "heading": 3.141592653589793, + "angularVelocity": -0.7118726480058678, + "velocityX": 0.14103743653568068, + "velocityY": -0.900305617371216, + "timestamp": 9.001851505526314 + }, + { + "x": 8.040216714979586, + "y": 4.080035987534699, + "heading": 3.0816900573414494, + "angularVelocity": -0.6725771689853446, + "velocityX": -0.5705144248724638, + "velocityY": -0.6880044032813033, + "timestamp": 9.090915778015244 + }, + { + "x": 7.92607322423097, + "y": 4.037808955209039, + "heading": 3.025407419661199, + "angularVelocity": -0.6319328290392299, + "velocityX": -1.281585618552088, + "velocityY": -0.4741186465190991, + "timestamp": 9.179980050504174 + }, + { + "x": 7.748670432062219, + "y": 4.014865360628201, + "heading": 2.9729438660433796, + "angularVelocity": -0.5890527385640573, + "velocityX": -1.9918513587005524, + "velocityY": -0.2576071632279864, + "timestamp": 9.269044322993103 + }, + { + "x": 7.508153207350264, + "y": 4.011667878946095, + "heading": 2.9246938540207905, + "angularVelocity": -0.5417437393718784, + "velocityX": -2.7004905333039093, + "velocityY": -0.03590083422625269, + "timestamp": 9.358108595482033 + }, + { + "x": 7.204966238284119, + "y": 4.029564783423129, + "heading": 2.881811620328617, + "angularVelocity": -0.4814751470350101, + "velocityX": -3.404136817081497, + "velocityY": 0.2009437002840627, + "timestamp": 9.447172867970963 + }, + { + "x": 6.877222132273021, + "y": 4.107681050730088, + "heading": 2.881811620606281, + "angularVelocity": 3.1175712181288514e-9, + "velocityX": -3.6798605866548297, + "velocityY": 0.8770774758943696, + "timestamp": 9.536237140459892 + }, + { + "x": 6.553507681979354, + "y": 4.201101101313888, + "heading": 2.8818116222683225, + "angularVelocity": 1.8661142988084573e-8, + "velocityX": -3.6346162299130933, + "velocityY": 1.0489060088085451, + "timestamp": 9.625301412948822 + }, + { + "x": 6.229793253222719, + "y": 4.2945212265266814, + "heading": 2.8818116239303677, + "angularVelocity": 1.866118842609661e-8, + "velocityX": -3.6346159880986018, + "velocityY": 1.0489068467314409, + "timestamp": 9.714365685437752 + }, + { + "x": 5.912076930566037, + "y": 4.386210365902842, + "heading": 2.8968730092713613, + "angularVelocity": 0.1691069260445136, + "velocityX": -3.567270172180123, + "velocityY": 1.0294716030780673, + "timestamp": 9.803429957926681 + }, + { + "x": 5.657903856180183, + "y": 4.459561682271931, + "heading": 2.908923259524863, + "angularVelocity": 0.13529836281994964, + "velocityX": -2.853816320314597, + "velocityY": 0.8235773371214163, + "timestamp": 9.892494230415611 + }, + { + "x": 5.467274045026463, + "y": 4.514575171136671, + "heading": 2.9179613240631452, + "angularVelocity": 0.1014780033082899, + "velocityX": -2.140362300465804, + "velocityY": 0.6176830206700082, + "timestamp": 9.98155850290454 + }, + { + "x": 5.340187502097666, + "y": 4.551250831022913, + "heading": 2.9239868528608652, + "angularVelocity": 0.06765371376574367, + "velocityX": -1.4269082245587752, + "velocityY": 0.41178868766711113, + "timestamp": 10.07062277539347 + }, + { + "x": 5.276644229888916, + "y": 4.569588661193848, + "heading": 2.926999670595623, + "angularVelocity": 0.033827455730154186, + "velocityX": -0.7134541206368397, + "velocityY": 0.2058943463914144, + "timestamp": 10.1596870478824 + }, + { + "x": 5.276644229888916, + "y": 4.569588661193848, + "heading": 2.926999670595623, + "angularVelocity": -1.4307348024093064e-32, + "velocityX": -8.4908818209571e-35, + "velocityY": -2.480999535551194e-34, + "timestamp": 10.24875132037133 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Choreo Test.traj b/src/main/deploy/choreo/Choreo Test.traj new file mode 100644 index 00000000..2437192e --- /dev/null +++ b/src/main/deploy/choreo/Choreo Test.traj @@ -0,0 +1,1390 @@ +{ + "samples": [ + { + "x": 1.4585455656051636, + "y": 4.074051856994629, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": 2.0171394626419585e-36, + "velocityY": -3.1684110491546193e-35, + "timestamp": 0 + }, + { + "x": 1.4905205741903755, + "y": 4.0671846353687675, + "heading": 3.1068407756328744, + "angularVelocity": -0.5198162463824533, + "velocityX": 0.501252042472078, + "velocityY": -0.1076531021687804, + "timestamp": 0.06379028088846767 + }, + { + "x": 1.5549156419284027, + "y": 4.054610420448303, + "heading": 3.0435644184480135, + "angularVelocity": -0.9919435422379561, + "velocityX": 1.0094808619923894, + "velocityY": -0.19711803656186136, + "timestamp": 0.12758056177693533 + }, + { + "x": 1.6523135702857736, + "y": 4.038202658793452, + "heading": 2.9552733306856536, + "angularVelocity": -1.3840836963350238, + "velocityX": 1.526845892521838, + "velocityY": -0.25721413084133044, + "timestamp": 0.191370842665403 + }, + { + "x": 1.783322512462502, + "y": 4.02131537659787, + "heading": 2.8504247025188674, + "angularVelocity": -1.6436458141657342, + "velocityX": 2.053744557196534, + "velocityY": -0.26473127191755363, + "timestamp": 0.25516112355387066 + }, + { + "x": 1.9476819873344473, + "y": 4.010657961922336, + "heading": 2.7442259551324777, + "angularVelocity": -1.664810781630976, + "velocityX": 2.576559823577434, + "velocityY": -0.16706956807680673, + "timestamp": 0.3189514044423383 + }, + { + "x": 2.1378010179584184, + "y": 4.0200538425068055, + "heading": 2.67135854139101, + "angularVelocity": -1.1422964866524252, + "velocityX": 2.9803761321631335, + "velocityY": 0.14729329379969647, + "timestamp": 0.382741685330806 + }, + { + "x": 2.3327531815626203, + "y": 4.054874437218412, + "heading": 2.6867523157379845, + "angularVelocity": 0.2413184913527656, + "velocityX": 3.0561421095646404, + "velocityY": 0.5458605014216376, + "timestamp": 0.44653196621927366 + }, + { + "x": 2.510732792290265, + "y": 4.113199213721838, + "heading": 2.7651162027533043, + "angularVelocity": 1.2284612314583365, + "velocityX": 2.79007410296292, + "velocityY": 0.9143207349314517, + "timestamp": 0.5103222471077413 + }, + { + "x": 2.666890859603882, + "y": 4.191041946411133, + "heading": 2.906047416060082, + "angularVelocity": 2.209289743576844, + "velocityX": 2.4479915300364854, + "velocityY": 1.220291423789085, + "timestamp": 0.5741125279962089 + }, + { + "x": 2.756813611394881, + "y": 4.2481449117517185, + "heading": 3.019078744416041, + "angularVelocity": 2.7742492473081195, + "velocityX": 2.207070642277403, + "velocityY": 1.401539386640599, + "timestamp": 0.6148555608962393 + }, + { + "x": 2.8357643538183357, + "y": 4.312490934812126, + "heading": 3.1475106055134408, + "angularVelocity": 3.152241057079095, + "velocityX": 1.937772836331876, + "velocityY": 1.5793135287274918, + "timestamp": 0.6555985937962696 + }, + { + "x": 2.9029140041003427, + "y": 4.3834707929163566, + "heading": 3.286187250098307, + "angularVelocity": 3.403689777467768, + "velocityX": 1.648125961726257, + "velocityY": 1.7421348645887855, + "timestamp": 0.6963416266963 + }, + { + "x": 2.957867862311061, + "y": 4.460664789436284, + "heading": 3.4321788864396923, + "angularVelocity": 3.583229473848938, + "velocityX": 1.3487915429751285, + "velocityY": 1.8946551355009582, + "timestamp": 0.7370846595963303 + }, + { + "x": 3.0002910660148934, + "y": 4.543635590242566, + "heading": 3.582061930766736, + "angularVelocity": 3.6787404780298614, + "velocityX": 1.0412382359439127, + "velocityY": 2.0364414453353255, + "timestamp": 0.7778276924963606 + }, + { + "x": 3.0299626612502326, + "y": 4.631989358642389, + "heading": 3.7319206979896067, + "angularVelocity": 3.6781446189971536, + "velocityX": 0.7282618186069594, + "velocityY": 2.168561398377315, + "timestamp": 0.818570725396391 + }, + { + "x": 3.0468078340815077, + "y": 4.7254089445273975, + "heading": 3.877554919455978, + "angularVelocity": 3.5744570568349348, + "velocityX": 0.4134491625257187, + "velocityY": 2.2928971958034863, + "timestamp": 0.8593137582964213 + }, + { + "x": 3.0508603788207704, + "y": 4.823595123995901, + "heading": 4.014992213293226, + "angularVelocity": 3.373271061446823, + "velocityX": 0.09946595652823349, + "velocityY": 2.4098888197503454, + "timestamp": 0.9000567911964517 + }, + { + "x": 3.0418255519972037, + "y": 4.925400743620523, + "heading": 4.142750839635198, + "angularVelocity": 3.1357171336618257, + "velocityX": -0.22175145492322643, + "velocityY": 2.4987246254941016, + "timestamp": 0.940799824096482 + }, + { + "x": 3.021206090160081, + "y": 5.020464579226095, + "heading": 4.282785145406025, + "angularVelocity": 3.4370123136002837, + "velocityX": -0.5060855898409946, + "velocityY": 2.3332537820349804, + "timestamp": 0.9815428569965123 + }, + { + "x": 2.9996081410347006, + "y": 5.103938301937717, + "heading": 4.452456839415369, + "angularVelocity": 4.164434553158123, + "velocityX": -0.5301016539042277, + "velocityY": 2.048785197617431, + "timestamp": 1.0222858898965426 + }, + { + "x": 2.9785438954530656, + "y": 5.173710724146211, + "heading": 4.628924606077659, + "angularVelocity": 4.331237860845608, + "velocityX": -0.5170023948221859, + "velocityY": 1.7124994690427882, + "timestamp": 1.0630289227965728 + }, + { + "x": 2.9587317784828078, + "y": 5.234319209722237, + "heading": 4.760996474258561, + "angularVelocity": 3.241581659003185, + "velocityX": -0.4862700579721273, + "velocityY": 1.487579133461626, + "timestamp": 1.103771955696603 + }, + { + "x": 2.940244887270436, + "y": 5.287568324744329, + "heading": 4.840301578000097, + "angularVelocity": 1.9464703066196472, + "velocityX": -0.45374361937493524, + "velocityY": 1.3069502006084648, + "timestamp": 1.1445149885966333 + }, + { + "x": 2.9231014391412486, + "y": 5.334086773278073, + "heading": 4.864342694978022, + "angularVelocity": 0.590066945603065, + "velocityX": -0.42077005340401513, + "velocityY": 1.1417522266416396, + "timestamp": 1.1852580214966635 + }, + { + "x": 2.9071312956135524, + "y": 5.374187557857939, + "heading": 4.831856104171122, + "angularVelocity": -0.7973532772244076, + "velocityX": -0.39197237885755587, + "velocityY": 0.9842366099318068, + "timestamp": 1.2260010543966937 + }, + { + "x": 2.8919732570648193, + "y": 5.408133029937744, + "heading": 4.741792239549658, + "angularVelocity": -2.210534125980521, + "velocityX": -0.3720400144467828, + "velocityY": 0.8331601666252006, + "timestamp": 1.266744087296724 + }, + { + "x": 2.882026462257207, + "y": 5.428283750424018, + "heading": 4.654897403574336, + "angularVelocity": -3.1758469572855184, + "velocityX": -0.3635371155251437, + "velocityY": 0.7364718930089131, + "timestamp": 1.2941052407603635 + }, + { + "x": 2.8722493985664026, + "y": 5.446002750654774, + "heading": 4.541017712722759, + "angularVelocity": -4.162093933755802, + "velocityX": -0.3573337543608044, + "velocityY": 0.6475969755552201, + "timestamp": 1.321466394224003 + }, + { + "x": 2.862908284982934, + "y": 5.461788364835415, + "heading": 4.399049387011055, + "angularVelocity": -5.188682045161824, + "velocityX": -0.34140057713143623, + "velocityY": 0.5769352597513185, + "timestamp": 1.3488275476876426 + }, + { + "x": 2.855341721346824, + "y": 5.475808186129583, + "heading": 4.230496751378239, + "angularVelocity": -6.160289837809847, + "velocityX": -0.27654402970127984, + "velocityY": 0.5123987668429125, + "timestamp": 1.3761887011512821 + }, + { + "x": 2.850499346368405, + "y": 5.487175210010101, + "heading": 4.040136228473314, + "angularVelocity": -6.957328138884789, + "velocityX": -0.17697992830800321, + "velocityY": 0.4154438845432298, + "timestamp": 1.4035498546149217 + }, + { + "x": 2.850511331304342, + "y": 5.495665667507396, + "heading": 3.8398105873897572, + "angularVelocity": -7.321534940029888, + "velocityX": 0.0004380274374277367, + "velocityY": 0.3103106566241056, + "timestamp": 1.4309110080785612 + }, + { + "x": 2.856241467196838, + "y": 5.505496640264063, + "heading": 3.647301887529118, + "angularVelocity": -7.03584006852873, + "velocityX": 0.2094259622537722, + "velocityY": 0.35930403189076365, + "timestamp": 1.4582721615422007 + }, + { + "x": 2.864610492964689, + "y": 5.5169142669590086, + "heading": 3.4794236810579164, + "angularVelocity": -6.1356406883319226, + "velocityX": 0.30587254952437387, + "velocityY": 0.41729332464432606, + "timestamp": 1.4856333150058403 + }, + { + "x": 2.87348931347585, + "y": 5.530255151802143, + "heading": 3.33971041673408, + "angularVelocity": -5.106263685465693, + "velocityX": 0.32450461282488824, + "velocityY": 0.48758488419952456, + "timestamp": 1.5129944684694798 + }, + { + "x": 2.8826084077382985, + "y": 5.545984706383625, + "heading": 3.227220485531993, + "angularVelocity": -4.111300766306338, + "velocityX": 0.333286177959096, + "velocityY": 0.5748863841718318, + "timestamp": 1.5403556219331194 + }, + { + "x": 2.8919732570648193, + "y": 5.564249038696289, + "heading": 3.141592653589793, + "angularVelocity": -3.1295402825758374, + "velocityX": 0.3422680750271005, + "velocityY": 0.6675278634336637, + "timestamp": 1.567716775396759 + }, + { + "x": 2.907609226294196, + "y": 5.599859473084939, + "heading": 3.0730087116624225, + "angularVelocity": -1.5751077296069407, + "velocityX": 0.35909770277085135, + "velocityY": 0.8178338672866402, + "timestamp": 1.6112591570626056 + }, + { + "x": 2.923994290533328, + "y": 5.642177598480751, + "heading": 3.0715517727837245, + "angularVelocity": -0.033460247762260184, + "velocityX": 0.37630151618426566, + "velocityY": 0.9718835712885416, + "timestamp": 1.6548015387284523 + }, + { + "x": 2.9408764364223456, + "y": 5.691439774506376, + "heading": 3.1364313706480664, + "angularVelocity": 1.4900332821075657, + "velocityX": 0.3877175580007244, + "velocityY": 1.1313615411227034, + "timestamp": 1.698343920394299 + }, + { + "x": 2.957708600687666, + "y": 5.747992956441908, + "heading": 3.2662643836700145, + "angularVelocity": 2.981761861772134, + "velocityX": 0.38656967353080685, + "velocityY": 1.2988077310408146, + "timestamp": 1.7418863020601456 + }, + { + "x": 2.9735616677098964, + "y": 5.813004702648798, + "heading": 3.4555164896398725, + "angularVelocity": 4.346388477833355, + "velocityX": 0.3640835989149611, + "velocityY": 1.493068218128355, + "timestamp": 1.7854286837259923 + }, + { + "x": 2.9912888649896243, + "y": 5.893296485362626, + "heading": 3.6575441846858756, + "angularVelocity": 4.639794318014249, + "velocityX": 0.40712511813822955, + "velocityY": 1.8439915237068092, + "timestamp": 1.828971065391839 + }, + { + "x": 3.013279885181438, + "y": 5.986062802451694, + "heading": 3.8282299654345575, + "angularVelocity": 3.9199918382637, + "velocityX": 0.5050486296449646, + "velocityY": 2.1304833024747283, + "timestamp": 1.8725134470576856 + }, + { + "x": 3.038931226373425, + "y": 6.091819637602481, + "heading": 3.9686109305861716, + "angularVelocity": 3.224007502137277, + "velocityX": 0.5891120377576196, + "velocityY": 2.4288252296897244, + "timestamp": 1.9160558287235323 + }, + { + "x": 3.0561859989868845, + "y": 6.209311599123153, + "heading": 4.084927810945441, + "angularVelocity": 2.6713486012756342, + "velocityX": 0.39627535181415346, + "velocityY": 2.6983356680470427, + "timestamp": 1.959598210389379 + }, + { + "x": 3.060479174814897, + "y": 6.319934118129855, + "heading": 4.222181718483572, + "angularVelocity": 3.1521910903139445, + "velocityX": 0.09859763439122565, + "velocityY": 2.540571157904074, + "timestamp": 2.003140592055226 + }, + { + "x": 3.0516520414546364, + "y": 6.423402292036398, + "heading": 4.3775834596079815, + "angularVelocity": 3.5689765965719165, + "velocityX": -0.202725092715463, + "velocityY": 2.376263537915287, + "timestamp": 2.0466829737210728 + }, + { + "x": 3.0294751087244585, + "y": 6.519092659204333, + "heading": 4.544867397627524, + "angularVelocity": 3.8418646757385675, + "velocityX": -0.5093183211788506, + "velocityY": 2.1976374168570505, + "timestamp": 2.0902253553869197 + }, + { + "x": 3.0006004811835547, + "y": 6.602636985626806, + "heading": 4.684421864644313, + "angularVelocity": 3.205026038487255, + "velocityX": -0.6631384512334111, + "velocityY": 1.918689865511037, + "timestamp": 2.1337677370527666 + }, + { + "x": 2.969939574786974, + "y": 6.676232256444707, + "heading": 4.772267122240108, + "angularVelocity": 2.0174656101712003, + "velocityX": -0.7041623637374467, + "velocityY": 1.690198560627341, + "timestamp": 2.1773101187186135 + }, + { + "x": 2.9384435097066675, + "y": 6.741492258657738, + "heading": 4.799993644312657, + "angularVelocity": 0.6367709117367123, + "velocityX": -0.7233427266798164, + "velocityY": 1.4987696978509293, + "timestamp": 2.2208525003844604 + }, + { + "x": 2.906165599822998, + "y": 6.798983573913574, + "heading": 4.76497199558932, + "angularVelocity": -0.8043117391257965, + "velocityX": -0.7412986761123197, + "velocityY": 1.3203530228786522, + "timestamp": 2.2643948820503073 + }, + { + "x": 2.8841461143685847, + "y": 6.834127530524869, + "heading": 4.712816195825582, + "angularVelocity": -1.7885707636427255, + "velocityX": -0.7551108044095549, + "velocityY": 1.205186261133561, + "timestamp": 2.2935554838134236 + }, + { + "x": 2.861839419847924, + "y": 6.865714456702858, + "heading": 4.63253644617705, + "angularVelocity": -2.753021021331392, + "velocityX": -0.7649600204367242, + "velocityY": 1.0832055673810859, + "timestamp": 2.32271608557654 + }, + { + "x": 2.8394465658695167, + "y": 6.8935981434744615, + "heading": 4.5249783872818, + "angularVelocity": -3.6884718555874985, + "velocityX": -0.7679146733772294, + "velocityY": 0.9562109519588466, + "timestamp": 2.351876687339656 + }, + { + "x": 2.820938490444669, + "y": 6.917249808372328, + "heading": 4.402250710323785, + "angularVelocity": -4.208681218411876, + "velocityX": -0.6346945640970328, + "velocityY": 0.8110828812793317, + "timestamp": 2.3810372891027725 + }, + { + "x": 2.8074623766708267, + "y": 6.937003163491629, + "heading": 4.267613712646861, + "angularVelocity": -4.617085709363492, + "velocityX": -0.46213428252662064, + "velocityY": 0.67739874779557, + "timestamp": 2.4101978908658888 + }, + { + "x": 2.800319635523422, + "y": 6.953877630904262, + "heading": 4.128017954879309, + "angularVelocity": -4.787135701161031, + "velocityX": -0.24494491593240095, + "velocityY": 0.5786734975399592, + "timestamp": 2.439358492629005 + }, + { + "x": 2.8000620488247647, + "y": 6.969065578302858, + "heading": 3.987893358621342, + "angularVelocity": -4.805271077608657, + "velocityX": -0.008833380763189107, + "velocityY": 0.5208379278992338, + "timestamp": 2.4685190943921214 + }, + { + "x": 2.806819200515747, + "y": 6.983483791351318, + "heading": 3.850218629262867, + "angularVelocity": -4.721258171448737, + "velocityX": 0.23172195642166132, + "velocityY": 0.49444154704302007, + "timestamp": 2.4976796961552377 + }, + { + "x": 2.8787618126815926, + "y": 7.022620957055028, + "heading": 3.50526697162818, + "angularVelocity": -4.279157773416301, + "velocityX": 0.8924548738228634, + "velocityY": 0.48550022341933585, + "timestamp": 2.5782917408378743 + }, + { + "x": 3.0036413022355246, + "y": 7.060885439295975, + "heading": 3.2022892030921404, + "angularVelocity": -3.7584677293429207, + "velocityX": 1.5491418192600497, + "velocityY": 0.4746745029429695, + "timestamp": 2.658903785520511 + }, + { + "x": 3.1809084432206887, + "y": 7.097904157350605, + "heading": 2.949774131290394, + "angularVelocity": -3.132473227740082, + "velocityX": 2.1990155650194754, + "velocityY": 0.45922068098346275, + "timestamp": 2.7395158302031475 + }, + { + "x": 3.409173750773556, + "y": 7.1327227997437035, + "heading": 2.7639734419701147, + "angularVelocity": -2.3048750351360234, + "velocityX": 2.831652620294277, + "velocityY": 0.4319285353718156, + "timestamp": 2.820127874885784 + }, + { + "x": 3.6828401651873204, + "y": 7.161792378430392, + "heading": 2.689447557134054, + "angularVelocity": -0.9245006143866398, + "velocityX": 3.394857623214588, + "velocityY": 0.36061085909845547, + "timestamp": 2.9007399195684207 + }, + { + "x": 3.98778430036543, + "y": 7.159811819402102, + "heading": 2.689447566809108, + "angularVelocity": 1.2001995625948416e-7, + "velocityX": 3.78286069257579, + "velocityY": -0.024569021119451855, + "timestamp": 2.9813519642510573 + }, + { + "x": 4.292728427914202, + "y": 7.1578300860217094, + "heading": 2.689447576484348, + "angularVelocity": 1.2002226411898335e-7, + "velocityX": 3.7828605979331416, + "velocityY": -0.024583589067793413, + "timestamp": 3.061964008933694 + }, + { + "x": 4.597672555462928, + "y": 7.155848352634247, + "heading": 2.6894475861595883, + "angularVelocity": 1.2002226360503321e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.024583589155504765, + "timestamp": 3.1425760536163305 + }, + { + "x": 4.902616683011654, + "y": 7.1538666192467835, + "heading": 2.6894475958348285, + "angularVelocity": 1.200222635014855e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.02458358915550535, + "timestamp": 3.223188098298967 + }, + { + "x": 5.207560810560381, + "y": 7.151884885859321, + "heading": 2.689447605510069, + "angularVelocity": 1.2002226399006615e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.024583589155505726, + "timestamp": 3.3038001429816037 + }, + { + "x": 5.512504938109107, + "y": 7.1499031524718575, + "heading": 2.6894476151853093, + "angularVelocity": 1.2002226414752478e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.02458358915550553, + "timestamp": 3.3844121876642403 + }, + { + "x": 5.817449065657833, + "y": 7.147921419084395, + "heading": 2.6894476248605494, + "angularVelocity": 1.2002226378664505e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.024583589155505473, + "timestamp": 3.465024232346877 + }, + { + "x": 6.122393193206559, + "y": 7.1459396856969315, + "heading": 2.6894476345357896, + "angularVelocity": 1.2002226381671276e-7, + "velocityX": 3.7828605979325713, + "velocityY": -0.02458358915550571, + "timestamp": 3.5456362770295136 + }, + { + "x": 6.427337320755285, + "y": 7.143957952309468, + "heading": 2.68944764421103, + "angularVelocity": 1.2002226400357947e-7, + "velocityX": 3.782860597932571, + "velocityY": -0.024583589155503634, + "timestamp": 3.62624832171215 + }, + { + "x": 6.732281448304175, + "y": 7.141976218947104, + "heading": 2.6894476538862704, + "angularVelocity": 1.2002226442354249e-7, + "velocityX": 3.7828605979345946, + "velocityY": -0.02458358884415627, + "timestamp": 3.7068603663947868 + }, + { + "x": 7.037225602914722, + "y": 7.13999865415615, + "heading": 2.689447663561551, + "angularVelocity": 1.2002276876636888e-7, + "velocityX": 3.78286093363701, + "velocityY": -0.024531877323536525, + "timestamp": 3.7874724110774234 + }, + { + "x": 7.337445623088021, + "y": 7.1893453075077804, + "heading": 2.6911915610396586, + "angularVelocity": 0.021633212319241236, + "velocityX": 3.7242576013949478, + "velocityY": 0.6121498784195986, + "timestamp": 3.86808445576006 + }, + { + "x": 7.586806915545534, + "y": 7.242263721634424, + "heading": 2.760497046001778, + "angularVelocity": 0.8597410627030903, + "velocityX": 3.0933502981995935, + "velocityY": 0.6564579069415494, + "timestamp": 3.9486965004426966 + }, + { + "x": 7.782285469357728, + "y": 7.296718330297362, + "heading": 2.849465104670445, + "angularVelocity": 1.1036571398099098, + "velocityX": 2.4249298548595077, + "velocityY": 0.6755145447224652, + "timestamp": 4.029308545125334 + }, + { + "x": 7.923630857992861, + "y": 7.352282841018828, + "heading": 2.9457862165386364, + "angularVelocity": 1.1948724566831164, + "velocityX": 1.7534028468276504, + "velocityY": 0.6892829842019053, + "timestamp": 4.10992058980797 + }, + { + "x": 8.010802097992581, + "y": 7.4087569649443, + "heading": 3.043737345702012, + "angularVelocity": 1.2150929746169037, + "velocityX": 1.0813674351383629, + "velocityY": 0.7005668215935522, + "timestamp": 4.190532634490607 + }, + { + "x": 8.043797492980957, + "y": 7.466023921966553, + "heading": 3.14, + "angularVelocity": 1.1941472850238053, + "velocityX": 0.40931097974595354, + "velocityY": 0.7104019907658747, + "timestamp": 4.2711446791732435 + }, + { + "x": 8.020274058203087, + "y": 7.5258000313901805, + "heading": 3.235103120292391, + "angularVelocity": 1.1448120726893416, + "velocityX": -0.2831653897583131, + "velocityY": 0.71956011029055, + "timestamp": 4.354217807559267 + }, + { + "x": 7.939231949152505, + "y": 7.586227133184176, + "heading": 3.3247503092396378, + "angularVelocity": 1.0791358251331902, + "velocityX": -0.97555142830298, + "velocityY": 0.7273964875043922, + "timestamp": 4.43729093594529 + }, + { + "x": 7.800688081182825, + "y": 7.647135861779641, + "heading": 3.406847460215054, + "angularVelocity": 0.9882515871308956, + "velocityX": -1.6677338468089835, + "velocityY": 0.7331941119688657, + "timestamp": 4.520364064331313 + }, + { + "x": 7.604687466154285, + "y": 7.7082307652906845, + "heading": 3.4777415630125312, + "angularVelocity": 0.8533939214140001, + "velocityX": -2.3593744311369353, + "velocityY": 0.7354352086892426, + "timestamp": 4.603437192717337 + }, + { + "x": 7.3513972375822965, + "y": 7.768865175468256, + "heading": 3.529440478672651, + "angularVelocity": 0.622330188648799, + "velocityX": -3.0490031312532504, + "velocityY": 0.7298919801818097, + "timestamp": 4.68651032110336 + }, + { + "x": 7.042446476619965, + "y": 7.826390272767311, + "heading": 3.5294407359210145, + "angularVelocity": 0.000003096649523888497, + "velocityX": -3.7190216254611523, + "velocityY": 0.6924633562822848, + "timestamp": 4.769583449489383 + }, + { + "x": 6.7282629808547805, + "y": 7.833355832469298, + "heading": 3.529440737430345, + "angularVelocity": 1.816869994863009e-8, + "velocityX": -3.7820111252490736, + "velocityY": 0.08384853005198241, + "timestamp": 4.852656577875407 + }, + { + "x": 6.416545562348172, + "y": 7.793455476408919, + "heading": 3.5294407390176192, + "angularVelocity": 1.910695113703758e-8, + "velocityX": -3.7523255060062675, + "velocityY": -0.4803040024563794, + "timestamp": 4.93572970626143 + }, + { + "x": 6.117328879898073, + "y": 7.697386963559072, + "heading": 3.529440736387819, + "angularVelocity": -3.165645417876012e-8, + "velocityX": -3.6018468097132645, + "velocityY": -1.1564330694690592, + "timestamp": 5.018802834647453 + }, + { + "x": 5.848961075575228, + "y": 7.552831079331328, + "heading": 3.519045211837445, + "angularVelocity": -0.12513702989572795, + "velocityX": -3.2305007592322372, + "velocityY": -1.7401040148148046, + "timestamp": 5.101875963033477 + }, + { + "x": 5.634955917705784, + "y": 7.389476234986078, + "heading": 3.5030562857096847, + "angularVelocity": -0.19246808731534845, + "velocityX": -2.576105679745284, + "velocityY": -1.9663981304059515, + "timestamp": 5.1849490914195 + }, + { + "x": 5.477332577410991, + "y": 7.214683999741706, + "heading": 3.4827939160779, + "angularVelocity": -0.24391003475431908, + "velocityX": -1.8974046524689734, + "velocityY": -2.104076716987816, + "timestamp": 5.268022219805523 + }, + { + "x": 5.376626668066582, + "y": 7.031496552279893, + "heading": 3.458815082623469, + "angularVelocity": -0.28864729089058233, + "velocityX": -1.2122561326503705, + "velocityY": -2.2051348133969344, + "timestamp": 5.351095348191547 + }, + { + "x": 5.3330583572387695, + "y": 6.8415608406066895, + "heading": 3.431423980439692, + "angularVelocity": -0.32972277216401125, + "velocityX": -0.5244573266262534, + "velocityY": -2.2863676301031086, + "timestamp": 5.43416847657757 + }, + { + "x": 5.34693872876846, + "y": 6.645346154308901, + "heading": 3.400717544814328, + "angularVelocity": -0.36860502765209313, + "velocityX": 0.16662222844571778, + "velocityY": -2.355392880859189, + "timestamp": 5.517472917162537 + }, + { + "x": 5.418495574903921, + "y": 6.444510062585301, + "heading": 3.3681952548408147, + "angularVelocity": -0.3904028374134781, + "velocityX": 0.858979973132117, + "velocityY": -2.4108689802526824, + "timestamp": 5.6007773577475035 + }, + { + "x": 5.547849243844485, + "y": 6.240899298718153, + "heading": 3.336181210288504, + "angularVelocity": -0.3843017770422251, + "velocityX": 1.552782397099573, + "velocityY": -2.4441765941573466, + "timestamp": 5.68408179833247 + }, + { + "x": 5.7350445619956005, + "y": 6.038062186003621, + "heading": 3.3091328137988687, + "angularVelocity": -0.32469333326891314, + "velocityX": 2.2471229244998536, + "velocityY": -2.434889560390793, + "timestamp": 5.767386238917437 + }, + { + "x": 5.979026091079565, + "y": 5.8453030071489485, + "heading": 3.298853791542748, + "angularVelocity": -0.12339104835157319, + "velocityX": 2.928793799834883, + "velocityY": -2.313912409724021, + "timestamp": 5.850690679502404 + }, + { + "x": 6.257928216078006, + "y": 5.698591940467709, + "heading": 3.298853780719763, + "angularVelocity": -1.299208718165422e-7, + "velocityX": 3.3479862902863196, + "velocityY": -1.7611434114559776, + "timestamp": 5.933995120087371 + }, + { + "x": 6.558953297615358, + "y": 5.605347982037956, + "heading": 3.2988537749203877, + "angularVelocity": -6.96166383865064e-8, + "velocityX": 3.61355384447146, + "velocityY": -1.1193155823986038, + "timestamp": 6.017299560672337 + }, + { + "x": 6.868955008416185, + "y": 5.548695751369894, + "heading": 3.298853769327315, + "angularVelocity": -6.714014967276945e-8, + "velocityX": 3.721310756353238, + "velocityY": -0.680062554531874, + "timestamp": 6.100604001257304 + }, + { + "x": 7.184039775897806, + "y": 5.543028549631772, + "heading": 3.2988537480257967, + "angularVelocity": -2.5570687177244914e-7, + "velocityX": 3.782328592198499, + "velocityY": -0.06803000774420709, + "timestamp": 6.183908441842271 + }, + { + "x": 7.453135749501623, + "y": 5.570966234514223, + "heading": 3.2626249853587836, + "angularVelocity": -0.43489593607031335, + "velocityX": 3.2302716603606547, + "velocityY": 0.3353684951999138, + "timestamp": 6.267212882427238 + }, + { + "x": 7.666594614732487, + "y": 5.614797465909912, + "heading": 3.2252844849339093, + "angularVelocity": -0.4482414162158435, + "velocityX": 2.5623947983078446, + "velocityY": 0.5261572022800323, + "timestamp": 6.3505173230122045 + }, + { + "x": 7.8237895341689345, + "y": 5.672106049462931, + "heading": 3.191367480553881, + "angularVelocity": -0.40714521509131607, + "velocityX": 1.8869932783008683, + "velocityY": 0.6879415208912717, + "timestamp": 6.433821763597171 + }, + { + "x": 7.924513457831174, + "y": 5.741949093717095, + "heading": 3.1626288175157167, + "angularVelocity": -0.34498356673858915, + "velocityX": 1.2091062967946495, + "velocityY": 0.838407217715205, + "timestamp": 6.517126204182138 + }, + { + "x": 7.968664646148682, + "y": 5.823824882507324, + "heading": 3.14, + "angularVelocity": -0.2716399912995765, + "velocityX": 0.5299980170021705, + "velocityY": 0.9828502324161114, + "timestamp": 6.600430644767105 + }, + { + "x": 7.953629359811841, + "y": 5.921217077492859, + "heading": 3.1237350830688406, + "angularVelocity": -0.18848196569172904, + "velocityX": -0.17423269577704348, + "velocityY": 1.1286053554161422, + "timestamp": 6.6867249322415985 + }, + { + "x": 7.877765712492472, + "y": 6.030911295018945, + "heading": 3.1145546861487188, + "angularVelocity": -0.10638475835188216, + "velocityX": -0.8791271072467286, + "velocityY": 1.271164299937107, + "timestamp": 6.773019219716092 + }, + { + "x": 7.740982038726218, + "y": 6.152452285533978, + "heading": 3.112307173835679, + "angularVelocity": -0.02604474037408179, + "velocityX": -1.5850837612708069, + "velocityY": 1.4084476976643152, + "timestamp": 6.859313507190586 + }, + { + "x": 7.543108693142391, + "y": 6.2849461115263425, + "heading": 3.1166940705371036, + "angularVelocity": 0.050836467045642465, + "velocityX": -2.293006308700471, + "velocityY": 1.5353719217106456, + "timestamp": 6.945607794665079 + }, + { + "x": 7.283736061719511, + "y": 6.425841206700315, + "heading": 3.126860221171615, + "angularVelocity": 0.11780792138200498, + "velocityX": -3.00567556687392, + "velocityY": 1.6327279510317223, + "timestamp": 7.031902082139573 + }, + { + "x": 6.974363478123194, + "y": 6.530031875169815, + "heading": 3.126860227088612, + "angularVelocity": 6.856765823922045e-8, + "velocityX": -3.5850876419572795, + "velocityY": 1.2073877833488817, + "timestamp": 7.118196369614067 + }, + { + "x": 6.654515721034659, + "y": 6.595335054105491, + "heading": 3.1268602314216754, + "angularVelocity": 5.0212631563865974e-8, + "velocityX": -3.7064765982693104, + "velocityY": 0.7567497321879761, + "timestamp": 7.20449065708856 + }, + { + "x": 6.328089013605824, + "y": 6.598897905700907, + "heading": 3.126860245581997, + "angularVelocity": 1.6409338766070953e-7, + "velocityX": -3.782715136564709, + "velocityY": 0.04128722421480937, + "timestamp": 7.290784944563054 + }, + { + "x": 6.025018317514451, + "y": 6.546337315021749, + "heading": 3.1724115352008173, + "angularVelocity": 0.527859849729727, + "velocityX": -3.512059777780222, + "velocityY": -0.6090854008695971, + "timestamp": 7.377079232037548 + }, + { + "x": 5.779136888025266, + "y": 6.469611949486163, + "heading": 3.2211889094820707, + "angularVelocity": 0.5652445336624501, + "velocityX": -2.8493361111749387, + "velocityY": -0.8891129155943783, + "timestamp": 7.463373519512041 + }, + { + "x": 5.5922330688018125, + "y": 6.373475998792455, + "heading": 3.2682230815265036, + "angularVelocity": 0.5450438658333501, + "velocityX": -2.165888666485556, + "velocityY": -1.1140476792525056, + "timestamp": 7.549667806986535 + }, + { + "x": 5.464860063299637, + "y": 6.259720552678007, + "heading": 3.3116817508923533, + "angularVelocity": 0.5036100376713217, + "velocityX": -1.476030560421781, + "velocityY": -1.3182268426293073, + "timestamp": 7.6359620944610285 + }, + { + "x": 5.397282123565674, + "y": 6.129282474517822, + "heading": 3.350614829351744, + "angularVelocity": 0.4511663471454952, + "velocityX": -0.7831102348917176, + "velocityY": -1.5115493965778382, + "timestamp": 7.722256381935522 + }, + { + "x": 5.389062072099223, + "y": 5.984597411179855, + "heading": 3.3841120445321926, + "angularVelocity": 0.3926696956274206, + "velocityX": -0.09635920747397948, + "velocityY": -1.6960645676573736, + "timestamp": 7.807562723508242 + }, + { + "x": 5.439577737943889, + "y": 5.8247498327009, + "heading": 3.4125162997310934, + "angularVelocity": 0.33296768651938463, + "velocityX": 0.5921677675229385, + "velocityY": -1.873806513466449, + "timestamp": 7.892869065080961 + }, + { + "x": 5.549063488192787, + "y": 5.650683264733526, + "heading": 3.43566101949846, + "angularVelocity": 0.2713130037071968, + "velocityX": 1.2834421009083707, + "velocityY": -2.0404880195101427, + "timestamp": 7.978175406653681 + }, + { + "x": 5.717925305048545, + "y": 5.464212491314424, + "heading": 3.4532309731984996, + "angularVelocity": 0.20596304302960114, + "velocityX": 1.9794755435832592, + "velocityY": -2.185895796036977, + "timestamp": 8.0634817482264 + }, + { + "x": 5.946971580653903, + "y": 5.270228748236446, + "heading": 3.464410638066347, + "angularVelocity": 0.13105315105228252, + "velocityX": 2.684985329140002, + "velocityY": -2.2739662667706324, + "timestamp": 8.148788089799119 + }, + { + "x": 6.226744570884399, + "y": 5.1093932756817635, + "heading": 3.464410636874865, + "angularVelocity": -1.3967096544667774e-8, + "velocityX": 3.279627107112591, + "velocityY": -1.8853870602055813, + "timestamp": 8.234094431371838 + }, + { + "x": 6.531679081607643, + "y": 5.003771669462237, + "heading": 3.464410634209207, + "angularVelocity": -3.124806173053443e-8, + "velocityX": 3.5745819724703742, + "velocityY": -1.2381448350998507, + "timestamp": 8.319400772944558 + }, + { + "x": 6.839623513036132, + "y": 4.907277074993315, + "heading": 3.4644106315608414, + "angularVelocity": -3.104535902985177e-8, + "velocityX": 3.609865641301503, + "velocityY": -1.1311538238533516, + "timestamp": 8.404707114517278 + }, + { + "x": 7.147567913336122, + "y": 4.81078238118367, + "heading": 3.464410628912475, + "angularVelocity": -3.104536108601854e-8, + "velocityX": 3.6098652763989647, + "velocityY": -1.1311549883707996, + "timestamp": 8.490013456089997 + }, + { + "x": 7.447305692554561, + "y": 4.691207128277643, + "heading": 3.4644106212813455, + "angularVelocity": -8.945560032337055e-8, + "velocityX": 3.5136635060469357, + "velocityY": -1.4017158713117939, + "timestamp": 8.575319797662717 + }, + { + "x": 7.693247872040493, + "y": 4.5498334622955285, + "heading": 3.399037241317186, + "angularVelocity": -0.7663366961813945, + "velocityX": 2.883046851520149, + "velocityY": -1.657246851473523, + "timestamp": 8.660626139235436 + }, + { + "x": 7.88024564060197, + "y": 4.422851480577848, + "heading": 3.3318555601258444, + "angularVelocity": -0.7875344312365365, + "velocityX": 2.1920734744212487, + "velocityY": -1.4885409381837746, + "timestamp": 8.745932480808156 + }, + { + "x": 8.008768514882867, + "y": 4.3120678627120395, + "heading": 3.265924152728589, + "angularVelocity": -0.772878149288027, + "velocityX": 1.5066039864262268, + "velocityY": -1.2986562994425228, + "timestamp": 8.831238822380875 + }, + { + "x": 8.07899777943964, + "y": 4.218114377697436, + "heading": 3.202319904856858, + "angularVelocity": -0.7455981196604378, + "velocityX": 0.8232595990170849, + "velocityY": -1.101365775186992, + "timestamp": 8.916545163953595 + }, + { + "x": 8.091029167175293, + "y": 4.141312599182129, + "heading": 3.141592653589793, + "angularVelocity": -0.7118726480058678, + "velocityX": 0.14103743653568068, + "velocityY": -0.900305617371216, + "timestamp": 9.001851505526314 + }, + { + "x": 8.040216714979586, + "y": 4.080035987534699, + "heading": 3.0816900573414494, + "angularVelocity": -0.6725771689853446, + "velocityX": -0.5705144248724638, + "velocityY": -0.6880044032813033, + "timestamp": 9.090915778015244 + }, + { + "x": 7.92607322423097, + "y": 4.037808955209039, + "heading": 3.025407419661199, + "angularVelocity": -0.6319328290392299, + "velocityX": -1.281585618552088, + "velocityY": -0.4741186465190991, + "timestamp": 9.179980050504174 + }, + { + "x": 7.748670432062219, + "y": 4.014865360628201, + "heading": 2.9729438660433796, + "angularVelocity": -0.5890527385640573, + "velocityX": -1.9918513587005524, + "velocityY": -0.2576071632279864, + "timestamp": 9.269044322993103 + }, + { + "x": 7.508153207350264, + "y": 4.011667878946095, + "heading": 2.9246938540207905, + "angularVelocity": -0.5417437393718784, + "velocityX": -2.7004905333039093, + "velocityY": -0.03590083422625269, + "timestamp": 9.358108595482033 + }, + { + "x": 7.204966238284119, + "y": 4.029564783423129, + "heading": 2.881811620328617, + "angularVelocity": -0.4814751470350101, + "velocityX": -3.404136817081497, + "velocityY": 0.2009437002840627, + "timestamp": 9.447172867970963 + }, + { + "x": 6.877222132273021, + "y": 4.107681050730088, + "heading": 2.881811620606281, + "angularVelocity": 3.1175712181288514e-9, + "velocityX": -3.6798605866548297, + "velocityY": 0.8770774758943696, + "timestamp": 9.536237140459892 + }, + { + "x": 6.553507681979354, + "y": 4.201101101313888, + "heading": 2.8818116222683225, + "angularVelocity": 1.8661142988084573e-8, + "velocityX": -3.6346162299130933, + "velocityY": 1.0489060088085451, + "timestamp": 9.625301412948822 + }, + { + "x": 6.229793253222719, + "y": 4.2945212265266814, + "heading": 2.8818116239303677, + "angularVelocity": 1.866118842609661e-8, + "velocityX": -3.6346159880986018, + "velocityY": 1.0489068467314409, + "timestamp": 9.714365685437752 + }, + { + "x": 5.912076930566037, + "y": 4.386210365902842, + "heading": 2.8968730092713613, + "angularVelocity": 0.1691069260445136, + "velocityX": -3.567270172180123, + "velocityY": 1.0294716030780673, + "timestamp": 9.803429957926681 + }, + { + "x": 5.657903856180183, + "y": 4.459561682271931, + "heading": 2.908923259524863, + "angularVelocity": 0.13529836281994964, + "velocityX": -2.853816320314597, + "velocityY": 0.8235773371214163, + "timestamp": 9.892494230415611 + }, + { + "x": 5.467274045026463, + "y": 4.514575171136671, + "heading": 2.9179613240631452, + "angularVelocity": 0.1014780033082899, + "velocityX": -2.140362300465804, + "velocityY": 0.6176830206700082, + "timestamp": 9.98155850290454 + }, + { + "x": 5.340187502097666, + "y": 4.551250831022913, + "heading": 2.9239868528608652, + "angularVelocity": 0.06765371376574367, + "velocityX": -1.4269082245587752, + "velocityY": 0.41178868766711113, + "timestamp": 10.07062277539347 + }, + { + "x": 5.276644229888916, + "y": 4.569588661193848, + "heading": 2.926999670595623, + "angularVelocity": 0.033827455730154186, + "velocityX": -0.7134541206368397, + "velocityY": 0.2058943463914144, + "timestamp": 10.1596870478824 + }, + { + "x": 5.276644229888916, + "y": 4.569588661193848, + "heading": 2.926999670595623, + "angularVelocity": -1.4307348024093064e-32, + "velocityX": -8.4908818209571e-35, + "velocityY": -2.480999535551194e-34, + "timestamp": 10.24875132037133 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Circle.1.traj b/src/main/deploy/choreo/Circle.1.traj new file mode 100644 index 00000000..68276f27 --- /dev/null +++ b/src/main/deploy/choreo/Circle.1.traj @@ -0,0 +1,365 @@ +{ + "samples": [ + { + "x": 1.5, + "y": 5.5, + "heading": -3.3219213067501535e-28, + "angularVelocity": -1.3665051940262503e-28, + "velocityX": -2.805794210798643e-23, + "velocityY": 1.2484693727650662e-22, + "timestamp": 0 + }, + { + "x": 1.5362282626284263, + "y": 5.5432844591906445, + "heading": -9.472390498067437e-18, + "angularVelocity": -1.1512814988371418e-16, + "velocityX": 0.44032104155440455, + "velocityY": 0.5260825877691788, + "timestamp": 0.08227692798993784 + }, + { + "x": 1.6086847866611431, + "y": 5.629853376109373, + "heading": -3.486400420928457e-17, + "angularVelocity": -3.086115917488839e-16, + "velocityX": 0.8806420682305758, + "velocityY": 1.0521651577622833, + "timestamp": 0.16455385597987568 + }, + { + "x": 1.7173695700579255, + "y": 5.759706748318585, + "heading": -7.301754283392471e-17, + "angularVelocity": -4.637209914966371e-16, + "velocityX": 1.3209630701096913, + "velocityY": 1.5782476981285976, + "timestamp": 0.2468307839698135 + }, + { + "x": 1.862282608738322, + "y": 5.932844570943078, + "heading": -1.192226659960793e-16, + "angularVelocity": -5.615805583767775e-16, + "velocityX": 1.7612840223946997, + "velocityY": 2.104330179241336, + "timestamp": 0.32910771195975136 + }, + { + "x": 2.043423890460986, + "y": 6.14926682935725, + "heading": -1.8781998178842608e-16, + "angularVelocity": -8.33736959658091e-16, + "velocityX": 2.201604825897445, + "velocityY": 2.6304124825934165, + "timestamp": 0.4113846399496892 + }, + { + "x": 2.2431937917892997, + "y": 6.3879460127932965, + "heading": -2.0530355513809777e-16, + "angularVelocity": -2.1249667162436542e-16, + "velocityX": 2.428018476245788, + "velocityY": 2.900924831141438, + "timestamp": 0.49366156793962707 + }, + { + "x": 2.4429638847633592, + "y": 6.626625035825442, + "heading": -2.0383025572099072e-16, + "angularVelocity": 1.7906592436584598e-17, + "velocityX": 2.4280208055226695, + "velocityY": 2.900922881580298, + "timestamp": 0.5759384959295649 + }, + { + "x": 2.6621600398809373, + "y": 6.847597458073065, + "heading": -1.9812607044504066e-16, + "angularVelocity": 6.932909886183889e-17, + "velocityX": 2.6641266327345763, + "velocityY": 2.6857155176557943, + "timestamp": 0.6582154239195027 + }, + { + "x": 2.9176597571753207, + "y": 7.025349134953072, + "heading": -2.0290881342506434e-16, + "angularVelocity": -5.812981960056516e-17, + "velocityX": 3.1053628706899454, + "velocityY": 2.1604073125061927, + "timestamp": 0.7404923519094405 + }, + { + "x": 3.2010604808925094, + "y": 7.154033982149684, + "heading": -1.9347228589546108e-16, + "angularVelocity": 1.1469226864191852e-16, + "velocityX": 3.444473811076744, + "velocityY": 1.5640453568265125, + "timestamp": 0.8227692798993783 + }, + { + "x": 3.5030417885342837, + "y": 7.229419779766539, + "heading": -1.8536764185493148e-16, + "angularVelocity": 9.850445607453505e-17, + "velocityX": 3.6703036321276663, + "velocityY": 0.9162446807211185, + "timestamp": 0.9050462078893161 + }, + { + "x": 3.804727969097522, + "y": 7.248361426815143, + "heading": -1.2128971877681688e-16, + "angularVelocity": 7.788079190947775e-16, + "velocityX": 3.6667166353140046, + "velocityY": 0.23021820954374964, + "timestamp": 0.9873231358792539 + }, + { + "x": 4.090299967419989, + "y": 7.2132072004642325, + "heading": -5.322461627386045e-17, + "angularVelocity": 8.272683991127659e-16, + "velocityX": 3.47086364670048, + "velocityY": -0.42726712347852686, + "timestamp": 1.0696000638691918 + }, + { + "x": 4.353217601776123, + "y": 7.126353740692139, + "heading": -1.7247782060093015e-27, + "angularVelocity": 6.468960080495983e-16, + "velocityX": 3.1955207951892257, + "velocityY": -1.0556235131034137, + "timestamp": 1.1518769918591296 + }, + { + "x": 4.58144387602119, + "y": 6.996965486159656, + "heading": 5.199201984564153e-17, + "angularVelocity": 6.587448099688971e-16, + "velocityX": 2.8916528747838215, + "velocityY": -1.639363913816324, + "timestamp": 1.230802881005533 + }, + { + "x": 4.782026897902942, + "y": 6.823603519584229, + "heading": 9.551997300692058e-17, + "angularVelocity": 5.515041215496502e-16, + "velocityX": 2.5414097205757624, + "velocityY": -2.196515851140449, + "timestamp": 1.3097287701519362 + }, + { + "x": 4.950456239944392, + "y": 6.6094496730444945, + "heading": 1.2540926885038495e-16, + "angularVelocity": 3.787007807807601e-16, + "velocityX": 2.134018936790482, + "velocityY": -2.713353613824885, + "timestamp": 1.3886546592983395 + }, + { + "x": 5.081311669842177, + "y": 6.35943446092387, + "heading": 1.3960950290458758e-16, + "angularVelocity": 1.7991858195137434e-16, + "velocityX": 1.6579531927103452, + "velocityY": -3.1677211980071323, + "timestamp": 1.4675805484447428 + }, + { + "x": 5.168573540307643, + "y": 6.08118030776853, + "heading": 1.3851795279191138e-16, + "angularVelocity": -1.3830064177977227e-17, + "velocityX": 1.1056178322375505, + "velocityY": -3.525511795492021, + "timestamp": 1.5465064375911461 + }, + { + "x": 5.206787103011429, + "y": 5.785835234655995, + "heading": 1.1611809267335485e-16, + "angularVelocity": -2.8380877758076653e-16, + "velocityX": 0.48417018949132823, + "velocityY": -3.742055696891635, + "timestamp": 1.6254323267375494 + }, + { + "x": 5.193141803403618, + "y": 5.487575652887037, + "heading": 8.296484960384974e-17, + "angularVelocity": -4.2005536367974825e-16, + "velocityX": -0.17288749934129347, + "velocityY": -3.778982853341056, + "timestamp": 1.7043582158839528 + }, + { + "x": 5.128449250262065, + "y": 5.1989084597897195, + "heading": 5.766159875503332e-17, + "angularVelocity": -3.2059506863832877e-16, + "velocityX": -0.8196620125692016, + "velocityY": -3.657446196923542, + "timestamp": 1.783284105030356 + }, + { + "x": 5.016690466802075, + "y": 4.9322095650866205, + "heading": 3.327908681576341e-17, + "angularVelocity": -3.0892920184187437e-16, + "velocityX": -1.4159965084800308, + "velocityY": -3.3791053555112245, + "timestamp": 1.8622099941767594 + }, + { + "x": 4.863164798870976, + "y": 4.6963872590942435, + "heading": 9.698495363712997e-18, + "angularVelocity": -2.987687779794426e-16, + "velocityX": -1.945187689255118, + "velocityY": -2.9878954617152007, + "timestamp": 1.9411358833231627 + }, + { + "x": 4.673014997916854, + "y": 4.4973957749437625, + "heading": 3.081671052827974e-18, + "angularVelocity": -8.383591723601681e-17, + "velocityX": -2.4092196237587498, + "velocityY": -2.5212447563481035, + "timestamp": 2.0200617724695658 + }, + { + "x": 4.450671672821045, + "y": 4.339164733886719, + "heading": 8.278658336073892e-28, + "angularVelocity": -3.904512302530694e-17, + "velocityX": -2.8171152393782313, + "velocityY": -2.0048053023962087, + "timestamp": 2.098987661615969 + }, + { + "x": 4.153965896620894, + "y": 4.213937531814997, + "heading": 8.290067362617875e-18, + "angularVelocity": 9.044327654116548e-17, + "velocityX": -3.2370114015394744, + "velocityY": -1.3662082554658637, + "timestamp": 2.1906480666608945 + }, + { + "x": 3.8249997993536913, + "y": 4.150894110176715, + "heading": 2.3888082893966074e-17, + "angularVelocity": 1.701717936252325e-16, + "velocityX": -3.588966218357502, + "velocityY": -0.6877934000769791, + "timestamp": 2.28230847170582 + }, + { + "x": 3.478286252707822, + "y": 4.155619248980717, + "heading": 3.558873113658147e-17, + "angularVelocity": 1.2765215510930902e-16, + "velocityX": -3.782587983065696, + "velocityY": 0.051550490112787714, + "timestamp": 2.373968876750746 + }, + { + "x": 3.1395982702697465, + "y": 4.229937042598315, + "heading": 4.921578190575193e-17, + "angularVelocity": 1.486688910228552e-16, + "velocityX": -3.695030392589622, + "velocityY": 0.8107949510060795, + "timestamp": 2.4656292817956715 + }, + { + "x": 2.8227601851385815, + "y": 4.370814002416527, + "heading": 5.890626112127518e-17, + "angularVelocity": 1.0572154039974704e-16, + "velocityX": -3.456651593192001, + "velocityY": 1.5369445481848356, + "timestamp": 2.557289686840597 + }, + { + "x": 2.54070424472652, + "y": 4.572499745217142, + "heading": 7.805444228664704e-17, + "angularVelocity": 2.0890351898005458e-16, + "velocityX": -3.0771840935441808, + "velocityY": 2.2003584066834767, + "timestamp": 2.648950091885523 + }, + { + "x": 2.2818433514518452, + "y": 4.803202434252147, + "heading": 7.067779216783429e-17, + "angularVelocity": -8.047804409499006e-17, + "velocityX": -2.8241299299059275, + "velocityY": 2.516928535521215, + "timestamp": 2.7406104969304486 + }, + { + "x": 2.022982633974282, + "y": 5.033905320540323, + "heading": 3.011799781222237e-17, + "angularVelocity": -4.4250071048418785e-16, + "velocityX": -2.8241280119882584, + "velocityY": 2.516930687520981, + "timestamp": 2.8322709019753742 + }, + { + "x": 1.8137895934579804, + "y": 5.220343180672855, + "heading": 1.710283920370377e-17, + "angularVelocity": -1.4199324781572106e-16, + "velocityX": -2.2822617946513546, + "velocityY": 2.0340065052205936, + "timestamp": 2.9239313070203 + }, + { + "x": 1.6568948001100603, + "y": 5.360171587323137, + "heading": 1.0302381022210203e-17, + "angularVelocity": -7.419188446827068e-17, + "velocityX": -1.7116964873875544, + "velocityY": 1.5255050049336634, + "timestamp": 3.0155917120652256 + }, + { + "x": 1.5522982674547041, + "y": 5.453390528438091, + "heading": 4.4011411079174696e-18, + "angularVelocity": -6.438156051477007e-17, + "velocityX": -1.1411310325771633, + "velocityY": 1.0170033731496684, + "timestamp": 3.1072521171101513 + }, + { + "x": 1.5, + "y": 5.5, + "heading": 1.7365292199524757e-28, + "angularVelocity": -4.801572833443273e-17, + "velocityX": -0.5705655285842443, + "velocityY": 0.508501697533024, + "timestamp": 3.198912522155077 + }, + { + "x": 1.5, + "y": 5.5, + "heading": 8.504010006620182e-29, + "angularVelocity": -3.897781011434181e-29, + "velocityX": -6.009517149045476e-23, + "velocityY": -7.563147799258595e-23, + "timestamp": 3.2905729272000026 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Circle.traj b/src/main/deploy/choreo/Circle.traj new file mode 100644 index 00000000..68276f27 --- /dev/null +++ b/src/main/deploy/choreo/Circle.traj @@ -0,0 +1,365 @@ +{ + "samples": [ + { + "x": 1.5, + "y": 5.5, + "heading": -3.3219213067501535e-28, + "angularVelocity": -1.3665051940262503e-28, + "velocityX": -2.805794210798643e-23, + "velocityY": 1.2484693727650662e-22, + "timestamp": 0 + }, + { + "x": 1.5362282626284263, + "y": 5.5432844591906445, + "heading": -9.472390498067437e-18, + "angularVelocity": -1.1512814988371418e-16, + "velocityX": 0.44032104155440455, + "velocityY": 0.5260825877691788, + "timestamp": 0.08227692798993784 + }, + { + "x": 1.6086847866611431, + "y": 5.629853376109373, + "heading": -3.486400420928457e-17, + "angularVelocity": -3.086115917488839e-16, + "velocityX": 0.8806420682305758, + "velocityY": 1.0521651577622833, + "timestamp": 0.16455385597987568 + }, + { + "x": 1.7173695700579255, + "y": 5.759706748318585, + "heading": -7.301754283392471e-17, + "angularVelocity": -4.637209914966371e-16, + "velocityX": 1.3209630701096913, + "velocityY": 1.5782476981285976, + "timestamp": 0.2468307839698135 + }, + { + "x": 1.862282608738322, + "y": 5.932844570943078, + "heading": -1.192226659960793e-16, + "angularVelocity": -5.615805583767775e-16, + "velocityX": 1.7612840223946997, + "velocityY": 2.104330179241336, + "timestamp": 0.32910771195975136 + }, + { + "x": 2.043423890460986, + "y": 6.14926682935725, + "heading": -1.8781998178842608e-16, + "angularVelocity": -8.33736959658091e-16, + "velocityX": 2.201604825897445, + "velocityY": 2.6304124825934165, + "timestamp": 0.4113846399496892 + }, + { + "x": 2.2431937917892997, + "y": 6.3879460127932965, + "heading": -2.0530355513809777e-16, + "angularVelocity": -2.1249667162436542e-16, + "velocityX": 2.428018476245788, + "velocityY": 2.900924831141438, + "timestamp": 0.49366156793962707 + }, + { + "x": 2.4429638847633592, + "y": 6.626625035825442, + "heading": -2.0383025572099072e-16, + "angularVelocity": 1.7906592436584598e-17, + "velocityX": 2.4280208055226695, + "velocityY": 2.900922881580298, + "timestamp": 0.5759384959295649 + }, + { + "x": 2.6621600398809373, + "y": 6.847597458073065, + "heading": -1.9812607044504066e-16, + "angularVelocity": 6.932909886183889e-17, + "velocityX": 2.6641266327345763, + "velocityY": 2.6857155176557943, + "timestamp": 0.6582154239195027 + }, + { + "x": 2.9176597571753207, + "y": 7.025349134953072, + "heading": -2.0290881342506434e-16, + "angularVelocity": -5.812981960056516e-17, + "velocityX": 3.1053628706899454, + "velocityY": 2.1604073125061927, + "timestamp": 0.7404923519094405 + }, + { + "x": 3.2010604808925094, + "y": 7.154033982149684, + "heading": -1.9347228589546108e-16, + "angularVelocity": 1.1469226864191852e-16, + "velocityX": 3.444473811076744, + "velocityY": 1.5640453568265125, + "timestamp": 0.8227692798993783 + }, + { + "x": 3.5030417885342837, + "y": 7.229419779766539, + "heading": -1.8536764185493148e-16, + "angularVelocity": 9.850445607453505e-17, + "velocityX": 3.6703036321276663, + "velocityY": 0.9162446807211185, + "timestamp": 0.9050462078893161 + }, + { + "x": 3.804727969097522, + "y": 7.248361426815143, + "heading": -1.2128971877681688e-16, + "angularVelocity": 7.788079190947775e-16, + "velocityX": 3.6667166353140046, + "velocityY": 0.23021820954374964, + "timestamp": 0.9873231358792539 + }, + { + "x": 4.090299967419989, + "y": 7.2132072004642325, + "heading": -5.322461627386045e-17, + "angularVelocity": 8.272683991127659e-16, + "velocityX": 3.47086364670048, + "velocityY": -0.42726712347852686, + "timestamp": 1.0696000638691918 + }, + { + "x": 4.353217601776123, + "y": 7.126353740692139, + "heading": -1.7247782060093015e-27, + "angularVelocity": 6.468960080495983e-16, + "velocityX": 3.1955207951892257, + "velocityY": -1.0556235131034137, + "timestamp": 1.1518769918591296 + }, + { + "x": 4.58144387602119, + "y": 6.996965486159656, + "heading": 5.199201984564153e-17, + "angularVelocity": 6.587448099688971e-16, + "velocityX": 2.8916528747838215, + "velocityY": -1.639363913816324, + "timestamp": 1.230802881005533 + }, + { + "x": 4.782026897902942, + "y": 6.823603519584229, + "heading": 9.551997300692058e-17, + "angularVelocity": 5.515041215496502e-16, + "velocityX": 2.5414097205757624, + "velocityY": -2.196515851140449, + "timestamp": 1.3097287701519362 + }, + { + "x": 4.950456239944392, + "y": 6.6094496730444945, + "heading": 1.2540926885038495e-16, + "angularVelocity": 3.787007807807601e-16, + "velocityX": 2.134018936790482, + "velocityY": -2.713353613824885, + "timestamp": 1.3886546592983395 + }, + { + "x": 5.081311669842177, + "y": 6.35943446092387, + "heading": 1.3960950290458758e-16, + "angularVelocity": 1.7991858195137434e-16, + "velocityX": 1.6579531927103452, + "velocityY": -3.1677211980071323, + "timestamp": 1.4675805484447428 + }, + { + "x": 5.168573540307643, + "y": 6.08118030776853, + "heading": 1.3851795279191138e-16, + "angularVelocity": -1.3830064177977227e-17, + "velocityX": 1.1056178322375505, + "velocityY": -3.525511795492021, + "timestamp": 1.5465064375911461 + }, + { + "x": 5.206787103011429, + "y": 5.785835234655995, + "heading": 1.1611809267335485e-16, + "angularVelocity": -2.8380877758076653e-16, + "velocityX": 0.48417018949132823, + "velocityY": -3.742055696891635, + "timestamp": 1.6254323267375494 + }, + { + "x": 5.193141803403618, + "y": 5.487575652887037, + "heading": 8.296484960384974e-17, + "angularVelocity": -4.2005536367974825e-16, + "velocityX": -0.17288749934129347, + "velocityY": -3.778982853341056, + "timestamp": 1.7043582158839528 + }, + { + "x": 5.128449250262065, + "y": 5.1989084597897195, + "heading": 5.766159875503332e-17, + "angularVelocity": -3.2059506863832877e-16, + "velocityX": -0.8196620125692016, + "velocityY": -3.657446196923542, + "timestamp": 1.783284105030356 + }, + { + "x": 5.016690466802075, + "y": 4.9322095650866205, + "heading": 3.327908681576341e-17, + "angularVelocity": -3.0892920184187437e-16, + "velocityX": -1.4159965084800308, + "velocityY": -3.3791053555112245, + "timestamp": 1.8622099941767594 + }, + { + "x": 4.863164798870976, + "y": 4.6963872590942435, + "heading": 9.698495363712997e-18, + "angularVelocity": -2.987687779794426e-16, + "velocityX": -1.945187689255118, + "velocityY": -2.9878954617152007, + "timestamp": 1.9411358833231627 + }, + { + "x": 4.673014997916854, + "y": 4.4973957749437625, + "heading": 3.081671052827974e-18, + "angularVelocity": -8.383591723601681e-17, + "velocityX": -2.4092196237587498, + "velocityY": -2.5212447563481035, + "timestamp": 2.0200617724695658 + }, + { + "x": 4.450671672821045, + "y": 4.339164733886719, + "heading": 8.278658336073892e-28, + "angularVelocity": -3.904512302530694e-17, + "velocityX": -2.8171152393782313, + "velocityY": -2.0048053023962087, + "timestamp": 2.098987661615969 + }, + { + "x": 4.153965896620894, + "y": 4.213937531814997, + "heading": 8.290067362617875e-18, + "angularVelocity": 9.044327654116548e-17, + "velocityX": -3.2370114015394744, + "velocityY": -1.3662082554658637, + "timestamp": 2.1906480666608945 + }, + { + "x": 3.8249997993536913, + "y": 4.150894110176715, + "heading": 2.3888082893966074e-17, + "angularVelocity": 1.701717936252325e-16, + "velocityX": -3.588966218357502, + "velocityY": -0.6877934000769791, + "timestamp": 2.28230847170582 + }, + { + "x": 3.478286252707822, + "y": 4.155619248980717, + "heading": 3.558873113658147e-17, + "angularVelocity": 1.2765215510930902e-16, + "velocityX": -3.782587983065696, + "velocityY": 0.051550490112787714, + "timestamp": 2.373968876750746 + }, + { + "x": 3.1395982702697465, + "y": 4.229937042598315, + "heading": 4.921578190575193e-17, + "angularVelocity": 1.486688910228552e-16, + "velocityX": -3.695030392589622, + "velocityY": 0.8107949510060795, + "timestamp": 2.4656292817956715 + }, + { + "x": 2.8227601851385815, + "y": 4.370814002416527, + "heading": 5.890626112127518e-17, + "angularVelocity": 1.0572154039974704e-16, + "velocityX": -3.456651593192001, + "velocityY": 1.5369445481848356, + "timestamp": 2.557289686840597 + }, + { + "x": 2.54070424472652, + "y": 4.572499745217142, + "heading": 7.805444228664704e-17, + "angularVelocity": 2.0890351898005458e-16, + "velocityX": -3.0771840935441808, + "velocityY": 2.2003584066834767, + "timestamp": 2.648950091885523 + }, + { + "x": 2.2818433514518452, + "y": 4.803202434252147, + "heading": 7.067779216783429e-17, + "angularVelocity": -8.047804409499006e-17, + "velocityX": -2.8241299299059275, + "velocityY": 2.516928535521215, + "timestamp": 2.7406104969304486 + }, + { + "x": 2.022982633974282, + "y": 5.033905320540323, + "heading": 3.011799781222237e-17, + "angularVelocity": -4.4250071048418785e-16, + "velocityX": -2.8241280119882584, + "velocityY": 2.516930687520981, + "timestamp": 2.8322709019753742 + }, + { + "x": 1.8137895934579804, + "y": 5.220343180672855, + "heading": 1.710283920370377e-17, + "angularVelocity": -1.4199324781572106e-16, + "velocityX": -2.2822617946513546, + "velocityY": 2.0340065052205936, + "timestamp": 2.9239313070203 + }, + { + "x": 1.6568948001100603, + "y": 5.360171587323137, + "heading": 1.0302381022210203e-17, + "angularVelocity": -7.419188446827068e-17, + "velocityX": -1.7116964873875544, + "velocityY": 1.5255050049336634, + "timestamp": 3.0155917120652256 + }, + { + "x": 1.5522982674547041, + "y": 5.453390528438091, + "heading": 4.4011411079174696e-18, + "angularVelocity": -6.438156051477007e-17, + "velocityX": -1.1411310325771633, + "velocityY": 1.0170033731496684, + "timestamp": 3.1072521171101513 + }, + { + "x": 1.5, + "y": 5.5, + "heading": 1.7365292199524757e-28, + "angularVelocity": -4.801572833443273e-17, + "velocityX": -0.5705655285842443, + "velocityY": 0.508501697533024, + "timestamp": 3.198912522155077 + }, + { + "x": 1.5, + "y": 5.5, + "heading": 8.504010006620182e-29, + "angularVelocity": -3.897781011434181e-29, + "velocityX": -6.009517149045476e-23, + "velocityY": -7.563147799258595e-23, + "timestamp": 3.2905729272000026 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Line.1.traj b/src/main/deploy/choreo/Line.1.traj new file mode 100644 index 00000000..e15fac05 --- /dev/null +++ b/src/main/deploy/choreo/Line.1.traj @@ -0,0 +1,131 @@ +{ + "samples": [ + { + "x": 1.5, + "y": 5.5, + "heading": -1.2874479772672074e-29, + "angularVelocity": -5.012654343497732e-30, + "velocityX": 2.633710619062863e-19, + "velocityY": -7.373916538898162e-46, + "timestamp": 0 + }, + { + "x": 1.5789016505791718, + "y": 5.5, + "heading": -6.693322095338404e-19, + "angularVelocity": -6.880708036173696e-18, + "velocityX": 0.8111057760332284, + "velocityY": 6.608245262710766e-34, + "timestamp": 0.09727664739985706 + }, + { + "x": 1.7367049486758532, + "y": 5.5, + "heading": -2.0081784525315156e-18, + "angularVelocity": -1.3763285215143903e-17, + "velocityX": 1.6222115206411716, + "velocityY": 9.05293138093418e-34, + "timestamp": 0.19455329479971412 + }, + { + "x": 1.9734098881938018, + "y": 5.5, + "heading": -4.016923229987951e-18, + "angularVelocity": -2.0649815049430785e-17, + "velocityX": 2.433317202398612, + "velocityY": 8.038324084205884e-34, + "timestamp": 0.29182994219957115 + }, + { + "x": 2.289016450766005, + "y": 5.5, + "heading": -6.696777202113547e-18, + "angularVelocity": -2.7548790422010714e-17, + "velocityX": 3.2444226956047455, + "velocityY": 8.752223368923682e-34, + "timestamp": 0.38910658959942823 + }, + { + "x": 2.6570082253839193, + "y": 5.5, + "heading": -5.1008819244864145e-18, + "angularVelocity": 1.640573889311827e-17, + "velocityX": 3.7829405561642684, + "velocityY": -7.164097231420146e-34, + "timestamp": 0.4863832369992853 + }, + { + "x": 3.0249999999998676, + "y": 5.5, + "heading": 1.4749694041473506e-17, + "angularVelocity": 2.0406311788662626e-16, + "velocityX": 3.7829405561646503, + "velocityY": 3.457561705895053e-33, + "timestamp": 0.5836598843991424 + }, + { + "x": 3.3929917746173133, + "y": 5.5, + "heading": 1.3703190360166544e-17, + "angularVelocity": -1.075801550800218e-17, + "velocityX": 3.7829405561646503, + "velocityY": -8.436403566578749e-34, + "timestamp": 0.6809365317989995 + }, + { + "x": 3.7609835492331904, + "y": 5.5, + "heading": 6.6978603967447e-18, + "angularVelocity": -7.201450862727049e-17, + "velocityX": 3.7829405561642684, + "velocityY": -1.6207126856714834e-33, + "timestamp": 0.7782131791988566 + }, + { + "x": 4.076590111811133, + "y": 5.5, + "heading": 4.0174424481780384e-18, + "angularVelocity": -2.7554588077011827e-17, + "velocityX": 3.2444226956047326, + "velocityY": -1.5328681912688769e-33, + "timestamp": 0.8754898265987137 + }, + { + "x": 4.3132950513190655, + "y": 5.5, + "heading": 2.0084039923467558e-18, + "angularVelocity": -2.0652834051193523e-17, + "velocityX": 2.433317202398599, + "velocityY": -1.0912710639486781e-33, + "timestamp": 0.9727664739985707 + }, + { + "x": 4.471098349419339, + "y": 5.5, + "heading": 6.69399083833827e-19, + "angularVelocity": -1.376491629039911e-17, + "velocityX": 1.6222115206411583, + "velocityY": -8.707726946074912e-34, + "timestamp": 1.0700431213984278 + }, + { + "x": 4.55, + "y": 5.5, + "heading": 2.6358429152716765e-29, + "angularVelocity": -6.881395501116776e-18, + "velocityX": 0.8111057760332147, + "velocityY": -5.515722786564321e-35, + "timestamp": 1.1673197687982848 + }, + { + "x": 4.55, + "y": 5.5, + "heading": 1.287447299074069e-29, + "angularVelocity": -6.265462343923493e-30, + "velocityX": 1.2222778711369996e-19, + "velocityY": -9.16410582597125e-46, + "timestamp": 1.2645964161981418 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Line.traj b/src/main/deploy/choreo/Line.traj new file mode 100644 index 00000000..e15fac05 --- /dev/null +++ b/src/main/deploy/choreo/Line.traj @@ -0,0 +1,131 @@ +{ + "samples": [ + { + "x": 1.5, + "y": 5.5, + "heading": -1.2874479772672074e-29, + "angularVelocity": -5.012654343497732e-30, + "velocityX": 2.633710619062863e-19, + "velocityY": -7.373916538898162e-46, + "timestamp": 0 + }, + { + "x": 1.5789016505791718, + "y": 5.5, + "heading": -6.693322095338404e-19, + "angularVelocity": -6.880708036173696e-18, + "velocityX": 0.8111057760332284, + "velocityY": 6.608245262710766e-34, + "timestamp": 0.09727664739985706 + }, + { + "x": 1.7367049486758532, + "y": 5.5, + "heading": -2.0081784525315156e-18, + "angularVelocity": -1.3763285215143903e-17, + "velocityX": 1.6222115206411716, + "velocityY": 9.05293138093418e-34, + "timestamp": 0.19455329479971412 + }, + { + "x": 1.9734098881938018, + "y": 5.5, + "heading": -4.016923229987951e-18, + "angularVelocity": -2.0649815049430785e-17, + "velocityX": 2.433317202398612, + "velocityY": 8.038324084205884e-34, + "timestamp": 0.29182994219957115 + }, + { + "x": 2.289016450766005, + "y": 5.5, + "heading": -6.696777202113547e-18, + "angularVelocity": -2.7548790422010714e-17, + "velocityX": 3.2444226956047455, + "velocityY": 8.752223368923682e-34, + "timestamp": 0.38910658959942823 + }, + { + "x": 2.6570082253839193, + "y": 5.5, + "heading": -5.1008819244864145e-18, + "angularVelocity": 1.640573889311827e-17, + "velocityX": 3.7829405561642684, + "velocityY": -7.164097231420146e-34, + "timestamp": 0.4863832369992853 + }, + { + "x": 3.0249999999998676, + "y": 5.5, + "heading": 1.4749694041473506e-17, + "angularVelocity": 2.0406311788662626e-16, + "velocityX": 3.7829405561646503, + "velocityY": 3.457561705895053e-33, + "timestamp": 0.5836598843991424 + }, + { + "x": 3.3929917746173133, + "y": 5.5, + "heading": 1.3703190360166544e-17, + "angularVelocity": -1.075801550800218e-17, + "velocityX": 3.7829405561646503, + "velocityY": -8.436403566578749e-34, + "timestamp": 0.6809365317989995 + }, + { + "x": 3.7609835492331904, + "y": 5.5, + "heading": 6.6978603967447e-18, + "angularVelocity": -7.201450862727049e-17, + "velocityX": 3.7829405561642684, + "velocityY": -1.6207126856714834e-33, + "timestamp": 0.7782131791988566 + }, + { + "x": 4.076590111811133, + "y": 5.5, + "heading": 4.0174424481780384e-18, + "angularVelocity": -2.7554588077011827e-17, + "velocityX": 3.2444226956047326, + "velocityY": -1.5328681912688769e-33, + "timestamp": 0.8754898265987137 + }, + { + "x": 4.3132950513190655, + "y": 5.5, + "heading": 2.0084039923467558e-18, + "angularVelocity": -2.0652834051193523e-17, + "velocityX": 2.433317202398599, + "velocityY": -1.0912710639486781e-33, + "timestamp": 0.9727664739985707 + }, + { + "x": 4.471098349419339, + "y": 5.5, + "heading": 6.69399083833827e-19, + "angularVelocity": -1.376491629039911e-17, + "velocityX": 1.6222115206411583, + "velocityY": -8.707726946074912e-34, + "timestamp": 1.0700431213984278 + }, + { + "x": 4.55, + "y": 5.5, + "heading": 2.6358429152716765e-29, + "angularVelocity": -6.881395501116776e-18, + "velocityX": 0.8111057760332147, + "velocityY": -5.515722786564321e-35, + "timestamp": 1.1673197687982848 + }, + { + "x": 4.55, + "y": 5.5, + "heading": 1.287447299074069e-29, + "angularVelocity": -6.265462343923493e-30, + "velocityX": 1.2222778711369996e-19, + "velocityY": -9.16410582597125e-46, + "timestamp": 1.2645964161981418 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlow.1.traj b/src/main/deploy/choreo/TestSlow.1.traj new file mode 100644 index 00000000..21c28ce9 --- /dev/null +++ b/src/main/deploy/choreo/TestSlow.1.traj @@ -0,0 +1,193 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -5.199035045340379e-36, + "velocityY": -7.094659994161304e-34, + "timestamp": 0 + }, + { + "x": 2.0394332924621374, + "y": 4.999539780571386, + "heading": 3.14, + "angularVelocity": 3.467230192636983e-17, + "velocityX": 0.4094219230255359, + "velocityY": -0.00477829548872164, + "timestamp": 0.09631456022367899 + }, + { + "x": 2.118299876878191, + "y": 4.998619341720087, + "heading": 3.14, + "angularVelocity": 6.719128759652369e-17, + "velocityX": 0.81884384077439, + "velocityY": -0.009556590915859996, + "timestamp": 0.19262912044735797 + }, + { + "x": 2.236599752604414, + "y": 4.9972386834536175, + "heading": 3.14, + "angularVelocity": 9.970677800237134e-17, + "velocityX": 1.2282657518394462, + "velocityY": -0.014334886264992857, + "timestamp": 0.28894368067103693 + }, + { + "x": 2.3943329187989826, + "y": 4.995397805781803, + "heading": 3.14, + "angularVelocity": 1.8923134419468757e-16, + "velocityX": 1.6376876541641503, + "velocityY": -0.019113181512118518, + "timestamp": 0.38525824089471594 + }, + { + "x": 2.591499374313956, + "y": 4.993096708718039, + "heading": 3.14, + "angularVelocity": 2.1985978708871568e-16, + "velocityX": 2.04710954457019, + "velocityY": -0.023891476620143418, + "timestamp": 0.48157280111839496 + }, + { + "x": 2.828099117491197, + "y": 4.990335392281679, + "heading": 3.14, + "angularVelocity": 3.641000187669834e-16, + "velocityX": 2.4565314177603748, + "velocityY": -0.02866977152724492, + "timestamp": 0.577887361342074 + }, + { + "x": 3.104132145725059, + "y": 4.987113856503132, + "heading": 3.14, + "angularVelocity": 4.534970646348135e-16, + "velocityX": 2.8659532638970524, + "velocityY": -0.03344806611860942, + "timestamp": 0.674201921565753 + }, + { + "x": 3.4195984543253704, + "y": 4.983432101437137, + "heading": 3.14, + "angularVelocity": 5.427823235427362e-16, + "velocityX": 3.275375061337336, + "velocityY": -0.038226360141646376, + "timestamp": 0.770516481789432 + }, + { + "x": 3.7744980323483457, + "y": 4.979290127211418, + "heading": 3.14, + "angularVelocity": 5.764340593314991e-16, + "velocityX": 3.6847967451521693, + "velocityY": -0.04300465283857951, + "timestamp": 0.866831042013111 + }, + { + "x": 4.168830825073529, + "y": 4.974687934464607, + "heading": 3.14, + "angularVelocity": 6.655918603013656e-16, + "velocityX": 4.094217860823843, + "velocityY": -0.047782938904807865, + "timestamp": 0.96314560223679 + }, + { + "x": 4.523730403096504, + "y": 4.970545960238888, + "heading": 3.14, + "angularVelocity": 5.770882548873366e-16, + "velocityX": 3.6847967451521697, + "velocityY": -0.0430046528385795, + "timestamp": 1.059460162460469 + }, + { + "x": 4.8391967116968155, + "y": 4.966864205172893, + "heading": 3.14, + "angularVelocity": 5.455400313133093e-16, + "velocityX": 3.2753750613373365, + "velocityY": -0.038226360141646376, + "timestamp": 1.155774722684148 + }, + { + "x": 5.115229739930678, + "y": 4.963642669394346, + "heading": 3.14, + "angularVelocity": 4.560243469996038e-16, + "velocityX": 2.865953263897053, + "velocityY": -0.033448066118609426, + "timestamp": 1.252089282907827 + }, + { + "x": 5.3518294831079185, + "y": 4.960881352957986, + "heading": 3.14, + "angularVelocity": 3.664668961069072e-16, + "velocityX": 2.456531417760375, + "velocityY": -0.028669771527244925, + "timestamp": 1.348403843131506 + }, + { + "x": 5.548995938622893, + "y": 4.958580255894223, + "heading": 3.14, + "angularVelocity": 2.7689730770284007e-16, + "velocityX": 2.0471095445701906, + "velocityY": -0.023891476620143418, + "timestamp": 1.444718403355185 + }, + { + "x": 5.7067291048174615, + "y": 4.956739378222408, + "heading": 3.14, + "angularVelocity": 2.4248632720368313e-16, + "velocityX": 1.6376876541641505, + "velocityY": -0.019113181512118518, + "timestamp": 1.541032963578864 + }, + { + "x": 5.825028980543684, + "y": 4.955358719955939, + "heading": 3.14, + "angularVelocity": 1.5364358713309854e-16, + "velocityX": 1.2282657518394462, + "velocityY": -0.014334886264992859, + "timestamp": 1.637347523802543 + }, + { + "x": 5.903895564959738, + "y": 4.95443828110464, + "heading": 3.14, + "angularVelocity": 1.2105238270710193e-16, + "velocityX": 0.8188438407743899, + "velocityY": -0.00955659091586, + "timestamp": 1.733662084026222 + }, + { + "x": 5.943328857421875, + "y": 4.953978061676025, + "heading": 3.14, + "angularVelocity": 8.963250403234999e-17, + "velocityX": 0.4094219230255359, + "velocityY": -0.00477829548872164, + "timestamp": 1.829976644249901 + }, + { + "x": 5.943328857421875, + "y": 4.953978061676025, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -2.7383639654683927e-37, + "velocityY": 0, + "timestamp": 1.92629120447358 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlow.traj b/src/main/deploy/choreo/TestSlow.traj new file mode 100644 index 00000000..21c28ce9 --- /dev/null +++ b/src/main/deploy/choreo/TestSlow.traj @@ -0,0 +1,193 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -5.199035045340379e-36, + "velocityY": -7.094659994161304e-34, + "timestamp": 0 + }, + { + "x": 2.0394332924621374, + "y": 4.999539780571386, + "heading": 3.14, + "angularVelocity": 3.467230192636983e-17, + "velocityX": 0.4094219230255359, + "velocityY": -0.00477829548872164, + "timestamp": 0.09631456022367899 + }, + { + "x": 2.118299876878191, + "y": 4.998619341720087, + "heading": 3.14, + "angularVelocity": 6.719128759652369e-17, + "velocityX": 0.81884384077439, + "velocityY": -0.009556590915859996, + "timestamp": 0.19262912044735797 + }, + { + "x": 2.236599752604414, + "y": 4.9972386834536175, + "heading": 3.14, + "angularVelocity": 9.970677800237134e-17, + "velocityX": 1.2282657518394462, + "velocityY": -0.014334886264992857, + "timestamp": 0.28894368067103693 + }, + { + "x": 2.3943329187989826, + "y": 4.995397805781803, + "heading": 3.14, + "angularVelocity": 1.8923134419468757e-16, + "velocityX": 1.6376876541641503, + "velocityY": -0.019113181512118518, + "timestamp": 0.38525824089471594 + }, + { + "x": 2.591499374313956, + "y": 4.993096708718039, + "heading": 3.14, + "angularVelocity": 2.1985978708871568e-16, + "velocityX": 2.04710954457019, + "velocityY": -0.023891476620143418, + "timestamp": 0.48157280111839496 + }, + { + "x": 2.828099117491197, + "y": 4.990335392281679, + "heading": 3.14, + "angularVelocity": 3.641000187669834e-16, + "velocityX": 2.4565314177603748, + "velocityY": -0.02866977152724492, + "timestamp": 0.577887361342074 + }, + { + "x": 3.104132145725059, + "y": 4.987113856503132, + "heading": 3.14, + "angularVelocity": 4.534970646348135e-16, + "velocityX": 2.8659532638970524, + "velocityY": -0.03344806611860942, + "timestamp": 0.674201921565753 + }, + { + "x": 3.4195984543253704, + "y": 4.983432101437137, + "heading": 3.14, + "angularVelocity": 5.427823235427362e-16, + "velocityX": 3.275375061337336, + "velocityY": -0.038226360141646376, + "timestamp": 0.770516481789432 + }, + { + "x": 3.7744980323483457, + "y": 4.979290127211418, + "heading": 3.14, + "angularVelocity": 5.764340593314991e-16, + "velocityX": 3.6847967451521693, + "velocityY": -0.04300465283857951, + "timestamp": 0.866831042013111 + }, + { + "x": 4.168830825073529, + "y": 4.974687934464607, + "heading": 3.14, + "angularVelocity": 6.655918603013656e-16, + "velocityX": 4.094217860823843, + "velocityY": -0.047782938904807865, + "timestamp": 0.96314560223679 + }, + { + "x": 4.523730403096504, + "y": 4.970545960238888, + "heading": 3.14, + "angularVelocity": 5.770882548873366e-16, + "velocityX": 3.6847967451521697, + "velocityY": -0.0430046528385795, + "timestamp": 1.059460162460469 + }, + { + "x": 4.8391967116968155, + "y": 4.966864205172893, + "heading": 3.14, + "angularVelocity": 5.455400313133093e-16, + "velocityX": 3.2753750613373365, + "velocityY": -0.038226360141646376, + "timestamp": 1.155774722684148 + }, + { + "x": 5.115229739930678, + "y": 4.963642669394346, + "heading": 3.14, + "angularVelocity": 4.560243469996038e-16, + "velocityX": 2.865953263897053, + "velocityY": -0.033448066118609426, + "timestamp": 1.252089282907827 + }, + { + "x": 5.3518294831079185, + "y": 4.960881352957986, + "heading": 3.14, + "angularVelocity": 3.664668961069072e-16, + "velocityX": 2.456531417760375, + "velocityY": -0.028669771527244925, + "timestamp": 1.348403843131506 + }, + { + "x": 5.548995938622893, + "y": 4.958580255894223, + "heading": 3.14, + "angularVelocity": 2.7689730770284007e-16, + "velocityX": 2.0471095445701906, + "velocityY": -0.023891476620143418, + "timestamp": 1.444718403355185 + }, + { + "x": 5.7067291048174615, + "y": 4.956739378222408, + "heading": 3.14, + "angularVelocity": 2.4248632720368313e-16, + "velocityX": 1.6376876541641505, + "velocityY": -0.019113181512118518, + "timestamp": 1.541032963578864 + }, + { + "x": 5.825028980543684, + "y": 4.955358719955939, + "heading": 3.14, + "angularVelocity": 1.5364358713309854e-16, + "velocityX": 1.2282657518394462, + "velocityY": -0.014334886264992859, + "timestamp": 1.637347523802543 + }, + { + "x": 5.903895564959738, + "y": 4.95443828110464, + "heading": 3.14, + "angularVelocity": 1.2105238270710193e-16, + "velocityX": 0.8188438407743899, + "velocityY": -0.00955659091586, + "timestamp": 1.733662084026222 + }, + { + "x": 5.943328857421875, + "y": 4.953978061676025, + "heading": 3.14, + "angularVelocity": 8.963250403234999e-17, + "velocityX": 0.4094219230255359, + "velocityY": -0.00477829548872164, + "timestamp": 1.829976644249901 + }, + { + "x": 5.943328857421875, + "y": 4.953978061676025, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -2.7383639654683927e-37, + "velocityY": 0, + "timestamp": 1.92629120447358 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlowAngle.1.traj b/src/main/deploy/choreo/TestSlowAngle.1.traj new file mode 100644 index 00000000..a98e7f3a --- /dev/null +++ b/src/main/deploy/choreo/TestSlowAngle.1.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": -4.007563632250384e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.0357142884817963, + "y": 5.035714288481796, + "heading": -1.2128236829393102e-18, + "angularVelocity": -1.2188889104369536e-17, + "velocityX": 0.35892894881563847, + "velocityY": 0.3589289488156384, + "timestamp": 0.09950239065320052 + }, + { + "x": 2.107142864670486, + "y": 5.107142864670486, + "heading": -5.373326257747517e-18, + "angularVelocity": -4.181308929900821e-17, + "velocityX": 0.717857889843493, + "velocityY": 0.717857889843493, + "timestamp": 0.19900478130640104 + }, + { + "x": 2.2142857274037135, + "y": 5.2142857274037135, + "heading": -1.2480917932573193e-17, + "angularVelocity": -7.143136207987212e-17, + "velocityX": 1.0767868191896721, + "velocityY": 1.0767868191896723, + "timestamp": 0.2985071719596015 + }, + { + "x": 2.3571428747442225, + "y": 5.3571428747442225, + "heading": -1.6672401590500304e-17, + "angularVelocity": -4.212444844143106e-17, + "velocityX": 1.4357157290663938, + "velocityY": 1.4357157290663938, + "timestamp": 0.39800956261280207 + }, + { + "x": 2.5357143028174978, + "y": 5.535714302817498, + "heading": -2.3808332434080198e-17, + "angularVelocity": -7.171616995817411e-17, + "velocityX": 1.794644600004205, + "velocityY": 1.794644600004205, + "timestamp": 0.4975119532660026 + }, + { + "x": 2.75, + "y": 5.75, + "heading": -3.1767176762840175e-17, + "angularVelocity": -7.998645710471698e-17, + "velocityX": 2.1535733541253332, + "velocityY": 2.1535733541253332, + "timestamp": 0.5970143439192032 + }, + { + "x": 2.9642856971825022, + "y": 5.964285697182502, + "heading": -2.6049355888382788e-17, + "angularVelocity": 5.746415217336393e-17, + "velocityX": 2.1535733541253332, + "velocityY": 2.1535733541253332, + "timestamp": 0.6965167345724037 + }, + { + "x": 3.1428571252557775, + "y": 6.1428571252557775, + "heading": -2.323188559090568e-17, + "angularVelocity": 2.831559968636768e-17, + "velocityX": 1.794644600004205, + "velocityY": 1.7946446000042047, + "timestamp": 0.7960191252256043 + }, + { + "x": 3.2857142725962865, + "y": 6.2857142725962865, + "heading": -1.7457981849578456e-17, + "angularVelocity": 5.802778493002341e-17, + "velocityX": 1.4357157290663938, + "velocityY": 1.4357157290663936, + "timestamp": 0.8955215158788048 + }, + { + "x": 3.392857135329514, + "y": 6.392857135329514, + "heading": -8.729542404310101e-18, + "angularVelocity": 8.772089729841888e-17, + "velocityX": 1.0767868191896721, + "velocityY": 1.0767868191896721, + "timestamp": 0.9950239065320053 + }, + { + "x": 3.4642857115182037, + "y": 6.464285711518204, + "heading": -2.9100030696757928e-18, + "angularVelocity": 5.848642418502831e-17, + "velocityX": 0.7178578898434931, + "velocityY": 0.717857889843493, + "timestamp": 1.0945262971852059 + }, + { + "x": 3.5, + "y": 6.5, + "heading": -2.657468488554102e-35, + "angularVelocity": 2.92455577143697e-17, + "velocityX": 0.3589289488156385, + "velocityY": 0.3589289488156383, + "timestamp": 1.1940286878384063 + }, + { + "x": 3.5, + "y": 6.5, + "heading": -4.035178290149187e-35, + "angularVelocity": -1.3845993613099983e-34, + "velocityX": 0, + "velocityY": 0, + "timestamp": 1.2935310784916068 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlowAngle.traj b/src/main/deploy/choreo/TestSlowAngle.traj new file mode 100644 index 00000000..a98e7f3a --- /dev/null +++ b/src/main/deploy/choreo/TestSlowAngle.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": -4.007563632250384e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.0357142884817963, + "y": 5.035714288481796, + "heading": -1.2128236829393102e-18, + "angularVelocity": -1.2188889104369536e-17, + "velocityX": 0.35892894881563847, + "velocityY": 0.3589289488156384, + "timestamp": 0.09950239065320052 + }, + { + "x": 2.107142864670486, + "y": 5.107142864670486, + "heading": -5.373326257747517e-18, + "angularVelocity": -4.181308929900821e-17, + "velocityX": 0.717857889843493, + "velocityY": 0.717857889843493, + "timestamp": 0.19900478130640104 + }, + { + "x": 2.2142857274037135, + "y": 5.2142857274037135, + "heading": -1.2480917932573193e-17, + "angularVelocity": -7.143136207987212e-17, + "velocityX": 1.0767868191896721, + "velocityY": 1.0767868191896723, + "timestamp": 0.2985071719596015 + }, + { + "x": 2.3571428747442225, + "y": 5.3571428747442225, + "heading": -1.6672401590500304e-17, + "angularVelocity": -4.212444844143106e-17, + "velocityX": 1.4357157290663938, + "velocityY": 1.4357157290663938, + "timestamp": 0.39800956261280207 + }, + { + "x": 2.5357143028174978, + "y": 5.535714302817498, + "heading": -2.3808332434080198e-17, + "angularVelocity": -7.171616995817411e-17, + "velocityX": 1.794644600004205, + "velocityY": 1.794644600004205, + "timestamp": 0.4975119532660026 + }, + { + "x": 2.75, + "y": 5.75, + "heading": -3.1767176762840175e-17, + "angularVelocity": -7.998645710471698e-17, + "velocityX": 2.1535733541253332, + "velocityY": 2.1535733541253332, + "timestamp": 0.5970143439192032 + }, + { + "x": 2.9642856971825022, + "y": 5.964285697182502, + "heading": -2.6049355888382788e-17, + "angularVelocity": 5.746415217336393e-17, + "velocityX": 2.1535733541253332, + "velocityY": 2.1535733541253332, + "timestamp": 0.6965167345724037 + }, + { + "x": 3.1428571252557775, + "y": 6.1428571252557775, + "heading": -2.323188559090568e-17, + "angularVelocity": 2.831559968636768e-17, + "velocityX": 1.794644600004205, + "velocityY": 1.7946446000042047, + "timestamp": 0.7960191252256043 + }, + { + "x": 3.2857142725962865, + "y": 6.2857142725962865, + "heading": -1.7457981849578456e-17, + "angularVelocity": 5.802778493002341e-17, + "velocityX": 1.4357157290663938, + "velocityY": 1.4357157290663936, + "timestamp": 0.8955215158788048 + }, + { + "x": 3.392857135329514, + "y": 6.392857135329514, + "heading": -8.729542404310101e-18, + "angularVelocity": 8.772089729841888e-17, + "velocityX": 1.0767868191896721, + "velocityY": 1.0767868191896721, + "timestamp": 0.9950239065320053 + }, + { + "x": 3.4642857115182037, + "y": 6.464285711518204, + "heading": -2.9100030696757928e-18, + "angularVelocity": 5.848642418502831e-17, + "velocityX": 0.7178578898434931, + "velocityY": 0.717857889843493, + "timestamp": 1.0945262971852059 + }, + { + "x": 3.5, + "y": 6.5, + "heading": -2.657468488554102e-35, + "angularVelocity": 2.92455577143697e-17, + "velocityX": 0.3589289488156385, + "velocityY": 0.3589289488156383, + "timestamp": 1.1940286878384063 + }, + { + "x": 3.5, + "y": 6.5, + "heading": -4.035178290149187e-35, + "angularVelocity": -1.3845993613099983e-34, + "velocityX": 0, + "velocityY": 0, + "timestamp": 1.2935310784916068 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlowOther.1.traj b/src/main/deploy/choreo/TestSlowOther.1.traj new file mode 100644 index 00000000..b7cfebda --- /dev/null +++ b/src/main/deploy/choreo/TestSlowOther.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": -1.0952306037480514e-39, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2, + "y": 4.953124995425155, + "heading": -3.164240520763351e-24, + "angularVelocity": -3.300992685598943e-23, + "velocityX": -6.527754769699201e-35, + "velocityY": -0.4890076415022641, + "timestamp": 0.09585740711707923 + }, + { + "x": 2, + "y": 4.859374987079833, + "heading": 2.5713915074059014e-24, + "angularVelocity": 5.983483597695269e-23, + "velocityX": -1.097538789626215e-34, + "velocityY": -0.9780152746132214, + "timestamp": 0.19171481423415845 + }, + { + "x": 2, + "y": 4.718749976060901, + "heading": 4.917772934580374e-23, + "angularVelocity": 4.862047876322287e-22, + "velocityX": -1.7992669720861215e-34, + "velocityY": -1.4670228962814877, + "timestamp": 0.28757222135123767 + }, + { + "x": 2, + "y": 4.531249963952723, + "heading": 1.1407687461769059e-22, + "angularVelocity": 6.770381391038879e-22, + "velocityX": -2.637563533921278e-34, + "velocityY": -1.9560305014214234, + "timestamp": 0.3834296284683169 + }, + { + "x": 2, + "y": 4.296874953245012, + "heading": 1.7398143338071676e-22, + "angularVelocity": 6.249336120704708e-22, + "velocityX": -1.4629288028921842e-34, + "velocityY": -2.4450380805882697, + "timestamp": 0.47928703558539615 + }, + { + "x": 2, + "y": 4.015624948419249, + "heading": -1.9887084571420204e-23, + "angularVelocity": -2.0224683409995954e-21, + "velocityX": -1.4112424782343753e-34, + "velocityY": -2.9340456130035593, + "timestamp": 0.5751444427024753 + }, + { + "x": 2, + "y": 3.6874999599322256, + "heading": -1.9829338642916739e-22, + "angularVelocity": -1.8611642258638035e-21, + "velocityX": -1.5151681626967904e-34, + "velocityY": -3.423053036331916, + "timestamp": 0.6710018498195546 + }, + { + "x": 2, + "y": 3.3125000400677727, + "heading": 5.600506325716781e-22, + "angularVelocity": 7.911168467222027e-21, + "velocityX": 1.4035532697888077e-34, + "velocityY": -3.912059914226877, + "timestamp": 0.7668592569366338 + }, + { + "x": 2, + "y": 2.98437505158075, + "heading": 5.045684206190348e-22, + "angularVelocity": -5.787987008981493e-22, + "velocityX": 2.835770758852267e-34, + "velocityY": -3.4230530363319156, + "timestamp": 0.8627166640537131 + }, + { + "x": 2, + "y": 2.7031250467549874, + "heading": 3.662231168806704e-22, + "angularVelocity": -1.4432401825905735e-21, + "velocityX": 2.8662185750679713e-34, + "velocityY": -2.9340456130035593, + "timestamp": 0.9585740711707923 + }, + { + "x": 2, + "y": 2.468750036047276, + "heading": 2.1828595317613215e-22, + "angularVelocity": -1.5433041868967679e-21, + "velocityX": 3.2780707212611828e-34, + "velocityY": -2.4450380805882697, + "timestamp": 1.0544314782878714 + }, + { + "x": 2, + "y": 2.281250023939098, + "heading": 1.5076855477401922e-22, + "angularVelocity": -7.043523061795029e-22, + "velocityX": 8.389359710572522e-35, + "velocityY": -1.9560305014214234, + "timestamp": 1.1502888854049507 + }, + { + "x": 2, + "y": 2.1406250129201667, + "heading": 8.548130994950006e-23, + "angularVelocity": -6.8108706582588185e-22, + "velocityX": -5.159202013607646e-35, + "velocityY": -1.4670228962814877, + "timestamp": 1.24614629252203 + }, + { + "x": 2, + "y": 2.0468750045748454, + "heading": 3.182367857810666e-23, + "angularVelocity": -5.597651120163003e-22, + "velocityX": 7.019719734335023e-37, + "velocityY": -0.9780152746132214, + "timestamp": 1.3420036996391091 + }, + { + "x": 2, + "y": 2, + "heading": 0, + "angularVelocity": -3.3198979387898458e-22, + "velocityX": -1.3716459797394807e-35, + "velocityY": -0.4890076415022641, + "timestamp": 1.4378611067561884 + }, + { + "x": 2, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 1.0385403263386622e-38, + "timestamp": 1.5337185138732676 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlowOther.traj b/src/main/deploy/choreo/TestSlowOther.traj new file mode 100644 index 00000000..b7cfebda --- /dev/null +++ b/src/main/deploy/choreo/TestSlowOther.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": -1.0952306037480514e-39, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2, + "y": 4.953124995425155, + "heading": -3.164240520763351e-24, + "angularVelocity": -3.300992685598943e-23, + "velocityX": -6.527754769699201e-35, + "velocityY": -0.4890076415022641, + "timestamp": 0.09585740711707923 + }, + { + "x": 2, + "y": 4.859374987079833, + "heading": 2.5713915074059014e-24, + "angularVelocity": 5.983483597695269e-23, + "velocityX": -1.097538789626215e-34, + "velocityY": -0.9780152746132214, + "timestamp": 0.19171481423415845 + }, + { + "x": 2, + "y": 4.718749976060901, + "heading": 4.917772934580374e-23, + "angularVelocity": 4.862047876322287e-22, + "velocityX": -1.7992669720861215e-34, + "velocityY": -1.4670228962814877, + "timestamp": 0.28757222135123767 + }, + { + "x": 2, + "y": 4.531249963952723, + "heading": 1.1407687461769059e-22, + "angularVelocity": 6.770381391038879e-22, + "velocityX": -2.637563533921278e-34, + "velocityY": -1.9560305014214234, + "timestamp": 0.3834296284683169 + }, + { + "x": 2, + "y": 4.296874953245012, + "heading": 1.7398143338071676e-22, + "angularVelocity": 6.249336120704708e-22, + "velocityX": -1.4629288028921842e-34, + "velocityY": -2.4450380805882697, + "timestamp": 0.47928703558539615 + }, + { + "x": 2, + "y": 4.015624948419249, + "heading": -1.9887084571420204e-23, + "angularVelocity": -2.0224683409995954e-21, + "velocityX": -1.4112424782343753e-34, + "velocityY": -2.9340456130035593, + "timestamp": 0.5751444427024753 + }, + { + "x": 2, + "y": 3.6874999599322256, + "heading": -1.9829338642916739e-22, + "angularVelocity": -1.8611642258638035e-21, + "velocityX": -1.5151681626967904e-34, + "velocityY": -3.423053036331916, + "timestamp": 0.6710018498195546 + }, + { + "x": 2, + "y": 3.3125000400677727, + "heading": 5.600506325716781e-22, + "angularVelocity": 7.911168467222027e-21, + "velocityX": 1.4035532697888077e-34, + "velocityY": -3.912059914226877, + "timestamp": 0.7668592569366338 + }, + { + "x": 2, + "y": 2.98437505158075, + "heading": 5.045684206190348e-22, + "angularVelocity": -5.787987008981493e-22, + "velocityX": 2.835770758852267e-34, + "velocityY": -3.4230530363319156, + "timestamp": 0.8627166640537131 + }, + { + "x": 2, + "y": 2.7031250467549874, + "heading": 3.662231168806704e-22, + "angularVelocity": -1.4432401825905735e-21, + "velocityX": 2.8662185750679713e-34, + "velocityY": -2.9340456130035593, + "timestamp": 0.9585740711707923 + }, + { + "x": 2, + "y": 2.468750036047276, + "heading": 2.1828595317613215e-22, + "angularVelocity": -1.5433041868967679e-21, + "velocityX": 3.2780707212611828e-34, + "velocityY": -2.4450380805882697, + "timestamp": 1.0544314782878714 + }, + { + "x": 2, + "y": 2.281250023939098, + "heading": 1.5076855477401922e-22, + "angularVelocity": -7.043523061795029e-22, + "velocityX": 8.389359710572522e-35, + "velocityY": -1.9560305014214234, + "timestamp": 1.1502888854049507 + }, + { + "x": 2, + "y": 2.1406250129201667, + "heading": 8.548130994950006e-23, + "angularVelocity": -6.8108706582588185e-22, + "velocityX": -5.159202013607646e-35, + "velocityY": -1.4670228962814877, + "timestamp": 1.24614629252203 + }, + { + "x": 2, + "y": 2.0468750045748454, + "heading": 3.182367857810666e-23, + "angularVelocity": -5.597651120163003e-22, + "velocityX": 7.019719734335023e-37, + "velocityY": -0.9780152746132214, + "timestamp": 1.3420036996391091 + }, + { + "x": 2, + "y": 2, + "heading": 0, + "angularVelocity": -3.3198979387898458e-22, + "velocityX": -1.3716459797394807e-35, + "velocityY": -0.4890076415022641, + "timestamp": 1.4378611067561884 + }, + { + "x": 2, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 1.0385403263386622e-38, + "timestamp": 1.5337185138732676 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlowRotate.1.traj b/src/main/deploy/choreo/TestSlowRotate.1.traj new file mode 100644 index 00000000..432b460e --- /dev/null +++ b/src/main/deploy/choreo/TestSlowRotate.1.traj @@ -0,0 +1,247 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.9208048805528434e-34, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.02366251246836, + "y": 4.9999997052109455, + "heading": 0.00950969246516044, + "angularVelocity": 0.1391965172175312, + "velocityX": 0.3463560295224064, + "velocityY": -0.000004314924996580056, + "timestamp": 0.0683184655424896 + }, + { + "x": 2.0709880449186984, + "y": 4.999999379781912, + "heading": 0.028512388083077194, + "angularVelocity": 0.27814874744366697, + "velocityX": 0.6927194876896832, + "velocityY": -0.000004763412508314695, + "timestamp": 0.1366369310849792 + }, + { + "x": 2.1419772416715244, + "y": 4.999999426144968, + "heading": 0.05698644572679429, + "angularVelocity": 0.4167842093292944, + "velocityX": 1.0390923769895746, + "velocityY": 6.786314042185358e-7, + "timestamp": 0.2049553966274688 + }, + { + "x": 2.2366309542982097, + "y": 5.000000358617276, + "heading": 0.09490253039039682, + "angularVelocity": 0.5549902850206919, + "velocityX": 1.385477731021595, + "velocityY": 0.000013648905905289525, + "timestamp": 0.2732738621699584 + }, + { + "x": 2.3549503342395997, + "y": 5.000002752941593, + "heading": 0.14222003904242062, + "angularVelocity": 0.6926020406971164, + "velocityX": 1.7318799390744954, + "velocityY": 0.000035046517766188305, + "timestamp": 0.341592327712448 + }, + { + "x": 2.4969369478040657, + "y": 5.000007160826493, + "heading": 0.1988827529646636, + "angularVelocity": 0.8293909043815195, + "velocityX": 2.0783050737016247, + "velocityY": 0.00006451967071180592, + "timestamp": 0.4099107932549376 + }, + { + "x": 2.662592916041132, + "y": 5.000013981415258, + "heading": 0.26481373895622135, + "angularVelocity": 0.9650536713321403, + "velocityX": 2.424761254832917, + "velocityY": 0.00009983521601619577, + "timestamp": 0.4782292587974272 + }, + { + "x": 2.851921094530713, + "y": 5.000023289694376, + "heading": 0.3399091190697772, + "angularVelocity": 1.0991959423744913, + "velocityX": 2.7712592340328714, + "velocityY": 0.00013624836335922213, + "timestamp": 0.5465477243399168 + }, + { + "x": 3.064925351863494, + "y": 5.000034634675967, + "heading": 0.4240287650700209, + "angularVelocity": 1.231287110040946, + "velocityX": 3.1178138390755596, + "velocityY": 0.00016606025181951199, + "timestamp": 0.6148661898824064 + }, + { + "x": 3.301611146859007, + "y": 5.000046829573855, + "heading": 0.51697668272804, + "angularVelocity": 1.3605094452862343, + "velocityX": 3.464448346667129, + "velocityY": 0.00017850075805292458, + "timestamp": 0.6831846554248959 + }, + { + "x": 3.56198711871686, + "y": 5.000057723571391, + "heading": 0.6184437013686873, + "angularVelocity": 1.4852063469947503, + "velocityX": 3.8112093090837407, + "velocityY": 0.00015945904887937467, + "timestamp": 0.7515031209673855 + }, + { + "x": 3.8460710207555886, + "y": 5.000063620513341, + "heading": 0.7277730701439165, + "angularVelocity": 1.6002901690939435, + "velocityX": 4.158230132701782, + "velocityY": 0.00008631549175548375, + "timestamp": 0.819821586509875 + }, + { + "x": 4.153930046387012, + "y": 5.000052301182086, + "heading": 0.8417150362226031, + "angularVelocity": 1.6678062830293265, + "velocityX": 4.506234488535969, + "velocityY": -0.00016568479933949432, + "timestamp": 0.8881400520523646 + }, + { + "x": 4.4380104929852475, + "y": 5.000038330055544, + "heading": 0.9512759924106793, + "angularVelocity": 1.6036799907330455, + "velocityX": 4.158179554275207, + "velocityY": -0.00020450000495440387, + "timestamp": 0.9564585175948541 + }, + { + "x": 4.69838319238425, + "y": 5.000025271205214, + "heading": 1.052957052286739, + "angularVelocity": 1.4883393394253035, + "velocityX": 3.811161409020038, + "velocityY": -0.00019114671598567193, + "timestamp": 1.0247769831373437 + }, + { + "x": 4.935066721530443, + "y": 5.000014536146546, + "heading": 1.146065613041752, + "angularVelocity": 1.36286083148494, + "velocityX": 3.4644151806804317, + "velocityY": -0.00015713260805078636, + "timestamp": 1.0930954486798332 + }, + { + "x": 5.148069980355881, + "y": 5.000006628646695, + "heading": 1.2302821189406512, + "angularVelocity": 1.232704880447324, + "velocityX": 3.117799223592987, + "velocityY": -0.00011574469345328521, + "timestamp": 1.1614139142223228 + }, + { + "x": 5.337398336214855, + "y": 5.000001474581276, + "heading": 1.3054155698448289, + "angularVelocity": 1.0997531971418355, + "velocityX": 2.7712618302474024, + "velocityY": -0.00007544176201745666, + "timestamp": 1.2297323797648123 + }, + { + "x": 5.5030553642442035, + "y": 4.999998653511926, + "heading": 1.3713394215549772, + "angularVelocity": 0.9649492444930273, + "velocityX": 2.424776767363443, + "velocityY": -0.00004129292611118568, + "timestamp": 1.2980508453073019 + }, + { + "x": 5.645043553923344, + "y": 4.999997581430543, + "heading": 1.427966299920808, + "angularVelocity": 0.8288663674773633, + "velocityX": 2.078328143813961, + "velocityY": -0.00001569241016637338, + "timestamp": 1.3663693108497914 + }, + { + "x": 5.763364679087044, + "y": 4.999997644896944, + "heading": 1.4752347555606973, + "angularVelocity": 0.6918840355173265, + "velocityX": 1.7319054844712716, + "velocityY": 9.289787179214142e-7, + "timestamp": 1.434687776392281 + }, + { + "x": 5.85802002824266, + "y": 4.999998289799587, + "heading": 1.51310089738515, + "angularVelocity": 0.554259255148266, + "velocityX": 1.385501685437416, + "velocityY": 0.000009439653500196435, + "timestamp": 1.5030062419347705 + }, + { + "x": 5.929010561462276, + "y": 4.999999073832564, + "heading": 1.5415326066657604, + "angularVelocity": 0.4161643423171998, + "velocityX": 1.039111939296464, + "velocityY": 0.000011476150277595035, + "timestamp": 1.57132470747726 + }, + { + "x": 5.976337019868017, + "y": 4.999999694057604, + "heading": 1.5605054658851873, + "angularVelocity": 0.2777120222003084, + "velocityX": 0.6927330412054942, + "velocityY": 0.000009078439254067869, + "timestamp": 1.6396431730197496 + }, + { + "x": 6, + "y": 5, + "heading": 1.57, + "angularVelocity": 0.1389746394246477, + "velocityX": 0.3463628748696997, + "velocityY": 0.000004478180145835614, + "timestamp": 1.7079616385622391 + }, + { + "x": 6, + "y": 5, + "heading": 1.57, + "angularVelocity": 0, + "velocityX": -2.0074835073437163e-36, + "velocityY": 0, + "timestamp": 1.7762801041047287 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/TestSlowRotate.traj b/src/main/deploy/choreo/TestSlowRotate.traj new file mode 100644 index 00000000..432b460e --- /dev/null +++ b/src/main/deploy/choreo/TestSlowRotate.traj @@ -0,0 +1,247 @@ +{ + "samples": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.9208048805528434e-34, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.02366251246836, + "y": 4.9999997052109455, + "heading": 0.00950969246516044, + "angularVelocity": 0.1391965172175312, + "velocityX": 0.3463560295224064, + "velocityY": -0.000004314924996580056, + "timestamp": 0.0683184655424896 + }, + { + "x": 2.0709880449186984, + "y": 4.999999379781912, + "heading": 0.028512388083077194, + "angularVelocity": 0.27814874744366697, + "velocityX": 0.6927194876896832, + "velocityY": -0.000004763412508314695, + "timestamp": 0.1366369310849792 + }, + { + "x": 2.1419772416715244, + "y": 4.999999426144968, + "heading": 0.05698644572679429, + "angularVelocity": 0.4167842093292944, + "velocityX": 1.0390923769895746, + "velocityY": 6.786314042185358e-7, + "timestamp": 0.2049553966274688 + }, + { + "x": 2.2366309542982097, + "y": 5.000000358617276, + "heading": 0.09490253039039682, + "angularVelocity": 0.5549902850206919, + "velocityX": 1.385477731021595, + "velocityY": 0.000013648905905289525, + "timestamp": 0.2732738621699584 + }, + { + "x": 2.3549503342395997, + "y": 5.000002752941593, + "heading": 0.14222003904242062, + "angularVelocity": 0.6926020406971164, + "velocityX": 1.7318799390744954, + "velocityY": 0.000035046517766188305, + "timestamp": 0.341592327712448 + }, + { + "x": 2.4969369478040657, + "y": 5.000007160826493, + "heading": 0.1988827529646636, + "angularVelocity": 0.8293909043815195, + "velocityX": 2.0783050737016247, + "velocityY": 0.00006451967071180592, + "timestamp": 0.4099107932549376 + }, + { + "x": 2.662592916041132, + "y": 5.000013981415258, + "heading": 0.26481373895622135, + "angularVelocity": 0.9650536713321403, + "velocityX": 2.424761254832917, + "velocityY": 0.00009983521601619577, + "timestamp": 0.4782292587974272 + }, + { + "x": 2.851921094530713, + "y": 5.000023289694376, + "heading": 0.3399091190697772, + "angularVelocity": 1.0991959423744913, + "velocityX": 2.7712592340328714, + "velocityY": 0.00013624836335922213, + "timestamp": 0.5465477243399168 + }, + { + "x": 3.064925351863494, + "y": 5.000034634675967, + "heading": 0.4240287650700209, + "angularVelocity": 1.231287110040946, + "velocityX": 3.1178138390755596, + "velocityY": 0.00016606025181951199, + "timestamp": 0.6148661898824064 + }, + { + "x": 3.301611146859007, + "y": 5.000046829573855, + "heading": 0.51697668272804, + "angularVelocity": 1.3605094452862343, + "velocityX": 3.464448346667129, + "velocityY": 0.00017850075805292458, + "timestamp": 0.6831846554248959 + }, + { + "x": 3.56198711871686, + "y": 5.000057723571391, + "heading": 0.6184437013686873, + "angularVelocity": 1.4852063469947503, + "velocityX": 3.8112093090837407, + "velocityY": 0.00015945904887937467, + "timestamp": 0.7515031209673855 + }, + { + "x": 3.8460710207555886, + "y": 5.000063620513341, + "heading": 0.7277730701439165, + "angularVelocity": 1.6002901690939435, + "velocityX": 4.158230132701782, + "velocityY": 0.00008631549175548375, + "timestamp": 0.819821586509875 + }, + { + "x": 4.153930046387012, + "y": 5.000052301182086, + "heading": 0.8417150362226031, + "angularVelocity": 1.6678062830293265, + "velocityX": 4.506234488535969, + "velocityY": -0.00016568479933949432, + "timestamp": 0.8881400520523646 + }, + { + "x": 4.4380104929852475, + "y": 5.000038330055544, + "heading": 0.9512759924106793, + "angularVelocity": 1.6036799907330455, + "velocityX": 4.158179554275207, + "velocityY": -0.00020450000495440387, + "timestamp": 0.9564585175948541 + }, + { + "x": 4.69838319238425, + "y": 5.000025271205214, + "heading": 1.052957052286739, + "angularVelocity": 1.4883393394253035, + "velocityX": 3.811161409020038, + "velocityY": -0.00019114671598567193, + "timestamp": 1.0247769831373437 + }, + { + "x": 4.935066721530443, + "y": 5.000014536146546, + "heading": 1.146065613041752, + "angularVelocity": 1.36286083148494, + "velocityX": 3.4644151806804317, + "velocityY": -0.00015713260805078636, + "timestamp": 1.0930954486798332 + }, + { + "x": 5.148069980355881, + "y": 5.000006628646695, + "heading": 1.2302821189406512, + "angularVelocity": 1.232704880447324, + "velocityX": 3.117799223592987, + "velocityY": -0.00011574469345328521, + "timestamp": 1.1614139142223228 + }, + { + "x": 5.337398336214855, + "y": 5.000001474581276, + "heading": 1.3054155698448289, + "angularVelocity": 1.0997531971418355, + "velocityX": 2.7712618302474024, + "velocityY": -0.00007544176201745666, + "timestamp": 1.2297323797648123 + }, + { + "x": 5.5030553642442035, + "y": 4.999998653511926, + "heading": 1.3713394215549772, + "angularVelocity": 0.9649492444930273, + "velocityX": 2.424776767363443, + "velocityY": -0.00004129292611118568, + "timestamp": 1.2980508453073019 + }, + { + "x": 5.645043553923344, + "y": 4.999997581430543, + "heading": 1.427966299920808, + "angularVelocity": 0.8288663674773633, + "velocityX": 2.078328143813961, + "velocityY": -0.00001569241016637338, + "timestamp": 1.3663693108497914 + }, + { + "x": 5.763364679087044, + "y": 4.999997644896944, + "heading": 1.4752347555606973, + "angularVelocity": 0.6918840355173265, + "velocityX": 1.7319054844712716, + "velocityY": 9.289787179214142e-7, + "timestamp": 1.434687776392281 + }, + { + "x": 5.85802002824266, + "y": 4.999998289799587, + "heading": 1.51310089738515, + "angularVelocity": 0.554259255148266, + "velocityX": 1.385501685437416, + "velocityY": 0.000009439653500196435, + "timestamp": 1.5030062419347705 + }, + { + "x": 5.929010561462276, + "y": 4.999999073832564, + "heading": 1.5415326066657604, + "angularVelocity": 0.4161643423171998, + "velocityX": 1.039111939296464, + "velocityY": 0.000011476150277595035, + "timestamp": 1.57132470747726 + }, + { + "x": 5.976337019868017, + "y": 4.999999694057604, + "heading": 1.5605054658851873, + "angularVelocity": 0.2777120222003084, + "velocityX": 0.6927330412054942, + "velocityY": 0.000009078439254067869, + "timestamp": 1.6396431730197496 + }, + { + "x": 6, + "y": 5, + "heading": 1.57, + "angularVelocity": 0.1389746394246477, + "velocityX": 0.3463628748696997, + "velocityY": 0.000004478180145835614, + "timestamp": 1.7079616385622391 + }, + { + "x": 6, + "y": 5, + "heading": 1.57, + "angularVelocity": 0, + "velocityX": -2.0074835073437163e-36, + "velocityY": 0, + "timestamp": 1.7762801041047287 + } + ] +} \ No newline at end of file diff --git a/src/main/java/org/wildstang/framework/auto/steps/PathHeadingStep.java b/src/main/java/org/wildstang/framework/auto/steps/PathHeadingStep.java index a4faa1ce..c2950d49 100644 --- a/src/main/java/org/wildstang/framework/auto/steps/PathHeadingStep.java +++ b/src/main/java/org/wildstang/framework/auto/steps/PathHeadingStep.java @@ -1,6 +1,7 @@ package org.wildstang.framework.auto.steps; import org.wildstang.framework.auto.AutoStep; +import org.wildstang.framework.core.Core; import org.wildstang.framework.subsystems.swerve.SwerveDriveTemplate; public class PathHeadingStep extends AutoStep { @@ -21,7 +22,7 @@ public PathHeadingStep(double heading, SwerveDriveTemplate drive) { @Override public void initialize() { //give robot heading controller a new value - m_drive.setAutoHeading(heading); + m_drive.setAutoHeading(Core.isBlue() ? heading : 360-heading); } @Override diff --git a/src/main/java/org/wildstang/framework/auto/steps/SetGyroStep.java b/src/main/java/org/wildstang/framework/auto/steps/SetGyroStep.java index 25135cf6..89085012 100644 --- a/src/main/java/org/wildstang/framework/auto/steps/SetGyroStep.java +++ b/src/main/java/org/wildstang/framework/auto/steps/SetGyroStep.java @@ -8,7 +8,8 @@ public class SetGyroStep extends AutoStep { private SwerveDriveTemplate m_drive; private double heading; - /** sets the gyro value to be the given argument value + /** DEPRECATED use AutoSetupStep instead + * sets the gyro value to be the given argument value * @param heading value you want the gyro to currently read * @param drive the swerveDrive subsystem */ @@ -20,7 +21,7 @@ public SetGyroStep(double heading, SwerveDriveTemplate drive) { @Override public void initialize() { m_drive.setToAuto(); - m_drive.setAutoValues(0.0, 0.0, 0,0, 0); + m_drive.setAutoValues(0.0, 0.0, 0,0); m_drive.setGyro(heading); m_drive.setAutoHeading(heading); } diff --git a/src/main/java/org/wildstang/framework/auto/steps/SplitGroup.java b/src/main/java/org/wildstang/framework/auto/steps/SplitGroup.java new file mode 100644 index 00000000..73e7250e --- /dev/null +++ b/src/main/java/org/wildstang/framework/auto/steps/SplitGroup.java @@ -0,0 +1,31 @@ +package org.wildstang.framework.auto.steps; + +import org.wildstang.framework.auto.AutoStep; + +public class SplitGroup{ + + AutoStep mainStep; + AutoStep[] steps; + AutoParallelStepGroup parallel = new AutoParallelStepGroup(); + AutoSerialStepGroup serial = new AutoSerialStepGroup(); + + public SplitGroup(AutoStep step, AutoStep[] otherSteps){ + this.mainStep = step; + this.steps = otherSteps; + AutoParallelStepGroup parallel = new AutoParallelStepGroup(); + AutoSerialStepGroup serial = new AutoSerialStepGroup(); + parallel.addStep(mainStep); + for (int i = 0; i < steps.length; i++){ + serial.addStep(steps[i]); + } + parallel.addStep(serial); + } + public AutoStep get(){ + return parallel; + } + + public String toString() { + return "Split Group"; + } + +} diff --git a/src/main/java/org/wildstang/framework/auto/steps/SwervePathFollowerStep.java b/src/main/java/org/wildstang/framework/auto/steps/SwervePathFollowerStep.java index b9999b7f..05de4c3d 100644 --- a/src/main/java/org/wildstang/framework/auto/steps/SwervePathFollowerStep.java +++ b/src/main/java/org/wildstang/framework/auto/steps/SwervePathFollowerStep.java @@ -1,23 +1,31 @@ package org.wildstang.framework.auto.steps; +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; + import org.wildstang.framework.auto.AutoStep; +import org.wildstang.framework.core.Core; import org.wildstang.framework.subsystems.swerve.SwerveDriveTemplate; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.choreo.lib.*; +import com.google.gson.Gson; public class SwervePathFollowerStep extends AutoStep { private static final double mToIn = 39.3701; private SwerveDriveTemplate m_drive; private ChoreoTrajectory pathtraj; - private boolean isBlue; - private double xOffset, yOffset, prevVelocity, prevTime; - private Pose2d localAutoPose, localRobotPose; + // x and y field relative + private Pose2d fieldAutoPose, fieldRobotPose; + private double xOffset, yOffset; private Timer timer; @@ -27,12 +35,9 @@ public class SwervePathFollowerStep extends AutoStep { * @param drive the swerveDrive subsystem * @param isBlue whether the robot is on the blue alliance */ - public SwervePathFollowerStep(ChoreoTrajectory pathData, SwerveDriveTemplate drive, boolean isBlue) { - this.pathtraj = pathData; + public SwervePathFollowerStep(String pathData, SwerveDriveTemplate drive) { + this.pathtraj = getTraj(pathData); m_drive = drive; - pathtraj = new ChoreoTrajectory(); - - this.isBlue = isBlue; timer = new Timer(); } @@ -41,29 +46,30 @@ public void initialize() { //start path m_drive.setToAuto(); timer.start(); - prevTime = 0.0; - prevVelocity = 0.0; } @Override public void update() { if (timer.get() >= pathtraj.getTotalTime()) { - m_drive.setAutoValues(0.0, -pathtraj.getFinalPose().getRotation().getDegrees(),0.0,0.0,0.0); + m_drive.setAutoValues(0.0,0.0,0.0,0.0); SmartDashboard.putNumber("auto final time", timer.get()); setFinished(); } else { - localRobotPose = m_drive.returnPose(); - localAutoPose = pathtraj.sample(timer.get()).getPose();//.poseMeters; - yOffset = -(localRobotPose.getX() - localAutoPose.getX()); - if (isBlue){ - xOffset = localRobotPose.getY() - localAutoPose.getY(); - } else { - xOffset = localRobotPose.getY() - (8.016 - localAutoPose.getY()); - } - //update values the robot is tracking to - m_drive.setAutoValues( getVelocity(),getHeading(), getAccel(), 2.0*xOffset,2.0*yOffset ); - prevVelocity = getVelocity(); - prevTime = timer.get(); + ChoreoTrajectoryState sample = pathtraj.sample(timer.get()); + + + fieldRobotPose = m_drive.returnPose(); + fieldAutoPose = sample.getPose(); + + xOffset = -fieldAutoPose.getY() + (Core.isBlue() ? fieldRobotPose.getY() : 8.016-fieldRobotPose.getY()); + yOffset = fieldAutoPose.getX() - fieldRobotPose.getY(); + SmartDashboard.putNumber("xOffset", xOffset); + SmartDashboard.putNumber("yOffset", yOffset); + + m_drive.setAutoHeading(getHeading()); + m_drive.setAutoValues((Core.isBlue() ? -1 : 1) * sample.velocityY * mToIn, sample.velocityX * mToIn, xOffset, yOffset); + SmartDashboard.putNumber("PF local X", fieldRobotPose.getX()); + SmartDashboard.putNumber("PF path X", fieldAutoPose.getX()); } } @@ -72,14 +78,22 @@ public String toString() { return "Swerve Path Follower"; } - public double getVelocity(){ - return mToIn * Math.hypot(pathtraj.sample(timer.get()).velocityX, pathtraj.sample(timer.get()).velocityY); - } public double getHeading(){ - if (isBlue) return ((-pathtraj.sample(timer.get()).heading*180/Math.PI)+360)%360; + if (Core.isBlue()) return ((-pathtraj.sample(timer.get()).heading*180/Math.PI)+360)%360; else return ((pathtraj.sample(timer.get()).heading*180/Math.PI)+360)%360; } - public double getAccel(){ - return (getVelocity() - prevVelocity) / (timer.get() - prevTime); + + public ChoreoTrajectory getTraj(String fileName){ + Gson gson = new Gson(); + var tempfile = Filesystem.getDeployDirectory(); + var traj_dir = new File(tempfile, "choreo"); + + var traj_file = new File(traj_dir, fileName + ".traj"); + try { + var reader = new BufferedReader(new FileReader(traj_file)); + return gson.fromJson(reader, ChoreoTrajectory.class); + } catch (Exception ex) { + DriverStation.reportError("Choreo Trajectory get Error", ex.getStackTrace()); + }return new ChoreoTrajectory(); } } diff --git a/src/main/java/org/wildstang/framework/core/Core.java b/src/main/java/org/wildstang/framework/core/Core.java index de73339a..6357ef4f 100644 --- a/src/main/java/org/wildstang/framework/core/Core.java +++ b/src/main/java/org/wildstang/framework/core/Core.java @@ -14,6 +14,9 @@ import org.wildstang.framework.subsystems.Subsystem; import org.wildstang.framework.subsystems.SubsystemManager; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; + /** * Core of robot framework. */ @@ -29,6 +32,9 @@ public class Core { private Class m_inputFactoryClass; private Class m_outputFactoryClass; + private static Alliance s_alliance; + + private static boolean inAuto = false; /** * Constructor collects I/O factory and initialized framework components. @@ -126,6 +132,14 @@ public void createSubsystems(Subsystems[] p_subsystems) { s_subsystemManager.addSubsystem(sub); } + for (Subsystems subsystem : p_subsystems) { + Log.info("Creating subsystem: " + subsystem.getName()); + + // Instantiate the class + Subsystem sub = (Subsystem) s_subsystemManager.getSubsystem(subsystem); + // Call the init method + sub.initSubsystems(); + } } /** @@ -144,6 +158,24 @@ public void createAutoPrograms(AutoPrograms[] p_programs) { } } + public static Alliance getAlliance() { + return s_alliance; + } + + public static void setAlliance(Alliance alliance) { + s_alliance = alliance; + } + + public static Boolean isBlue() { + return s_alliance == Alliance.Blue; + } + public static boolean getIsInAuto(){ + return inAuto; + } + public static void setToTeleop(){ + inAuto = false; + } + /** * Returns the framework's InputManager. * @return InputManager belonging to the framework. diff --git a/src/main/java/org/wildstang/framework/subsystems/Subsystem.java b/src/main/java/org/wildstang/framework/subsystems/Subsystem.java index 425e4364..b50ae86c 100644 --- a/src/main/java/org/wildstang/framework/subsystems/Subsystem.java +++ b/src/main/java/org/wildstang/framework/subsystems/Subsystem.java @@ -33,6 +33,12 @@ public interface Subsystem extends InputListener { */ public void resetState(); + /** + * Initialize references to other subsystems here + * Called after all subsystems are initialized + */ + public void initSubsystems(); + /** * Returns the name of the subsystem. * diff --git a/src/main/java/org/wildstang/framework/subsystems/swerve/SwerveDriveTemplate.java b/src/main/java/org/wildstang/framework/subsystems/swerve/SwerveDriveTemplate.java index 8b3606f7..1b545462 100644 --- a/src/main/java/org/wildstang/framework/subsystems/swerve/SwerveDriveTemplate.java +++ b/src/main/java/org/wildstang/framework/subsystems/swerve/SwerveDriveTemplate.java @@ -5,10 +5,6 @@ import edu.wpi.first.math.geometry.Pose2d; public abstract class SwerveDriveTemplate implements Subsystem{ - - public abstract void setAutoValues(double velocity, double heading, double accel, double xOffset, double yOffset); - - public abstract void resetDriveEncoders(); public abstract void setAutoHeading(double headingTarget); @@ -20,4 +16,6 @@ public abstract class SwerveDriveTemplate implements Subsystem{ public abstract Pose2d returnPose(); + public abstract void setAutoValues(double xVelocity, double yVelocity, double xOffset, double yOffset); + } diff --git a/src/main/java/org/wildstang/hardware/roborio/outputs/WsSpark.java b/src/main/java/org/wildstang/hardware/roborio/outputs/WsSpark.java index 69bf8d34..f27af0b5 100644 --- a/src/main/java/org/wildstang/hardware/roborio/outputs/WsSpark.java +++ b/src/main/java/org/wildstang/hardware/roborio/outputs/WsSpark.java @@ -66,18 +66,6 @@ public WsSpark(String name, int channel, WsMotorControllers controller, double p isUsingController = false; isChanged = true; controlType = ControlType.kDutyCycle; - - warnFollower(); - } - - /** - * WsSpark should never be a follower. It is possible that follower state can get burned into the flash, this warns in that case. - */ - public void warnFollower() { - if (motor.isFollower()) { - // NOTE: this may also display if the motor cannot be found. - Log.error("WsSpark (" + this.getName() + ") is set as a follower!"); - } } /** @@ -154,6 +142,14 @@ public void setCurrentLimit(int stallLimitAmps, int freeLimitAmps, int limitRPM) motor.burnFlash(); } + /** + * Sets the current limit, but does not burn flash. Never use in init + * @param limit the amount of amps drawn before limiting + */ + public void tempCurrentLimit(int limit){ + motor.setSmartCurrentLimit(limit, limit, 0); + } + /** * Enables voltage compensation. */ @@ -221,20 +217,18 @@ public void sendDataToOutput() { } /** - * Wraps CANSparkBase.setSpeed(). + * Wraps setValue(). * @param value New motor percent speed, from -1.0 to 1.0. */ @Override public void setSpeed(double value){ - if (controlType == ControlType.kDutyCycle && super.getValue() == value){ + if (controlType == ControlType.kDutyCycle && super.getValue()==value){ isChanged = false; } else { isChanged = true; } controlType = ControlType.kDutyCycle; super.setSpeed(value); - - warnFollower(); } /** @@ -298,10 +292,6 @@ public void initClosedLoop(double P, double I, double D, double FF, AbsoluteEnco slotID = 0; } - /** - * Returns the motor controller's PID controller. - * @return PID Controller object - */ public SparkPIDController getPIDController(){ return motor.getPIDController(); } diff --git a/src/main/java/org/wildstang/hardware/roborio/outputs/config/WsSparkFollowerConfig.java b/src/main/java/org/wildstang/hardware/roborio/outputs/config/WsSparkFollowerConfig.java index 3468c4e5..7df6889b 100644 --- a/src/main/java/org/wildstang/hardware/roborio/outputs/config/WsSparkFollowerConfig.java +++ b/src/main/java/org/wildstang/hardware/roborio/outputs/config/WsSparkFollowerConfig.java @@ -1,6 +1,5 @@ package org.wildstang.hardware.roborio.outputs.config; -import org.wildstang.framework.core.Outputs; import org.wildstang.framework.hardware.OutputConfig; /** @@ -8,7 +7,7 @@ */ public class WsSparkFollowerConfig implements OutputConfig { - private Outputs following; + private String following; private int m_channel = 0; private boolean oppose; private WsMotorControllers controller; @@ -19,7 +18,7 @@ public class WsSparkFollowerConfig implements OutputConfig { * @param channel Hardware port number. * @param controller Enumeration representing type of controller. */ - public WsSparkFollowerConfig(Outputs following, int channel, WsMotorControllers controller) { + public WsSparkFollowerConfig(String following, int channel, WsMotorControllers controller) { this(following, channel, controller, false); } @@ -30,7 +29,7 @@ public WsSparkFollowerConfig(Outputs following, int channel, WsMotorControllers * @param controller Enumeration representing type of controller. * @param oppose True if the follow should oppose the direction of this motor. */ - public WsSparkFollowerConfig(Outputs following, int channel, WsMotorControllers controller, boolean oppose) { + public WsSparkFollowerConfig(String following, int channel, WsMotorControllers controller, boolean oppose) { this.following = following; m_channel = channel; this.controller = controller; @@ -41,7 +40,7 @@ public WsSparkFollowerConfig(Outputs following, int channel, WsMotorControllers * Returns the name of the followed motor controller. * @return The name of followed motor controller. */ - public Outputs getFollowing() { + public String getFollowing() { return following; } diff --git a/src/main/java/org/wildstang/sample/auto/Programs/Jubilee.java b/src/main/java/org/wildstang/sample/auto/Programs/Jubilee.java new file mode 100644 index 00000000..d5bbbd89 --- /dev/null +++ b/src/main/java/org/wildstang/sample/auto/Programs/Jubilee.java @@ -0,0 +1,29 @@ +package org.wildstang.sample.auto.Programs; + +import org.wildstang.framework.auto.AutoProgram; +import org.wildstang.framework.auto.steps.SwervePathFollowerStep; +import org.wildstang.framework.core.Core; +import org.wildstang.sample.auto.Steps.AutoSetupStep; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +// Test swerve auto program, named per Arms +public class Jubilee extends AutoProgram { + + @Override + protected void defineSteps() { + SwerveDrive swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + addStep(new AutoSetupStep(1.5, 5.5, 0, Alliance.Red)); + addStep(new SwervePathFollowerStep("Circle", swerve)); + addStep(new SwervePathFollowerStep("Circle", swerve)); + addStep(new SwervePathFollowerStep("Circle", swerve)); + } + + @Override + public String toString() { + return "Jubilee"; + } + +} diff --git a/src/main/java/org/wildstang/sample/auto/Programs/Line.java b/src/main/java/org/wildstang/sample/auto/Programs/Line.java new file mode 100644 index 00000000..f1726235 --- /dev/null +++ b/src/main/java/org/wildstang/sample/auto/Programs/Line.java @@ -0,0 +1,27 @@ +package org.wildstang.sample.auto.Programs; + +import org.wildstang.framework.auto.AutoProgram; +import org.wildstang.framework.auto.steps.SwervePathFollowerStep; +import org.wildstang.framework.core.Core; +import org.wildstang.sample.auto.Steps.AutoSetupStep; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +// Test swerve auto program, named per Arms +public class Line extends AutoProgram { + + @Override + protected void defineSteps() { + SwerveDrive swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + addStep(new AutoSetupStep(1.5, 5.5, 0, Alliance.Blue)); + addStep(new SwervePathFollowerStep("Line", swerve)); + } + + @Override + public String toString() { + return "Line"; + } + +} diff --git a/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java b/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java index fcc8859b..9cb3ba59 100644 --- a/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java +++ b/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java @@ -2,27 +2,29 @@ import org.wildstang.framework.auto.AutoProgram; import org.wildstang.framework.auto.steps.AutoParallelStepGroup; +import org.wildstang.framework.auto.steps.AutoSerialStepGroup; import org.wildstang.framework.auto.steps.PathHeadingStep; -import org.wildstang.framework.auto.steps.SetGyroStep; import org.wildstang.framework.auto.steps.SwervePathFollowerStep; +import org.wildstang.framework.auto.steps.control.AutoStepDelay; import org.wildstang.framework.core.Core; -import org.wildstang.sample.auto.Steps.StartOdometryStep; +import org.wildstang.sample.auto.Steps.AutoSetupStep; +import org.wildstang.sample.auto.Steps.ObjectOnStep; import org.wildstang.sample.robot.WsSubsystems; import org.wildstang.sample.subsystems.swerve.SwerveDrive; -import com.choreo.lib.Choreo; +import com.choreo.lib.*; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; public class TestProgram extends AutoProgram{ protected void defineSteps(){ SwerveDrive swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); - addStep(new SetGyroStep(180.0, swerve)); - AutoParallelStepGroup group1 = new AutoParallelStepGroup(); - group1.addStep(new PathHeadingStep(180.0, swerve)); - addStep(group1); - addStep(new StartOdometryStep(3.0, 5.0, 180.0, true)); - addStep(new SwervePathFollowerStep(Choreo.getTrajectory("ChoreoTest"), swerve, true)); + addStep(new AutoSetupStep(0.7, 4.0, 180.0, Alliance.Blue)); + addStep(new SwervePathFollowerStep("TestSlow", swerve)); + + } public String toString(){ diff --git a/src/main/java/org/wildstang/sample/auto/Steps/AlignOnStep.java b/src/main/java/org/wildstang/sample/auto/Steps/AlignOnStep.java new file mode 100644 index 00000000..4f9a4e8f --- /dev/null +++ b/src/main/java/org/wildstang/sample/auto/Steps/AlignOnStep.java @@ -0,0 +1,27 @@ +package org.wildstang.sample.auto.Steps; + +import org.wildstang.framework.auto.AutoStep; +import org.wildstang.framework.core.Core; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; + +public class AlignOnStep extends AutoStep{ + + private SwerveDrive swerve; + private boolean on; + + public AlignOnStep(boolean isOn){ + on = isOn; + } + public void update(){ + swerve.setAutoAlign(on); + this.setFinished(); + } + public void initialize(){ + swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + } + public String toString(){ + return "Align in auto"; + } + +} diff --git a/src/main/java/org/wildstang/sample/auto/Steps/AutoSetupStep.java b/src/main/java/org/wildstang/sample/auto/Steps/AutoSetupStep.java new file mode 100644 index 00000000..e1a0f4ab --- /dev/null +++ b/src/main/java/org/wildstang/sample/auto/Steps/AutoSetupStep.java @@ -0,0 +1,53 @@ +package org.wildstang.sample.auto.Steps; + +import org.wildstang.framework.auto.AutoStep; +import org.wildstang.framework.core.Core; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.LED.LedController; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; +import org.wildstang.sample.subsystems.swerve.SwerveDrive.driveType; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class AutoSetupStep extends AutoStep{ + + private double x, y, heading; + private SwerveDrive swerve; + + // Setup based on starting Choreo position + public AutoSetupStep(double x, double y, double pathHeading, Alliance alliance){ + this.x = x; + this.y = y; + heading = pathHeading; + Core.setAlliance(alliance); + LedController led = (LedController) Core.getSubsystemManager().getSubsystem(WsSubsystems.LED); + led.setAuto(); + } + + public void update(){ + double difference = Math.min(Math.abs(swerve.getGyroAngle() - heading), Math.abs(360 - (swerve.getGyroAngle() - heading))); + // Gyro reset and reads within 1 degree of what we told it to + if (difference < 1) { + // Sets odometry field relative, flipping for red + if (Core.isBlue()){ + swerve.setOdo(new Pose2d(new Translation2d(x, y), new Rotation2d(Math.toRadians(360.0-heading)))); + } else { + swerve.setOdo(new Pose2d(new Translation2d(x, 8.016-y), new Rotation2d(Math.toRadians(360.0-heading)))); + } + this.setFinished(); + } + } + public void initialize(){ + swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + swerve.setAutoValues(0, 0, 0, 0); + swerve.setAutoHeading(heading); + swerve.setGyro(heading); + } + public String toString(){ + return "Auto Setup Step"; + } + +} diff --git a/src/main/java/org/wildstang/sample/auto/Steps/ObjectOnStep.java b/src/main/java/org/wildstang/sample/auto/Steps/ObjectOnStep.java new file mode 100644 index 00000000..e4973adf --- /dev/null +++ b/src/main/java/org/wildstang/sample/auto/Steps/ObjectOnStep.java @@ -0,0 +1,32 @@ +package org.wildstang.sample.auto.Steps; + +import org.wildstang.framework.auto.AutoStep; +import org.wildstang.framework.core.Core; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; + +public class ObjectOnStep extends AutoStep{ + + private SwerveDrive swerve; + private boolean isOn; + + public ObjectOnStep(boolean isOn){ + this.isOn = isOn; + } + @Override + public void initialize() { + swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + } + + @Override + public void update() { + swerve.setAutoObject(isOn); + setFinished(); + } + + @Override + public String toString() { + return "Object On Step"; + } + +} diff --git a/src/main/java/org/wildstang/sample/auto/Steps/TagOnStep.java b/src/main/java/org/wildstang/sample/auto/Steps/TagOnStep.java deleted file mode 100644 index aabaa269..00000000 --- a/src/main/java/org/wildstang/sample/auto/Steps/TagOnStep.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.wildstang.sample.auto.Steps; - -import org.wildstang.framework.auto.AutoStep; -import org.wildstang.framework.core.Core; -import org.wildstang.sample.robot.WsSubsystems; -import org.wildstang.sample.subsystems.swerve.SwerveDrive; -import org.wildstang.sample.subsystems.targeting.WsVision; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -public class TagOnStep extends AutoStep{ - - private SwerveDrive swerve; - private WsVision limelight; - private boolean color, on;//true for blue, false for red - - public TagOnStep(boolean isOn, boolean isBlue){ - color = isBlue; - on = isOn; - } - public void update(){ - swerve.setAutoTag(on, color); - limelight.setGamePiece(false); - this.setFinished(); - } - public void initialize(){ - swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); - limelight = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); - } - public String toString(){ - return "Tag Align On"; - } - -} diff --git a/src/main/java/org/wildstang/sample/auto/Steps/VisionOnStep.java b/src/main/java/org/wildstang/sample/auto/Steps/VisionOnStep.java new file mode 100644 index 00000000..3e052c3c --- /dev/null +++ b/src/main/java/org/wildstang/sample/auto/Steps/VisionOnStep.java @@ -0,0 +1,27 @@ +package org.wildstang.sample.auto.Steps; + +import org.wildstang.framework.auto.AutoStep; +import org.wildstang.framework.core.Core; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; + +public class VisionOnStep extends AutoStep{ + + private SwerveDrive swerve; + private boolean on; + + public VisionOnStep(boolean isOn){ + on = isOn; + } + public void update(){ + swerve.setVisionAuto(on); + this.setFinished(); + } + public void initialize(){ + swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + } + public String toString(){ + return "Vision in auto"; + } + +} diff --git a/src/main/java/org/wildstang/sample/robot/CANConstants.java b/src/main/java/org/wildstang/sample/robot/CANConstants.java index 54aa354d..b53268db 100644 --- a/src/main/java/org/wildstang/sample/robot/CANConstants.java +++ b/src/main/java/org/wildstang/sample/robot/CANConstants.java @@ -26,5 +26,5 @@ public final class CANConstants { public static final int ANGLE3 = 16; public static final int DRIVE4 = 17; public static final int ANGLE4 = 18; - + } \ No newline at end of file diff --git a/src/main/java/org/wildstang/sample/robot/KalmanFilter.java b/src/main/java/org/wildstang/sample/robot/KalmanFilter.java new file mode 100644 index 00000000..030d670d --- /dev/null +++ b/src/main/java/org/wildstang/sample/robot/KalmanFilter.java @@ -0,0 +1,135 @@ +package org.wildstang.sample.robot; +import org.ejml.simple.SimpleMatrix; + +public class KalmanFilter { + private final double deltaT = 0.02; + + // State vector: [x, vx, ax, y, vy, ay, theta, omega] + private SimpleMatrix x; // State vector + + // Covariance matrix + private SimpleMatrix P; // State covariance matrix + + // State transition matrix (F) + private final SimpleMatrix F; + + // Measurement matrix (H) + private final SimpleMatrix H; + + // Process noise covariance (Q) + private final SimpleMatrix Q; + + // Measurement noise covariance (R) + private final SimpleMatrix R; + + // Identity matrix for update step + private final SimpleMatrix I; + + public KalmanFilter() { + // Initial state (assume starting at origin with zero velocity and acceleration) + x = new SimpleMatrix(8, 1); // 8x1 state vector initialized to zero + + // Initial state covariance (P) - assuming some initial uncertainty + P = SimpleMatrix.identity(8).scale(1e-3); // Small initial uncertainty + + // State transition matrix (F) + F = new SimpleMatrix(new double[][]{ + {1, deltaT, 0.5 * deltaT * deltaT, 0, 0, 0, 0, 0}, + {0, 1, deltaT, 0, 0, 0, 0, 0}, + {0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 0, 1, deltaT, 0.5 * deltaT * deltaT, 0, 0}, + {0, 0, 0, 0, 1, deltaT, 0, 0}, + {0, 0, 0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 0, 0, 1, deltaT}, + {0, 0, 0, 0, 0, 0, 0, 1} + }); + + // Measurement matrix (H) - mapping the state to measurements (x, y, ax, ay, theta) + H = new SimpleMatrix(new double[][]{ + {1, 0, 0, 0, 0, 0, 0, 0}, // x + {0, 0, 0, 1, 0, 0, 0, 0}, // y + {0, 0, 1, 0, 0, 0, 0, 0}, // ax + {0, 0, 0, 0, 0, 1, 0, 0}, // ay + {0, 0, 0, 0, 0, 0, 1, 0} // theta + }); + + // Process noise covariance matrix (Q) - assume small process noise + Q = SimpleMatrix.diag(1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4); + + // Measurement noise covariance matrix (R) - assume some measurement noise + R = SimpleMatrix.diag(1e-2, 1e-2, 1e-2, 1e-2, 1e-2); + + // Identity matrix for the update step + I = SimpleMatrix.identity(8); + } + + /** + * Kalman filter prediction step + */ + public void predict() { + // Predict the next state: x(t+1) = F * x(t) + x = F.mult(x); + + // Predict the next state covariance: P(t+1) = F * P * F' + Q + P = F.mult(P).mult(F.transpose()).plus(Q); + } + + /** + * Kalman filter update step + * @param z Measurement vector [x_meas, y_meas, ax_meas, ay_meas, theta_meas] + */ + public void update(SimpleMatrix z) { + // Innovation (residual): y = z - H * x + SimpleMatrix y = z.minus(H.mult(x)); + + // Innovation covariance: S = H * P * H' + R + SimpleMatrix S = H.mult(P).mult(H.transpose()).plus(R); + + // Kalman gain: K = P * H' * S^(-1) + SimpleMatrix K = P.mult(H.transpose()).mult(S.invert()); + + // Update the state: x = x + K * y + x = x.plus(K.mult(y)); + + // Update the covariance: P = (I - K * H) * P + P = I.minus(K.mult(H)).mult(P); + } + + /** + * Get the current state estimate + * @return The current state vector [x, vx, ax, y, vy, ay, theta, omega] + */ + public SimpleMatrix getState() { + return x; + } + + public static void main(String[] args) { + // Create a Kalman filter instance + KalmanFilter kf = new KalmanFilter(); + + // Simulate some sensor measurements (these would come from encoders and IMU in reality) + SimpleMatrix z = new SimpleMatrix(5, 1); + z.set(0, 0, 2.0); // x measurement + z.set(1, 0, 1.5); // y measurement + z.set(2, 0, 0.1); // ax measurement + z.set(3, 0, 0.05); // ay measurement + z.set(4, 0, 0.3); // theta measurement + + // Run the prediction step + kf.predict(); + + // Run the update step with the simulated measurements + kf.update(z); + + // Get the updated state + SimpleMatrix updatedState = kf.getState(); + + // Print the updated state + System.out.println("Updated state: "); + updatedState.print(); + } +} + + + + diff --git a/src/main/java/org/wildstang/sample/robot/KalmanFilterJenny.java b/src/main/java/org/wildstang/sample/robot/KalmanFilterJenny.java new file mode 100644 index 00000000..54f637dc --- /dev/null +++ b/src/main/java/org/wildstang/sample/robot/KalmanFilterJenny.java @@ -0,0 +1,183 @@ +package org.wildstang.sample.robot; +import org.ejml.simple.SimpleMatrix; +import org.wildstang.framework.core.Core; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; + +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; + +public class KalmanFilterJenny { + private final double deltaT = 0.02; + + // State vector: [x, vx, ax, y, vy, ay, theta, omega] + private SimpleMatrix x; // State vector + + // Measurement vector + private SimpleMatrix z; + + // Covariance matrix + private SimpleMatrix P; // State covariance matrix + + // State transition matrix (F) + private final SimpleMatrix F = new SimpleMatrix(new double[][]{ + {1, deltaT, 0.5 * deltaT * deltaT, 0, 0, 0, 0, 0}, + {0, 1, deltaT, 0, 0, 0, 0, 0}, + {0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 0, 1, deltaT, 0.5 * deltaT * deltaT, 0, 0}, + {0, 0, 0, 0, 1, deltaT, 0, 0}, + {0, 0, 0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 0, 0, 1, deltaT}, + {0, 0, 0, 0, 0, 0, 0, 1} + }); + + // Measurement matrix (H) mapping the state to measurements (x, y, ax, ay, theta) + private final SimpleMatrix + H = new SimpleMatrix(new double[][]{ + {1, 0, 0, 0, 0, 0, 0, 0}, // x + {0, 0, 0, 1, 0, 0, 0, 0}, // y + {0, 0, 1, 0, 0, 0, 0, 0}, // ax + {0, 0, 0, 0, 0, 1, 0, 0}, // ay + {0, 0, 0, 0, 0, 0, 1, 0} // theta + }); + + + // Process noise covariance (Q) + // values will be tuned after testing + private final SimpleMatrix Q = SimpleMatrix.diag(1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4); + + // Measurement noise covariance (R) + //values will be tuned after testing + private final SimpleMatrix R = SimpleMatrix.diag(1e-2, 1e-2, 1e-2, 1e-2, 1e-2); + + + + // Identity matrix for update step + private final SimpleMatrix I = SimpleMatrix.identity(8); + + private SwerveDrive swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem("SwerveDrive"); + private SwerveDriveOdometry odometry; + private com.ctre.phoenix.sensors.Pigeon2 gyro; + private ChassisSpeeds robotSpeeds; + + private final int ACCL_X = 0; + private final int ACCL_Y = 1; + + public KalmanFilterJenny() { + odometry = swerve.odometry; + gyro = swerve.gyro; + robotSpeeds = swerve.speeds; + kfInit(); + } + + public void kfInit(){ + initStateMatrix(); + initCovarianceMatrix(); + z = new SimpleMatrix(5, 1); + } + + private void initStateMatrix(){ + + //getting x,y acceleration values from gyro + short[] shortAcceleration = {0,0,0}; + gyro.getBiasedAccelerometer(shortAcceleration); + double[] acceleration = new double[2]; + for (int i=0;i<2;i++) { + acceleration[i] = shortAcceleration[i]; + } + //geting x,y velocity values from chassisspeeds class + double currSpeedX = robotSpeeds.vxMetersPerSecond; + double currSpeedY = robotSpeeds.vyMetersPerSecond; + + //initalizing state matrix + x.set(1,0,odometry.getPoseMeters().getX()); //position in x direction + x.set(2,0,currSpeedX); //velocity in x direction + x.set(3,0,acceleration[ACCL_X]); //acceleration in x direction + x.set(4,0,odometry.getPoseMeters().getY()); //position in y direction + x.set(4,0,currSpeedY); //velocity in y direction + x.set(5,0,acceleration[ACCL_Y]); //acceleration in y direction; + + } + + public void initCovarianceMatrix(){ + // Initial state covariance (P) - assuming some initial uncertainty + P = SimpleMatrix.identity(8).scale(1e-3); // Small initial uncertainty + } + + + private void updateMeasurementMatrix(){ + + short[] shortAcceleration = {0,0,0}; + gyro.getBiasedAccelerometer(shortAcceleration); + double[] acceleration = new double[2]; + for (int i=0;i<2;i++) { + acceleration[i] = shortAcceleration[i]; + } + + z.set(0, 0, odometry.getPoseMeters().getX()); // x measurement + z.set(1, 0, odometry.getPoseMeters().getY()); // y measurement + z.set(2, 0, acceleration[ACCL_X]); // ax measurement + z.set(3, 0, acceleration[ACCL_Y]); // ay measurement + z.set(4, 0, gyro.getYaw()); // theta measurement + } + + + /** + * Kalman filter prediction step + */ + private void predict() { + // Predict the next state: x(t+1) = F * x(t) + x = F.mult(x); + + // Predict the next state covariance: P(t+1) = F * P * F' + Q + P = F.mult(P).mult(F.transpose()).plus(Q); + } + + /** + * Kalman filter update step + * @param z Measurement vector [x_meas, y_meas, ax_meas, ay_meas, theta_meas] + */ + private void update() { + // Innovation (residual): y = z - H * x + SimpleMatrix y = z.minus(H.mult(x)); + + // Innovation covariance: S = H * P * H' + R + SimpleMatrix S = H.mult(P).mult(H.transpose()).plus(R); + + // Kalman gain: K = P * H' * S^(-1) + SimpleMatrix K = P.mult(H.transpose()).mult(S.invert()); + + // Update the state: x = x + K * y + x = x.plus(K.mult(y)); + + // Update the covariance: P = (I - K * H) * P + P = I.minus(K.mult(H)).mult(P); + } + + /** + * Get the current state estimate + * @return The current state vector [x, vx, ax, y, vy, ay, theta, omega] + */ + public SimpleMatrix getState() { + return x; + } + + public void kfPeriodic() { + + //get Measurements + updateMeasurementMatrix(); + + // Run the prediction step + predict(); + + // Run the update step + + update(); + + // Get the updated state + SimpleMatrix updatedState = getState(); + + + } +} \ No newline at end of file diff --git a/src/main/java/org/wildstang/sample/robot/Robot.java b/src/main/java/org/wildstang/sample/robot/Robot.java index 201729a1..10fc9a89 100644 --- a/src/main/java/org/wildstang/sample/robot/Robot.java +++ b/src/main/java/org/wildstang/sample/robot/Robot.java @@ -6,6 +6,8 @@ import org.wildstang.hardware.roborio.RoboRIOInputFactory; import org.wildstang.hardware.roborio.RoboRIOOutputFactory; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -46,6 +48,11 @@ public void robotInit() { logChooser.addOption(LogLevel.ERROR.toString(), LogLevel.ERROR); logChooser.addOption(LogLevel.NONE.toString(), LogLevel.NONE); SmartDashboard.putData("Log Level", logChooser); + + DataLogManager.start(); + // Record both DS control and joystick data + DriverStation.startDataLog(DataLogManager.getLog()); + } /** @@ -65,6 +72,7 @@ public void autonomousInit() { Log.danger("Engaging autonomous mode."); Core.getSubsystemManager().resetState(); Core.getAutoManager().startCurrentProgram(); + } /** @@ -78,6 +86,8 @@ public void teleopInit() { // tell AutoManager not to preload or run any more programs Core.getAutoManager().endPeriod(); Core.getSubsystemManager().resetState(); + Core.setToTeleop(); + } /** @@ -122,6 +132,7 @@ public void autonomousPeriodic() { @Override public void teleopPeriodic() { update(); + } /** diff --git a/src/main/java/org/wildstang/sample/robot/WsAutoPrograms.java b/src/main/java/org/wildstang/sample/robot/WsAutoPrograms.java index c809af18..4f7f0e28 100644 --- a/src/main/java/org/wildstang/sample/robot/WsAutoPrograms.java +++ b/src/main/java/org/wildstang/sample/robot/WsAutoPrograms.java @@ -1,6 +1,10 @@ package org.wildstang.sample.robot; import org.wildstang.framework.core.AutoPrograms; +import org.wildstang.sample.auto.Programs.Jubilee; +import org.wildstang.sample.auto.Programs.Line; +import org.wildstang.sample.auto.Programs.TestProgram; + /** * All active AutoPrograms are enumerated here. @@ -10,8 +14,9 @@ public enum WsAutoPrograms implements AutoPrograms { // enumerate programs //SAMPLE_PROGRAM("Sample", SampleAutoProgram.class), - //TEST_PROGRAM("Test Program", Testprogram.class), - + Line("Line", Line.class), + TEST_PROGRAM("Test Program", TestProgram.class), + Jubilee("Jubilee", Jubilee.class), ; /** diff --git a/src/main/java/org/wildstang/sample/robot/WsInputs.java b/src/main/java/org/wildstang/sample/robot/WsInputs.java index bab12bfd..79990b30 100644 --- a/src/main/java/org/wildstang/sample/robot/WsInputs.java +++ b/src/main/java/org/wildstang/sample/robot/WsInputs.java @@ -7,6 +7,7 @@ import org.wildstang.hardware.JoystickConstants; import org.wildstang.hardware.roborio.inputs.config.WsJSButtonInputConfig; import org.wildstang.hardware.roborio.inputs.config.WsDPadButtonInputConfig; +import org.wildstang.hardware.roborio.inputs.config.WsDigitalInputConfig; import org.wildstang.hardware.roborio.inputs.config.WsJSJoystickInputConfig; /** diff --git a/src/main/java/org/wildstang/sample/robot/WsOutputs.java b/src/main/java/org/wildstang/sample/robot/WsOutputs.java index 6b3bd474..2dd1d4e7 100644 --- a/src/main/java/org/wildstang/sample/robot/WsOutputs.java +++ b/src/main/java/org/wildstang/sample/robot/WsOutputs.java @@ -28,8 +28,6 @@ public enum WsOutputs implements Outputs { DRIVE4("Module 4 Drive Motor", new WsSparkConfig(CANConstants.DRIVE4, WsMotorControllers.SPARK_FLEX_BRUSHLESS)), ANGLE4("Module 4 Angle Motor", new WsSparkConfig(CANConstants.ANGLE4, WsMotorControllers.SPARK_MAX_BRUSHLESS)), - TEST_FOLLOWER("Test Follower", new WsSparkFollowerConfig(ANGLE4, CANConstants.ANGLE4, WsMotorControllers.SPARK_FLEX_BRUSHLESS, true)), - // --------------------------------- // Other Motors // --------------------------------- diff --git a/src/main/java/org/wildstang/sample/robot/WsSubsystems.java b/src/main/java/org/wildstang/sample/robot/WsSubsystems.java index 96120698..9b61f930 100644 --- a/src/main/java/org/wildstang/sample/robot/WsSubsystems.java +++ b/src/main/java/org/wildstang/sample/robot/WsSubsystems.java @@ -1,6 +1,7 @@ package org.wildstang.sample.robot; import org.wildstang.framework.core.Subsystems; +import org.wildstang.sample.subsystems.LED.LedController; import org.wildstang.sample.subsystems.swerve.SwerveDrive; import org.wildstang.sample.subsystems.targeting.WsVision; @@ -12,7 +13,8 @@ public enum WsSubsystems implements Subsystems { // enumerate subsystems WS_VISION("Ws Vision", WsVision.class), - SWERVE_DRIVE("Swerve Drive", SwerveDrive.class) + SWERVE_DRIVE("Swerve Drive", SwerveDrive.class), + LED("Led Controller", LedController.class), ; /** diff --git a/src/main/java/org/wildstang/sample/subsystems/InputTestSubsystem.java b/src/main/java/org/wildstang/sample/subsystems/InputTestSubsystem.java index 42e1e150..d3234705 100644 --- a/src/main/java/org/wildstang/sample/subsystems/InputTestSubsystem.java +++ b/src/main/java/org/wildstang/sample/subsystems/InputTestSubsystem.java @@ -103,4 +103,9 @@ public void resetState() {} public String getName() { return "Input Test"; } + + @Override + public void initSubsystems() { + // TODO Auto-generated method stub + } } \ No newline at end of file diff --git a/src/main/java/org/wildstang/sample/subsystems/LED/LedController.java b/src/main/java/org/wildstang/sample/subsystems/LED/LedController.java new file mode 100644 index 00000000..725739bf --- /dev/null +++ b/src/main/java/org/wildstang/sample/subsystems/LED/LedController.java @@ -0,0 +1,128 @@ +package org.wildstang.sample.subsystems.LED; + +import org.wildstang.framework.core.Core; +import org.wildstang.framework.io.inputs.AnalogInput; +import org.wildstang.framework.io.inputs.DigitalInput; +import org.wildstang.framework.io.inputs.Input; +import org.wildstang.framework.subsystems.Subsystem; +import org.wildstang.sample.robot.Robot; +import org.wildstang.sample.robot.WsInputs; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.targeting.WsVision; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class LedController implements Subsystem { + + private AddressableLED led; + private AddressableLEDBuffer ledBuffer; + private WsVision vision; + private Timer timer = new Timer(); + + private int port = 0;//port + private int length = 45;//length + private int initialHue = 0; + private int initialRed = 0; + private int initialBlue = 0; + + private int[] white = {255,255,255}; + private int[] blue = {0,0,255}; + private int[] red = {255,0,0}; + private int[] green = {0,255,0}; + private int[] orange = {255,100,0}; + private int[] cyan = {0,155,155}; + private int[] purple = {128,0,128}; + + private boolean isAutoLocked = false; + + + @Override + public void update(){ + if (Core.getIsInAuto()){ + if (isAutoLocked){ + if (Core.isBlue()) cycleBlue(); + else cycleRed(); + } else rainbow(); + } else { + if (isAutoLocked){ + if (Core.isBlue()) cycleBlue(); + else cycleRed(); + } else rainbow(); + } + led.setData(ledBuffer); + led.start(); + } + + @Override + public void inputUpdate(Input source) { + isAutoLocked = false; + } + + @Override + public void initSubsystems() { + vision = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); + } + + @Override + public void init() { + + //Outputs + led = new AddressableLED(port); + ledBuffer = new AddressableLEDBuffer(length); + led.setLength(ledBuffer.getLength()); + setRGB(255, 255, 255); + led.setData(ledBuffer); + led.start(); + resetState(); + timer.start(); + } + + @Override + public void selfTest() { + } + + @Override + public void resetState() { + } + + @Override + public String getName() { + return "Led Controller"; + } + + public void setAuto(){ + isAutoLocked = true; + } + + public void setRGB(int red, int green, int blue){ + for (int i = 0; i < length; i++){ + ledBuffer.setRGB(i, (int)(0.75*red), (int)(0.75*green), (int)(0.75*blue)); + } + } + public void setRGB(int[] color){ + setRGB(color[0],color[1],color[2]); + } + + private void rainbow(){ + for (int i = 0; i < ledBuffer.getLength(); i++){ + ledBuffer.setHSV(i, 180-(initialHue + (i*180/ledBuffer.getLength()))%180, 255, 128); + } + initialHue = (initialHue + 3) % 180; + led.setData(ledBuffer); + } + private void cycleRed(){ + for (int i = 0; i < ledBuffer.getLength(); i++){ + ledBuffer.setRGB(i, (initialRed + 255-(i*255/ledBuffer.getLength()))%255, 0, 0); + } + initialRed = (initialRed + 5) % 255; + } + private void cycleBlue(){ + for (int i = 0; i < ledBuffer.getLength(); i++){ + ledBuffer.setRGB(i, 0, 0, 255-(initialBlue + (i*255/ledBuffer.getLength()))%255); + } + initialBlue = (initialBlue + 5) % 255; + } +} diff --git a/src/main/java/org/wildstang/sample/subsystems/drive/Drive.java b/src/main/java/org/wildstang/sample/subsystems/drive/Drive.java index 61045a81..1f3aef0d 100644 --- a/src/main/java/org/wildstang/sample/subsystems/drive/Drive.java +++ b/src/main/java/org/wildstang/sample/subsystems/drive/Drive.java @@ -146,4 +146,10 @@ public void setGyro(double degrees){ gyro.reset(); gyro.setAngleAdjustment(degrees); } + + @Override + public void initSubsystems() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'initSubsystems'"); + } } diff --git a/src/main/java/org/wildstang/sample/subsystems/swerve/DriveConstants.java b/src/main/java/org/wildstang/sample/subsystems/swerve/DriveConstants.java index 25293e92..804a43a5 100644 --- a/src/main/java/org/wildstang/sample/subsystems/swerve/DriveConstants.java +++ b/src/main/java/org/wildstang/sample/subsystems/swerve/DriveConstants.java @@ -1,19 +1,27 @@ package org.wildstang.sample.subsystems.swerve; -public final class DriveConstants { +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +public final class DriveConstants { + public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(new Translation2d(0.2794, 0.33), new Translation2d(0.2794, -0.33), + new Translation2d(-0.2794, 0.33), new Translation2d(-0.2794, -0.33)); + /**P Loop for Translating to a point */ + public static final double TRANSLATION_P = 0.0; /** robot length from swerve pod to swerve pod, in inches */ - public static final double ROBOT_LENGTH = 11; + public static final double ROBOT_LENGTH = 11.5; /** robot width from swerve pod to swerve pod, in inches */ - public static final double ROBOT_WIDTH = 11; + public static final double ROBOT_WIDTH = 13.08; /**speed with which the robot rotates relative to drive speed */ - public static final double ROTATION_SPEED = 0.50; + public static final double ROTATION_SPEED = 0.75; + /**speed with which the rotation PID is controlled by */ + public static final double PID_ROTATION = 1.5; /**drive motor gear ratio */ - public static final double DRIVE_RATIO = (45.0*22.0)/(15.0*14.0); + public static final double DRIVE_RATIO = 4.0; /**angle motor gear ratio */ //public static final double ANGLE_RATIO = 12.8; /**diameter of drive wheel, in inches */ - public static final double WHEEL_DIAMETER = 3.0; + public static final double WHEEL_DIAMETER = 2.73; /**offset of module 1, the front left module, in degrees */ public static final double FRONT_LEFT_OFFSET = -90; /**offset of module 2, the front right module, in degrees */ @@ -34,7 +42,7 @@ public final class DriveConstants { */ public static final double SLEW_RATE_LIMIT = 3.0; /**second order correction for rotation plus driving */ - public static final double ROT_CORRECTION_FACTOR = 0.2; + public static final double ROT_CORRECTION_FACTOR = -0.5; /**encoder ticks per revolution, 1.0 for neos */ public static final double TICKS_PER_REV = 1.0; /**PID values for drive P */ @@ -44,11 +52,11 @@ public final class DriveConstants { /**PID values for drive D */ public static final double DRIVE_D = 0.1; /**PID values for driveF coefficient of momentum */ - public static final double DRIVE_F_V = 0.00536;//0.00558*1.2325;//0.00581 on old treads + public static final double DRIVE_F_V = 0.00375 * 157.5/144.5*157.5/142;//free speed of 22.2 ft/s becomes 266.4 in/s /**PID values for drive F coefficient of kinetic friction */ public static final double DRIVE_F_K = 0.016; /**PID values for drive F coefficient of inertia */ - public static final double DRIVE_F_I = 0;//0.003; + public static final double DRIVE_F_I = 0.000;// /**PID values for angle P */ public static final double ANGLE_P = 0.01; /**PID values for angle I */ @@ -56,7 +64,7 @@ public final class DriveConstants { /**PID values for angle D */ public static final double ANGLE_D = 0.0; /**Drive motor current limit */ - public static final int DRIVE_CURRENT_LIMIT = 50; + public static final int DRIVE_CURRENT_LIMIT = 60; /**Angle motor current limit */ public static final int ANGLE_CURRENT_LIMIT = 10; /**Swerve Module Names */ diff --git a/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java index 6f27c355..4e90d09a 100644 --- a/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java @@ -2,26 +2,35 @@ import com.ctre.phoenix.sensors.Pigeon2; +import java.util.Arrays; + import org.wildstang.framework.core.Core; import org.wildstang.framework.io.inputs.Input; import org.wildstang.framework.io.inputs.AnalogInput; import org.wildstang.framework.io.inputs.DigitalInput; import org.wildstang.framework.subsystems.swerve.SwerveDriveTemplate; +import org.wildstang.hardware.roborio.outputs.WsSpark; import org.wildstang.sample.robot.CANConstants; import org.wildstang.sample.robot.WsInputs; import org.wildstang.sample.robot.WsOutputs; import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.targeting.VisionConsts; import org.wildstang.sample.subsystems.targeting.WsVision; -import org.wildstang.sample.subsystems.targeting.LimeConsts; -import org.wildstang.hardware.roborio.outputs.WsSpark; +import edu.wpi.first.math.estimator.KalmanFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /**Class: SwerveDrive @@ -34,188 +43,202 @@ public class SwerveDrive extends SwerveDriveTemplate { private AnalogInput leftStickX;//translation joystick x private AnalogInput leftStickY;//translation joystick y private AnalogInput rightStickX;//rot joystick - private AnalogInput rightTrigger;//thrust - private AnalogInput leftTrigger;//scoring autodrive - // private DigitalInput rightBumper;//robot centric control - private DigitalInput leftBumper;//hp station pickup + private AnalogInput rightTrigger;//intake, score when aiming + private AnalogInput leftTrigger;//scoring to speaker + private DigitalInput rightBumper;//slowdown + private DigitalInput leftBumper;//lift up to amp private DigitalInput select;//gyro reset - private DigitalInput start;//snake mode + private DigitalInput start;//endgame toggle private DigitalInput faceUp;//rotation lock 0 degrees private DigitalInput faceRight;//rotation lock 90 degrees private DigitalInput faceLeft;//rotation lock 270 degrees private DigitalInput faceDown;//rotation lock 180 degrees private DigitalInput dpadLeft;//defense mode - private DigitalInput rightStickButton;//auto drive override + private DigitalInput dpadRight; - private double xSpeed; - private double ySpeed; + private double xPower; + private double yPower; private double rotSpeed; private double thrustValue; private boolean rotLocked; private boolean isSnake; - private boolean isFieldCentric; + + /**Direction to face */ private double rotTarget; - private double pathVel; - private double pathHeading; - private double pathAccel; - private double pathTarget; - private double aimOffset; - private double vertOffset; - private double pathXOffset = 0; - private double pathYOffset = 0; - private boolean autoOverride; - private boolean isBlue; - private boolean autoTag = false; + + private boolean isAutoVision = false; + private boolean isAutoAlign = false; + private boolean isAutoObject = false; + private double yObject = 0; private final double mToIn = 39.37; //private final AHRS gyro = new AHRS(SerialPort.Port.kUSB); - private final Pigeon2 gyro = new Pigeon2(CANConstants.GYRO); + public final Pigeon2 gyro = new Pigeon2(CANConstants.GYRO); public SwerveModule[] modules; private SwerveSignal swerveSignal; private WsSwerveHelper swerveHelper = new WsSwerveHelper(); - private SwerveDriveOdometry odometry; - private Timer autoTimer = new Timer(); + public SwerveDriveOdometry odometry; + StructPublisher publisher; + public ChassisSpeeds speeds; - private WsVision limelight; - private LimeConsts LC; + private WsVision vision; + //private KalmanFilterJenny kf; - public enum driveType {TELEOP, AUTO, CROSS}; + public enum driveType {TELEOP, AUTO, CROSS, OBJECT}; public driveType driveState; @Override public void inputUpdate(Input source) { //determine if we are in cross or teleop - if (driveState != driveType.AUTO && dpadLeft.getValue()) { - driveState = driveType.CROSS; - for (int i = 0; i < modules.length; i++) { - modules[i].setDriveBrake(true); - } - this.swerveSignal = new SwerveSignal(new double[]{0, 0, 0, 0 }, swerveHelper.setCross().getAngles()); - } - else if (driveState == driveType.CROSS || driveState == driveType.AUTO) { - driveState = driveType.TELEOP; - } + // if (driveState != driveType.AUTO && dpadLeft.getValue()) { + // driveState = driveType.CROSS; + // for (int i = 0; i < modules.length; i++) { + // modules[i].setDriveBrake(true); + // } + // this.swerveSignal = new SwerveSignal(new double[]{0, 0, 0, 0 }, swerveHelper.setCross().getAngles()); + // } + // else if (driveState == driveType.CROSS || driveState == driveType.AUTO) { + // driveState = driveType.TELEOP; + // } + if (driveState == driveType.AUTO) driveState = driveType.TELEOP; //get x and y speeds - xSpeed = swerveHelper.scaleDeadband(leftStickX.getValue(), DriveConstants.DEADBAND); - ySpeed = swerveHelper.scaleDeadband(leftStickY.getValue(), DriveConstants.DEADBAND); + xPower = swerveHelper.scaleDeadband(leftStickX.getValue(), DriveConstants.DEADBAND); + yPower = swerveHelper.scaleDeadband(leftStickY.getValue(), DriveConstants.DEADBAND); + //reset gyro if (source == select && select.getValue()) { gyro.setYaw(0.0); + odometry.resetPosition(new Rotation2d(), odoPosition(), + new Pose2d(new Translation2d(odometry.getPoseMeters().getX(),odometry.getPoseMeters().getY()),new Rotation2d())); if (rotLocked) rotTarget = 0.0; } //determine snake or pid locks - if (start.getValue() && (Math.abs(xSpeed) > 0.1 || Math.abs(ySpeed) > 0.1)) { - rotLocked = true; - isSnake = true; - rotTarget = swerveHelper.getDirection(xSpeed, ySpeed); - } - else { - isSnake = false; - } - if ((source == faceUp && faceUp.getValue()) || leftBumper.getValue()){ - if (faceLeft.getValue() || faceRight.getValue()){ rotTarget = getClosestRotation(); - } else rotTarget = 0.0; + // if (start.getValue() && (Math.abs(xSpeed) > 0.1 || Math.abs(ySpeed) > 0.1)) { + // rotLocked = true; + // isSnake = true; + // rotTarget = swerveHelper.getDirection(xSpeed, ySpeed); + // } + // else { + // isSnake = false; + // } + if (source == faceUp && faceUp.getValue()){ + if (faceLeft.getValue()) rotTarget = 315.0; + else if (faceRight.getValue()) rotTarget = 45.0; + else rotTarget = 0.0; rotLocked = true; } if (source == faceLeft && faceLeft.getValue()){ - if (faceUp.getValue() || faceDown.getValue()){ rotTarget = getClosestRotation(); - } else rotTarget = 270.0; + if (faceUp.getValue()) rotTarget = 315.0; + else if (faceDown.getValue()) rotTarget = 225.0; + else rotTarget = 270.0; rotLocked = true; } - if ((source == faceDown && faceDown.getValue() && !leftBumper.getValue()) || Math.abs(leftTrigger.getValue()) > 0.15){ - if (faceLeft.getValue() || faceRight.getValue()){ rotTarget = getClosestRotation(); - } else rotTarget = 180.0; + if (source == faceDown && faceDown.getValue()){ + if (faceLeft.getValue()) rotTarget = 225.0; + else if (faceRight.getValue()) rotTarget = 135.0; + else rotTarget = 180.0; rotLocked = true; } if (source == faceRight && faceRight.getValue()){ - if (faceUp.getValue() || faceDown.getValue()){ rotTarget = getClosestRotation(); - } else rotTarget = 90.0; + if (faceUp.getValue()) rotTarget = 45.0; + else if (faceDown.getValue()) rotTarget = 135.0; + else rotTarget = 90.0; rotLocked = true; } //get rotational joystick rotSpeed = rightStickX.getValue()*Math.abs(rightStickX.getValue()); rotSpeed = swerveHelper.scaleDeadband(rotSpeed, DriveConstants.DEADBAND); - if (Math.abs(leftTrigger.getValue()) > 0.15) rotSpeed = 0; - rotSpeed *=1.5; + // if (rotSpeed == 0 && rotLocked == false){ + // if (Math.abs(getGyroAngle() - rotTarget) < 1.0) rotLocked = true; + // rotTarget = getGyroAngle(); + // } //if the rotational joystick is being used, the robot should not be auto tracking heading if (rotSpeed != 0) { rotLocked = false; } - //assign thrust - thrustValue = 1 - DriveConstants.DRIVE_THRUST + DriveConstants.DRIVE_THRUST * Math.abs(rightTrigger.getValue()); - xSpeed *= thrustValue; - ySpeed *= thrustValue; - rotSpeed *= thrustValue; + //assign thrust - no thrust this year, low cg robot means max speed all the time + // thrustValue = 1 - DriveConstants.DRIVE_THRUST + DriveConstants.DRIVE_THRUST * Math.abs(rightTrigger.getValue()); + // xSpeed *= thrustValue; + // ySpeed *= thrustValue; + // rotSpeed *= thrustValue; } @Override public void init() { + publisher = NetworkTableInstance.getDefault() + .getStructTopic("MyPose", Pose2d.struct).publish(); + initInputs(); initOutputs(); resetState(); gyro.setYaw(0.0); + + //kf = new KalmanFilterJenny(); + + } + + public void initSubsystems() { + vision = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); } public void initInputs() { - LC = new LimeConsts(); - leftStickX = (AnalogInput) Core.getInputManager().getInput(WsInputs.DRIVER_LEFT_JOYSTICK_X); + leftStickX = (AnalogInput) WsInputs.DRIVER_LEFT_JOYSTICK_X.get(); leftStickX.addInputListener(this); - leftStickY = (AnalogInput) Core.getInputManager().getInput(WsInputs.DRIVER_LEFT_JOYSTICK_Y); + leftStickY = (AnalogInput) WsInputs.DRIVER_LEFT_JOYSTICK_Y.get(); leftStickY.addInputListener(this); - rightStickX = (AnalogInput) Core.getInputManager().getInput(WsInputs.DRIVER_RIGHT_JOYSTICK_X); + rightStickX = (AnalogInput) WsInputs.DRIVER_RIGHT_JOYSTICK_X.get(); rightStickX.addInputListener(this); - rightTrigger = (AnalogInput) Core.getInputManager().getInput(WsInputs.DRIVER_RIGHT_TRIGGER); + rightTrigger = (AnalogInput) WsInputs.DRIVER_RIGHT_TRIGGER.get(); rightTrigger.addInputListener(this); - leftTrigger = (AnalogInput) Core.getInputManager().getInput(WsInputs.DRIVER_LEFT_TRIGGER); + leftTrigger = (AnalogInput) WsInputs.DRIVER_LEFT_TRIGGER.get(); leftTrigger.addInputListener(this); - // rightBumper = (DigitalInput) Core.getInputManager().getInput(WSInputs.DRIVER_RIGHT_SHOULDER); - // rightBumper.addInputListener(this); - leftBumper = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_LEFT_SHOULDER); + rightBumper = (DigitalInput) WsInputs.DRIVER_RIGHT_SHOULDER.get(); + rightBumper.addInputListener(this); + leftBumper = (DigitalInput) WsInputs.DRIVER_LEFT_SHOULDER.get(); leftBumper.addInputListener(this); - select = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_SELECT); + select = (DigitalInput) WsInputs.DRIVER_SELECT.get(); select.addInputListener(this); - start = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_START); + start = (DigitalInput) WsInputs.DRIVER_START.get(); start.addInputListener(this); - faceUp = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_FACE_UP); + faceUp = (DigitalInput) WsInputs.DRIVER_FACE_UP.get(); faceUp.addInputListener(this); - faceLeft = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_FACE_LEFT); + faceLeft = (DigitalInput) WsInputs.DRIVER_FACE_LEFT.get(); faceLeft.addInputListener(this); - faceRight = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_FACE_RIGHT); + faceRight = (DigitalInput) WsInputs.DRIVER_FACE_RIGHT.get(); faceRight.addInputListener(this); - faceDown = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_FACE_DOWN); + faceDown = (DigitalInput) WsInputs.DRIVER_FACE_DOWN.get(); faceDown.addInputListener(this); - dpadLeft = (DigitalInput) Core.getInputManager().getInput(WsInputs.DRIVER_DPAD_LEFT); + dpadLeft = (DigitalInput) WsInputs.DRIVER_DPAD_LEFT.get(); dpadLeft.addInputListener(this); - rightStickButton = (DigitalInput) WsInputs.DRIVER_RIGHT_JOYSTICK_BUTTON.get(); - rightStickButton.addInputListener(this); + dpadRight = (DigitalInput) WsInputs.DRIVER_DPAD_RIGHT.get(); + dpadRight.addInputListener(this); } public void initOutputs() { //create four swerve modules modules = new SwerveModule[]{ - new SwerveModule((WsSpark) Core.getOutputManager().getOutput(WsOutputs.DRIVE1), - (WsSpark) Core.getOutputManager().getOutput(WsOutputs.ANGLE1), DriveConstants.FRONT_LEFT_OFFSET), - new SwerveModule((WsSpark) Core.getOutputManager().getOutput(WsOutputs.DRIVE2), - (WsSpark) Core.getOutputManager().getOutput(WsOutputs.ANGLE2), DriveConstants.FRONT_RIGHT_OFFSET), - new SwerveModule((WsSpark) Core.getOutputManager().getOutput(WsOutputs.DRIVE3), - (WsSpark) Core.getOutputManager().getOutput(WsOutputs.ANGLE3), DriveConstants.REAR_LEFT_OFFSET), - new SwerveModule((WsSpark) Core.getOutputManager().getOutput(WsOutputs.DRIVE4), - (WsSpark) Core.getOutputManager().getOutput(WsOutputs.ANGLE4), DriveConstants.REAR_RIGHT_OFFSET) + new SwerveModule((WsSpark) WsOutputs.DRIVE1.get(), + (WsSpark) WsOutputs.ANGLE1.get(), DriveConstants.FRONT_LEFT_OFFSET), + new SwerveModule((WsSpark) WsOutputs.DRIVE2.get(), + (WsSpark) WsOutputs.ANGLE2.get(), DriveConstants.FRONT_RIGHT_OFFSET), + new SwerveModule((WsSpark) WsOutputs.DRIVE3.get(), + (WsSpark) WsOutputs.ANGLE3.get(), DriveConstants.REAR_LEFT_OFFSET), + new SwerveModule((WsSpark) WsOutputs.DRIVE4.get(), + (WsSpark) WsOutputs.ANGLE4.get(), DriveConstants.REAR_RIGHT_OFFSET) }; //create default swerveSignal swerveSignal = new SwerveSignal(new double[]{0.0, 0.0, 0.0, 0.0}, new double[]{0.0, 0.0, 0.0, 0.0}); - limelight = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); - odometry = new SwerveDriveOdometry(new SwerveDriveKinematics(new Translation2d(0.2794, 0.2794), new Translation2d(0.2794, -0.2794), - new Translation2d(-0.2794, 0.2794), new Translation2d(-0.2794, -0.2794)), odoAngle(), odoPosition(), new Pose2d()); + odometry = new SwerveDriveOdometry(new SwerveDriveKinematics(new Translation2d(0.2794, 0.33), new Translation2d(0.2794, -0.33), + new Translation2d(-0.2794, 0.33), new Translation2d(-0.2794, -0.33)), odoAngle(), odoPosition(), new Pose2d()); } @Override @@ -225,6 +248,13 @@ public void selfTest() { @Override public void update() { odometry.update(odoAngle(), odoPosition()); + SmartDashboard.putNumber("Drive Speed", robotSpeed()); + if (robotSpeed() < 0.5) { + setOdo(new Pose2d(vision.getCameraPose(), odoAngle())); + } + vision.setOdometry(odometry.getPoseMeters().getTranslation()); + publisher.set(odometry.getPoseMeters()); + //kf.kfPeriodic(); //calling periodic kalman filter method if (driveState == driveType.CROSS) { //set to cross - done in inputupdate @@ -233,78 +263,90 @@ public void update() { } if (driveState == driveType.TELEOP) { if (rotLocked){ - //if rotation tracking, replace rotational joystick value with controller generated one rotSpeed = swerveHelper.getRotControl(rotTarget, getGyroAngle()); - if (isSnake) { - if (Math.abs(rotSpeed) < 0.05) { - rotSpeed = 0; - } - else { - rotSpeed *= 4; - if (Math.abs(rotSpeed) > 1) rotSpeed = 1.0 * Math.signum(rotSpeed); - } - - } + if (Math.abs(rotTarget - getGyroAngle()) < 1.0) rotSpeed = 0; + // if (isSnake) { + // if (Math.abs(rotSpeed) < 0.05) { + // rotSpeed = 0; + // } + // else { + // rotSpeed *= 4; + // if (Math.abs(rotSpeed) > 1) rotSpeed = 1.0 * Math.signum(rotSpeed); + // } + // } } - this.swerveSignal = swerveHelper.setDrive(xSpeed, ySpeed, rotSpeed, getGyroAngle()); + this.swerveSignal = swerveHelper.setDrive(xPower, yPower, rotSpeed, getGyroAngle()); SmartDashboard.putNumber("FR signal", swerveSignal.getSpeed(0)); drive(); } if (driveState == driveType.AUTO) { - //get controller generated rotation value - rotSpeed = Math.max(-0.2, Math.min(0.2, swerveHelper.getRotControl(pathTarget, getGyroAngle()))); - //ensure rotation is never more than 0.2 to prevent normalization of translation from occuring - if (autoTag){ - xSpeed = limelight.getScoreX(aimOffset); - ySpeed = limelight.getScoreY(vertOffset); - if (Math.abs(xSpeed) > 0.3) xSpeed = Math.signum(xSpeed) * 0.3; - if (Math.abs(ySpeed) > 0.3) ySpeed = Math.signum(ySpeed) * 0.3; - if (Math.abs(pathVel * DriveConstants.DRIVE_F_V) > Math.abs(ySpeed*0.5)){ - ySpeed = 0.0;//no adjustment when coming towards tag - } else { - pathVel = 0.0;//adjustment when close enough to tag - } - pathXOffset = 0;//disables odometry tracking - pathYOffset = 0; + //auto Align to reset robot to a specific X/Y location on the field, in case of any serious collisions + if (isAutoAlign){ + this.swerveSignal = swerveHelper.setDrive(vision.getYAdjust(VisionConsts.shot), + vision.getXAdjust(VisionConsts.shot), swerveHelper.getRotControl(180, getGyroAngle()), getGyroAngle()); + //shooting at speaker during auto + } else if (isAutoVision && vision.aprilTagsInView()) { + rotTarget = vision.turnToTarget(VisionConsts.speaker); + rotSpeed = swerveHelper.getRotControl(rotTarget, getGyroAngle()); + this.swerveSignal = swerveHelper.setDrive(xPower, yPower, rotSpeed, getGyroAngle()); + //picking up a game piece with vision assistance in auto + } else if (isAutoObject && vision.back.TargetInView()){ + yObject = swerveHelper.adjustObjectAuto(vision.back.ty, xPower, yPower); + if (yObject > 0) this.swerveSignal = swerveHelper.setObject(0, yObject, vision.back.tx); + else this.swerveSignal = swerveHelper.setObject(xPower, yPower, vision.back.tx); } else { - xSpeed = 0;//no LL adjustments if tag is off - ySpeed = 0; + //get controller generated rotation value + rotSpeed = swerveHelper.getAutoRotation(rotTarget, getGyroAngle()); + this.swerveSignal = swerveHelper.setDrive(xPower, yPower, rotSpeed, getGyroAngle()); } - //update where the robot is, to determine error in path - this.swerveSignal = swerveHelper.setAuto(swerveHelper.getAutoPower(pathVel, pathAccel), pathHeading, rotSpeed,getGyroAngle(),pathXOffset+xSpeed, pathYOffset+ySpeed); + // Pre generated power values in set auto drive(); } + if (driveState == driveType.OBJECT) { + //for teleop game piece pickup + if (vision.back.TargetInView()) { + //force the controller input to point at the gamepiece, only thing driver controls is speed of the robot + this.swerveSignal = swerveHelper.setObject(xPower, yPower, vision.back.tx); + drive(); + } + else { + this.swerveSignal = swerveHelper.setDrive(xPower, yPower, rotSpeed, getGyroAngle()); + drive(); + } + } SmartDashboard.putNumber("Gyro Reading", getGyroAngle()); - SmartDashboard.putNumber("X speed", xSpeed); - SmartDashboard.putNumber("Y speed", ySpeed); + SmartDashboard.putNumber("X Power", xPower); + SmartDashboard.putNumber("Y Power", yPower); SmartDashboard.putNumber("rotSpeed", rotSpeed); SmartDashboard.putString("Drive mode", driveState.toString()); SmartDashboard.putBoolean("rotLocked", rotLocked); - SmartDashboard.putNumber("Auto velocity", pathVel); - SmartDashboard.putNumber("Auto translate direction", pathHeading); - SmartDashboard.putNumber("Auto rotation target", pathTarget); + SmartDashboard.putNumber("Rotation target", rotTarget); + SmartDashboard.putNumber("Odo X", odometry.getPoseMeters().getX()); + SmartDashboard.putNumber("Odo Y", odometry.getPoseMeters().getY()); + SmartDashboard.putNumber("Yaw", gyro.getYaw()); + SmartDashboard.putNumber("Roll", gyro.getRoll()); + SmartDashboard.putNumber("Pitch", gyro.getPitch()); + short[] shortAcceleration = {0,0,0}; + gyro.getBiasedAccelerometer(shortAcceleration); + double[] acceleration = new double[3]; + for (int i=0;i<3;i++) { + acceleration[i] = shortAcceleration[i]; + } + SmartDashboard.putNumberArray("Accelerometer", acceleration); + SmartDashboard.putBoolean("Alliance Color", DriverStation.getAlliance().isPresent()); } @Override public void resetState() { - xSpeed = 0; - ySpeed = 0; + xPower = 0; + yPower = 0; rotSpeed = 0; - setToTeleop(); rotLocked = false; rotTarget = 0.0; - pathVel = 0.0; - pathHeading = 0.0; - pathAccel = 0.0; - pathTarget = 0.0; - aimOffset = 0.0; - vertOffset = 0.0; - autoOverride = false; - autoTag = false; - - isFieldCentric = true; + isSnake = false; + setToTeleop(); } @Override @@ -312,13 +354,6 @@ public String getName() { return "Swerve Drive"; } - /** resets the drive encoders on each module */ - public void resetDriveEncoders() { - for (int i = 0; i < modules.length; i++) { - modules[i].resetDriveEncoders(); - } - } - /** sets the drive to teleop/cross, and sets drive motors to coast */ public void setToTeleop() { driveState = driveType.TELEOP; @@ -326,11 +361,8 @@ public void setToTeleop() { modules[i].setDriveBrake(true); } rotSpeed = 0; - xSpeed = 0; - ySpeed = 0; - pathHeading = 0; - pathVel = 0; - pathAccel = 0; + xPower = 0; + yPower = 0; rotLocked = false; } @@ -342,6 +374,13 @@ public void setToAuto() { } } + /** Sets the drive state = OBJECT */ + public void setToObject() { + driveState = driveType.AUTO; + xPower = 0; + yPower = 0; + } + /**drives the robot at the current swerveSignal, and displays information for each swerve module */ private void drive() { if (driveState == driveType.CROSS) { @@ -358,18 +397,18 @@ private void drive() { } } - /**sets autonomous values from the path data file */ - public void setAutoValues(double velocity, double heading, double accel, double xOffset, double yOffset) { - pathVel = velocity; - pathHeading = heading; - pathAccel = accel; - pathXOffset = xOffset; - pathYOffset = yOffset; + /**sets autonomous values from the path data file in field relative */ + public void setAutoValues(double xVelocity, double yVelocity, double xOffset, double yOffset) { + SmartDashboard.putNumber("Offset X Power", xOffset * DriveConstants.TRANSLATION_P); + SmartDashboard.putNumber("Offset Y Power", yOffset * DriveConstants.TRANSLATION_P); + // accel of 0 because currently not using acceleration for power since + xPower = swerveHelper.getAutoPower(xVelocity, 0) + (xOffset) * DriveConstants.TRANSLATION_P; + yPower = swerveHelper.getAutoPower(yVelocity, 0) + (yOffset) * DriveConstants.TRANSLATION_P; } /**sets the autonomous heading controller to a new target */ public void setAutoHeading(double headingTarget) { - pathTarget = headingTarget; + rotTarget = headingTarget; } /** @@ -380,22 +419,34 @@ public void setAutoHeading(double headingTarget) { public void setGyro(double degrees) { resetState(); setToAuto(); - gyro.setYaw(degrees); + + // Make degrees clockwise + gyro.setYaw((360-degrees)%360); } public double getGyroAngle() { - if (!isFieldCentric) return 0; - return (359.99 - gyro.getYaw()+360)%360; + return (360 - gyro.getYaw()+360)%360; } + + public double getFieldYaw(){ + return Math.toRadians(360-getGyroAngle()); + } + + // Magnitude of robot speed for vision confidence + public double robotSpeed() { + speeds = DriveConstants.kinematics.toChassisSpeeds(new SwerveModuleState[] + {modules[0].moduleState(), modules[1].moduleState(), modules[2].moduleState(), modules[3].moduleState()}); + return Math.sqrt(speeds.vxMetersPerSecond * speeds.vxMetersPerSecond + speeds.vyMetersPerSecond + * speeds.vyMetersPerSecond + speeds.omegaRadiansPerSecond); + } public Rotation2d odoAngle(){ - return new Rotation2d(Math.toRadians(360-getGyroAngle())); + return new Rotation2d(getFieldYaw()); } public SwerveModulePosition[] odoPosition(){ return new SwerveModulePosition[]{modules[0].odoPosition(), modules[1].odoPosition(), modules[2].odoPosition(), modules[3].odoPosition()}; } public void setOdo(Pose2d starting){ - this.odometry.resetPosition(odoAngle(), odoPosition(), starting); - autoTimer.start(); + this.odometry = new SwerveDriveOdometry(DriveConstants.kinematics, odoAngle(), odoPosition(), starting); } public Pose2d returnPose(){ return odometry.getPoseMeters(); @@ -403,14 +454,13 @@ public Pose2d returnPose(){ public double getRotTarget(){ return rotTarget; } - public void setAutoTag(boolean isOn, boolean isBlue){ - autoTag = isOn; - this.isBlue = isBlue; + public void setVisionAuto(boolean isOn){ + this.isAutoVision = isOn; + } + public void setAutoObject(boolean isOn){ + this.isAutoObject = isOn; } - private double getClosestRotation(){ - if (getGyroAngle() < 90.0) return 45.0; - if (getGyroAngle() < 180.0) return 135.0; - if (getGyroAngle() < 270.0) return 225.0; - return 315.0; + public void setAutoAlign(boolean isOn){ + this.isAutoAlign = isOn; } } diff --git a/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveModule.java b/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveModule.java index 3236dc26..aa5657d8 100644 --- a/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveModule.java +++ b/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveModule.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -31,6 +32,7 @@ public class SwerveModule { */ public SwerveModule(WsSpark driveMotor, WsSpark angleMotor, double offset) { this.driveMotor = driveMotor; + // this.driveMotor.getController().getAbsoluteEncoder(Type.kDutyCycle).setVelocityConversionFactor(); this.angleMotor = angleMotor; this.absEncoder = angleMotor.getController().getAbsoluteEncoder(Type.kDutyCycle); this.absEncoder.setInverted(true); @@ -71,7 +73,7 @@ public void displayNumbers(String name) { /** resets drive encoder */ public void resetDriveEncoders() { - driveMotor.resetEncoder(); + //driveMotor.resetEncoder(); } /**sets drive to brake mode if true, coast if false @@ -159,6 +161,15 @@ public WsSpark getDriveMotor() { return driveMotor; } public SwerveModulePosition odoPosition(){ - return new SwerveModulePosition(getPosition()*0.0254*0.98, new Rotation2d(Math.toRadians(360-getAngle()))); + return new SwerveModulePosition(getPosition()*0.0254, new Rotation2d(Math.toRadians(360-getAngle()))); + } + public SwerveModuleState moduleState(){ + return new SwerveModuleState(driveMotor.getVelocity(), Rotation2d.fromDegrees(360-getAngle())); + } + public void setDriveCurrent(int newCurrentLimit){ + driveMotor.setCurrentLimit(newCurrentLimit, newCurrentLimit, 0); + } + public void tempDriveCurrent(int limit){ + driveMotor.tempCurrentLimit(limit); } } diff --git a/src/main/java/org/wildstang/sample/subsystems/swerve/WsSwerveHelper.java b/src/main/java/org/wildstang/sample/subsystems/swerve/WsSwerveHelper.java index a1593f2c..b3be0e4f 100644 --- a/src/main/java/org/wildstang/sample/subsystems/swerve/WsSwerveHelper.java +++ b/src/main/java/org/wildstang/sample/subsystems/swerve/WsSwerveHelper.java @@ -53,8 +53,8 @@ public SwerveSignal setDrive(double i_tx, double i_ty, double i_rot, double i_gy double yTrans = i_tx * Math.sin(Math.toRadians(i_gyro)) + i_ty * Math.cos(Math.toRadians(i_gyro)); //account for slight second order skew due to rotation and translation at the same time - xTrans += Math.cos(Math.atan2(xTrans,yTrans)) * i_rot * DriveConstants.ROT_CORRECTION_FACTOR; - yTrans += -Math.sin(Math.atan2(xTrans,yTrans)) * i_rot * DriveConstants.ROT_CORRECTION_FACTOR; + xTrans += Math.cos(Math.atan2(xTrans,yTrans)) * i_rot * DriveConstants.ROT_CORRECTION_FACTOR * Math.hypot(i_tx, i_ty); + yTrans += -Math.sin(Math.atan2(xTrans,yTrans)) * i_rot * DriveConstants.ROT_CORRECTION_FACTOR * Math.hypot(i_tx, i_ty); //cartesian vector addition of translation and rotation vectors //note rotation vector angle advances in the cos -> sin -> -cos -> -sin fashion @@ -81,8 +81,37 @@ public SwerveSignal setDrive(double i_tx, double i_ty, double i_rot, double i_gy * @param i_gyro the gyro value, field centric, in bearing degrees * @return SwerveSignal that is the command for the robot to move */ - public SwerveSignal setAuto(double i_power, double i_heading, double i_rot, double i_gyro, double xOffset, double yOffset) { - return setDrive(i_power * -Math.sin(Math.toRadians(i_heading))+ xOffset*.5, i_power * -Math.cos(Math.toRadians(i_heading))+ yOffset*.5, i_rot, i_gyro); //TODO: change xxoffset multiplier + public SwerveSignal setAuto(double i_X, double i_Y, double i_rot, double i_gyro, double xOffset, double yOffset) { + //return setDrive(i_power * -Math.sin(Math.toRadians(i_heading))+ xOffset*.5, i_power * -Math.cos(Math.toRadians(i_heading))+ yOffset*.5, i_rot, i_gyro); //TODO: change xxoffset multiplier + return setDrive(i_X - xOffset*Math.cos(Math.toRadians(i_gyro)) - yOffset*Math.sin(Math.toRadians(i_gyro)), + i_Y + xOffset*Math.sin(Math.toRadians(i_gyro)) - yOffset*Math.cos(Math.toRadians(i_gyro)), i_rot, i_gyro); + } + + /* + * Sets the robot to automatially drive at a detected game piece + * Adjusts driver's joysticks to point at gamepiece, and rotates the robot to face the gamepiece + * + * @param xPower X value of joystick + * @param yPower Y value of joystick + * @param tx limelight camera's tx value looking at gamepiece + * @return the swerveSignal to control the swerveDrive + */ + public SwerveSignal setObject(double xPower, double yPower, double tx){ + return setDrive(0, -Math.hypot(yPower, xPower) , getRotControl(tx, 0.0), 360.0-1.0*tx); + } + /* + * Determines if the limelight should take over for object detection in auto + * If the robot is moving too slowly to pick up a detected gamepiece, gives power + * to move enough to pick up the gamepiece + * @param ty ty value of limelight in object detection mode + * @param xPower X value of the normal auto path + * @param yPower Y value of the normal auto path + * @return the new power value to use if the robot is moving too slowly, otherwise 0 if the robot is fine + */ + public double adjustObjectAuto( double ty, double xPower, double yPower){ + double yObject = ty*0.03; + if (Math.hypot(xPower, yPower) > yObject) return yObject; + else return 0; } /**automatically creates a rotational joystick value to rotate the robot towards a specific target @@ -102,7 +131,17 @@ else if (Math.abs(rotDelta) <= 180.0) { else { rotPID = (rotDelta + 360) / 180; } - return Math.signum(rotPID) * Math.min(Math.abs(rotPID*2.5), 1.0); + return Math.signum(rotPID) * Math.min(Math.abs(rotPID*DriveConstants.PID_ROTATION), 1.0/DriveConstants.ROTATION_SPEED); + } + /* + * getRotControl, but capped to -0.2 to 0.2 + * + * @param i_target target direction for the robot to face, field centric, bearing degrees + * @param i_gyro the gyro value, field centric, in bearing degrees + * @return double that indicates what the rotational joystick value should be + */ + public double getAutoRotation(double i_target, double i_gyro){ + return Math.max(-0.2, Math.min(0.2, getRotControl(i_target, i_gyro))); } /**determines the translational magnitude of the robot in autonomous @@ -112,8 +151,8 @@ else if (Math.abs(rotDelta) <= 180.0) { */ public double getAutoPower(double pathVel, double pathAccel) { if (pathVel == 0) return 0; - double guess = pathVel * DriveConstants.DRIVE_F_V + DriveConstants.DRIVE_F_K + pathAccel * DriveConstants.DRIVE_F_I; - return -(guess); + // I don't know why this was negative + return (pathVel * DriveConstants.DRIVE_F_V + DriveConstants.DRIVE_F_K + pathAccel * DriveConstants.DRIVE_F_I); } /**returns magnitude of vector components */ diff --git a/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java b/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java index ea90bfa1..9d352b98 100644 --- a/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java +++ b/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.5.0 (March 27, 2024) package org.wildstang.sample.subsystems.targeting; @@ -303,6 +303,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -362,9 +374,60 @@ public Results() { public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + + public String error; public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -385,7 +448,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -397,7 +460,7 @@ private static Pose3d toPose3D(double[] inData){ private static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -405,6 +468,86 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -544,8 +687,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -604,6 +747,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -618,6 +783,26 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -643,6 +828,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -696,6 +886,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -765,7 +977,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - //System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); diff --git a/src/main/java/org/wildstang/sample/subsystems/targeting/TargetCoordinate.java b/src/main/java/org/wildstang/sample/subsystems/targeting/TargetCoordinate.java new file mode 100644 index 00000000..00567610 --- /dev/null +++ b/src/main/java/org/wildstang/sample/subsystems/targeting/TargetCoordinate.java @@ -0,0 +1,34 @@ +package org.wildstang.sample.subsystems.targeting; + +import org.wildstang.framework.core.Core; + +public class TargetCoordinate{ + private double blueX, blueY, redX, redY, blueHeading, redHeading; + + public TargetCoordinate(double i_blueX, double i_blueY){ + this(i_blueX, i_blueY, i_blueX, 322-i_blueY, 0, 0); + } + public TargetCoordinate(double i_blueX, double i_blueY, double i_redX, double i_redY, double i_blueHeading, double i_redHeading){ + blueX = i_blueX; + blueY = i_blueY; + redX = i_redX; + redY = i_redY; + blueHeading = i_blueHeading; + redHeading = i_redHeading; + } + public TargetCoordinate(double i_blueX, double i_blueY, double i_redX, double i_redY){ + this(i_blueX, i_blueY, i_redX, i_redY,0,0); + } + public TargetCoordinate(double i_blueX, double i_blueY, double i_blueHeading){ + this(i_blueX, i_blueY, i_blueX, 322-i_blueY, i_blueHeading, 360-i_blueHeading); + } + public double getX(){ + return Core.isBlue() ? blueX : redX; + } + public double getY(){ + return Core.isBlue() ? blueY : redY; + } + public double getHeading(){ + return Core.isBlue() ? blueHeading : redHeading; + } + } diff --git a/src/main/java/org/wildstang/sample/subsystems/targeting/VisionConsts.java b/src/main/java/org/wildstang/sample/subsystems/targeting/VisionConsts.java new file mode 100644 index 00000000..691e7f4d --- /dev/null +++ b/src/main/java/org/wildstang/sample/subsystems/targeting/VisionConsts.java @@ -0,0 +1,41 @@ +package org.wildstang.sample.subsystems.targeting; + +import java.util.List; + +import org.wildstang.framework.core.Core; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; + +public class VisionConsts { + //652.73, 323.00 + + public static final double inToM = 1/39.71; + public final double mToIn = 39.71; + + public static final TargetCoordinate speaker = new TargetCoordinate + (9.5, 208); + + public static final TargetCoordinate truss = new TargetCoordinate + (140, 218); + + public static final TargetCoordinate shot = new TargetCoordinate + (67, 218); + + //TODO: what are the actual pipeline indices? + public final int notePipelineIndex = 0; + public final int ATPipelineIndex = 0; + + /* + * April Tag IDs: + * - Stage: 11, 12, 13, 14, 15, 16 + * - Speaker: 3, 4, 7, 8 + * - Amp: 5, 6 + */ + public final List stageATs = List.of(11, 12, 13, 14, 15, 16); + public final List speakerATs = List.of(3, 4, 7, 8); + public final List ampATs = List.of(5, 6); + + public static Pose3d cameraPose = new Pose3d(-11.8*inToM,8.5*inToM,22.3*inToM, new Rotation3d(0.0,-65*Math.PI/180.0,11*Math.PI/180.0)); + +} \ No newline at end of file diff --git a/src/main/java/org/wildstang/sample/subsystems/targeting/WsLL.java b/src/main/java/org/wildstang/sample/subsystems/targeting/WsLL.java index 31651687..b5450b9f 100644 --- a/src/main/java/org/wildstang/sample/subsystems/targeting/WsLL.java +++ b/src/main/java/org/wildstang/sample/subsystems/targeting/WsLL.java @@ -1,5 +1,7 @@ package org.wildstang.sample.subsystems.targeting; +import org.wildstang.framework.core.Core; + import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -10,11 +12,12 @@ public class WsLL { public NetworkTable limelight; - public LimelightHelpers.Results result; + //public LimelightHelpers.Results result; - public double[] blue3D; - public double[] red3D; - public double tid; + private double[] blue3D; + private double[] red3D; + public double[] target3D; + public int tid; public double tv; public double tx; public double ty; @@ -29,10 +32,12 @@ public class WsLL { */ public WsLL(String CameraID){ limelight = NetworkTableInstance.getDefault().getTable(CameraID); - red3D = limelight.getEntry("botpose_wpired").getDoubleArray(new double[7]); - blue3D = limelight.getEntry("botpose_wpiblue").getDoubleArray(new double[7]); + red3D = limelight.getEntry("botpose_wpired").getDoubleArray(new double[11]); + //red3D = limelight.getEntry("botpose_orb_wpired").getDoubleArray(new double[11]); + blue3D = limelight.getEntry("botpose_wpiblue").getDoubleArray(new double[11]); + //blue3D = limelight.getEntry("botpose_orb_wpiblue").getDoublearray(new double[11]); setToIn(); - tid = limelight.getEntry("tid").getDouble(0); + tid = (int) limelight.getEntry("tid").getInteger(0); tv = limelight.getEntry("tv").getDouble(0); tx = limelight.getEntry("tx").getDouble(0); ty = limelight.getEntry("ty").getDouble(0); @@ -41,21 +46,28 @@ public WsLL(String CameraID){ ta = limelight.getEntry("ta").getDouble(0); this.CameraID = CameraID; - result = LimelightHelpers.getLatestResults(CameraID).targetingResults; + //result = LimelightHelpers.getLatestResults(CameraID).targetingResults; } /* * updates all values to the latest value */ - public void update(){ - result = LimelightHelpers.getLatestResults(CameraID).targetingResults; + public void update(double yaw){ + LimelightHelpers.SetRobotOrientation(CameraID, yaw, 0, 0, 0, 0, 0); + + //result = LimelightHelpers.getLatestResults(CameraID).targetingResults; tv = limelight.getEntry("tv").getDouble(0); + tx = limelight.getEntry("tx").getDouble(0); + ty = limelight.getEntry("ty").getDouble(0); if (tv > 0){ - blue3D = limelight.getEntry("botpose_wpiblue").getDoubleArray(new double[7]); - red3D = limelight.getEntry("botpose_wpired").getDoubleArray(new double[7]); + blue3D = limelight.getEntry("botpose_wpiblue").getDoubleArray(new double[11]); + //blue3D = limelight.getEntry("botpose_orb_wpiblue").getDoubleArray(new double[11]); + red3D = limelight.getEntry("botpose_wpired").getDoubleArray(new double[11]); + //red3D = limelight.getEntry("botpose_orb_wpired").getDoubleArray(new double[11]); + target3D = Core.isBlue() ? blue3D : red3D; setToIn(); - tid = limelight.getEntry("tid").getDouble(0); - numTargets = result.targets_Fiducials.length; + tid = (int) limelight.getEntry("tid").getInteger(0); + //numTargets = result.targets_Fiducials.length; } updateDashboard(); } @@ -66,17 +78,12 @@ public boolean TargetInView(){ return tv>0; } - /* - * returns true if seeing a blue apriltag, false if a red one - */ - public boolean isSeeingBlue(){ - return tid > 4.5; - } - public void updateDashboard(){ SmartDashboard.putBoolean(CameraID + " tv", TargetInView()); SmartDashboard.putNumber(CameraID + " tid", tid); SmartDashboard.putNumber(CameraID + " numTargets", numTargets); + SmartDashboard.putNumber(CameraID + "Vision x", target3D[0]); + SmartDashboard.putNumber(CameraID + "Vision y", target3D[1]); } /* * returns total latency, capture latency + pipeline latency @@ -106,10 +113,68 @@ public void setCam(int cameraMode){ limelight.getEntry("camMode").setNumber(cameraMode); } + /* + * sets measurements to in from m + */ private void setToIn(){ for (int i = 0; i < 7; i++){ this.red3D[i] *= mToIn; this.blue3D[i] *= mToIn; + this.target3D[i] *= mToIn; } } + /** + * returns distance to selected alliances' center of speaker for lookup table use + */ + public double distanceToTarget(TargetCoordinate target){ + return Math.hypot(target3D[0] - target.getX(), + target3D[1] - target.getY()); + } + /* + * gets control value for aligning robot to certain x value on the field + */ + public double getAlignX(TargetCoordinate target){ + return -target3D[0]+target.getX(); + } + /* + * gets control value for aligning robot to certain y value on the field + */ + public double getAlignY(TargetCoordinate target){ + return target3D[1] - target.getY(); + } + + /** + * input of X and Y in frc field coordinates, returns controller bearing degrees (aka what to plug into rotLocked) for turnToTarget + */ + public double getDirection(double x, double y) { + double measurement = 90 + Math.toDegrees(Math.atan2(x,y)); + if (measurement < 0) { + measurement = 360 + measurement; + } + else if (measurement >= 360) { + measurement = measurement - 360; + } + return measurement; + } + /** + * returns what to set rotLocked to + */ + public double turnToTarget(TargetCoordinate target){ + return getDirection(target3D[0] - target.getX(), + target3D[1] - target.getY()); + } + + /* + * determine how many april tags a camera can see + */ + public double getNumTags(){ + return target3D[7]; + } + /* + * determine the distance to the first april tag that a camera can see + */ + public double getTagDist(){ + return target3D[9]; + } + } \ No newline at end of file diff --git a/src/main/java/org/wildstang/sample/subsystems/targeting/WsPV.java b/src/main/java/org/wildstang/sample/subsystems/targeting/WsPV.java new file mode 100644 index 00000000..cca4bf7e --- /dev/null +++ b/src/main/java/org/wildstang/sample/subsystems/targeting/WsPV.java @@ -0,0 +1,135 @@ +package org.wildstang.sample.subsystems.targeting; + +import java.lang.annotation.Target; +import java.util.OptionalDouble; + +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +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.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class WsPV { + + public PhotonCamera camera; + public String cameraID; + AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public PhotonTrackedTarget target; + public PhotonPipelineResult result; + private Transform3d robotToCamera = new Transform3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0,0,0)); + + private VisionConsts VC = new VisionConsts(); + + public int tid = 0; + public double tx = 0; + public double ty = 0; + public boolean tv = false; + public Transform3d aprilTag = new Transform3d(); + public Pose3d estimatedPose = new Pose3d(); + + //whether the cam detects ATs or Notes + public boolean isAT; + + public WsPV(String cameraID, boolean isAprilTag){ + this.cameraID = cameraID; + camera = new PhotonCamera(cameraID); + isAT = isAprilTag; + } + public WsPV(String cameraID){ + this(cameraID, false); + } + + public void update(){ + result = camera.getLatestResult(); + SmartDashboard.putBoolean("hasTargets", result.hasTargets()); + tv = result.hasTargets(); + if(tv) { + target = result.getBestTarget(); + tx = target.getYaw(); + ty = target.getPitch(); + + //only used if it is an AT cam + if(isAT){ + tid = target.getFiducialId(); + aprilTag = target.getBestCameraToTarget(); + estimatedPose = PhotonUtils.estimateFieldToRobotAprilTag(aprilTag, + aprilTagFieldLayout.getTagPose(target.getFiducialId()).get(), robotToCamera); + } + + } + updateDashboard(); + } + + public void updateDashboard(){ + SmartDashboard.putBoolean(cameraID + " tv", tv); + SmartDashboard.putNumber(cameraID + " tid", tid); + SmartDashboard.putNumber(cameraID + " Y", ty); + SmartDashboard.putNumber(cameraID + " X", tx); + SmartDashboard.putBoolean(cameraID + " isAT", isAT); + SmartDashboard.putNumber(cameraID + "poseX", estimatedPose.getX()*VC.mToIn); + SmartDashboard.putNumber(cameraID + "posey", estimatedPose.getY()*VC.mToIn); + } + + /** + * Toggles between Note and AT detection pipelines + */ + public void togglePipeline() { + isAT = !isAT; + camera.setPipelineIndex(isAT ? VC.notePipelineIndex : VC.ATPipelineIndex); + } + + /** + * returns what to set rotLocked to + */ + public double turnToTarget(boolean isBlue){ + return getDirection(estimatedPose.getX()*VC.mToIn - (VisionConsts.speaker.getX()), + estimatedPose.getY()*VC.mToIn - (VisionConsts.speaker.getY()), isBlue); + } + + /** + * returns distance to selected alliances' center of speaker for lookup table use + */ + public double distanceToTarget(boolean isBlue){ + return Math.hypot(estimatedPose.getX()*VC.mToIn - VisionConsts.speaker.getX(), + estimatedPose.getY()*VC.mToIn - VisionConsts.speaker.getY()); + } + + /** + * input of X and Y in frc field coordinates, returns controller bearing degrees (aka what to plug into rotLocked) + */ + private double getDirection(double x, double y, boolean isBlue) { + double measurement = Math.toDegrees(Math.atan2(x,y)); + if (isBlue) measurement += 90; + else measurement -= 90; + if (measurement < 0) { + measurement = 360 + measurement; + } + else if (measurement >= 360) { + measurement = measurement - 360; + } + return measurement; + } + public boolean TargetInView(){ + return tv; + } + public double getPoseX(){ + return estimatedPose.getX(); + } + public double getPoseY(){ + return estimatedPose.getY(); + } + + public int getAprilTagID() { + return tid; + } + +} diff --git a/src/main/java/org/wildstang/sample/subsystems/targeting/WsVision.java b/src/main/java/org/wildstang/sample/subsystems/targeting/WsVision.java index 18621cb4..32fe6174 100644 --- a/src/main/java/org/wildstang/sample/subsystems/targeting/WsVision.java +++ b/src/main/java/org/wildstang/sample/subsystems/targeting/WsVision.java @@ -2,177 +2,55 @@ // ton of imports import org.wildstang.framework.subsystems.Subsystem; +import org.wildstang.sample.robot.WsInputs; +import org.wildstang.sample.robot.WsSubsystems; +import org.wildstang.sample.subsystems.swerve.SwerveDrive; import org.wildstang.framework.core.Core; import org.wildstang.framework.io.inputs.DigitalInput; import org.wildstang.framework.io.inputs.Input; -import org.wildstang.sample.robot.WsInputs; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class WsVision implements Subsystem { public WsLL left = new WsLL("limelight-left"); public WsLL right = new WsLL("limelight-right"); + public WsLL back = new WsLL("limelight-back"); - public boolean gamepiece, isLow; + private final double Align_P = 0.006; + + public SwerveDrive swerve; - private DigitalInput rightBumper, leftBumper, high, mid, low;//, dup, ddown; + public VisionConsts VC; + public Translation2d robotOdometry = new Translation2d(); - public LimeConsts LC; + private double inputDistance = 0; - ShuffleboardTab tab = Shuffleboard.getTab("Tab"); + private DigitalInput driverLeftShoulder; - public boolean TargetInView(){ - return left.TargetInView() || right.TargetInView(); - } + private Timer lastUpdate = new Timer(); + + @Override + public void inputUpdate(Input source) { - //get ySpeed value for auto drive - public double getScoreY(double offset){ - if (right.TargetInView() && left.TargetInView()){ - return LC.VERT_AUTOAIM_P * (offset*LC.OFFSET_VERTICAL + (getLeftVertical() + getRightVertical())/2.0); - } else if (left.tv > 0.0){ - return (getLeftVertical()+offset*LC.OFFSET_VERTICAL) * LC.VERT_AUTOAIM_P; - } else { - return (getRightVertical()+offset*LC.OFFSET_VERTICAL) * LC.VERT_AUTOAIM_P; - } - } - //get ySpeed value for station auto drive - public double getStationY(double offset){ - if (left.TargetInView()){ - return (offset*LC.STATION_OFFSETS + (left.tid > 4.5 ? -left.blue3D[0] + LC.STATION_VERTICAL - : -left.red3D[0] + LC.STATION_VERTICAL)) * -LC.VERT_AUTOAIM_P; - } else { - return (offset*LC.STATION_OFFSETS + (right.tid > 4.5 ? -right.blue3D[0] + LC.STATION_VERTICAL - : -right.red3D[0] + LC.STATION_VERTICAL)) * -LC.VERT_AUTOAIM_P; - } - } - //get xSpeed value for station auto drive - public double getStationX(double offset){ - if (left.TargetInView()){ - if (left.isSeeingBlue()){ - //basically this garbage line determines whether we're going to the left or right double substation - // by which one's closer, adds on the driver offset, and then calculates the xSpeed for that - // desired displacement. - return -LC.HORI_AUTOAIM_P * (-offset*LC.STATION_OFFSETS + (left.blue3D[1] - LC.BLUE_STATION_X - - LC.STATION_HORIZONTAL*Math.signum(left.blue3D[1]-LC.BLUE_STATION_X))); - } else { - return -LC.HORI_AUTOAIM_P * (-offset*LC.STATION_OFFSETS + (left.red3D[1] - LC.RED_STATION_X - - LC.STATION_HORIZONTAL*Math.signum(left.red3D[1]-LC.RED_STATION_X))); - } - } else { - if (right.isSeeingBlue()){ - return -LC.HORI_AUTOAIM_P * (-offset*LC.STATION_OFFSETS + (right.blue3D[1] - LC.BLUE_STATION_X - - LC.STATION_HORIZONTAL*Math.signum(right.blue3D[1]-LC.BLUE_STATION_X))); - } else { - return -LC.HORI_AUTOAIM_P * (-offset*LC.STATION_OFFSETS + (right.red3D[1] - LC.RED_STATION_X - - LC.STATION_HORIZONTAL*Math.signum(right.red3D[1]-LC.RED_STATION_X))); - } - } - } - //gets the vertical distance to target for grid targets from the left limelight - private double getLeftVertical(){ - if (left.isSeeingBlue()){ - return -left.blue3D[0] + (LC.VERTICAL_APRILTAG_DISTANCE + (isLow ? 10.0 : 0.0)); - } else { - return -left.red3D[0] + (LC.VERTICAL_APRILTAG_DISTANCE + (isLow ? 10.0 : 0.0)); - } - } - //gets the vertical distance to target for grid targets from the right limelight - private double getRightVertical(){ - if (right.isSeeingBlue()){ - return -right.blue3D[0] + (LC.VERTICAL_APRILTAG_DISTANCE); - } else { - return -right.red3D[0] + (LC.VERTICAL_APRILTAG_DISTANCE); - } - } - - //get xSpeed value for autodrive - public double getScoreX(double offset){ - if (right.TargetInView() && left.TargetInView()){ - return LC.HORI_AUTOAIM_P * (offset*LC.OFFSET_HORIZONTAL + (getLeftHorizontal() + getRightHorizontal())/2.0); - } else if (left.TargetInView()){ - return (getLeftHorizontal()+offset*LC.OFFSET_HORIZONTAL) * LC.HORI_AUTOAIM_P; - } else { - return (getRightHorizontal()+offset*LC.OFFSET_HORIZONTAL) * LC.HORI_AUTOAIM_P; - } - } - //get the horizontal distance to targets on the grid from the left limelight - private double getLeftHorizontal(){ - if (left.isSeeingBlue()){ - if (gamepiece) return getCone(left.blue3D[1], true); - else return getCube(left.blue3D[1], true); - } else { - if (gamepiece) return getCone(left.red3D[1], false); - else return getCube(left.red3D[1], false); - } - } - //gets the horizontal distance to targets on the grid from the right limelight - private double getRightHorizontal(){ - if (right.isSeeingBlue()){ - if (gamepiece) return getCone(right.blue3D[1], true); - else return getCube(right.blue3D[1], true); - } else { - if (gamepiece) return getCone(right.red3D[1], false); - else return getCube(right.red3D[1], false); - } - } - //determines which cone node is the closest to score on - private double getCone(double target, boolean color){ - int i = color ? 6 : 0; - double minimum = 1000; - while (i < (color ? 12 : 6)){ - if (Math.abs(minimum) > Math.abs(target - LC.CONES[i]*39.3701)){ - minimum = target - LC.CONES[i]*39.3701; - } - i++; - } - return minimum; - } - //determines which cube node is the closest to score on - private double getCube(double target, boolean color){ - int i = color ? 3 : 0; - double minimum = 1000; - while (i < (color ? 6 : 3)){ - if (Math.abs(minimum) > Math.abs(target - LC.CUBES[i]*39.3701)){ - minimum = target - LC.CUBES[i]*39.3701; - } - i++; - } - return minimum; } @Override - public void inputUpdate(Input source) { - if (rightBumper.getValue()) - { - gamepiece = LC.CUBE; - } - if (leftBumper.getValue()){ - gamepiece = LC.CONE; - } - if (source == low && low.getValue()) isLow = true; - if ((source == mid && mid.getValue()) || (source == high && high.getValue())) isLow = false; + public void initSubsystems() { + swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); } @Override public void init() { - LC = new LimeConsts(); - - rightBumper = (DigitalInput) Core.getInputManager().getInput(WsInputs.OPERATOR_RIGHT_SHOULDER); - rightBumper.addInputListener(this); - leftBumper = (DigitalInput) WsInputs.OPERATOR_LEFT_SHOULDER.get(); - leftBumper.addInputListener(this); - high = (DigitalInput) WsInputs.OPERATOR_FACE_UP.get(); - high.addInputListener(this); - mid = (DigitalInput) WsInputs.OPERATOR_FACE_RIGHT.get(); - mid.addInputListener(this); - low = (DigitalInput) WsInputs.OPERATOR_FACE_DOWN.get(); - low.addInputListener(this); - - resetState(); + VC = new VisionConsts(); + //same as update() + //resetState(); + driverLeftShoulder = (DigitalInput) WsInputs.DRIVER_LEFT_SHOULDER.get(); + driverLeftShoulder.addInputListener(this); + lastUpdate.start(); } @Override @@ -181,24 +59,98 @@ public void selfTest() { @Override public void update() { - left.update(); - right.update(); - SmartDashboard.putBoolean("limelight target in view", TargetInView()); + + left.update(swerve.getFieldYaw()); + right.update(swerve.getFieldYaw()); + back.update(swerve.getFieldYaw()); + if (aprilTagsInView()) lastUpdate.reset(); + SmartDashboard.putBoolean("Vision targetinView", aprilTagsInView()); + SmartDashboard.putNumber("GP X", back.tx); + SmartDashboard.putNumber("GP Y", back.ty); + SmartDashboard.putBoolean("GP tv", back.TargetInView()); + SmartDashboard.putNumber("Vision back tx", back.tx); + SmartDashboard.putNumber("Vision back ty", back.ty); + } @Override public void resetState() { - gamepiece = LC.CONE; - isLow = false; - left.update(); - right.update(); - } - public void setGamePiece(boolean newPiece){ - gamepiece = newPiece; + left.update(swerve.getFieldYaw()); + right.update(swerve.getFieldYaw()); + back.update(swerve.getFieldYaw()); } @Override public String getName() { return "Ws Vision"; } -} \ No newline at end of file + + /** + * determine whether to take data from left or right camera + */ + public boolean isLeftBetter(){ + if (left.TargetInView() && !right.TargetInView()) return true; + if (!left.TargetInView() && right.TargetInView()) return false; + if (left.getTagDist() < right.getTagDist()) return true; + if (left.getTagDist() > right.getTagDist()) return false; + if (left.getNumTags() > right.getNumTags()) return true; + if (left.getNumTags() < right.getNumTags()) return false; + return false; + } + /** + * gets bearing degrees for what the robot's heading should be to be pointing at the target + */ + public double turnToTarget(TargetCoordinate target){ + if (!aprilTagsInView()) return left.getDirection(robotOdometry.getX() - target.getX(), robotOdometry.getY() - target.getY()); + return isLeftBetter() ? left.turnToTarget(target) : right.turnToTarget(target); + } + /* + * returns distance from current location to target, in inches + */ + public double distanceToTarget(TargetCoordinate target){ + if (!aprilTagsInView()) return Math.hypot(robotOdometry.getX() - target.getX(), robotOdometry.getY() - target.getY()); + else return isLeftBetter() ? left.distanceToTarget(target) : right.distanceToTarget(target); + } + /** + * can either camera see an April Tag + */ + public boolean aprilTagsInView(){ + return left.TargetInView() || right.TargetInView(); + } + /* + * get the control value to use for driving the robot to a specific X on the field + */ + public double getXAdjust(TargetCoordinate target){ + if (!aprilTagsInView()) return Align_P * robotOdometry.getX(); + if (isLeftBetter()) return Align_P * left.getAlignX(target); + else return Align_P * right.getAlignX(target); + } + /* + * get the control value to use for driving the robot to a specific y on the field + */ + public double getYAdjust(TargetCoordinate target){ + if (!aprilTagsInView()) return Align_P * robotOdometry.getY(); + if (isLeftBetter()) return Align_P * left.getAlignY(target); + else return Align_P * right.getAlignY(target); + } + /* + * get Y value from cameras to use for determining the direction of the robot + * to feed a note to the desired location + */ + public double getYValue(){ + if (!aprilTagsInView()) return robotOdometry.getY(); + if (isLeftBetter()) return left.target3D[1]; + else return right.target3D[1]; + } + public double getUpdateTime(){ + return lastUpdate.get(); + } + public Translation2d getCameraPose(){ + if (isLeftBetter()) return new Translation2d(left.target3D[0], left.target3D[1]); + else return new Translation2d(right.target3D[0], right.target3D[1]); + } + public void setOdometry(Translation2d update){ + robotOdometry = update; + robotOdometry.times(39.37); + } + } \ No newline at end of file diff --git a/src/main/test.chor b/src/main/test.chor new file mode 100644 index 00000000..95ebe687 --- /dev/null +++ b/src/main/test.chor @@ -0,0 +1,898 @@ +{ + "version": "v0.2.2", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 0.75, + "motorMaxVelocity": 4200, + "gearing": 4, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, + "wheelRadius": 0.03809997942601111 + }, + "paths": { + "TestSlow": { + "waypoints": [ + { + "x": 2, + "y": 5, + "heading": 3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 5.943328857421875, + "y": 4.953978061676025, + "heading": 3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2, + "y": 5, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -5.199035045340379e-36, + "velocityY": -7.094659994161304e-34, + "timestamp": 0 + }, + { + "x": 2.0394332924621374, + "y": 4.999539780571386, + "heading": 3.14, + "angularVelocity": 3.467230192636983e-17, + "velocityX": 0.4094219230255359, + "velocityY": -0.00477829548872164, + "timestamp": 0.09631456022367899 + }, + { + "x": 2.118299876878191, + "y": 4.998619341720087, + "heading": 3.14, + "angularVelocity": 6.719128759652369e-17, + "velocityX": 0.81884384077439, + "velocityY": -0.009556590915859996, + "timestamp": 0.19262912044735797 + }, + { + "x": 2.236599752604414, + "y": 4.9972386834536175, + "heading": 3.14, + "angularVelocity": 9.970677800237134e-17, + "velocityX": 1.2282657518394462, + "velocityY": -0.014334886264992857, + "timestamp": 0.28894368067103693 + }, + { + "x": 2.3943329187989826, + "y": 4.995397805781803, + "heading": 3.14, + "angularVelocity": 1.8923134419468757e-16, + "velocityX": 1.6376876541641503, + "velocityY": -0.019113181512118518, + "timestamp": 0.38525824089471594 + }, + { + "x": 2.591499374313956, + "y": 4.993096708718039, + "heading": 3.14, + "angularVelocity": 2.1985978708871568e-16, + "velocityX": 2.04710954457019, + "velocityY": -0.023891476620143418, + "timestamp": 0.48157280111839496 + }, + { + "x": 2.828099117491197, + "y": 4.990335392281679, + "heading": 3.14, + "angularVelocity": 3.641000187669834e-16, + "velocityX": 2.4565314177603748, + "velocityY": -0.02866977152724492, + "timestamp": 0.577887361342074 + }, + { + "x": 3.104132145725059, + "y": 4.987113856503132, + "heading": 3.14, + "angularVelocity": 4.534970646348135e-16, + "velocityX": 2.8659532638970524, + "velocityY": -0.03344806611860942, + "timestamp": 0.674201921565753 + }, + { + "x": 3.4195984543253704, + "y": 4.983432101437137, + "heading": 3.14, + "angularVelocity": 5.427823235427362e-16, + "velocityX": 3.275375061337336, + "velocityY": -0.038226360141646376, + "timestamp": 0.770516481789432 + }, + { + "x": 3.7744980323483457, + "y": 4.979290127211418, + "heading": 3.14, + "angularVelocity": 5.764340593314991e-16, + "velocityX": 3.6847967451521693, + "velocityY": -0.04300465283857951, + "timestamp": 0.866831042013111 + }, + { + "x": 4.168830825073529, + "y": 4.974687934464607, + "heading": 3.14, + "angularVelocity": 6.655918603013656e-16, + "velocityX": 4.094217860823843, + "velocityY": -0.047782938904807865, + "timestamp": 0.96314560223679 + }, + { + "x": 4.523730403096504, + "y": 4.970545960238888, + "heading": 3.14, + "angularVelocity": 5.770882548873366e-16, + "velocityX": 3.6847967451521697, + "velocityY": -0.0430046528385795, + "timestamp": 1.059460162460469 + }, + { + "x": 4.8391967116968155, + "y": 4.966864205172893, + "heading": 3.14, + "angularVelocity": 5.455400313133093e-16, + "velocityX": 3.2753750613373365, + "velocityY": -0.038226360141646376, + "timestamp": 1.155774722684148 + }, + { + "x": 5.115229739930678, + "y": 4.963642669394346, + "heading": 3.14, + "angularVelocity": 4.560243469996038e-16, + "velocityX": 2.865953263897053, + "velocityY": -0.033448066118609426, + "timestamp": 1.252089282907827 + }, + { + "x": 5.3518294831079185, + "y": 4.960881352957986, + "heading": 3.14, + "angularVelocity": 3.664668961069072e-16, + "velocityX": 2.456531417760375, + "velocityY": -0.028669771527244925, + "timestamp": 1.348403843131506 + }, + { + "x": 5.548995938622893, + "y": 4.958580255894223, + "heading": 3.14, + "angularVelocity": 2.7689730770284007e-16, + "velocityX": 2.0471095445701906, + "velocityY": -0.023891476620143418, + "timestamp": 1.444718403355185 + }, + { + "x": 5.7067291048174615, + "y": 4.956739378222408, + "heading": 3.14, + "angularVelocity": 2.4248632720368313e-16, + "velocityX": 1.6376876541641505, + "velocityY": -0.019113181512118518, + "timestamp": 1.541032963578864 + }, + { + "x": 5.825028980543684, + "y": 4.955358719955939, + "heading": 3.14, + "angularVelocity": 1.5364358713309854e-16, + "velocityX": 1.2282657518394462, + "velocityY": -0.014334886264992859, + "timestamp": 1.637347523802543 + }, + { + "x": 5.903895564959738, + "y": 4.95443828110464, + "heading": 3.14, + "angularVelocity": 1.2105238270710193e-16, + "velocityX": 0.8188438407743899, + "velocityY": -0.00955659091586, + "timestamp": 1.733662084026222 + }, + { + "x": 5.943328857421875, + "y": 4.953978061676025, + "heading": 3.14, + "angularVelocity": 8.963250403234999e-17, + "velocityX": 0.4094219230255359, + "velocityY": -0.00477829548872164, + "timestamp": 1.829976644249901 + }, + { + "x": 5.943328857421875, + "y": 4.953978061676025, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -2.7383639654683927e-37, + "velocityY": 0, + "timestamp": 1.92629120447358 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "TestSlowAngle": { + "waypoints": [ + { + "x": 2, + "y": 5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 3.5, + "y": 6.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": -4.007563632250384e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.0357142884817963, + "y": 5.035714288481796, + "heading": -1.2128236829393102e-18, + "angularVelocity": -1.2188889104369536e-17, + "velocityX": 0.35892894881563847, + "velocityY": 0.3589289488156384, + "timestamp": 0.09950239065320052 + }, + { + "x": 2.107142864670486, + "y": 5.107142864670486, + "heading": -5.373326257747517e-18, + "angularVelocity": -4.181308929900821e-17, + "velocityX": 0.717857889843493, + "velocityY": 0.717857889843493, + "timestamp": 0.19900478130640104 + }, + { + "x": 2.2142857274037135, + "y": 5.2142857274037135, + "heading": -1.2480917932573193e-17, + "angularVelocity": -7.143136207987212e-17, + "velocityX": 1.0767868191896721, + "velocityY": 1.0767868191896723, + "timestamp": 0.2985071719596015 + }, + { + "x": 2.3571428747442225, + "y": 5.3571428747442225, + "heading": -1.6672401590500304e-17, + "angularVelocity": -4.212444844143106e-17, + "velocityX": 1.4357157290663938, + "velocityY": 1.4357157290663938, + "timestamp": 0.39800956261280207 + }, + { + "x": 2.5357143028174978, + "y": 5.535714302817498, + "heading": -2.3808332434080198e-17, + "angularVelocity": -7.171616995817411e-17, + "velocityX": 1.794644600004205, + "velocityY": 1.794644600004205, + "timestamp": 0.4975119532660026 + }, + { + "x": 2.75, + "y": 5.75, + "heading": -3.1767176762840175e-17, + "angularVelocity": -7.998645710471698e-17, + "velocityX": 2.1535733541253332, + "velocityY": 2.1535733541253332, + "timestamp": 0.5970143439192032 + }, + { + "x": 2.9642856971825022, + "y": 5.964285697182502, + "heading": -2.6049355888382788e-17, + "angularVelocity": 5.746415217336393e-17, + "velocityX": 2.1535733541253332, + "velocityY": 2.1535733541253332, + "timestamp": 0.6965167345724037 + }, + { + "x": 3.1428571252557775, + "y": 6.1428571252557775, + "heading": -2.323188559090568e-17, + "angularVelocity": 2.831559968636768e-17, + "velocityX": 1.794644600004205, + "velocityY": 1.7946446000042047, + "timestamp": 0.7960191252256043 + }, + { + "x": 3.2857142725962865, + "y": 6.2857142725962865, + "heading": -1.7457981849578456e-17, + "angularVelocity": 5.802778493002341e-17, + "velocityX": 1.4357157290663938, + "velocityY": 1.4357157290663936, + "timestamp": 0.8955215158788048 + }, + { + "x": 3.392857135329514, + "y": 6.392857135329514, + "heading": -8.729542404310101e-18, + "angularVelocity": 8.772089729841888e-17, + "velocityX": 1.0767868191896721, + "velocityY": 1.0767868191896721, + "timestamp": 0.9950239065320053 + }, + { + "x": 3.4642857115182037, + "y": 6.464285711518204, + "heading": -2.9100030696757928e-18, + "angularVelocity": 5.848642418502831e-17, + "velocityX": 0.7178578898434931, + "velocityY": 0.717857889843493, + "timestamp": 1.0945262971852059 + }, + { + "x": 3.5, + "y": 6.5, + "heading": -2.657468488554102e-35, + "angularVelocity": 2.92455577143697e-17, + "velocityX": 0.3589289488156385, + "velocityY": 0.3589289488156383, + "timestamp": 1.1940286878384063 + }, + { + "x": 3.5, + "y": 6.5, + "heading": -4.035178290149187e-35, + "angularVelocity": -1.3845993613099983e-34, + "velocityX": 0, + "velocityY": 0, + "timestamp": 1.2935310784916068 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "TestSlowRotate": { + "waypoints": [ + { + "x": 2, + "y": 5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 26 + }, + { + "x": 6, + "y": 5, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": 0, + "velocityX": 1.9208048805528434e-34, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.02366251246836, + "y": 4.9999997052109455, + "heading": 0.00950969246516044, + "angularVelocity": 0.1391965172175312, + "velocityX": 0.3463560295224064, + "velocityY": -0.000004314924996580056, + "timestamp": 0.0683184655424896 + }, + { + "x": 2.0709880449186984, + "y": 4.999999379781912, + "heading": 0.028512388083077194, + "angularVelocity": 0.27814874744366697, + "velocityX": 0.6927194876896832, + "velocityY": -0.000004763412508314695, + "timestamp": 0.1366369310849792 + }, + { + "x": 2.1419772416715244, + "y": 4.999999426144968, + "heading": 0.05698644572679429, + "angularVelocity": 0.4167842093292944, + "velocityX": 1.0390923769895746, + "velocityY": 6.786314042185358e-7, + "timestamp": 0.2049553966274688 + }, + { + "x": 2.2366309542982097, + "y": 5.000000358617276, + "heading": 0.09490253039039682, + "angularVelocity": 0.5549902850206919, + "velocityX": 1.385477731021595, + "velocityY": 0.000013648905905289525, + "timestamp": 0.2732738621699584 + }, + { + "x": 2.3549503342395997, + "y": 5.000002752941593, + "heading": 0.14222003904242062, + "angularVelocity": 0.6926020406971164, + "velocityX": 1.7318799390744954, + "velocityY": 0.000035046517766188305, + "timestamp": 0.341592327712448 + }, + { + "x": 2.4969369478040657, + "y": 5.000007160826493, + "heading": 0.1988827529646636, + "angularVelocity": 0.8293909043815195, + "velocityX": 2.0783050737016247, + "velocityY": 0.00006451967071180592, + "timestamp": 0.4099107932549376 + }, + { + "x": 2.662592916041132, + "y": 5.000013981415258, + "heading": 0.26481373895622135, + "angularVelocity": 0.9650536713321403, + "velocityX": 2.424761254832917, + "velocityY": 0.00009983521601619577, + "timestamp": 0.4782292587974272 + }, + { + "x": 2.851921094530713, + "y": 5.000023289694376, + "heading": 0.3399091190697772, + "angularVelocity": 1.0991959423744913, + "velocityX": 2.7712592340328714, + "velocityY": 0.00013624836335922213, + "timestamp": 0.5465477243399168 + }, + { + "x": 3.064925351863494, + "y": 5.000034634675967, + "heading": 0.4240287650700209, + "angularVelocity": 1.231287110040946, + "velocityX": 3.1178138390755596, + "velocityY": 0.00016606025181951199, + "timestamp": 0.6148661898824064 + }, + { + "x": 3.301611146859007, + "y": 5.000046829573855, + "heading": 0.51697668272804, + "angularVelocity": 1.3605094452862343, + "velocityX": 3.464448346667129, + "velocityY": 0.00017850075805292458, + "timestamp": 0.6831846554248959 + }, + { + "x": 3.56198711871686, + "y": 5.000057723571391, + "heading": 0.6184437013686873, + "angularVelocity": 1.4852063469947503, + "velocityX": 3.8112093090837407, + "velocityY": 0.00015945904887937467, + "timestamp": 0.7515031209673855 + }, + { + "x": 3.8460710207555886, + "y": 5.000063620513341, + "heading": 0.7277730701439165, + "angularVelocity": 1.6002901690939435, + "velocityX": 4.158230132701782, + "velocityY": 0.00008631549175548375, + "timestamp": 0.819821586509875 + }, + { + "x": 4.153930046387012, + "y": 5.000052301182086, + "heading": 0.8417150362226031, + "angularVelocity": 1.6678062830293265, + "velocityX": 4.506234488535969, + "velocityY": -0.00016568479933949432, + "timestamp": 0.8881400520523646 + }, + { + "x": 4.4380104929852475, + "y": 5.000038330055544, + "heading": 0.9512759924106793, + "angularVelocity": 1.6036799907330455, + "velocityX": 4.158179554275207, + "velocityY": -0.00020450000495440387, + "timestamp": 0.9564585175948541 + }, + { + "x": 4.69838319238425, + "y": 5.000025271205214, + "heading": 1.052957052286739, + "angularVelocity": 1.4883393394253035, + "velocityX": 3.811161409020038, + "velocityY": -0.00019114671598567193, + "timestamp": 1.0247769831373437 + }, + { + "x": 4.935066721530443, + "y": 5.000014536146546, + "heading": 1.146065613041752, + "angularVelocity": 1.36286083148494, + "velocityX": 3.4644151806804317, + "velocityY": -0.00015713260805078636, + "timestamp": 1.0930954486798332 + }, + { + "x": 5.148069980355881, + "y": 5.000006628646695, + "heading": 1.2302821189406512, + "angularVelocity": 1.232704880447324, + "velocityX": 3.117799223592987, + "velocityY": -0.00011574469345328521, + "timestamp": 1.1614139142223228 + }, + { + "x": 5.337398336214855, + "y": 5.000001474581276, + "heading": 1.3054155698448289, + "angularVelocity": 1.0997531971418355, + "velocityX": 2.7712618302474024, + "velocityY": -0.00007544176201745666, + "timestamp": 1.2297323797648123 + }, + { + "x": 5.5030553642442035, + "y": 4.999998653511926, + "heading": 1.3713394215549772, + "angularVelocity": 0.9649492444930273, + "velocityX": 2.424776767363443, + "velocityY": -0.00004129292611118568, + "timestamp": 1.2980508453073019 + }, + { + "x": 5.645043553923344, + "y": 4.999997581430543, + "heading": 1.427966299920808, + "angularVelocity": 0.8288663674773633, + "velocityX": 2.078328143813961, + "velocityY": -0.00001569241016637338, + "timestamp": 1.3663693108497914 + }, + { + "x": 5.763364679087044, + "y": 4.999997644896944, + "heading": 1.4752347555606973, + "angularVelocity": 0.6918840355173265, + "velocityX": 1.7319054844712716, + "velocityY": 9.289787179214142e-7, + "timestamp": 1.434687776392281 + }, + { + "x": 5.85802002824266, + "y": 4.999998289799587, + "heading": 1.51310089738515, + "angularVelocity": 0.554259255148266, + "velocityX": 1.385501685437416, + "velocityY": 0.000009439653500196435, + "timestamp": 1.5030062419347705 + }, + { + "x": 5.929010561462276, + "y": 4.999999073832564, + "heading": 1.5415326066657604, + "angularVelocity": 0.4161643423171998, + "velocityX": 1.039111939296464, + "velocityY": 0.000011476150277595035, + "timestamp": 1.57132470747726 + }, + { + "x": 5.976337019868017, + "y": 4.999999694057604, + "heading": 1.5605054658851873, + "angularVelocity": 0.2777120222003084, + "velocityX": 0.6927330412054942, + "velocityY": 0.000009078439254067869, + "timestamp": 1.6396431730197496 + }, + { + "x": 6, + "y": 5, + "heading": 1.57, + "angularVelocity": 0.1389746394246477, + "velocityX": 0.3463628748696997, + "velocityY": 0.000004478180145835614, + "timestamp": 1.7079616385622391 + }, + { + "x": 6, + "y": 5, + "heading": 1.57, + "angularVelocity": 0, + "velocityX": -2.0074835073437163e-36, + "velocityY": 0, + "timestamp": 1.7762801041047287 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "TestSlowOther": { + "waypoints": [ + { + "x": 2, + "y": 5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2, + "y": 2, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2, + "y": 5, + "heading": 0, + "angularVelocity": -1.0952306037480514e-39, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2, + "y": 4.953124995425155, + "heading": -3.164240520763351e-24, + "angularVelocity": -3.300992685598943e-23, + "velocityX": -6.527754769699201e-35, + "velocityY": -0.4890076415022641, + "timestamp": 0.09585740711707923 + }, + { + "x": 2, + "y": 4.859374987079833, + "heading": 2.5713915074059014e-24, + "angularVelocity": 5.983483597695269e-23, + "velocityX": -1.097538789626215e-34, + "velocityY": -0.9780152746132214, + "timestamp": 0.19171481423415845 + }, + { + "x": 2, + "y": 4.718749976060901, + "heading": 4.917772934580374e-23, + "angularVelocity": 4.862047876322287e-22, + "velocityX": -1.7992669720861215e-34, + "velocityY": -1.4670228962814877, + "timestamp": 0.28757222135123767 + }, + { + "x": 2, + "y": 4.531249963952723, + "heading": 1.1407687461769059e-22, + "angularVelocity": 6.770381391038879e-22, + "velocityX": -2.637563533921278e-34, + "velocityY": -1.9560305014214234, + "timestamp": 0.3834296284683169 + }, + { + "x": 2, + "y": 4.296874953245012, + "heading": 1.7398143338071676e-22, + "angularVelocity": 6.249336120704708e-22, + "velocityX": -1.4629288028921842e-34, + "velocityY": -2.4450380805882697, + "timestamp": 0.47928703558539615 + }, + { + "x": 2, + "y": 4.015624948419249, + "heading": -1.9887084571420204e-23, + "angularVelocity": -2.0224683409995954e-21, + "velocityX": -1.4112424782343753e-34, + "velocityY": -2.9340456130035593, + "timestamp": 0.5751444427024753 + }, + { + "x": 2, + "y": 3.6874999599322256, + "heading": -1.9829338642916739e-22, + "angularVelocity": -1.8611642258638035e-21, + "velocityX": -1.5151681626967904e-34, + "velocityY": -3.423053036331916, + "timestamp": 0.6710018498195546 + }, + { + "x": 2, + "y": 3.3125000400677727, + "heading": 5.600506325716781e-22, + "angularVelocity": 7.911168467222027e-21, + "velocityX": 1.4035532697888077e-34, + "velocityY": -3.912059914226877, + "timestamp": 0.7668592569366338 + }, + { + "x": 2, + "y": 2.98437505158075, + "heading": 5.045684206190348e-22, + "angularVelocity": -5.787987008981493e-22, + "velocityX": 2.835770758852267e-34, + "velocityY": -3.4230530363319156, + "timestamp": 0.8627166640537131 + }, + { + "x": 2, + "y": 2.7031250467549874, + "heading": 3.662231168806704e-22, + "angularVelocity": -1.4432401825905735e-21, + "velocityX": 2.8662185750679713e-34, + "velocityY": -2.9340456130035593, + "timestamp": 0.9585740711707923 + }, + { + "x": 2, + "y": 2.468750036047276, + "heading": 2.1828595317613215e-22, + "angularVelocity": -1.5433041868967679e-21, + "velocityX": 3.2780707212611828e-34, + "velocityY": -2.4450380805882697, + "timestamp": 1.0544314782878714 + }, + { + "x": 2, + "y": 2.281250023939098, + "heading": 1.5076855477401922e-22, + "angularVelocity": -7.043523061795029e-22, + "velocityX": 8.389359710572522e-35, + "velocityY": -1.9560305014214234, + "timestamp": 1.1502888854049507 + }, + { + "x": 2, + "y": 2.1406250129201667, + "heading": 8.548130994950006e-23, + "angularVelocity": -6.8108706582588185e-22, + "velocityX": -5.159202013607646e-35, + "velocityY": -1.4670228962814877, + "timestamp": 1.24614629252203 + }, + { + "x": 2, + "y": 2.0468750045748454, + "heading": 3.182367857810666e-23, + "angularVelocity": -5.597651120163003e-22, + "velocityX": 7.019719734335023e-37, + "velocityY": -0.9780152746132214, + "timestamp": 1.3420036996391091 + }, + { + "x": 2, + "y": 2, + "heading": 0, + "angularVelocity": -3.3198979387898458e-22, + "velocityX": -1.3716459797394807e-35, + "velocityY": -0.4890076415022641, + "timestamp": 1.4378611067561884 + }, + { + "x": 2, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 1.0385403263386622e-38, + "timestamp": 1.5337185138732676 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + }, + "splitTrajectoriesAtStopPoints": false, + "usesObstacles": false +} \ No newline at end of file diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json index ff7359e1..88a68dd0 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix5.json", "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", + "version": "5.33.0", "frcYear": 2024, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -20,19 +20,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.33.1" + "version": "5.33.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.33.1" + "version": "5.33.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.33.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +45,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.33.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -60,7 +60,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.33.1", + "version": "5.33.0", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -75,7 +75,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.33.1", + "version": "5.33.0", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.33.0", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -105,7 +105,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", + "version": "5.33.0", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -120,7 +120,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.33.1", + "version": "5.33.0", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -135,7 +135,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.33.0", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d1720..69a40798 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.1.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf84..0f3520e7 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.0", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..4143e090 --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/libgrapplefrc2024.json b/vendordeps/libgrapplefrc2024.json new file mode 100644 index 00000000..1cd5b6e0 --- /dev/null +++ b/vendordeps/libgrapplefrc2024.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2024.json", + "name": "libgrapplefrc", + "version": "2024.2.1", + "frcYear": "2024", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2024.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2024.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.2.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2024.2.1", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.2.1", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file