From 0593a8595c3df61870815e4d6d3a45264c90f9c9 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 5 Feb 2021 12:29:07 -0700 Subject: [PATCH] FIXED AUTO and anded figureEightpath --- PathWeaver/Groups/FigureEight | 1 + PathWeaver/Paths/FigureEight | 6 ++++ src/main/deploy/paths/FigureEight.wpilib.json | 1 + src/main/java/frc4388/robot/Robot.java | 5 ++-- .../java/frc4388/robot/RobotContainer.java | 13 ++++++++- .../robot/commands/auto/FigureEight.java | 28 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 3 +- 7 files changed, 52 insertions(+), 5 deletions(-) create mode 100644 PathWeaver/Groups/FigureEight create mode 100644 PathWeaver/Paths/FigureEight create mode 100644 src/main/deploy/paths/FigureEight.wpilib.json create mode 100644 src/main/java/frc4388/robot/commands/auto/FigureEight.java diff --git a/PathWeaver/Groups/FigureEight b/PathWeaver/Groups/FigureEight new file mode 100644 index 0000000..109dc07 --- /dev/null +++ b/PathWeaver/Groups/FigureEight @@ -0,0 +1 @@ +FigureEight3 diff --git a/PathWeaver/Paths/FigureEight b/PathWeaver/Paths/FigureEight new file mode 100644 index 0000000..3516cba --- /dev/null +++ b/PathWeaver/Paths/FigureEight @@ -0,0 +1,6 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +8.144025699763882,-4.085312310773085,0.0567858762907818,3.1232231959930674,true, +10.661532881988599,-4.142098187063867,-0.0567858762907818,-2.744650687387848,true, +8.162954325194141,-4.085312310773085,0.13250037801182657,2.915008316260198,true, +5.853662022702296,-4.142098187063867,0.0,-2.9717941925509805,true, +8.162954325194141,-4.142098187063867,0.0,2.650007560236543,true, diff --git a/src/main/deploy/paths/FigureEight.wpilib.json b/src/main/deploy/paths/FigureEight.wpilib.json new file mode 100644 index 0000000..018b231 --- /dev/null +++ b/src/main/deploy/paths/FigureEight.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":8.144025699763882,"y":-4.085312310773085},"rotation":{"radians":1.5526165117219188}},"curvature":0.0},{"time":0.26858342272014163,"velocity":0.7251752413443824,"acceleration":2.7,"pose":{"translation":{"x":8.146529565775754,"y":-3.9879594803434175},"rotation":{"radians":1.5302724845258593}},"curvature":-0.4492184430763076},{"time":0.3786202826968416,"velocity":1.0222747632814724,"acceleration":2.699999999999998,"pose":{"translation":{"x":8.153132407432002,"y":-3.892044527728496},"rotation":{"radians":1.4673386094875254}},"curvature":-0.8560459009725254},{"time":0.4224297994342931,"velocity":1.1405604584725912,"acceleration":1.885794890972377,"pose":{"translation":{"x":8.159051660960351,"y":-3.84503937804659},"rotation":{"radians":1.4221986537949538}},"curvature":-1.0482940848705655},{"time":0.46222383593023403,"velocity":1.215603849187805,"acceleration":-1.1505152855002447,"pose":{"translation":{"x":8.167196120595225,"y":-3.798871612449995},"rotation":{"radians":1.3687470360380933}},"curvature":-1.2298824565130255},{"time":0.5011375046601508,"velocity":1.1708330784991428,"acceleration":-0.8911200732641882,"pose":{"translation":{"x":8.177868480023559,"y":-3.7536822492583015},"rotation":{"radians":1.307726566264961}},"curvature":-1.394930899367627},{"time":0.5411084879876529,"velocity":1.1352141329078975,"acceleration":-0.6316400205622513,"pose":{"translation":{"x":8.191327623042152,"y":-3.7096038196508734},"rotation":{"radians":1.2400774386228697}},"curvature":-1.5355396007904818},{"time":0.5820043465644814,"velocity":1.1093826719555187,"acceleration":-0.3736739666099782,"pose":{"translation":{"x":8.20779031167778,"y":-3.66676053267107},"rotation":{"radians":1.1669733412403345}},"curvature":-1.6431597805456788},{"time":0.6236772115092954,"velocity":1.093810607211588,"acceleration":-0.12310675377583101,"pose":{"translation":{"x":8.227432874307313,"y":-3.6252684402304682},"rotation":{"radians":1.0898111642431112}},"curvature":-1.7104925694432371},{"time":0.6659694977557876,"velocity":1.0886041411420242,"acceleration":0.11171386676449169,"pose":{"translation":{"x":8.250392893777828,"y":-3.585235602113083},"rotation":{"radians":1.0101479978291668}},"curvature":-1.7334347577943885},{"time":0.7087265233893024,"velocity":1.0933806938068926,"acceleration":0.32158438852791293,"pose":{"translation":{"x":8.27677089552672,"y":-3.546762250979591},"rotation":{"radians":0.9295935188326374}},"curvature":-1.7123786983279752},{"time":0.7518123974283091,"velocity":1.1072364382639173,"acceleration":0.4979654592766971,"pose":{"translation":{"x":8.306632035701826,"y":-3.5099409573715517},"rotation":{"radians":0.8496809518685422}},"curvature":-1.652327454758158},{"time":0.7951242584618977,"velocity":1.1288042490356367,"acceleration":0.6344197090420162,"pose":{"translation":{"x":8.340007789281533,"y":-3.4748567947156266},"rotation":{"radians":0.7717474460023444}},"curvature":-1.5617852698212031},{"time":0.8386007297394951,"velocity":1.1563865792937436,"acceleration":0.7277646697196621,"pose":{"translation":{"x":8.376897638194889,"y":-3.4415875043278046},"rotation":{"radians":0.6968502884164062}},"curvature":-1.450915489468398},{"time":0.8822228237104323,"velocity":1.1881331981049827,"acceleration":0.7786220728538547,"pose":{"translation":{"x":8.417270759441731,"y":-3.410203660417623},"rotation":{"radians":0.6257314902519716}},"curvature":-1.329679038375259},{"time":0.9260081257772728,"velocity":1.2222254007607982,"acceleration":0.7911613211924509,"pose":{"translation":{"x":8.461067713212788,"y":-3.380768835092387},"rotation":{"radians":0.5588276310075535}},"curvature":-1.2064983755835206},{"time":0.9700007259050741,"velocity":1.2570306444006007,"acceleration":0.7721260701266061,"pose":{"translation":{"x":8.508202131009803,"y":-3.3533397633613937},"rotation":{"radians":0.4963114230895469}},"curvature":-1.087634080540555},{"time":1.0142596981545526,"velocity":1.2912041507114331,"acceleration":0.7294938841400632,"pose":{"translation":{"x":8.558562403765636,"y":-3.327966508140153},"rotation":{"radians":0.4381485950172553}},"curvature":-0.9771619643019163},{"time":1.0588483278705898,"velocity":1.323731283391468,"acceleration":0.6711886821682149,"pose":{"translation":{"x":8.612013369964398,"y":-3.304692625254609},"rotation":{"radians":0.38415648858283175}},"curvature":-0.8773103831418712},{"time":1.1038253519281693,"velocity":1.353919352896523,"acceleration":0.6041438056934345,"pose":{"translation":{"x":8.66839800376155,"y":-3.283555328445364},"rotation":{"radians":0.33405584818257916}},"curvature":-0.788931883642805},{"time":1.1492386466218387,"velocity":1.3813555135818338,"acceleration":0.5338150791461915,"pose":{"translation":{"x":8.72753910310402,"y":-3.2645856543718965},"rotation":{"radians":0.2875120513742578}},"curvature":-0.7119605542696107},{"time":1.195121248941006,"velocity":1.4058483385702734,"acceleration":0.4640832248687823,"pose":{"translation":{"x":8.789240977850326,"y":-3.247808627616785},"rotation":{"radians":0.24416533945207405}},"curvature":-0.6457848071742879},{"time":1.2414893252100092,"velocity":1.427366984936154,"acceleration":0.3974112246089385,"pose":{"translation":{"x":8.853291137890686,"y":-3.2332434256899303},"rotation":{"radians":0.20365138546211842}},"curvature":-0.589518975790378},{"time":1.2883416253348423,"velocity":1.4459866149045095,"acceleration":0.3351216144989501,"pose":{"translation":{"x":8.919461981267126,"y":-3.2209035440327773},"rotation":{"radians":0.16561419173476635}},"curvature":-0.5421847762808252},{"time":1.3356599945629195,"velocity":1.4618440231956802,"acceleration":0.27769505585874443,"pose":{"translation":{"x":8.98751248229361,"y":-3.210796961022535},"rotation":{"radians":0.12971330903276887}},"curvature":-0.502823351507489},{"time":1.3834105940302592,"velocity":1.4751041285820516,"acceleration":0.22503430625890894,"pose":{"translation":{"x":9.057189879676137,"y":-3.2029263029763992},"rotation":{"radians":0.09562707053970072}},"curvature":-0.47055868564286807},{"time":1.431545568866449,"velocity":1.4859361492511036,"acceleration":0.1766712840574132,"pose":{"translation":{"x":9.128231364632873,"y":-3.1972890091557744},"rotation":{"radians":0.06305315149950098}},"curvature":-0.4446293992552219},{"time":1.4800049802824873,"velocity":1.4944975356906416,"acceleration":0.13191557653190647,"pose":{"translation":{"x":9.20036576901425,"y":-3.1938774967704973},"rotation":{"radians":0.031707406812833294}},"curvature":-0.4244014125183763},{"time":1.5287188798032278,"velocity":1.5009236578310374,"acceleration":0.0899527650674221,"pose":{"translation":{"x":9.273315253423094,"y":-3.192679325983053},"rotation":{"radians":0.0013216452842124452}},"curvature":-0.4093700246755956},{"time":1.5776094495978845,"velocity":1.5053214997697886,"acceleration":0.0499041742866857,"pose":{"translation":{"x":9.346796995334731,"y":-3.1936773649128045},"rotation":{"radians":-0.02835922309542794}},"curvature":-0.399156969925464},{"time":1.626593165132263,"velocity":1.5077659916470256,"acceleration":0.010859398003298921,"pose":{"translation":{"x":9.420524877217108,"y":-3.196849954640208},"rotation":{"radians":-0.05758038981598413}},"curvature":-0.39350591794916745},{"time":1.6755829580735122,"velocity":1.5082979913066739,"acceleration":-0.028108580267543207,"pose":{"translation":{"x":9.494211174650903,"y":-3.202171074211038},"rotation":{"radians":-0.08658053239961737}},"curvature":-0.3922784951963284},{"time":1.7244903711434338,"velocity":1.50692327336072,"acceleration":-0.06793741553955096,"pose":{"translation":{"x":9.567568244449642,"y":-3.2096105056406072},"rotation":{"radians":-0.11559528301875487}},"curvature":-0.3954520004404316},{"time":1.7732277044030849,"velocity":1.5036121848987696,"acceleration":-0.1095712817452024,"pose":{"translation":{"x":9.640310212779813,"y":-3.2191339989179903},"rotation":{"radians":-0.14486072390491975}},"curvature":-0.4031193933392665},{"time":1.8217101554956687,"velocity":1.498299900590406,"acceleration":-0.15395139611813355,"pose":{"translation":{"x":9.71215466328098,"y":-3.2307034370102445},"rotation":{"radians":-0.1746168688510383}},"curvature":-0.4154916988060637},{"time":1.869857955202921,"velocity":1.4908874796054583,"acceleration":-0.20199312669480646,"pose":{"translation":{"x":9.782824325185901,"y":-3.2442770008666306},"rotation":{"radians":-0.20511108519447085}},"curvature":-0.4329025646752345},{"time":1.9175984941159532,"velocity":1.4812442188803199,"acceleration":-0.2545399336395223,"pose":{"translation":{"x":9.85204876144064,"y":-3.259809334422836},"rotation":{"radians":-0.23660138350856535}},"curvature":-0.45581421034000424},{"time":1.9648684254200068,"velocity":1.4692121337030413,"acceleration":-0.3122822570242224,"pose":{"translation":{"x":9.919566056824687,"y":-3.277251709605196},"rotation":{"radians":-0.26935944882543106}},"curvature":-0.48482326886062455},{"time":2.0116157111556734,"velocity":1.4546137858037511,"acceleration":-0.37562778867237384,"pose":{"translation":{"x":9.98512450607106,"y":-3.296552191334916},"rotation":{"radians":-0.3036732002349346}},"curvature":-0.5206638806959428},{"time":2.0578015525417652,"velocity":1.4372651003359205,"acceleration":-0.4445101596899729,"pose":{"translation":{"x":10.048484301986434,"y":-3.3176558025322915},"rotation":{"radians":-0.3398485346469549}},"curvature":-0.5642036280981605},{"time":2.103402106160669,"velocity":1.4169951909648304,"acceleration":-0.5559126882526984,"pose":{"translation":{"x":10.10941922357125,"y":-3.3405046891209316},"rotation":{"radians":-0.37820972265054004}},"curvature":-0.6164252626925653},{"time":2.1928692104204783,"velocity":1.3672592925255755,"acceleration":-0.6707612394302929,"pose":{"translation":{"x":10.223187619440479,"y":-3.3911934772083416},"rotation":{"radians":-0.4628649114294588}},"curvature":-0.7511212378070588},{"time":2.23673681010184,"velocity":1.3378346069924736,"acceleration":-0.7415918018956962,"pose":{"translation":{"x":10.27565177577564,"y":-3.41890477060889},"rotation":{"radians":-0.5098658804426618}},"curvature":-0.8355251217305552},{"time":2.28008955253411,"velocity":1.3056845686150065,"acceleration":-0.8004150692263593,"pose":{"translation":{"x":10.324955798121955,"y":-3.448104453212708},"rotation":{"radians":-0.5604405033440728}},"curvature":-0.93209567274306},{"time":2.322981748490907,"velocity":1.271353008618976,"acceleration":-0.8390030097356673,"pose":{"translation":{"x":10.370966718250422,"y":-3.478722761023296},"rotation":{"radians":-0.6148891064034518}},"curvature":-1.0406114842458793},{"time":2.3654746618947975,"velocity":1.2357013263806746,"acceleration":-0.848331626965323,"pose":{"translation":{"x":10.413575282846484,"y":-3.510688043072797},"rotation":{"radians":-0.6734367301486227}},"curvature":-1.1596820097197569},{"time":2.40762861755472,"velocity":1.1999407925926684,"acceleration":-0.8197735400765805,"pose":{"translation":{"x":10.45269764163016,"y":-3.543926926426223},"rotation":{"radians":-0.7361860933121319}},"curvature":-1.2862239745345891},{"time":2.449492691547849,"velocity":1.1656217324532934,"acceleration":-0.7465361503385792,"pose":{"translation":{"x":10.488277035476148,"y":-3.5783644811856714},"rotation":{"radians":-0.8030608642665602}},"curvature":-1.4149664463658604},{"time":2.491092771839681,"velocity":1.1345657686584532,"acceleration":-0.6249180018486429,"pose":{"translation":{"x":10.520285484533956,"y":-3.6139243854945473},"rotation":{"radians":-0.873744949513414}},"curvature":-1.5381808872546259},{"time":2.532419711454165,"velocity":1.1087398201320504,"acceleration":-0.45490783555761566,"pose":{"translation":{"x":10.54872547634799,"y":-3.650529090541787},"rotation":{"radians":-0.9476287036826585}},"curvature":-1.6459020098516235},{"time":2.5734201757738124,"velocity":1.0900883876515424,"acceleration":-0.23984627286440718,"pose":{"translation":{"x":10.5736316539777,"y":-3.688099985566081},"rotation":{"radians":-1.0237774982520436}},"curvature":-1.7268721227684594},{"time":2.6139930603878025,"velocity":1.0803571324975192,"acceleration":0.014786020553848042,"pose":{"translation":{"x":10.595072504117672,"y":-3.7265575628600898},"rotation":{"radians":-1.1009387882404207}},"curvature":-1.7702275743740314},{"time":2.6539934469507624,"velocity":1.080948579035401,"acceleration":0.30314254140685704,"pose":{"translation":{"x":10.613152045217749,"y":-3.7658215827746746},"rotation":{"radians":-1.177597546787089}},"curvature":-1.7675702349748694},{"time":2.6932438543082973,"velocity":1.0928470472730185,"acceleration":0.6210814265691578,"pose":{"translation":{"x":10.628011515603152,"y":-3.805811238723111},"rotation":{"radians":-1.2520763790082095}},"curvature":-1.714721992539867},{"time":2.731549794664244,"velocity":1.116638155355363,"acceleration":0.9685816109671112,"pose":{"translation":{"x":10.639831061594592,"y":-3.846445322185315},"rotation":{"radians":-1.3226604560805812}},"curvature":-1.6124287996724787},{"time":2.7687147839885298,"velocity":1.1526354805866552,"acceleration":1.3529700442030284,"pose":{"translation":{"x":10.648831425628375,"y":-3.887642387712064},"rotation":{"radians":-1.3877167952849379}},"curvature":-1.4656816429042703},{"time":2.8045501012314262,"velocity":1.2011195913408066,"acceleration":1.793886298206407,"pose":{"translation":{"x":10.655275634376533,"y":-3.929320917929216},"rotation":{"radians":-1.4457785338445088}},"curvature":-1.281932589239291},{"time":2.8388766490922634,"velocity":1.2626975152130888,"acceleration":2.688680763473646,"pose":{"translation":{"x":10.659470686866918,"y":-3.971399488541936},"rotation":{"radians":-1.4955769816542777}},"curvature":-1.0689013091883093},{"time":2.9020190110680275,"velocity":1.4324671692176159,"acceleration":2.644896001582353,"pose":{"translation":{"x":10.662571309685688,"y":-4.056432509196593},"rotation":{"radians":-1.5661311718221727}},"curvature":-0.5764310912457891},{"time":2.958845106220003,"velocity":1.582766281070615,"acceleration":-2.7000000000000033,"pose":{"translation":{"x":10.661532881988586,"y":-4.142098187063867},"rotation":{"radians":-1.5914830305747611}},"curvature":-2.2642413606889E-14},{"time":3.0157147759187786,"velocity":1.4292181728839217,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":10.659025781329863,"y":-4.227706764444544},"rotation":{"radians":-1.616923972078659}},"curvature":-0.5794830740542293},{"time":3.0788995728014443,"velocity":1.2586192213007241,"acceleration":-1.8548021073885574,"pose":{"translation":{"x":10.652402093294093,"y":-4.312363264645151},"rotation":{"radians":-1.6879559814329343}},"curvature":-1.0823657631751815},{"time":3.1132238684432814,"velocity":1.194954445409617,"acceleration":-1.4183306392913186,"pose":{"translation":{"x":10.646461601924281,"y":-4.354050723759024},"rotation":{"radians":-1.7382919033002837}},"curvature":-1.304470293310562},{"time":3.149067743718721,"velocity":1.1441159788755246,"acceleration":-1.0336991671477538,"pose":{"translation":{"x":10.63828706692055,"y":-4.39516665630448},"rotation":{"radians":-1.7971803998456843}},"curvature":-1.4995782376100788},{"time":3.1862581452638237,"velocity":1.1056722917724613,"acceleration":-0.6814327896018239,"pose":{"translation":{"x":10.627574937631728,"y":-4.435607225653632},"rotation":{"radians":-1.8634033636797305}},"curvature":-1.6590311542848646},{"time":3.2246089650095238,"velocity":1.0795387856896321,"acceleration":-0.35465764723964244,"pose":{"translation":{"x":10.614065749149994,"y":-4.475272148269502},"rotation":{"radians":-1.9355141776661098}},"curvature":-1.7739091668190143},{"time":3.2639211301352775,"velocity":1.065596425698236,"acceleration":-0.054079758213387966,"pose":{"translation":{"x":10.59754242149813,"y":-4.514064712744968},"rotation":{"radians":-2.01185980641995}},"curvature":-1.8375021499068773},{"time":3.3039919342248054,"velocity":1.0634294063016583,"acceleration":0.21524900491993268,"pose":{"translation":{"x":10.57782855881677,"y":-4.551891798841717},"rotation":{"radians":-2.0906632400310574}},"curvature":-1.847535946421992},{"time":3.3446312961236675,"velocity":1.0721769885109693,"acceleration":0.4463283632103242,"pose":{"translation":{"x":10.55478674855166,"y":-4.5886638965291855},"rotation":{"radians":-2.1701510366295764}},"curvature":-1.807281227694547},{"time":3.385679883055164,"velocity":1.090498137128201,"acceleration":0.6323129159963308,"pose":{"translation":{"x":10.528316860640897,"y":-4.624295125023517},"rotation":{"radians":-2.2486945731280255}},"curvature":-1.7250635512909043},{"time":3.4270233171290854,"velocity":1.1166401244847841,"acceleration":0.7684392300348157,"pose":{"translation":{"x":10.498354346702202,"y":-4.658703251826505},"rotation":{"radians":-2.3249291475283167}},"curvature":-1.6124205135284229},{"time":3.468598472578368,"velocity":1.1485881049268087,"acceleration":0.8534948715289253,"pose":{"translation":{"x":10.464868539220152,"y":-4.6918097117645425},"rotation":{"radians":-2.397825149811876}},"curvature":-1.481722262089039},{"time":3.5103910400071667,"velocity":1.1842578468953153,"acceleration":0.8904109923073181,"pose":{"translation":{"x":10.427860950733448,"y":-4.723539626027573},"rotation":{"radians":-2.4667039905771437}},"curvature":-1.3441302272260385},{"time":3.5524263190934353,"velocity":1.221686521458435,"acceleration":0.8857916888142122,"pose":{"translation":{"x":10.387363573022162,"y":-4.753821821208035},"rotation":{"radians":-2.531208327766738}},"curvature":-1.2083919584999614},{"time":3.594756499071095,"velocity":1.2591822430686554,"acceleration":0.8486092516329284,"pose":{"translation":{"x":10.343437176294985,"y":-4.7825888483398185},"rotation":{"radians":-2.5912447431944896}},"curvature":-1.0805017621503796},{"time":3.637447506181832,"velocity":1.295410226664354,"acceleration":0.7885850191383549,"pose":{"translation":{"x":10.29616960837649,"y":-4.809777001937202},"rotation":{"radians":-2.6469170088069935}},"curvature":-0.9639678941977714},{"time":3.6805675094494257,"velocity":1.3294140152663751,"acceleration":0.7147764033317778,"pose":{"translation":{"x":10.245674093894376,"y":-4.835326339033813},"rotation":{"radians":-2.6984630342937583}},"curvature":-0.8603669664710593},{"time":3.7241780705293572,"velocity":1.3605858152623695,"acceleration":0.5937425357936439,"pose":{"translation":{"x":10.192087533466728,"y":-4.859180698221566},"rotation":{"radians":-2.746202458240151}},"curvature":-0.769943823095007},{"time":3.813087466358601,"velocity":1.4133751053979056,"acceleration":0.4759554448872352,"pose":{"translation":{"x":10.076297052322575,"y":-4.90159885926333},"rotation":{"radians":-2.831722650737655}},"curvature":-0.6259093803531824},{"time":3.858396418119853,"velocity":1.4349401476908066,"acceleration":0.4031735134591288,"pose":{"translation":{"x":10.01447000547942,"y":-4.920069417443172},"rotation":{"radians":-2.8702518630879292}},"curvature":-0.5701185274060226},{"time":3.9042888372814035,"velocity":1.453442755565308,"acceleration":0.3364309742769644,"pose":{"translation":{"x":9.950302258811936,"y":-4.936658548443725},"rotation":{"radians":-2.906442861019111}},"curvature":-0.523570086662142},{"time":3.9507439988549558,"velocity":1.469071710833692,"acceleration":0.27584627387272914,"pose":{"translation":{"x":9.884023580698905,"y":-4.951329284232595},"rotation":{"radians":-2.940634407809564}},"curvature":-0.48516463013563027},{"time":3.9977252193046904,"velocity":1.4820313054367444,"acceleration":0.22100602778282308,"pose":{"translation":{"x":9.815877210633007,"y":-4.964048552569377},"rotation":{"radians":-2.973144040627538}},"curvature":-0.4539329783295228},{"time":4.045181636510048,"velocity":1.4925194596961049,"acceleration":0.17118196871039504,"pose":{"translation":{"x":9.746118158408077,"y":-4.974787196044596},"rotation":{"radians":-3.0042686129008187}},"curvature":-0.42905439779349464},{"time":4.093050207504643,"velocity":1.500713695918313,"acceleration":0.1254775815972625,"pose":{"translation":{"x":9.675011503306354,"y":-4.9835199911186585},"rotation":{"radians":-3.0342862554389693}},"curvature":-0.40985911377381434},{"time":4.141257817023227,"velocity":1.50676267017529,"acceleration":0.0829189844148324,"pose":{"translation":{"x":9.602830693285734,"y":-4.990225667160804},"rotation":{"radians":-3.0634591737212666}},"curvature":-0.39582312703252504},{"time":4.189723433259794,"velocity":1.5107813898526652,"acceleration":0.0425053874218333,"pose":{"translation":{"x":9.529855844167018,"y":-4.994886925488047},"rotation":{"radians":-3.092036911109639}},"curvature":-0.38656026605361216},{"time":4.238360276974222,"velocity":1.5128487177377221,"acceleration":0.003232434336177768,"pose":{"translation":{"x":9.456372038821176,"y":-4.997490458404134},"rotation":{"radians":-3.120259855026401}},"curvature":-0.38181439268661055},{"time":4.287077989659751,"velocity":1.5130061945449869,"acceleration":-0.0359009200660863,"pose":{"translation":{"x":9.382667626356588,"y":-4.9980269682384835},"rotation":{"radians":3.1348224460979797}},"curvature":-0.38145341171899005},{"time":4.335784798687746,"velocity":1.5112575752873987,"acceleration":-0.07589351668537668,"pose":{"translation":{"x":9.309032521306303,"y":-4.996491186385141},"rotation":{"radians":3.1066063755265203}},"curvature":-0.3854659572294401},{"time":4.384389684141806,"velocity":1.5075687796022001,"acceleration":-0.11774384070158458,"pose":{"translation":{"x":9.235756502815294,"y":-4.992881892341725},"rotation":{"radians":3.0780423874866325}},"curvature":-0.3939611431214523},{"time":4.432804554606986,"velocity":1.5018682268065602,"acceleration":-0.16244551074150326,"pose":{"translation":{"x":9.163127513827702,"y":-4.987201932748377},"rotation":{"radians":3.0488900661557334}},"curvature":-0.40717141995776074},{"time":4.480946437912586,"velocity":1.4940477939849242,"acceleration":-0.2109683427027063,"pose":{"translation":{"x":9.091429960274098,"y":-4.97945824042671},"rotation":{"radians":3.0188997361155576}},"curvature":-0.42545824798632054},{"time":4.52873968744721,"velocity":1.4839649313382275,"acceleration":-0.26421497288470236,"pose":{"translation":{"x":9.020943010258732,"y":-4.969661853418753},"rotation":{"radians":2.9878088338200004}},"curvature":-0.449319851969671},{"time":4.576118194131091,"velocity":1.4714468204794282,"acceleration":-0.32294046938203386,"pose":{"translation":{"x":8.95193889324678,"y":-4.957827934025907},"rotation":{"radians":2.9553384546289747}},"curvature":-0.47939962089364646},{"time":4.623027576588021,"velocity":1.4562978824903658,"acceleration":-0.38762011870073887,"pose":{"translation":{"x":8.884681199251611,"y":-4.943975787847888},"rotation":{"radians":2.9211902724844974}},"curvature":-0.5164925654251911},{"time":4.669427294771506,"velocity":1.4383124182204028,"acceleration":-0.45825054460406,"pose":{"translation":{"x":8.819423178022028,"y":-4.928128882821678},"rotation":{"radians":2.8850441667430697}},"curvature":-0.5615453995323849},{"time":4.715292591709941,"velocity":1.4172946209199375,"acceleration":-0.5340748478034238,"pose":{"translation":{"x":8.756406038229517,"y":-4.910314868260474},"rotation":{"radians":2.846557092143872}},"curvature":-0.6156429675363732},{"time":4.760616110184929,"velocity":1.3930884696884924,"acceleration":-0.6132384481763581,"pose":{"translation":{"x":8.695857246655518,"y":-4.890565593892633},"rotation":{"radians":2.8053640106936144}},"curvature":-0.6799696000751724},{"time":4.805408952905852,"velocity":1.3656197763289062,"acceleration":-0.6924140277262646,"pose":{"translation":{"x":8.637988827378656,"y":-4.868917128900628},"rotation":{"radians":2.761082079831413}},"curvature":-0.7557284577075098},{"time":4.849700856514036,"velocity":1.3349514409559,"acceleration":-0.7664831791944419,"pose":{"translation":{"x":8.582995660962013,"y":-4.845409780959987},"rotation":{"radians":2.713319749982488}},"curvature":-0.8439955558703085},{"time":4.893539040744565,"velocity":1.3013502101367729,"acceleration":-0.8284199398425929,"pose":{"translation":{"x":8.531053783640369,"y":-4.820088115278253},"rotation":{"radians":2.6616929070272644}},"curvature":-0.9454799917912167},{"time":4.936985200749209,"velocity":1.2653585448793339,"acceleration":-0.8695587839650004,"pose":{"translation":{"x":8.482318686507448,"y":-4.793000973633919},"rotation":{"radians":2.6058505454045235}},"curvature":-1.060162732549907},{"time":4.980110087590327,"velocity":1.2278589207191437,"acceleration":-0.8803985258822774,"pose":{"translation":{"x":8.4369236147032,"y":-4.764201493415391},"rotation":{"radians":2.5455123641215733}},"curvature":-1.1868021062896175},{"time":5.022985260209067,"velocity":1.190111681948657,"acceleration":-0.8519487630239296,"pose":{"translation":{"x":8.394977866601014,"y":-4.733747126659927},"rotation":{"radians":2.4805196342898674}},"curvature":-1.3223375592153612},{"time":5.065672010148422,"velocity":1.1537447581403115,"acceleration":-0.7773763494395487,"pose":{"translation":{"x":8.356565092995005,"y":-4.701699659092588},"rotation":{"radians":2.410898054771754}},"curvature":-1.4613049869195633},{"time":5.108208253352713,"velocity":1.1206780886792864,"acceleration":-0.653466769172971,"pose":{"translation":{"x":8.321741596287247,"y":-4.668125229165187},"rotation":{"radians":2.336926630114393}},"curvature":-1.595489916826352},{"time":5.150595309734591,"velocity":1.0929795558906683,"acceleration":-0.4813608267749898,"pose":{"translation":{"x":8.290534629675028,"y":-4.6330943470952395},"rotation":{"radians":2.2592002506013067}},"curvature":-1.7141399206681425},{"time":5.192787618594875,"velocity":1.0726698312141363,"acceleration":-0.266268847625778,"pose":{"translation":{"x":8.262940696338115,"y":-4.596681913904908},"rotation":{"radians":2.178667690150449}},"curvature":-1.8050327960246364},{"time":5.234688883615397,"velocity":1.0615128296630596,"acceleration":-0.016285750583754106,"pose":{"translation":{"x":8.23892384862599,"y":-4.558967240459958},"rotation":{"radians":2.0966251340150075}},"curvature":-1.8564442742022231},{"time":5.276157130029359,"velocity":1.0608374881448162,"acceleration":0.25927046065235587,"pose":{"translation":{"x":8.218413987245118,"y":-4.520034066508694},"rotation":{"radians":2.0146523546388706}},"curvature":-1.859590959094079},{"time":5.317018471473968,"velocity":1.0714316269640332,"acceleration":0.5516203673466193,"pose":{"translation":{"x":8.201305160446186,"y":-4.4799705797209235},"rotation":{"radians":1.9344950887502468}},"curvature":-1.8106856227501722},{"time":5.357085968942132,"velocity":1.0935336746360818,"acceleration":0.8543260456307139,"pose":{"translation":{"x":8.18745386321136,"y":-4.438869434726895},"rotation":{"radians":1.8579172526497472}},"curvature":-1.7117073663121114},{"time":5.396177667975818,"velocity":1.126930731288517,"acceleration":1.1653871400160365,"pose":{"translation":{"x":8.176677336441552,"y":-4.396827772156248},"rotation":{"radians":1.7865597648566511}},"curvature":-1.5695128917245569},{"time":5.434128167969031,"velocity":1.1711577559377855,"acceleration":1.4900227679995155,"pose":{"translation":{"x":8.168751866143648,"y":-4.353947237676968},"rotation":{"radians":1.721840907045295}},"curvature":-1.3936885445745835},{"time":5.470790717108042,"velocity":1.2257857888878136,"acceleration":1.844663886303964,"pose":{"translation":{"x":8.163411082617777,"y":-4.310334001034326},"rotation":{"radians":1.6649176084574433}},"curvature":-1.1940292681145606},{"time":5.506030185344113,"velocity":1.2907907633154503,"acceleration":2.5373687891473744,"pose":{"translation":{"x":8.16034425964457,"y":-4.266098775089834},"rotation":{"radians":1.6167072273804692}},"curvature":-0.9784633630353562},{"time":5.571450847131285,"velocity":1.4567871086995856,"acceleration":2.6999999999999966,"pose":{"translation":{"x":8.159557603004586,"y":-4.176228036556234},"rotation":{"radians":1.5493297512083974}},"curvature":-0.5152826157179136},{"time":5.630654617722215,"velocity":1.6166372892950975,"acceleration":-2.476914536380259,"pose":{"translation":{"x":8.162954325194143,"y":-4.085312310773087},"rotation":{"radians":1.525373047373322}},"curvature":6.327903279913163E-15},{"time":5.689589087447211,"velocity":1.470661644539394,"acceleration":-2.475079244516706,"pose":{"translation":{"x":8.166399581189568,"y":-3.9944034085491387},"rotation":{"radians":1.547742682470547}},"curvature":0.4813033868546016},{"time":5.754177454413548,"velocity":1.310800318023785,"acceleration":-1.8479780860986792,"pose":{"translation":{"x":8.165934722270187,"y":-3.9045795684462368},"rotation":{"radians":1.6107126060224006}},"curvature":0.9164123397203398},{"time":5.788794827141088,"velocity":1.2468281718249798,"acceleration":-1.5290272430493592,"pose":{"translation":{"x":8.163203690204126,"y":-3.860394700097871},"rotation":{"radians":1.6558573322652212}},"curvature":1.1217892839030288},{"time":5.824722578283459,"velocity":1.1918936615467979,"acceleration":-1.2371482722621954,"pose":{"translation":{"x":8.158347662867941,"y":-3.816855771202598},"rotation":{"radians":1.7093004429284517}},"curvature":1.3157461024125117},{"time":5.861843249191268,"velocity":1.1459698876679887,"acceleration":-0.9556639062502005,"pose":{"translation":{"x":8.151076473039424,"y":-3.7740777999628534},"rotation":{"radians":1.770301323403295}},"curvature":1.492159175304283},{"time":5.9000337788742305,"velocity":1.1094725768894047,"acceleration":-0.678177410971513,"pose":{"translation":{"x":8.141141464327683,"y":-3.7321710989991397},"rotation":{"radians":1.8379278746375904}},"curvature":1.6427765240829797},{"time":5.9391567561608065,"velocity":1.0829402574436975,"acceleration":-0.4051950916429352,"pose":{"translation":{"x":8.128333898247767,"y":-3.6912412943889734},"rotation":{"radians":1.911020790455274}},"curvature":1.758643069337746},{"time":5.9790583037254095,"velocity":1.0667723462215633,"acceleration":-0.14169603594921562,"pose":{"translation":{"x":8.112483361295299,"y":-3.651389344705835},"rotation":{"radians":1.988202960063321}},"curvature":1.832074430636221},{"time":6.01957386409901,"velocity":1.061031451922363,"acceleration":0.10473427929157636,"pose":{"translation":{"x":8.093456172021098,"y":-3.6127115600581154},"rotation":{"radians":2.067940151152443}},"curvature":1.858686794641853},{"time":6.06054083477254,"velocity":1.0653220980706142,"acceleration":0.3254487277089871,"pose":{"translation":{"x":8.071153788105814,"y":-3.5752996211280683},"rotation":{"radians":2.1486453015698412}},"curvature":1.8387700930169368},{"time":6.101814521869443,"velocity":1.0787545670241603,"acceleration":0.5121706515576572,"pose":{"translation":{"x":8.045511213434553,"y":-3.5392405982107547},"rotation":{"radians":2.22880418136507}},"curvature":1.7774424641749056},{"time":6.143282604800894,"velocity":1.0999933020780086,"acceleration":0.6583893479431816,"pose":{"translation":{"x":8.016495405171506,"y":-3.5046169702529943},"rotation":{"radians":2.3070926307510735}},"curvature":1.6835307278255236},{"time":6.184874017542705,"velocity":1.127376645193125,"acceleration":0.7605608025111811,"pose":{"translation":{"x":7.984103680834579,"y":-3.4715066438923134},"rotation":{"radians":2.382459571871696}},"curvature":1.567671319864707},{"time":6.226560423633062,"velocity":1.1590816916630142,"acceleration":0.8187941387856714,"pose":{"translation":{"x":7.94836212537002,"y":-3.4399829724958946},"rotation":{"radians":2.4541632010977663}},"curvature":1.440365216417158},{"time":6.268350999071865,"velocity":1.193299569888787,"acceleration":0.8367733489121008,"pose":{"translation":{"x":7.909323998227046,"y":-3.410114775199522},"rotation":{"radians":2.5217629022488226}},"curvature":1.31055960787649},{"time":6.310282878731931,"velocity":1.2283870492581195,"acceleration":0.8209343102323394,"pose":{"translation":{"x":7.867068140432475,"y":-3.3819663559465347},"rotation":{"radians":2.5850797113775825}},"curvature":1.1849648918557187},{"time":6.352409999089653,"velocity":1.2629706477510616,"acceleration":0.779196902860242,"pose":{"translation":{"x":7.821697381665355,"y":-3.355597522526771},"rotation":{"radians":2.6441412040319996}},"curvature":1.0680026712113686},{"time":6.394792523390575,"velocity":1.2959949794217387,"acceleration":0.719663712623116,"pose":{"translation":{"x":7.773336947331586,"y":-3.331063605615521},"rotation":{"radians":2.6991241642022574}},"curvature":0.9621403598455297},{"time":6.437488138616451,"velocity":1.3267214643879204,"acceleration":0.6496107134953284,"pose":{"translation":{"x":7.722132865638557,"y":-3.3084154778124724},"rotation":{"radians":2.7503035430340894}},"curvature":0.8683768798809708},{"time":6.480545698743085,"velocity":1.354692116743151,"acceleration":0.5370053924885482,"pose":{"translation":{"x":7.668250374669767,"y":-3.2876995726806597},"rotation":{"radians":2.7980115671337464}},"curvature":0.7867212472916952},{"time":6.567906873176035,"velocity":1.401605538507778,"acceleration":0.39267502685289,"pose":{"translation":{"x":7.553197609067247,"y":-3.2522280837333115},"rotation":{"radians":2.8844573867685455}},"curvature":0.6570825634305726},{"time":6.656947741398939,"velocity":1.4365696638282113,"acceleration":0.29487004071388556,"pose":{"translation":{"x":7.429824221550183,"y":-3.224932550024751},"rotation":{"radians":2.961341444736131}},"curvature":0.5659708774276553},{"time":6.702048055958604,"velocity":1.4498683954186287,"acceleration":0.23578171383223953,"pose":{"translation":{"x":7.365589096343066,"y":-3.214420228138204},"rotation":{"radians":2.997049022629304}},"curvature":0.532469793596845},{"time":6.74749983928141,"velocity":1.4605850947872114,"acceleration":0.18102216507151228,"pose":{"translation":{"x":7.2999811939387715,"y":-3.2060265767125213},"rotation":{"radians":3.0313530315952613}},"curvature":0.5059170432926517},{"time":6.793254795376655,"velocity":1.4688677560023247,"acceleration":0.1299169083383581,"pose":{"translation":{"x":7.233255619643192,"y":-3.19976748914473},"rotation":{"radians":3.064546662474821}},"curvature":0.48566055081168685},{"time":6.839255705834929,"velocity":1.4748440520698132,"acceleration":0.08164230915873075,"pose":{"translation":{"x":7.165673945235362,"y":-3.195654572106795},"rotation":{"radians":3.096908408356383}},"curvature":0.47118592927934255},{"time":6.885438498454438,"velocity":1.4786145219026687,"acceleration":0.0353003925989222,"pose":{"translation":{"x":7.097502616042085,"y":-3.193695164584567},"rotation":{"radians":3.128704870372477}},"curvature":0.4621140280475768},{"time":6.93173434576252,"velocity":1.4802487834883438,"acceleration":-0.010033975007457422,"pose":{"translation":{"x":7.029011358012566,"y":-3.193892356916729},"rotation":{"radians":-3.12299140212966}},"curvature":0.4581962863080353},{"time":6.978071767008269,"velocity":1.4797838349616539,"acceleration":-0.05528245333367036,"pose":{"translation":{"x":6.960471584793034,"y":-3.1962450098337474},"rotation":{"radians":-3.0915573833240027}},"curvature":0.45931000573533276},{"time":7.024378719629345,"velocity":1.4772238730143548,"acceleration":-0.1013393081743746,"pose":{"translation":{"x":6.892154804801375,"y":-3.20074777349682},"rotation":{"radians":-3.059928088346626}},"curvature":0.4654545945304411},{"time":7.070584672050683,"velocity":1.4725413937624383,"acceleration":-0.14904523277384052,"pose":{"translation":{"x":6.824331028301758,"y":-3.207391106536825},"rotation":{"radians":-3.0278520882435016}},"curvature":0.4767490668003416},{"time":7.116622649960156,"velocity":1.4656796526284839,"acceleration":-0.19915022509020616,"pose":{"translation":{"x":6.757267174479267,"y":-3.216161295093267},"rotation":{"radians":-2.995073722574345}},"curvature":0.49343043507532147},{"time":7.162431243215652,"velocity":1.4565568609705863,"acceleration":-0.25225670175655496,"pose":{"translation":{"x":6.691225478514523,"y":-3.22704047185323},"rotation":{"radians":-2.9613302823689853}},"curvature":0.5158519610096561},{"time":7.207956549897071,"velocity":1.4450727972606758,"acceleration":-0.30873399333891455,"pose":{"translation":{"x":6.626461898658322,"y":-3.2400066350903227},"rotation":{"radians":-2.9263496374437588}},"curvature":0.5444793892911293},{"time":7.253154016774942,"velocity":1.4311188028226676,"acceleration":-0.3685964015199292,"pose":{"translation":{"x":6.5632245233062525,"y":-3.255033667703629},"rotation":{"radians":-2.889848557689294}},"curvature":0.5798821429533514},{"time":7.297990110261217,"velocity":1.4145923801054154,"acceleration":-0.46326533503013145,"pose":{"translation":{"x":6.501751978073334,"y":-3.272091356256655},"rotation":{"radians":-2.85153209800565}},"curvature":0.6227148739896239},{"time":7.386533209549777,"velocity":1.3735734315488943,"acceleration":-0.5596625923229855,"pose":{"translation":{"x":6.384999008969924,"y":-3.3121574799917015},"rotation":{"radians":-2.76822290076295}},"curvature":0.7334805592909766},{"time":7.430212501142492,"velocity":1.3491277659852838,"acceleration":-0.6198109644022372,"pose":{"translation":{"x":6.330134186098258,"y":-3.3350851779733883},"rotation":{"radians":-2.7226031638558825}},"curvature":0.8026956535741429},{"time":7.473527022074999,"velocity":1.3222809509934854,"acceleration":-0.6716928216399525,"pose":{"translation":{"x":6.277862209492651,"y":-3.3598820955720266},"rotation":{"radians":-2.6739318019730742}},"curvature":0.8816579661794339},{"time":7.516507736916445,"velocity":1.2934111133655328,"acceleration":-0.7096709991712431,"pose":{"translation":{"x":6.22835049698468,"y":-3.38649782325747},"rotation":{"radians":-2.621932577055205}},"curvature":0.9702282268386422},{"time":7.559194075220186,"velocity":1.2631178570105548,"acceleration":-0.7273237896174178,"pose":{"translation":{"x":6.181747446073123,"y":-3.414877969397684},"rotation":{"radians":-2.566380702394667}},"curvature":1.0675184967178264},{"time":7.601629058935087,"velocity":1.23225388384268,"acceleration":-0.7181064012668297,"pose":{"translation":{"x":6.138180840998581,"y":-3.4449641792977017},"rotation":{"radians":-2.507134825153775}},"curvature":1.1715612109376943},{"time":7.643852654530874,"velocity":1.2019328495608437,"acceleration":-0.6762611712966131,"pose":{"translation":{"x":6.097756259818113,"y":-3.4766941542385665},"rotation":{"radians":-2.444176346910186}},"curvature":1.2789768513611621},{"time":7.685893662120396,"velocity":1.1735021485258637,"acceleration":-0.5977931762315961,"pose":{"translation":{"x":6.060555481479861,"y":-3.5100016705162824},"rotation":{"radians":-2.3776533818672845}},"curvature":1.3847382987654744},{"time":7.727760983383845,"velocity":1.1484741495674773,"acceleration":-0.4812402030604427,"pose":{"translation":{"x":6.026634892897671,"y":-3.5448165984807662},"rotation":{"radians":-2.3079236265766285}},"curvature":1.4821755280724418},{"time":7.769435701052831,"velocity":1.1284185999739678,"acceleration":-0.3279827070721275,"pose":{"translation":{"x":5.9960238960257435,"y":-3.5810649215747894},"rotation":{"radians":-2.235587217484583}},"curvature":1.5633738433395497},{"time":7.810865810434139,"velocity":1.114830240544792,"acceleration":-0.14197561416046384,"pose":{"translation":{"x":5.968723314933232,"y":-3.618668755372934},"rotation":{"radians":-2.1614987374843175}},"curvature":1.6200488996502214},{"time":7.851965329977103,"velocity":1.1089951110159797,"acceleration":0.07103247335439015,"pose":{"translation":{"x":5.944703802878903,"y":-3.657546366620535},"rotation":{"radians":-2.086748814570102}},"curvature":1.6448126284883529},{"time":7.892618636072855,"velocity":1.111882815897994,"acceleration":0.3046140946985791,"pose":{"translation":{"x":5.923904249385733,"y":-3.6976121922726337},"rotation":{"radians":-2.0126115212627367}},"curvature":1.6325250001062348},{"time":7.932689323614408,"velocity":1.1240889121074138,"acceleration":0.5529589644045386,"pose":{"translation":{"x":5.906230187315568,"y":-3.7387768585329235},"rotation":{"radians":-1.9404632282976844}},"curvature":1.5812835964734753},{"time":7.97203126771206,"velocity":1.1458433927733127,"acceleration":0.8123254618330228,"pose":{"translation":{"x":5.891552199943725,"y":-3.7809471998927013},"rotation":{"radians":-1.8716882255514158}},"curvature":1.4926646255804414},{"time":8.010498657462144,"velocity":1.1770914329175604,"acceleration":1.082758465716829,"pose":{"translation":{"x":5.879704328033638,"y":-3.8240262781698124},"rotation":{"radians":-1.8075916741542633}},"curvature":1.3711044744426328},{"time":8.047952087472204,"velocity":1.2176444513310858,"acceleration":1.3702967890657312,"pose":{"translation":{"x":5.870482476911477,"y":-3.8679134015476007},"rotation":{"radians":-1.7493385162056108}},"curvature":1.222648936473306},{"time":8.084259144196894,"velocity":1.267395894581356,"acceleration":1.6900744201882945,"pose":{"translation":{"x":5.86364282354079,"y":-3.9125041436138615},"rotation":{"radians":-1.6979289536895217}},"curvature":1.0534970658646665},{"time":8.119289594227576,"velocity":1.3265999621058944,"acceleration":2.3193098426615095,"pose":{"translation":{"x":5.858900223597107,"y":-3.957690362399782},"rotation":{"radians":-1.6542111715069086}},"curvature":0.8687390966980826},{"time":8.184757920182378,"velocity":1.4784412948754382,"acceleration":2.7000000000000215,"pose":{"translation":{"x":5.854349442700676,"y":-4.0493981987060375},"rotation":{"radians":-1.5927619924860474}},"curvature":0.46252980509605585},{"time":8.244231040407984,"velocity":1.6390187194845751,"acceleration":-2.6470074164761126,"pose":{"translation":{"x":5.8536620227023,"y":-4.14209818706386},"rotation":{"radians":-1.570796326794887}},"curvature":1.287279126407328E-14},{"time":8.303609027487825,"velocity":1.4818447473088134,"acceleration":-2.3632604540666833,"pose":{"translation":{"x":5.854334141027116,"y":-4.234751044574789},"rotation":{"radians":-1.5492857726355629}},"curvature":0.4543786941030466},{"time":8.368743453818249,"velocity":1.3279151333638015,"acceleration":-1.785876066371605,"pose":{"translation":{"x":5.858784602177313,"y":-4.3261488038193034},"rotation":{"radians":-1.4887394318507872}},"curvature":0.8648218951322589},{"time":8.403522799571604,"velocity":1.2658035321788206,"acceleration":-1.4909180739818382,"pose":{"translation":{"x":5.863423494411472,"y":-4.371013536065665},"rotation":{"radians":-1.445332879856721}},"curvature":1.0587050209102449},{"time":8.43954800944357,"velocity":1.2120928956617194,"acceleration":-1.2202403726158404,"pose":{"translation":{"x":5.8701145378051915,"y":-4.415142524324043},"rotation":{"radians":-1.3939354910078332}},"curvature":1.2423850761692288},{"time":8.476707249151167,"velocity":1.1667496911548008,"acceleration":-0.9581065083622506,"pose":{"translation":{"x":5.879137365859245,"y":-4.458409728744621},"rotation":{"radians":-1.3352348596582246}},"curvature":1.4106147179951498},{"time":8.514886046185921,"velocity":1.1301703372343608,"acceleration":-0.6981694369685343,"pose":{"translation":{"x":5.890731351044297,"y":-4.500695935575307},"rotation":{"radians":-1.2700874070940888}},"curvature":1.5561667733717361},{"time":8.553958742418882,"velocity":1.1028909749045523,"acceleration":-0.44055096040320013,"pose":{"translation":{"x":5.905097153302065,"y":-4.541888649274365},"rotation":{"radians":-1.1995542542965447}},"curvature":1.6709984320619333},{"time":8.593785493766013,"velocity":1.0853452613488341,"acceleration":-0.18968381677718937,"pose":{"translation":{"x":5.922398268546473,"y":-4.581881984623031},"rotation":{"radians":-1.124898606398946}},"curvature":1.7479069511424339},{"time":8.634216102645986,"velocity":1.0776762291418551,"acceleration":0.047404874361174194,"pose":{"translation":{"x":5.9427625771648165,"y":-4.6205765588381436},"rotation":{"radians":-1.0475382470365942}},"curvature":1.7823093126228504},{"time":8.675100121472118,"velocity":1.0796143309176878,"acceleration":0.262541247128277,"pose":{"translation":{"x":5.966283892518912,"y":-4.657879383684765},"rotation":{"radians":-0.9689578217323415}},"curvature":1.7735690688357202},{"time":8.716300537215266,"velocity":1.0904311394490973,"acceleration":0.4477300205634343,"pose":{"translation":{"x":5.993023509446263,"y":-4.693703757588805},"rotation":{"radians":-0.8905981589487968}},"curvature":1.725359175840478},{"time":8.757707023764633,"velocity":1.1089700665233049,"acceleration":0.5964229559570596,"pose":{"translation":{"x":6.023011752761212,"y":-4.727969157749648},"rotation":{"radians":-0.8137476930546436}},"curvature":1.6449194765614434},{"time":8.799245006629523,"velocity":1.1337442730480767,"acceleration":0.70462580600936,"pose":{"translation":{"x":6.0562495257561,"y":-4.760601132252773},"rotation":{"radians":-0.7394596595799129}},"curvature":1.541531809694632},{"time":8.840878476745305,"velocity":1.1630802904853756,"acceleration":0.7715962436107516,"pose":{"translation":{"x":6.092709858702428,"y":-4.791531192182384},"rotation":{"radians":-0.6685088821752051}},"curvature":1.4248024068706338},{"time":8.882606665176045,"velocity":1.195277603931217,"acceleration":0.7999066518500789,"pose":{"translation":{"x":6.13233945735201,"y":-4.8206967037340265},"rotation":{"radians":-0.6013890247776947}},"curvature":1.3032831602703094},{"time":8.924456318247433,"velocity":1.228753419800638,"acceleration":0.7948421998177476,"pose":{"translation":{"x":6.175060251438132,"y":-4.848040780327219},"rotation":{"radians":-0.5383411070650799}},"curvature":1.1836913169590342},{"time":8.966471914550594,"velocity":1.2621491887928964,"acceleration":0.7633444997134565,"pose":{"translation":{"x":6.220770943176713,"y":-4.873512174718073},"rotation":{"radians":-0.47939983987352636}},"curvature":1.0707065413613832},{"time":9.00870588959083,"velocity":1.294388261340896,"acceleration":0.7128405202593145,"pose":{"translation":{"x":6.269348555767459,"y":-4.897065171111916},"rotation":{"radians":-0.42444530576451334}},"curvature":0.9671658185344763},{"time":9.05121022713575,"velocity":1.3246870754296936,"acceleration":0.6502612218084417,"pose":{"translation":{"x":6.3206499818950235,"y":-4.918659477275924},"rotation":{"radians":-0.3732512292106936}},"curvature":0.8744504611450831},{"time":9.094030042160236,"velocity":1.3525311406651275,"acceleration":0.5457360274747628,"pose":{"translation":{"x":6.374513532230166,"y":-4.9382601166517315},"rotation":{"radians":-0.3255252083330618}},"curvature":0.7929094708356739},{"time":9.180766062422345,"velocity":1.3998661118019413,"acceleration":0.4077165099754065,"pose":{"translation":{"x":6.489196629143686,"y":-4.9713664198533865},"rotation":{"radians":-0.23915290570151532}},"curvature":0.6617341139237327},{"time":9.268988401498161,"velocity":1.4358358159918003,"acceleration":0.3123081186237476,"pose":{"translation":{"x":6.611791534640181,"y":-4.99620648201905},"rotation":{"radians":-0.16262612247565691}},"curvature":0.5678375943127466},{"time":9.31360992112943,"velocity":1.4497714788379743,"acceleration":0.25406970630091197,"pose":{"translation":{"x":6.675498390669304,"y":-5.005492635568483},"rotation":{"radians":-0.12723941387058146}},"curvature":0.5327117147587316},{"time":9.358537830285739,"velocity":1.461186299522032,"acceleration":0.19981878656826185,"pose":{"translation":{"x":6.740493728703598,"y":-5.01268085045031},"rotation":{"radians":-0.09336499474097489}},"curvature":0.5044389772228018},{"time":9.403725256240742,"velocity":1.4702155961445038,"acceleration":0.1490123384599986,"pose":{"translation":{"x":6.806529143348976,"y":-5.017770338980916},"rotation":{"radians":-0.0607183972762435}},"curvature":0.4823857976140558},{"time":9.449116180453652,"velocity":1.4769794039063302,"acceleration":0.10091640326504217,"pose":{"translation":{"x":6.87335003520672,"y":-5.020764766052148},"rotation":{"radians":-0.029029253956322994}},"curvature":0.4660424995149525},{"time":9.494647371973604,"velocity":1.4815742479908955,"acceleration":0.05468462035374717,"pose":{"translation":{"x":6.940697159374636,"y":-5.0216721412439345},"rotation":{"radians":0.0019610529046543137}},"curvature":0.45502515752946493},{"time":9.540250372199525,"velocity":1.4840680307452419,"acceleration":0.009407728162280068,"pose":{"translation":{"x":7.008308173948219,"y":-5.020504710936918},"rotation":{"radians":0.03250221131972893}},"curvature":0.449074221278162},{"time":9.58585350040829,"velocity":1.4844970525787795,"acceleration":-0.035856068239308855,"pose":{"translation":{"x":7.075919188521802,"y":-5.01727885042507},"rotation":{"radians":0.06283806504008764}},"curvature":0.4480524583149358},{"time":9.631383864089777,"velocity":1.4828645127516555,"acceleration":-0.08206073252558314,"pose":{"translation":{"x":7.143266312689718,"y":-5.012014956028322},"rotation":{"radians":0.09320983319449506}},"curvature":0.45194368816321656},{"time":9.676769366470293,"velocity":1.4791401451802688,"acceleration":-0.13015347500748914,"pose":{"translation":{"x":7.210087204547461,"y":-5.004737337205187},"rotation":{"radians":0.12385937570635763}},"curvature":0.4608530305938923},{"time":9.721940706761016,"velocity":1.4732609382706854,"acceleration":-0.18104868491282325,"pose":{"translation":{"x":7.276122619192839,"y":-4.995474108665384},"rotation":{"radians":0.1550324327505232}},"curvature":0.4750088057985688},{"time":9.76683336708675,"velocity":1.4651331811564732,"acceleration":-0.23558543091106707,"pose":{"translation":{"x":7.341117957227133,"y":-4.984257082482462},"rotation":{"radians":0.18698175946257517}},"curvature":0.4947656624983722},{"time":9.8113895735365,"velocity":1.4546363880602464,"acceleration":-0.32602176757399176,"pose":{"translation":{"x":7.404824813256256,"y":-4.971121660206426},"rotation":{"radians":0.21997004540831627}},"curvature":0.5206078335420556},{"time":9.899335792680683,"velocity":1.4259640062434102,"acceleration":-0.46254979519344736,"pose":{"translation":{"x":7.527419718752752,"y":-4.939254533633044},"rotation":{"radians":0.29017847443513733}},"curvature":0.5931356615481289},{"time":9.985494934982379,"velocity":1.3861111126177175,"acceleration":-0.5747031106868311,"pose":{"translation":{"x":7.642102815666272,"y":-4.900223631154084},"rotation":{"radians":0.36803446154541325}},"curvature":0.6989287175264021},{"time":10.02786431729719,"velocity":1.361761296803515,"acceleration":-0.6502631704334794,"pose":{"translation":{"x":7.6959663660014135,"y":-4.878145331222145},"rotation":{"radians":0.4106334768387333}},"curvature":0.7666149840316651},{"time":10.069787225278716,"velocity":1.3345003737456567,"acceleration":-0.7218737179944148,"pose":{"translation":{"x":7.747267792128979,"y":-4.85443038180962},"rotation":{"radians":0.4561239109973504}},"curvature":0.845324054163293},{"time":10.111293540702226,"velocity":1.3045380555106383,"acceleration":-0.7839136593286343,"pose":{"translation":{"x":7.795845404719723,"y":-4.8291362899551755},"rotation":{"radians":0.5048317700335061}},"curvature":0.9356274061228237},{"time":10.152424951149351,"velocity":1.2722945810336845,"acceleration":-0.8293277318794265,"pose":{"translation":{"x":7.841556096458306,"y":-4.802323289074925},"rotation":{"radians":0.5570561378807112}},"curvature":1.0375572397134882},{"time":10.193231128724204,"velocity":1.2384528863388624,"acceleration":-0.8501119636590367,"pose":{"translation":{"x":7.884276890544428,"y":-4.774054231075055},"rotation":{"radians":0.6130418720169789}},"curvature":1.1502481396728574},{"time":10.233763862581347,"velocity":1.203995524367098,"acceleration":-0.8382017878429927,"pose":{"translation":{"x":7.9239064891940085,"y":-4.744394478464448},"rotation":{"radians":0.6729427831056884}},"curvature":1.2714980913383336},{"time":10.274069062181395,"velocity":1.1702116340029691,"acceleration":-0.7866518820481041,"pose":{"translation":{"x":7.960366822140338,"y":-4.713411796467312},"rotation":{"radians":0.7367756993078343}},"curvature":1.3973107344482827},{"time":10.31417700201825,"velocity":1.1386606476452348,"acceleration":-0.6908214173719566,"pose":{"translation":{"x":7.993604595135223,"y":-4.681176245135796},"rotation":{"radians":0.804368512670424}},"curvature":1.5215497864753451},{"time":10.354091858324736,"velocity":1.11108661003739,"acceleration":-0.5491779435297587,"pose":{"translation":{"x":8.023592838450174,"y":-4.647760071462619},"rotation":{"radians":0.8753091616038282}},"curvature":1.6359066009953251},{"time":10.393782358314775,"velocity":1.0892894628751921,"acceleration":-0.3634030941154679,"pose":{"translation":{"x":8.050332455377525,"y":-4.613237601493697},"rotation":{"radians":0.9489066508944368}},"curvature":1.7304023672686455},{"time":10.433175889330965,"velocity":1.0749737318157753,"acceleration":-0.13771841611472463,"pose":{"translation":{"x":8.07385377073162,"y":-4.577685132440763},"rotation":{"radians":1.0241776811155843}},"curvature":1.794549358023086},{"time":10.472158231102664,"velocity":1.069605145450534,"acceleration":0.12239260490231733,"pose":{"translation":{"x":8.094218079349965,"y":-4.541180824793992},"rotation":{"radians":1.099870630012684}},"curvature":1.819048028705929},{"time":10.510579848121393,"velocity":1.0743076672420155,"acceleration":0.4117963586371772,"pose":{"translation":{"x":8.111519194594372,"y":-4.503804594434625},"rotation":{"radians":1.1745308109175374}},"curvature":1.7975755307943106},{"time":10.548267600298342,"velocity":1.089827346353703,"acceleration":0.7275302192585646,"pose":{"translation":{"x":8.12588499685214,"y":-4.4656380047475945},"rotation":{"radians":1.2465984768132268}},"curvature":1.7280250282835923},{"time":10.585038654655786,"velocity":1.1165793995927427,"acceleration":1.0152170598119943,"pose":{"translation":{"x":8.137478982037191,"y":-4.42676415873415},"rotation":{"radians":1.3145189759702494}},"curvature":1.6126760587838824},{"time":10.620743207395932,"velocity":1.152827270647497,"acceleration":-2.699999999999999,"pose":{"translation":{"x":8.146501810091241,"y":-4.387267591124479},"rotation":{"radians":1.376839033227379}},"curvature":1.4572035866994433},{"time":10.657536485046869,"velocity":1.053485420989971,"acceleration":-2.7,"pose":{"translation":{"x":8.15319285348496,"y":-4.347234160490331},"rotation":{"radians":1.4322673966427992}},"curvature":1.268911736666696},{"time":10.698350469833187,"velocity":0.9432876620669106,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":8.157831745719122,"y":-4.306750941357647},"rotation":{"radians":1.479689560375334}},"curvature":1.0548121629757579},{"time":10.800220049061643,"velocity":0.668239798150081,"acceleration":-2.7,"pose":{"translation":{"x":8.162282206869317,"y":-4.224788868147113},"rotation":{"radians":1.5467352565172034}},"curvature":0.5670449966398566},{"time":11.04771627059871,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":8.162954325194136,"y":-4.14209818706387},"rotation":{"radians":1.5707963267949074}},"curvature":1.6188848564701006E-14}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index c8ac8f8..8afb971 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,6 +7,7 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.TimedRobot; @@ -71,7 +72,8 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - } + m_robotContainer.resetOdometry(new Pose2d()); + } /** * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. @@ -83,7 +85,6 @@ public class Robot extends TimedRobot { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); - m_robotContainer.resetOdometry(new Pose2d()); //m_robotContainer.resetGyroYawRobotContainer(0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2bc3f90..07fa991 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -37,6 +37,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.auto.DriveOffLineBackward; import frc4388.robot.commands.auto.DriveOffLineForward; import frc4388.robot.commands.auto.EightBallAutoMiddle; +import frc4388.robot.commands.auto.FigureEight; import frc4388.robot.commands.auto.FiveBallAutoMiddle; import frc4388.robot.commands.auto.SixBallAutoMiddle; import frc4388.robot.commands.auto.TenBallAutoMiddle; @@ -109,6 +110,8 @@ public class RobotContainer { SixBallAutoMiddle m_sixBallAutoMiddle; + FigureEight m_figureEight; + EightBallAutoMiddle m_eightBallAutoMiddle; DriveOffLineForward m_driveOffLineForward; @@ -270,12 +273,19 @@ public class RobotContainer { } public void buildAutos() { + System.out.println("BUILDING AUTOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOS"); String[] sixBallAutoMiddlePaths = new String[]{ "SixBallMidComplete" }; m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); + String[] figureEight = new String[]{ + "FigureEight" + }; + + m_figureEight = new FigureEight(m_robotDrive, buildPaths(figureEight)); + String[] eightBallAutoMiddlePaths = new String[]{ "EightBallMidComplete" }; @@ -323,8 +333,9 @@ public class RobotContainer { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_figureEight.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); diff --git a/src/main/java/frc4388/robot/commands/auto/FigureEight.java b/src/main/java/frc4388/robot/commands/auto/FigureEight.java new file mode 100644 index 0000000..c0b528d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/FigureEight.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class FigureEight extends SequentialCommandGroup { + /** + * Creates a new FigureEight. + */ + public FigureEight(Drive drive, RamseteCommand[] paths) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + addCommands( + paths[0] + ); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 350d77a..c6c4805 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -833,8 +833,7 @@ public class Drive extends SubsystemBase { SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro); SmartDashboard.putData("Drive Train", m_driveTrain); - - System.out.println("yooooooooooooooooooooooooooooooooooooo"); + //SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); //SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());