From 9eb5663b73506ff34e819eace26c04a8ebf68e64 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 5 Feb 2021 10:47:45 -0700 Subject: [PATCH 1/9] extended path --- PathWeaver/Paths/DriveOffLineForward | 2 +- src/main/deploy/paths/DriveOffLineForward.wpilib.json | 2 +- src/main/java/frc4388/robot/subsystems/Drive.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/PathWeaver/Paths/DriveOffLineForward b/PathWeaver/Paths/DriveOffLineForward index f5cce7f..9b1f469 100644 --- a/PathWeaver/Paths/DriveOffLineForward +++ b/PathWeaver/Paths/DriveOffLineForward @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.3,0.5,0.0,true, -3.7,-2.3,0.5,0.0,true, +4.926159376619506,-2.306021520328549,0.5,0.0,true, diff --git a/src/main/deploy/paths/DriveOffLineForward.wpilib.json b/src/main/deploy/paths/DriveOffLineForward.wpilib.json index 25c61f9..574d08b 100644 --- a/src/main/deploy/paths/DriveOffLineForward.wpilib.json +++ b/src/main/deploy/paths/DriveOffLineForward.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.24673072577541286,"velocity":0.6661729595936147,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2821826120636555,"y":-2.300096658926172},"rotation":{"radians":-0.0022988506389506674}},"curvature":-0.017838043885837547},{"time":0.33725349511063807,"velocity":0.9105844367987228,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.353548563195771,"y":-2.300293663507997},"rotation":{"radians":-0.0030970284585935958}},"curvature":-0.0069332281418155},{"time":0.43198725573191693,"velocity":1.1663655904761758,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.4519266542203786,"y":-2.3006233214402596},"rotation":{"radians":-0.0035415696301389923}},"curvature":-0.0029369290271881927},{"time":0.5284273014978229,"velocity":1.4267537140441218,"acceleration":2.7,"pose":{"translation":{"x":3.576966078125889,"y":-2.3010839099521765},"rotation":{"radians":-0.0037936885562888066}},"curvature":-0.0013707072882595045},{"time":0.5763438583279685,"velocity":1.556128417485515,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.6484302726022984,"y":-2.3013581294794916},"rotation":{"radians":-0.0038756571162190407}},"curvature":-9.542625124576716E-4},{"time":0.6235805335204395,"velocity":1.6836674405051866,"acceleration":0.3371724938324549,"pose":{"translation":{"x":3.7249482805894383,"y":-2.3016571676734268},"rotation":{"radians":-0.003936728545992743}},"curvature":-6.608939907291656E-4},{"time":0.6712952623779239,"velocity":1.699755534626604,"acceleration":0.0018965134247352152,"pose":{"translation":{"x":3.805667202540378,"y":-2.3019768360475115},"rotation":{"radians":-0.0039808512652144586}},"curvature":-4.439007504459168E-4},{"time":0.7206906310513733,"velocity":1.699849213606413,"acceleration":0.0015545280583909033,"pose":{"translation":{"x":3.8896288968047106,"y":-2.302312429284039},"rotation":{"radians":-0.004010692104327395}},"curvature":-2.737831684939188E-4},{"time":0.771381063968029,"velocity":1.6999280133066739,"acceleration":0.001402028284918414,"pose":{"translation":{"x":3.9757962901544204,"y":-2.302658854441877},"rotation":{"radians":-0.0040279621163264315}},"curvature":-1.3070032995605277E-4},{"time":0.8227257437439109,"velocity":1.6999999999999997,"acceleration":-0.001402028284905696,"pose":{"translation":{"x":4.063079688309753,"y":-2.3030107601642746},"rotation":{"radians":-0.0040336164409507415}},"curvature":-5.668115247084055E-16},{"time":0.8740704235197836,"velocity":1.6999280133066745,"acceleration":-0.0015545280583857497,"pose":{"translation":{"x":4.150363086465086,"y":-2.303362665886672},"rotation":{"radians":-0.004027962116326537}},"curvature":1.3070032995489233E-4},{"time":0.9247608564364644,"velocity":1.6998492136064138,"acceleration":-0.0018965134247457959,"pose":{"translation":{"x":4.236530479814796,"y":-2.3037090910445097},"rotation":{"radians":-0.0040106921043276}},"curvature":2.7378316849265206E-4},{"time":0.9741562251098724,"velocity":1.6997555346266044,"acceleration":-0.33717249383246317,"pose":{"translation":{"x":4.320492174079129,"y":-2.3040446842810374},"rotation":{"radians":-0.003980851265214779}},"curvature":4.4390075044446666E-4},{"time":1.021870953967357,"velocity":1.6836674405051866,"acceleration":-2.699999999999997,"pose":{"translation":{"x":4.401211096030068,"y":-2.304364352655122},"rotation":{"radians":-0.003936728545993193}},"curvature":6.608939907274078E-4},{"time":1.069107629159828,"velocity":1.556128417485515,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":4.477729104017208,"y":-2.3046633908490572},"rotation":{"radians":-0.00387565711621964}},"curvature":9.542625124554455E-4},{"time":1.1170241859899734,"velocity":1.4267537140441227,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":4.549193298493617,"y":-2.3049376103763723},"rotation":{"radians":-0.0037936885562895833}},"curvature":0.001370707288256562},{"time":1.2134642317558797,"velocity":1.166365590476176,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":4.674232722399128,"y":-2.3053981988882892},"rotation":{"radians":-0.003541569630140284}},"curvature":0.0029369290271822348},{"time":1.3081979923771587,"velocity":0.9105844367987229,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":4.772610813423736,"y":-2.305727856820553},"rotation":{"radians":-0.003097028458595807}},"curvature":0.006933228141801077},{"time":1.3987207617123838,"velocity":0.6661729595936149,"acceleration":-2.7,"pose":{"translation":{"x":4.843976764555851,"y":-2.3059248614023775},"rotation":{"radians":-0.0022988506389546087}},"curvature":0.017838043885796937},{"time":1.6454514874877968,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":4.926159376619506,"y":-2.3060215203285503},"rotation":{"radians":-1.0658141036401503E-14}},"curvature":-5.6843418860808015E-14}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fe96ca2..350d77a 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -834,7 +834,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()); 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 2/9] 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()); From cbc0c9443c8cdd5fe53e9a602741d68b6db1c15e Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 5 Feb 2021 13:54:16 -0700 Subject: [PATCH 3/9] Added Run Intake to end of Drive Forward Auto --- PathWeaver/Paths/FigureEight3 | 3 +++ PathWeaver/pathweaver.json | 1 - src/main/java/frc4388/robot/RobotContainer.java | 11 ++++++----- .../robot/commands/auto/DriveOffLineForward.java | 6 +++++- 4 files changed, 14 insertions(+), 7 deletions(-) create mode 100644 PathWeaver/Paths/FigureEight3 diff --git a/PathWeaver/Paths/FigureEight3 b/PathWeaver/Paths/FigureEight3 new file mode 100644 index 0000000..610bd7c --- /dev/null +++ b/PathWeaver/Paths/FigureEight3 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,10.0,0.0,true, +10.0,-10.0,0.0,-10.0,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index ddd52e0..89479dc 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -1,6 +1,5 @@ { "lengthUnit": "Meter", - "exportUnit": "Always Meters", "maxVelocity": 1.7, "maxAcceleration": 2.7, "wheelBase": 0.648, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 07fa991..a67dc99 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.auto.DriveOffLineBackward; import frc4388.robot.commands.auto.DriveOffLineForward; @@ -161,8 +162,8 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the storage not - //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); - m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + //m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -230,7 +231,7 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - // safety for climber and leveler + // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); @@ -332,8 +333,8 @@ public class RobotContainer { try { 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_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_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); diff --git a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java index a5fb618..47ebfe6 100644 --- a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java +++ b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java @@ -9,7 +9,9 @@ package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.intake.RunIntake; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -21,9 +23,11 @@ public class DriveOffLineForward extends SequentialCommandGroup { public DriveOffLineForward(Drive drive, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); + Intake m_intake = new Intake(); addCommands( - paths[0] + paths[0], + new RunIntake(m_intake) ); } } From 26b56c8c21c39c0d5e3a68a26ce23573950b48b1 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 5 Feb 2021 14:27:50 -0700 Subject: [PATCH 4/9] Added code to help intake run in between the path --- .../java/frc4388/robot/RobotContainer.java | 19 ++++++++++++++++++- .../commands/auto/SixBallAutoMiddle.java | 7 ++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a67dc99..ab2aeef 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -52,6 +52,7 @@ import frc4388.robot.commands.drive.DriveStraightAtVelocityPID; import frc4388.robot.commands.drive.DriveStraightToPositionMM; import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.TurnDegrees; +import frc4388.robot.commands.intake.RunIntake; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; import frc4388.robot.commands.shooter.TrackTarget; @@ -111,6 +112,10 @@ public class RobotContainer { SixBallAutoMiddle m_sixBallAutoMiddle; + SixBallAutoMiddle m_sixBallAuto0; + + SixBallAutoMiddle m_sixBallAuto1; + FigureEight m_figureEight; EightBallAutoMiddle m_eightBallAutoMiddle; @@ -281,6 +286,18 @@ public class RobotContainer { m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); + String[] sixBallAutoMiddle0 = new String[]{ + "SixBallMid0" + }; + + m_sixBallAuto0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0)); + + String[] sixBallAutoMiddle1 = new String[]{ + "SixBallMid1" + }; + + m_sixBallAuto1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1)); + String[] figureEight = new String[]{ "FigureEight" }; @@ -333,7 +350,7 @@ public class RobotContainer { try { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); - return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_sixBallAutoMiddle.andThen(m_sixBallAuto1); //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)); diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index 9900c65..261e4e5 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -15,7 +15,9 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.RobotContainer; +import frc4388.robot.commands.intake.RunIntake; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -27,9 +29,12 @@ public class SixBallAutoMiddle extends SequentialCommandGroup { public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); + Intake m_intake = new Intake(); addCommands( - paths[0] + paths[0], + new RunIntake(m_intake)//, + //paths[1] ); } } From 0b8ecf39f8ecc178ad38139af25f5018767babe7 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 5 Feb 2021 15:20:45 -0700 Subject: [PATCH 5/9] 6 Ball Auto Path with Intake WORKING --- src/main/java/frc4388/robot/RobotContainer.java | 6 ++++-- .../java/frc4388/robot/commands/auto/SixBallAutoMiddle.java | 6 +++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ab2aeef..04d83f4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -350,14 +352,14 @@ public class RobotContainer { try { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); - return m_sixBallAutoMiddle.andThen(m_sixBallAuto1); + //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_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)); - + return new ParallelRaceGroup(m_sixBallAutoMiddle, new RunIntake(m_robotIntake)); } catch (Exception e) { System.err.println("ERROR"); } diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index 261e4e5..17b544c 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -12,9 +12,11 @@ import java.nio.file.Path; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.RobotContainer; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.commands.intake.RunIntake; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; @@ -32,9 +34,7 @@ public class SixBallAutoMiddle extends SequentialCommandGroup { Intake m_intake = new Intake(); addCommands( - paths[0], - new RunIntake(m_intake)//, - //paths[1] + paths[0] ); } } From 0b985a37ad3b87380e8e61f9880bdf3bbafb7047 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 5 Feb 2021 15:31:15 -0700 Subject: [PATCH 6/9] Storage also works in the 6 ball auto 6 ball auto, with intake and storage (no shooter), WORKING! --- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 04d83f4..0015d47 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -169,8 +169,8 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the storage not - m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); - //m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); + //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -359,7 +359,7 @@ public class RobotContainer { //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)); - return new ParallelRaceGroup(m_sixBallAutoMiddle, new RunIntake(m_robotIntake)); + return new ParallelRaceGroup(m_sixBallAutoMiddle, new RunIntake(m_robotIntake));//, new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)); } catch (Exception e) { System.err.println("ERROR"); } From 554465489e1bd22f56f7659c934b0a4203f64f1e Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 8 Feb 2021 17:41:03 -0700 Subject: [PATCH 7/9] Tried Fixing Auto Issue --- src/main/deploy/paths/FigureEight3.wpilib.json | 1 + src/main/java/frc4388/robot/RobotContainer.java | 12 +++++++++--- 2 files changed, 10 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/paths/FigureEight3.wpilib.json diff --git a/src/main/deploy/paths/FigureEight3.wpilib.json b/src/main/deploy/paths/FigureEight3.wpilib.json new file mode 100644 index 0000000..3ee5daa --- /dev/null +++ b/src/main/deploy/paths/FigureEight3.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":0.0,"y":0.0},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2405915837300212,"velocity":0.6495972760710574,"acceleration":2.700000000000001,"pose":{"translation":{"x":0.07814381358912215,"y":-2.831307938322425E-5},"rotation":{"radians":-0.0010826509167209514}},"curvature":-0.02748258627930911},{"time":0.3403687473786084,"velocity":0.9189956179222429,"acceleration":2.7,"pose":{"translation":{"x":0.15639844350516796,"y":-2.2414140403270721E-4},"rotation":{"radians":-0.004261296220402157}},"curvature":-0.05347409186319095},{"time":0.4171073902948127,"velocity":1.1261899537959945,"acceleration":2.6999999999999997,"pose":{"translation":{"x":0.23486907390179113,"y":-7.485484820790589E-4},"rotation":{"radians":-0.0094212046777475}},"curvature":-0.07770893881369795},{"time":0.4820198850766026,"velocity":1.3014536897068272,"acceleration":2.6999999999999993,"pose":{"translation":{"x":0.3136548399925232,"y":-0.0017556548118591309},"rotation":{"radians":-0.01643557600211064}},"curvature":-0.0999786645789153},{"time":0.5394608356419908,"velocity":1.4565442562333752,"acceleration":2.6999999999999997,"pose":{"translation":{"x":0.39284893282456324,"y":-0.0033927426557056606},"rotation":{"radians":-0.0251682723352039}},"curvature":-0.12012966946706},{"time":0.5916708442330667,"velocity":1.5975112794292803,"acceleration":0.4372208985685985,"pose":{"translation":{"x":0.4725387040525675,"y":-0.005800360813736916},"rotation":{"radians":-0.03547638991817513}},"curvature":-0.13806039182086433},{"time":0.6416172926181855,"velocity":1.619348910472532,"acceleration":-0.1327710001684345,"pose":{"translation":{"x":0.552805770712439,"y":-0.009112429397646338},"rotation":{"radians":-0.047212672643734674}},"curvature":-0.15371802470468285},{"time":0.691763244869097,"velocity":1.61269098223778,"acceleration":-0.10816235523707908,"pose":{"translation":{"x":0.6337261199951172,"y":-0.013456344604492188},"rotation":{"radians":-0.06022776783721897}},"curvature":-0.1670947999411318},{"time":0.7425904865170331,"velocity":1.607193388070935,"acceleration":-0.08527371232841424,"pose":{"translation":{"x":0.7153702140203677,"y":-0.018953083490487188},"rotation":{"radians":-0.07437232026484512}},"curvature":-0.1782238294413945},{"time":0.7941232830888397,"velocity":1.602798995200592,"acceleration":-0.06425369682292294,"pose":{"translation":{"x":0.7978030946105719,"y":-0.02571730874478817},"rotation":{"radians":-0.08949889521362152}},"curvature":-0.18717450044020895},{"time":0.8463856188020151,"velocity":1.59944094692642,"acceleration":-0.045195912914832825,"pose":{"translation":{"x":0.8810844880645163,"y":-0.033857473463285714},"rotation":{"radians":-0.10546371724200888}},"curvature":-0.19404745661562758},{"time":0.8994014127537447,"velocity":1.5970448497198668,"acceleration":-0.028144903720509953,"pose":{"translation":{"x":0.9652689099311829,"y":-0.0434759259223938},"rotation":{"radians":-0.12212820899172197}},"curvature":-0.19896924595594245},{"time":0.9531946524545232,"velocity":1.595530844167674,"acceleration":-0.013101546569886206,"pose":{"translation":{"x":1.0504057697835378,"y":-0.05466901435283944},"rotation":{"radians":-0.1393603148117705}},"curvature":-0.20208676461993152},{"time":1.0077894527571787,"velocity":1.5948155678490352,"acceleration":-2.8300772275892307E-5,"pose":{"translation":{"x":1.136539475992322,"y":-0.06752719171345234},"rotation":{"radians":-0.15703559687254381}},"curvature":-0.20356166296129774},{"time":1.0632100485727427,"velocity":1.5948139994033736,"acceleration":0.011145438289388458,"pose":{"translation":{"x":1.22370954049984,"y":-0.08213512046495453},"rotation":{"radians":-0.17503809651401303}},"curvature":-0.20356489854681434},{"time":1.11948073014329,"velocity":1.59544116081232,"acceleration":0.020517784327446633,"pose":{"translation":{"x":1.31195068359375,"y":-0.09857177734375},"rotation":{"radians":-0.19326096007443874}},"curvature":-0.20227161900745516},{"time":1.1766257299402767,"velocity":1.5966136495935466,"acceleration":0.028208643483979043,"pose":{"translation":{"x":1.4012929386808537,"y":-0.11691055813571438},"rotation":{"radians":-0.2116068355586935}},"curvature":-0.199856536473444},{"time":1.2346690704589236,"velocity":1.5982509734928563,"acceleration":0.03435396942968972,"pose":{"translation":{"x":1.4917617570608854,"y":-0.13721938244998455},"rotation":{"radians":-0.22998805342187695}},"curvature":-0.1964899195916967},{"time":1.2936343821175844,"velocity":1.60027666600699,"acceleration":0.03910017590330624,"pose":{"translation":{"x":1.5833781127003022,"y":-0.1595607984927483},"rotation":{"radians":-0.2483266107864853}},"curvature":-0.19233428470069036},{"time":1.3535447002134353,"velocity":1.6026191699829608,"acceleration":0.042598904638657606,"pose":{"translation":{"x":1.676158607006073,"y":-0.18399208784103394},"rotation":{"radians":-0.26655398309896333}},"curvature":-0.18754182089294194},{"time":1.4144222493608867,"velocity":1.6052124868937283,"acceleration":0.04500232668776041,"pose":{"translation":{"x":1.770115573599469,"y":-0.21056537021650001},"rotation":{"radians":-0.2846107903049497}},"curvature":-0.18225253988851078},{"time":1.4762882231020171,"velocity":1.607996599654883,"acceleration":0.04645911315598688,"pose":{"translation":{"x":1.8652571830898523,"y":-0.2393277082592249},"rotation":{"radians":-0.30244634603314535}},"curvature":-0.17659310488419677},{"time":1.5391625654678787,"velocity":1.6109176858414669,"acceleration":0.047111157975952694,"pose":{"translation":{"x":1.9615875478484668,"y":-0.2703212123014964},"rotation":{"radians":-0.3200181181490893}},"curvature":-0.17067626514133666},{"time":1.603063760245367,"velocity":1.6139281451234813,"acceleration":0.04709108007037938,"pose":{"translation":{"x":2.0591068267822266,"y":-0.30358314514160156},"rotation":{"radians":-0.33729112761652036}},"curvature":-0.1646008057287766},{"time":1.6680086326282912,"velocity":1.6169864693090261,"acceleration":0.04652048240038413,"pose":{"translation":{"x":2.1578113301075064,"y":-0.3391460268176161},"rotation":{"radians":-0.3542373101961703}},"curvature":-0.15845191395296837},{"time":1.7340121668609523,"velocity":1.6200569855616598,"acceleration":0.04550890564454909,"pose":{"translation":{"x":2.257693624123931,"y":-0.3770377393811941},"rotation":{"radians":-0.3708348624485024}},"curvature":-0.15230186412129473},{"time":1.8010873424668827,"velocity":1.6231095033994016,"acceleration":0.04415338673725915,"pose":{"translation":{"x":2.3587426359881647,"y":-0.4172816316713579},"rotation":{"radians":-0.38706759009738423}},"curvature":-0.14621092848984082},{"time":1.8692449907312363,"velocity":1.6261188944023197,"acceleration":0.04253851724936418,"pose":{"translation":{"x":2.4609437584877014,"y":-0.45989662408828735},"rotation":{"radians":-0.4029242733210075}},"curvature":-0.1402284325466594},{"time":1.9384936722968376,"velocity":1.6290646306375938,"acceleration":0.04073689217796292,"pose":{"translation":{"x":2.564278954814654,"y":-0.5048973133671097},"rotation":{"radians":-0.41839806017259407}},"curvature":-0.13439388534690774},{"time":2.008839576053657,"velocity":1.6319303041340967,"acceleration":0.03880984385024885,"pose":{"translation":{"x":2.6687268633395433,"y":-0.5522940773516893},"rotation":{"radians":-0.4334858962447088}},"curvature":-0.1287381289353431},{"time":2.080286438951921,"velocity":1.6347031457267684,"acceleration":0.03680836577891133,"pose":{"translation":{"x":2.7742629023850895,"y":-0.602093179768417},"rotation":{"radians":-0.448187995969697}},"curvature":-0.12328446383071472},{"time":2.152835485944844,"velocity":1.6373735575853954,"acceleration":0.03477414499532014,"pose":{"translation":{"x":2.880859375,"y":-0.654296875},"rotation":{"radians":-0.4625073586397917}},"curvature":-0.11804971934383038},{"time":2.2264853889572405,"velocity":1.6399346699916397,"acceleration":0.032740636566912784,"pose":{"translation":{"x":2.98848557373276,"y":-0.7089035128592514},"rotation":{"radians":-0.47644933034151526}},"curvature":-0.11304524771977777},{"time":2.301232243567186,"velocity":1.6423819295929438,"acceleration":0.030734129097070333,"pose":{"translation":{"x":3.0971078854054213,"y":-0.7659076433628798},"rotation":{"radians":-0.490021211510979}},"curvature":-0.10827782955645215},{"time":2.3770695619636633,"velocity":1.6447127235269168,"acceleration":0.028774763928721563,"pose":{"translation":{"x":3.2066898958873935,"y":-0.8253001215052791},"rotation":{"radians":-0.5032319086929682}},"curvature":-0.10375048466516042},{"time":2.453988280688158,"velocity":1.6469260414999138,"acceleration":0.027344752776362884,"pose":{"translation":{"x":3.317192494869232,"y":-0.8870682120323181},"rotation":{"radians":-0.5160916282809809}},"curvature":-0.09946318763650631},{"time":2.4928496108418328,"velocity":1.6479886949655267,"acceleration":0.02641341993660787,"pose":{"translation":{"x":3.3727760816236696,"y":-0.9188381762487552},"rotation":{"radians":-0.5223933499305692}},"curvature":-0.09740887163401273},{"time":2.53197673400173,"velocity":1.6490221761004604,"acceleration":0.02550085277916446,"pose":{"translation":{"x":3.428573980636429,"y":-0.9511956942151301},"rotation":{"radians":-0.5286116094776955}},"curvature":-0.09541349105745087},{"time":2.571367804568202,"velocity":1.6500266819917897,"acceleration":0.024607920377304648,"pose":{"translation":{"x":3.4845805914756056,"y":-0.9841382109243568},"rotation":{"radians":-0.5347479353268761}},"curvature":-0.09347644950906146},{"time":2.611020821933997,"velocity":1.651002460285847,"acceleration":0.02373530153336793,"pose":{"translation":{"x":3.5407901648432016,"y":-1.0176629666239023},"rotation":{"radians":-0.5408038933963395}},"curvature":-0.09159706182401153},{"time":2.650933632064671,"velocity":1.6519498028693427,"acceleration":0.02288350430585584,"pose":{"translation":{"x":3.5971968058493076,"y":-1.051767000089967},"rotation":{"radians":-0.546781081651562}},"curvature":-0.08977456657954339},{"time":2.6911039292123218,"velocity":1.6528690400370885,"acceleration":0.022052884108780306,"pose":{"translation":{"x":3.653794477286283,"y":-1.086447151901666},"rotation":{"radians":-0.5526811250772102}},"curvature":-0.08800813754015926},{"time":2.731529257753552,"velocity":1.6537605351224676,"acceleration":0.021243660430409737,"pose":{"translation":{"x":3.7105770029029372,"y":-1.12170006771521},"rotation":{"radians":-0.5585056710629697}},"curvature":-0.08629689410238194},{"time":2.772207014143035,"velocity":1.6546246795662767,"acceleration":0.020455932226803017,"pose":{"translation":{"x":3.767538070678711,"y":-1.157522201538086},"rotation":{"radians":-0.5642563851793584}},"curvature":-0.08463991080199534},{"time":2.8131344489743197,"velocity":1.6554618883994023,"acceleration":0.01968969204989398,"pose":{"translation":{"x":3.8246712360978563,"y":-1.1939098190032382},"rotation":{"radians":-0.5699349473203195}},"curvature":-0.08303622594547336},{"time":2.8543086691401407,"velocity":1.6562725961148619,"acceleration":0.018944838974633928,"pose":{"translation":{"x":3.8819699254236184,"y":-1.2308590006432496},"rotation":{"radians":-0.5755430481901774}},"curvature":-0.08148484942566686},{"time":2.895726640084726,"velocity":1.6570572529050631,"acceleration":0.018221190390968898,"pose":{"translation":{"x":3.939427438972416,"y":-1.268365645164522},"rotation":{"radians":-0.5810823861133668}},"curvature":-0.07998476977982921},{"time":2.937385188141221,"velocity":1.6578163212406118,"acceleration":0.017518492728726003,"pose":{"translation":{"x":3.9970369543880224,"y":-1.3064254727214575},"rotation":{"radians":-0.5865546641462306}},"curvature":-0.07853496054580741},{"time":2.9792810029475167,"velocity":1.65855027276766,"acceleration":0.016836431182176145,"pose":{"translation":{"x":4.054791529915747,"y":-1.3450340281906392},"rotation":{"radians":-0.5919615874710755}},"curvature":-0.07713438596977641},{"time":3.021410639934429,"velocity":1.6592595855015202,"acceleration":0.01617463850185506,"pose":{"translation":{"x":4.112684107676614,"y":-1.3841866844450124},"rotation":{"radians":-0.5973048610536026}},"curvature":-0.07578200611631837},{"time":3.0637705228802665,"velocity":1.65994474129515,"acceleration":0.01553270291821187,"pose":{"translation":{"x":4.1707075169415475,"y":-1.423878645628065},"rotation":{"radians":-0.6025861875457454}},"curvature":-0.07447678142898713},{"time":3.1063569465264984,"velocity":1.660606223561996,"acceleration":0.014910175261208886,"pose":{"translation":{"x":4.228854477405548,"y":-1.464104950428009},"rotation":{"radians":-0.6078072654168749}},"curvature":-0.07321767678680903},{"time":3.1491660792493343,"velocity":1.661244515233674,"acceleration":0.014306575336558616,"pose":{"translation":{"x":4.287117602461876,"y":-1.5048604753519612},"rotation":{"radians":-0.6129697872972361}},"curvature":-0.072003665099477},{"time":3.1921939657825407,"velocity":1.661860096933934,"acceleration":0.013721397617188226,"pose":{"translation":{"x":4.3454894024762325,"y":-1.546139938000124},"rotation":{"radians":-0.6180754385183802}},"curvature":-0.07083373048133693},{"time":3.235436529987009,"velocity":1.6624534453513704,"acceleration":0.013154116305517162,"pose":{"translation":{"x":4.403962288060939,"y":-1.5879379003399663},"rotation":{"radians":-0.6231258958362255}},"curvature":-0.06970687104165842},{"time":3.2788895776630684,"velocity":1.6630250317943305,"acceleration":0.012604189819139052,"pose":{"translation":{"x":4.462528573349118,"y":-1.6302487719804049},"rotation":{"radians":-0.6281228263232281}},"curvature":-0.06862210132615378},{"time":3.322548799401602,"velocity":1.6635753209124788,"acceleration":0.012071064749408054,"pose":{"translation":{"x":4.521180479268878,"y":-1.6730668134459847},"rotation":{"radians":-0.6330678864169678}},"curvature":-0.06757845444226444},{"time":3.3664097734705654,"velocity":1.6641047695704374,"acceleration":0.011554179339399952,"pose":{"translation":{"x":4.579910136817489,"y":-1.7163861394510604},"rotation":{"radians":-0.6379627211132425}},"curvature":-0.066574983898393},{"time":3.4104679687335056,"velocity":1.6646138258598757,"acceleration":0.01105296652453745,"pose":{"translation":{"x":4.638709590335566,"y":-1.7602007221739768},"rotation":{"radians":-0.6428089632925209}},"curvature":-0.06561076518502294},{"time":3.4547187475972008,"velocity":1.6651029282373409,"acceleration":0.0105668565762716,"pose":{"translation":{"x":4.69757080078125,"y":-1.80450439453125},"rotation":{"radians":-0.647608233169335}},"curvature":-0.06468489712354483},{"time":3.499157368985429,"velocity":1.6655725047759975,"acceleration":0.010095279386269354,"pose":{"translation":{"x":4.756485649004389,"y":-1.8492908534517483},"rotation":{"radians":-0.6523621378548796}},"curvature":-0.06379650300659852},{"time":3.5437789913365902,"velocity":1.666022972520301,"acceleration":0.009637666425594585,"pose":{"translation":{"x":4.815445939020719,"y":-1.894553663150873},"rotation":{"radians":-0.6570722710237484}},"curvature":-0.06294473155184836},{"time":3.588578675622476,"velocity":1.6664547369334204,"acceleration":0.009193452410798338,"pose":{"translation":{"x":4.874443401286044,"y":-1.9402862584047398},"rotation":{"radians":-0.6617402126763631}},"curvature":-0.062128757689329704},{"time":3.633551388386291,"velocity":1.666868191427999,"acceleration":0.008762076706344615,"pose":{"translation":{"x":4.933469695970416,"y":-1.986481947824359},"rotation":{"radians":-0.6663675289892359}},"curvature":-0.06134778320084009},{"time":3.678692004797843,"velocity":1.6672637169715687,"acceleration":0.008342984489951926,"pose":{"translation":{"x":4.99251641623232,"y":-2.033133917129817},"rotation":{"radians":-0.6709557722457755}},"curvature":-0.060601037228293915},{"time":3.723995311723959,"velocity":1.6676416817585968,"acceleration":0.007935627705738952,"pose":{"translation":{"x":5.05157509149285,"y":-2.0802352324244566},"rotation":{"radians":-0.6755064808408625}},"curvature":-0.05988777666651089},{"time":3.7694560108127284,"velocity":1.668002440941808,"acceleration":0.007539465827406847,"pose":{"translation":{"x":5.110637190709895,"y":-2.127778843469059},"rotation":{"radians":-0.6800211793529247}},"curvature":-0.059207286454563475},{"time":3.8150687215898125,"velocity":1.6683463364160072,"acceleration":0.0071539664518024465,"pose":{"translation":{"x":5.169694125652313,"y":-2.175757586956024},"rotation":{"radians":-0.6845013786777033}},"curvature":-0.058558879778560675},{"time":3.8608279845655145,"velocity":1.6686736966481945,"acceleration":0.006778605741540854,"pose":{"translation":{"x":5.228737254174121,"y":-2.2241641897835507},"rotation":{"radians":-0.6889485762183403}},"curvature":-0.057941898197592274},{"time":3.9067282643515098,"velocity":1.6689848365482902,"acceleration":0.006412868733427135,"pose":{"translation":{"x":5.287757883488666,"y":-2.2729912723298185},"rotation":{"radians":-0.6933642561268223}},"curvature":-0.05735571170349064},{"time":3.9527639527857867,"velocity":1.6692800573752722,"acceleration":0.006056249527731615,"pose":{"translation":{"x":5.346747273442816,"y":-2.3222313517271687},"rotation":{"radians":-0.697749889592203}},"curvature":-0.05679971872408576},{"time":3.9989293720651164,"velocity":1.6695596466739802,"acceleration":0.005708251372146678,"pose":{"translation":{"x":5.405696639791131,"y":-2.371876845136285},"rotation":{"radians":-0.702106935171379}},"curvature":-0.056273346078724217},{"time":4.045218777883857,"velocity":1.6698238782382608,"acceleration":0.005368386652712271,"pose":{"translation":{"x":5.4645971574700525,"y":-2.421920073020374},"rotation":{"radians":-0.7064368391585247}},"curvature":-0.055776048893992014},{"time":4.091626362578495,"velocity":1.6700730120965201,"acceleration":0.005036176802774174,"pose":{"translation":{"x":5.523439963872079,"y":-2.4723532624193467},"rotation":{"radians":-0.7107410359896046}},"curvature":-0.05530731048681846},{"time":4.138146258276857,"velocity":1.6703072945161037,"acceleration":0.004711152139866201,"pose":{"translation":{"x":5.582216162119948,"y":-2.5231685502239998},"rotation":{"radians":-0.7150209486786628}},"curvature":-0.05486664222143903},{"time":4.184772540051715,"velocity":1.6705269580232613,"acceleration":0.0043928516394809835,"pose":{"translation":{"x":5.64091682434082,"y":-2.5743579864501953},"rotation":{"radians":-0.7192779892828582}},"curvature":-0.05445358334605495},{"time":4.231499229077968,"velocity":1.6707322214357578,"acceleration":0.004080822653578123,"pose":{"translation":{"x":5.699532994940455,"y":-2.6259135375130427},"rotation":{"radians":-0.7235135593934571}},"curvature":-0.05406770081444218},{"time":4.278320295792755,"velocity":1.6709232899054722,"acceleration":0.003774620580993549,"pose":{"translation":{"x":5.758055693877395,"y":-2.677827089501079},"rotation":{"radians":-0.7277290506502201}},"curvature":-0.053708589097225895},{"time":4.3252296630584075,"velocity":1.6711003549685945,"acceleration":0.003473808495880602,"pose":{"translation":{"x":5.816475919937147,"y":-2.730090451450451},"rotation":{"radians":-0.7319258452768336}},"curvature":-0.05337586998704912},{"time":4.372221209327352,"velocity":1.6712635946012582,"acceleration":0.0031779567398719836,"pose":{"translation":{"x":5.874784654006362,"y":-2.782695358619094},"rotation":{"radians":-0.7361053166352199}},"curvature":-0.05306919240141616},{"time":4.419288771809292,"velocity":1.671413173278677,"acceleration":0.002886642483009199,"pose":{"translation":{"x":5.932972862347015,"y":-2.8356334757609147},"rotation":{"radians":-0.7402688297967409}},"curvature":-0.05278823218658412},{"time":4.466426149639281,"velocity":1.6715492420360587,"acceleration":0.0025994492574276926,"pose":{"translation":{"x":5.991031499870587,"y":-2.8888964003999718},"rotation":{"radians":-0.744417742128466}},"curvature":-0.052532691925503534},{"time":4.513627107047846,"velocity":1.6716719385297443,"acceleration":0.0023159664681472866,"pose":{"translation":{"x":6.0489515134122485,"y":-2.942475666104656},"rotation":{"radians":-0.7485534038928245}},"curvature":-0.052302300752467605},{"time":4.560885376531426,"velocity":1.671781387097211,"acceleration":0.002035788883939062,"pose":{"translation":{"x":6.106723845005035,"y":-2.9963627457618713},"rotation":{"radians":-0.7526771588590875}},"curvature":-0.052096814176818704},{"time":4.608194662024063,"velocity":1.671877698814724,"acceleration":0.0017585161115803924,"pose":{"translation":{"x":6.164339435154034,"y":-3.050549054851217},"rotation":{"radians":-0.7567903449252481}},"curvature":-0.05191601391777436},{"time":4.655548642070016,"velocity":1.6719609715515822,"acceleration":0.0014837520559655183,"pose":{"translation":{"x":6.221789226110559,"y":-3.105025954719167},"rotation":{"radians":-0.7608942947489714}},"curvature":-0.05175970775217297},{"time":4.70294097299591,"velocity":1.6720312900200305,"acceleration":0.0012111043684684662,"pose":{"translation":{"x":6.2790641651463375,"y":-3.159784755853252},"rotation":{"radians":-0.7649903363863789}},"curvature":-0.05162772937669655},{"time":4.750365292084858,"velocity":1.6720887258200507,"acceleration":9.401838856545614E-4,"pose":{"translation":{"x":6.336155207827687,"y":-3.2148167211562395},"rotation":{"radians":-0.7690797939375139}},"curvature":-0.05151993828590486},{"time":4.797815220749009,"velocity":1.6721333374783562,"acceleration":6.706040601600467E-4,"pose":{"translation":{"x":6.393053321289699,"y":-3.270113069220315},"rotation":{"radians":-0.7731639881974063}},"curvature":-0.05143621966720566},{"time":4.845284367704559,"velocity":1.672165170481037,"acceleration":4.0198038547065336E-4,"pose":{"translation":{"x":6.449749487510417,"y":-3.3256649776012637},"rotation":{"radians":-0.7772442373117127}},"curvature":-0.05137648431369161},{"time":4.892766332144535,"velocity":1.6721842572994055,"acceleration":1.339298160045948E-4,"pose":{"translation":{"x":6.506234706585019,"y":-3.38146358609265},"rotation":{"radians":-0.7813218574359585}},"curvature":-0.05134066855558977},{"time":4.940254706913932,"velocity":1.6721906174087007,"acceleration":-1.339298160045948E-4,"pose":{"translation":{"x":6.5625,"y":-3.4375},"rotation":{"radians":-0.7853981633974483}},"curvature":-0.05132873421089494},{"time":4.9877430816833295,"velocity":1.6721842572994055,"acceleration":-4.0198038547065336E-4,"pose":{"translation":{"x":6.61853641390735,"y":-3.493765293414981},"rotation":{"radians":-0.7894744693589382}},"curvature":-0.05134066855558977},{"time":5.035225046123306,"velocity":1.672165170481037,"acceleration":-6.706040601600467E-4,"pose":{"translation":{"x":6.674335022398736,"y":-3.5502505124895833},"rotation":{"radians":-0.793552089483184}},"curvature":-0.05137648431369161},{"time":5.082694193078855,"velocity":1.6721333374783562,"acceleration":-9.401838856545614E-4,"pose":{"translation":{"x":6.729886930779685,"y":-3.606946678710301},"rotation":{"radians":-0.7976323385974903}},"curvature":-0.05143621966720566},{"time":5.130144121743006,"velocity":1.6720887258200507,"acceleration":-0.0012111043684684662,"pose":{"translation":{"x":6.7851832788437605,"y":-3.6638447921723127},"rotation":{"radians":-0.8017165328573828}},"curvature":-0.05151993828590486},{"time":5.177568440831954,"velocity":1.6720312900200305,"acceleration":-0.0014837520559655183,"pose":{"translation":{"x":6.840215244146748,"y":-3.7209358348536625},"rotation":{"radians":-0.8058059904085177}},"curvature":-0.05162772937669655},{"time":5.224960771757848,"velocity":1.6719609715515822,"acceleration":-0.0017585161115803924,"pose":{"translation":{"x":6.894974045280833,"y":-3.778210773889441},"rotation":{"radians":-0.8099020320459251}},"curvature":-0.05175970775217297},{"time":5.272314751803801,"velocity":1.671877698814724,"acceleration":-0.002035788883939062,"pose":{"translation":{"x":6.949450945148783,"y":-3.8356605648459663},"rotation":{"radians":-0.8140059818696486}},"curvature":-0.05191601391777436},{"time":5.319624037296438,"velocity":1.671781387097211,"acceleration":-0.0023159664681472606,"pose":{"translation":{"x":7.003637254238129,"y":-3.8932761549949646},"rotation":{"radians":-0.8181191679358092}},"curvature":-0.052096814176818704},{"time":5.3668823067800195,"velocity":1.6716719385297443,"acceleration":-0.0025994492574276636,"pose":{"translation":{"x":7.057524333895344,"y":-3.9510484865877515},"rotation":{"radians":-0.8222429229020721}},"curvature":-0.052302300752467605},{"time":5.4140832641885845,"velocity":1.6715492420360587,"acceleration":-0.0028866424830091665,"pose":{"translation":{"x":7.111103599600028,"y":-4.008968500129413},"rotation":{"radians":-0.8263785846664307}},"curvature":-0.052532691925503534},{"time":5.461220642018574,"velocity":1.671413173278677,"acceleration":-0.0031779567398720196,"pose":{"translation":{"x":7.164366524239085,"y":-4.067027137652985},"rotation":{"radians":-0.8305274969981558}},"curvature":-0.05278823218658412},{"time":5.508288204500514,"velocity":1.6712635946012582,"acceleration":-0.0034738084958805624,"pose":{"translation":{"x":7.217304641380906,"y":-4.125215345993638},"rotation":{"radians":-0.8346910101596766}},"curvature":-0.05306919240141616},{"time":5.555279750769459,"velocity":1.6711003549685945,"acceleration":-0.0037746205809935916,"pose":{"translation":{"x":7.269909548549549,"y":-4.183524080062853},"rotation":{"radians":-0.838870481518063}},"curvature":-0.05337586998704912},{"time":5.602189118035111,"velocity":1.6709232899054722,"acceleration":-0.004080822653578123,"pose":{"translation":{"x":7.322172910498921,"y":-4.241944306122605},"rotation":{"radians":-0.8430672761446766}},"curvature":-0.053708589097225895},{"time":5.649010184749898,"velocity":1.6707322214357578,"acceleration":-0.004392851639480933,"pose":{"translation":{"x":7.374086462486957,"y":-4.300467005059545},"rotation":{"radians":-0.8472827674014396}},"curvature":-0.05406770081444218},{"time":5.695736873776151,"velocity":1.6705269580232613,"acceleration":-0.0047111521398662546,"pose":{"translation":{"x":7.425642013549805,"y":-4.35908317565918},"rotation":{"radians":-0.8515183375120384}},"curvature":-0.05445358334605495},{"time":5.7423631555510095,"velocity":1.6703072945161037,"acceleration":-0.005036176802774117,"pose":{"translation":{"x":7.476831449776,"y":-4.417783837880052},"rotation":{"radians":-0.8557753781162338}},"curvature":-0.05486664222143903},{"time":5.788883051249372,"velocity":1.6700730120965201,"acceleration":-0.005368386652712331,"pose":{"translation":{"x":7.527646737580653,"y":-4.476560036127921},"rotation":{"radians":-0.8600552908052921}},"curvature":-0.05530731048681846},{"time":5.83529063594401,"velocity":1.6698238782382608,"acceleration":-0.005708251372146744,"pose":{"translation":{"x":7.578079926979626,"y":-4.5354028425299475},"rotation":{"radians":-0.8643594876363718}},"curvature":-0.055776048893992014},{"time":5.88158004176275,"velocity":1.6695596466739802,"acceleration":-0.006056249527731615,"pose":{"translation":{"x":7.628123154863715,"y":-4.594303360208869},"rotation":{"radians":-0.8686893916235175}},"curvature":-0.056273346078724217},{"time":5.92774546104208,"velocity":1.6692800573752722,"acceleration":-0.006412868733427062,"pose":{"translation":{"x":7.677768648272831,"y":-4.653252726557184},"rotation":{"radians":-0.8730464372026936}},"curvature":-0.05679971872408576},{"time":5.973781149476357,"velocity":1.6689848365482902,"acceleration":-0.006778605741540775,"pose":{"translation":{"x":7.7270087276701815,"y":-4.712242116511334},"rotation":{"radians":-0.8774320706680744}},"curvature":-0.05735571170349064},{"time":6.019681429262353,"velocity":1.6686736966481945,"acceleration":-0.007153966451802529,"pose":{"translation":{"x":7.775835810216449,"y":-4.771262745825879},"rotation":{"radians":-0.8818477505765563}},"curvature":-0.057941898197592274},{"time":6.065440692238055,"velocity":1.6683463364160072,"acceleration":-0.007539465827406847,"pose":{"translation":{"x":7.824242413043976,"y":-4.830305874347687},"rotation":{"radians":-0.8862949481171933}},"curvature":-0.058558879778560675},{"time":6.1110534030151396,"velocity":1.668002440941808,"acceleration":-0.007935627705738952,"pose":{"translation":{"x":7.872221156530941,"y":-4.889362809290105},"rotation":{"radians":-0.890775147441972}},"curvature":-0.059207286454563475},{"time":6.156514102103909,"velocity":1.6676416817585968,"acceleration":-0.008342984489951926,"pose":{"translation":{"x":7.919764767575543,"y":-4.94842490850715},"rotation":{"radians":-0.895289845954034}},"curvature":-0.05988777666651089},{"time":6.201817409030025,"velocity":1.6672637169715687,"acceleration":-0.008762076706344615,"pose":{"translation":{"x":7.966866082870183,"y":-5.00748358376768},"rotation":{"radians":-0.899840554549121}},"curvature":-0.060601037228293915},{"time":6.246958025441577,"velocity":1.666868191427999,"acceleration":-0.009193452410798338,"pose":{"translation":{"x":8.013518052175641,"y":-5.066530304029584},"rotation":{"radians":-0.9044287978056607}},"curvature":-0.06134778320084009},{"time":6.291930738205392,"velocity":1.6664547369334204,"acceleration":-0.0096376664255947,"pose":{"translation":{"x":8.05971374159526,"y":-5.125556598713956},"rotation":{"radians":-0.9090561141185335}},"curvature":-0.062128757689329704},{"time":6.336730422491277,"velocity":1.666022972520301,"acceleration":-0.010095279386269354,"pose":{"translation":{"x":8.105446336849127,"y":-5.184554060979281},"rotation":{"radians":-0.9137240557711481}},"curvature":-0.06294473155184836},{"time":6.381352044842438,"velocity":1.6655725047759975,"acceleration":-0.010566856576271473,"pose":{"translation":{"x":8.150709146548252,"y":-5.243514350995611},"rotation":{"radians":-0.918434188940017}},"curvature":-0.06379650300659852},{"time":6.425790666230667,"velocity":1.6651029282373409,"acceleration":-0.01105296652453745,"pose":{"translation":{"x":8.19549560546875,"y":-5.30242919921875},"rotation":{"radians":-0.9231880936255615}},"curvature":-0.06468489712354483},{"time":6.470041445094362,"velocity":1.6646138258598757,"acceleration":-0.011554179339400093,"pose":{"translation":{"x":8.239799277826023,"y":-5.361290409664434},"rotation":{"radians":-0.9279873635023757}},"curvature":-0.06561076518502294},{"time":6.514099640357302,"velocity":1.6641047695704374,"acceleration":-0.012071064749407907,"pose":{"translation":{"x":8.28361386054894,"y":-5.420089863182511},"rotation":{"radians":-0.9328336056816542}},"curvature":-0.066574983898393},{"time":6.557960614426266,"velocity":1.6635753209124788,"acceleration":-0.012604189819139052,"pose":{"translation":{"x":8.326933186554015,"y":-5.478819520731122},"rotation":{"radians":-0.9377284403779288}},"curvature":-0.06757845444226444},{"time":6.6016198361648,"velocity":1.6630250317943305,"acceleration":-0.013154116305517162,"pose":{"translation":{"x":8.369751228019595,"y":-5.537471426650882},"rotation":{"radians":-0.9426735004716684}},"curvature":-0.06862210132615378},{"time":6.64507288384086,"velocity":1.6624534453513704,"acceleration":-0.013721397617188056,"pose":{"translation":{"x":8.412062099660034,"y":-5.596037711939061},"rotation":{"radians":-0.9476704309586711}},"curvature":-0.06970687104165842},{"time":6.688315448045329,"velocity":1.661860096933934,"acceleration":-0.014306575336558616,"pose":{"translation":{"x":8.453860061999876,"y":-5.6545105975237675},"rotation":{"radians":-0.9527208882765165}},"curvature":-0.07083373048133693},{"time":6.731343334578535,"velocity":1.661244515233674,"acceleration":-0.014910175261208886,"pose":{"translation":{"x":8.495139524648039,"y":-5.712882397538124},"rotation":{"radians":-0.9578265394976606}},"curvature":-0.072003665099477},{"time":6.774152467301372,"velocity":1.660606223561996,"acceleration":-0.01553270291821187,"pose":{"translation":{"x":8.535895049571991,"y":-5.771145522594452},"rotation":{"radians":-0.9629890613780216}},"curvature":-0.07321767678680903},{"time":6.816738890947604,"velocity":1.65994474129515,"acceleration":-0.01617463850185506,"pose":{"translation":{"x":8.576121354371935,"y":-5.8292924830584525},"rotation":{"radians":-0.9682101392491511}},"curvature":-0.07447678142898713},{"time":6.859098773893441,"velocity":1.6592595855015202,"acceleration":-0.016836431182175933,"pose":{"translation":{"x":8.615813315554988,"y":-5.887315892323386},"rotation":{"radians":-0.9734914657412941}},"curvature":-0.07578200611631837},{"time":6.901228410880354,"velocity":1.65855027276766,"acceleration":-0.017518492728726003,"pose":{"translation":{"x":8.65496597180936,"y":-5.945208470084253},"rotation":{"radians":-0.9788347393238211}},"curvature":-0.07713438596977641},{"time":6.94312422568665,"velocity":1.6578163212406118,"acceleration":-0.018221190390968898,"pose":{"translation":{"x":8.693574527278543,"y":-6.002963045611978},"rotation":{"radians":-0.9842416626486661}},"curvature":-0.07853496054580741},{"time":6.984782773743145,"velocity":1.6570572529050631,"acceleration":-0.018944838974633928,"pose":{"translation":{"x":8.731634354835478,"y":-6.060572561027584},"rotation":{"radians":-0.9897139406815298}},"curvature":-0.07998476977982921},{"time":7.02620074468773,"velocity":1.6562725961148619,"acceleration":-0.019689692049893723,"pose":{"translation":{"x":8.76914099935675,"y":-6.118030074576382},"rotation":{"radians":-0.9952532786047192}},"curvature":-0.08148484942566686},{"time":7.067374964853552,"velocity":1.6554618883994023,"acceleration":-0.020455932226803017,"pose":{"translation":{"x":8.806090180996762,"y":-6.175328763902144},"rotation":{"radians":-1.000861379474577}},"curvature":-0.08303622594547336},{"time":7.1083023996848365,"velocity":1.6546246795662767,"acceleration":-0.021243660430409737,"pose":{"translation":{"x":8.842477798461914,"y":-6.232461929321289},"rotation":{"radians":-1.0065399416155383}},"curvature":-0.08463991080199534},{"time":7.14898015607432,"velocity":1.6537605351224676,"acceleration":-0.022052884108780306,"pose":{"translation":{"x":8.87829993228479,"y":-6.289422997097063},"rotation":{"radians":-1.012290655731927}},"curvature":-0.08629689410238194},{"time":7.189405484615549,"velocity":1.6528690400370885,"acceleration":-0.022883504305855687,"pose":{"translation":{"x":8.913552848098334,"y":-6.346205522713717},"rotation":{"radians":-1.0181152017176864}},"curvature":-0.08800813754015926},{"time":7.2295757817632005,"velocity":1.6519498028693427,"acceleration":-0.02373530153336793,"pose":{"translation":{"x":8.948232999910033,"y":-6.402803194150692},"rotation":{"radians":-1.0240152451433346}},"curvature":-0.08977456657954339},{"time":7.269488591893874,"velocity":1.651002460285847,"acceleration":-0.024607920377304814,"pose":{"translation":{"x":8.982337033376098,"y":-6.459209835156798},"rotation":{"radians":-1.029992433398557}},"curvature":-0.09159706182401153},{"time":7.30914160925967,"velocity":1.6500266819917897,"acceleration":-0.02550085277916446,"pose":{"translation":{"x":9.015861789075643,"y":-6.515419408524394},"rotation":{"radians":-1.0360483914680205}},"curvature":-0.09347644950906146},{"time":7.348532679826142,"velocity":1.6490221761004604,"acceleration":-0.026413419936608052,"pose":{"translation":{"x":9.04880430578487,"y":-6.571426019363571},"rotation":{"radians":-1.0421847173172012}},"curvature":-0.09541349105745087},{"time":7.387659802986039,"velocity":1.6479886949655267,"acceleration":-0.027344752776362502,"pose":{"translation":{"x":9.081161823751245,"y":-6.62722391837633},"rotation":{"radians":-1.0484029768643275}},"curvature":-0.09740887163401273},{"time":7.426521133139714,"velocity":1.6469260414999138,"acceleration":-0.028774763928721664,"pose":{"translation":{"x":9.112931787967682,"y":-6.682807505130768},"rotation":{"radians":-1.0547046985139157}},"curvature":-0.09946318763650631},{"time":7.503439851864208,"velocity":1.6447127235269168,"acceleration":-0.030734129097070226,"pose":{"translation":{"x":9.174699878494721,"y":-6.7933101041126065},"rotation":{"radians":-1.0675644181019284}},"curvature":-0.10375048466516042},{"time":7.579277170260685,"velocity":1.6423819295929438,"acceleration":-0.03274063656691302,"pose":{"translation":{"x":9.23409235663712,"y":-6.902892114594579},"rotation":{"radians":-1.0807751152839176}},"curvature":-0.10827782955645215},{"time":7.65402402487063,"velocity":1.6399346699916397,"acceleration":-0.03477414499532001,"pose":{"translation":{"x":9.291096487140749,"y":-7.01151442626724},"rotation":{"radians":-1.0943469964533814}},"curvature":-0.11304524771977777},{"time":7.727673927883027,"velocity":1.6373735575853954,"acceleration":-0.03680836577891133,"pose":{"translation":{"x":9.345703125,"y":-7.119140625},"rotation":{"radians":-1.108288968155105}},"curvature":-0.11804971934383038},{"time":7.8002229748759495,"velocity":1.6347031457267684,"acceleration":-0.0388098438502487,"pose":{"translation":{"x":9.397906820231583,"y":-7.2257370976149105},"rotation":{"radians":-1.1226083308251997}},"curvature":-0.12328446383071472},{"time":7.871669837774214,"velocity":1.6319303041340967,"acceleration":-0.04073689217796276,"pose":{"translation":{"x":9.44770592264831,"y":-7.331273136660457},"rotation":{"radians":-1.1373104305501878}},"curvature":-0.1287381289353431},{"time":7.9420157415310335,"velocity":1.6290646306375938,"acceleration":-0.04253851724936385,"pose":{"translation":{"x":9.49510268663289,"y":-7.435721045185346},"rotation":{"radians":-1.1523982666223025}},"curvature":-0.13439388534690774},{"time":8.011264423096636,"velocity":1.6261188944023197,"acceleration":-0.04415338673725898,"pose":{"translation":{"x":9.540103375911713,"y":-7.539056241512299},"rotation":{"radians":-1.1678720534738891}},"curvature":-0.1402284325466594},{"time":8.07942207136099,"velocity":1.6231095033994016,"acceleration":-0.04550890564454946,"pose":{"translation":{"x":9.582718368328642,"y":-7.641257364011835},"rotation":{"radians":-1.1837287366975124}},"curvature":-0.14621092848984082},{"time":8.14649724696692,"velocity":1.6200569855616598,"acceleration":-0.046520482400384325,"pose":{"translation":{"x":9.622962260618806,"y":-7.742306375876069},"rotation":{"radians":-1.1999614643463943}},"curvature":-0.15230186412129473},{"time":8.21250078119958,"velocity":1.6169864693090261,"acceleration":-0.04709108007037938,"pose":{"translation":{"x":9.660853973182384,"y":-7.842188669892494},"rotation":{"radians":-1.2165590165987263}},"curvature":-0.15845191395296837},{"time":8.277445653582504,"velocity":1.6139281451234813,"acceleration":-0.04711115797595239,"pose":{"translation":{"x":9.696416854858398,"y":-7.940893173217773},"rotation":{"radians":-1.2335051991783763}},"curvature":-0.1646008057287766},{"time":8.341346848359992,"velocity":1.6109176858414669,"acceleration":-0.046459113155986775,"pose":{"translation":{"x":9.729678787698504,"y":-8.038412452151533},"rotation":{"radians":-1.2507782086458072}},"curvature":-0.17067626514133666},{"time":8.404221190725853,"velocity":1.607996599654883,"acceleration":-0.045002326687760316,"pose":{"translation":{"x":9.760672291740775,"y":-8.134742816910148},"rotation":{"radians":-1.2683499807617513}},"curvature":-0.17659310488419677},{"time":8.466087164466984,"velocity":1.6052124868937283,"acceleration":-0.04259890463865741,"pose":{"translation":{"x":9.7894346297835,"y":-8.229884426400531},"rotation":{"radians":-1.2861855364899468}},"curvature":-0.18225253988851078},{"time":8.526964713614436,"velocity":1.6026191699829608,"acceleration":-0.039100175903306514,"pose":{"translation":{"x":9.816007912158966,"y":-8.323841392993927},"rotation":{"radians":-1.3042423436959334}},"curvature":-0.18754182089294194},{"time":8.586875031710287,"velocity":1.60027666600699,"acceleration":-0.03435396942968964,"pose":{"translation":{"x":9.840439201507252,"y":-8.416621887299698},"rotation":{"radians":-1.3224697160084113}},"curvature":-0.19233428470069036},{"time":8.645840343368947,"velocity":1.5982509734928563,"acceleration":-0.028208643483979248,"pose":{"translation":{"x":9.862780617550015,"y":-8.508238242939115},"rotation":{"radians":-1.3408082733730198}},"curvature":-0.1964899195916967},{"time":8.703883683887593,"velocity":1.5966136495935466,"acceleration":-0.020517784327446734,"pose":{"translation":{"x":9.883089441864286,"y":-8.598707061319146},"rotation":{"radians":-1.359189491236203}},"curvature":-0.199856536473444},{"time":8.761028683684579,"velocity":1.59544116081232,"acceleration":-0.011145438289388403,"pose":{"translation":{"x":9.90142822265625,"y":-8.68804931640625},"rotation":{"radians":-1.377535366720458}},"curvature":-0.20227161900745516},{"time":8.817299365255126,"velocity":1.5948139994033736,"acceleration":2.8300772275892165E-5,"pose":{"translation":{"x":9.917864879535045,"y":-8.77629045950016},"rotation":{"radians":-1.3957582302808835}},"curvature":-0.20356489854681434},{"time":8.87271996107069,"velocity":1.5948155678490352,"acceleration":0.013101546569886274,"pose":{"translation":{"x":9.932472808286548,"y":-8.863460524007678},"rotation":{"radians":-1.4137607299223527}},"curvature":-0.20356166296129774},{"time":8.927314761373346,"velocity":1.595530844167674,"acceleration":0.028144903720509915,"pose":{"translation":{"x":9.94533098564716,"y":-8.949594230216462},"rotation":{"radians":-1.4314360119831262}},"curvature":-0.20208676461993152},{"time":8.981108001074125,"velocity":1.5970448497198668,"acceleration":0.04519591291483318,"pose":{"translation":{"x":9.956524074077606,"y":-9.034731090068817},"rotation":{"radians":-1.4486681178031746}},"curvature":-0.19896924595594245},{"time":9.034123795025854,"velocity":1.59944094692642,"acceleration":0.06425369682292277,"pose":{"translation":{"x":9.966142526536714,"y":-9.118915511935484},"rotation":{"radians":-1.4653326095528878}},"curvature":-0.19404745661562758},{"time":9.08638613073903,"velocity":1.602798995200592,"acceleration":0.08527371232841367,"pose":{"translation":{"x":9.974282691255212,"y":-9.202196905389428},"rotation":{"radians":-1.481297431581275}},"curvature":-0.18717450044020895},{"time":9.137918927310837,"velocity":1.607193388070935,"acceleration":0.10816235523708011,"pose":{"translation":{"x":9.981046916509513,"y":-9.284629785979632},"rotation":{"radians":-1.4964240065300516}},"curvature":-0.1782238294413945},{"time":9.188746168958772,"velocity":1.61269098223778,"acceleration":0.13277100016843377,"pose":{"translation":{"x":9.986543655395508,"y":-9.366273880004883},"rotation":{"radians":-1.5105685589576776}},"curvature":-0.1670947999411318},{"time":9.238892121209684,"velocity":1.619348910472532,"acceleration":-0.43722089856853985,"pose":{"translation":{"x":9.990887570602354,"y":-9.447194229287561},"rotation":{"radians":-1.523583654151162}},"curvature":-0.15371802470468285},{"time":9.288838569594802,"velocity":1.5975112794292834,"acceleration":-2.699999999999996,"pose":{"translation":{"x":9.994199639186263,"y":-9.527461295947433},"rotation":{"radians":-1.5353199368767214}},"curvature":-0.13806039182086433},{"time":9.341048578185877,"velocity":1.4565442562333788,"acceleration":-2.700000000000001,"pose":{"translation":{"x":9.996607257344294,"y":-9.607151067175437},"rotation":{"radians":-1.5456280544596928}},"curvature":-0.12012966946706},{"time":9.398489528751265,"velocity":1.3014536897068296,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":9.99824434518814,"y":-9.686345160007477},"rotation":{"radians":-1.554360750792786}},"curvature":-0.0999786645789153},{"time":9.463402023533055,"velocity":1.1261899537959958,"acceleration":-2.7,"pose":{"translation":{"x":9.999251451517921,"y":-9.765130926098209},"rotation":{"radians":-1.561375122117149}},"curvature":-0.07770893881369795},{"time":9.54014066644926,"velocity":0.9189956179222442,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":9.999775858595967,"y":-9.843601556494832},"rotation":{"radians":-1.5665350305744945}},"curvature":-0.05347409186319095},{"time":9.639917830097847,"velocity":0.6495972760710598,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":9.999971686920617,"y":-9.921856186410878},"rotation":{"radians":-1.5697136758781756}},"curvature":-0.02748258627930911},{"time":9.88050941382787,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":10.0,"y":-10.0},"rotation":{"radians":-1.5707963267948966}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0015d47..40f69d1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -169,8 +169,8 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the storage not - //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); - m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + //m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -359,7 +359,13 @@ public class RobotContainer { //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)); - return new ParallelRaceGroup(m_sixBallAutoMiddle, new RunIntake(m_robotIntake));//, new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)); + return new SequentialCommandGroup( + m_sixBallAuto0, + m_sixBallAuto1 + /**new ParallelRaceGroup( + m_sixBallAuto0, + new RunIntake(m_robotIntake) + )**/); //ParallelRaceGroup(m_sixBallAuto0, new RunIntake(m_robotIntake)); } catch (Exception e) { System.err.println("ERROR"); } From 12b6f19be4ddd22966afc586333990e37e08f924 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 9 Feb 2021 17:55:35 -0700 Subject: [PATCH 8/9] Added a reset odometry in between sequential commands --- .../java/frc4388/robot/RobotContainer.java | 26 ++++++++++--------- .../frc4388/robot/commands/auto/Wait.java | 1 + 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 40f69d1..cb582c9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -281,7 +281,6 @@ public class RobotContainer { } public void buildAutos() { - System.out.println("BUILDING AUTOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOS"); String[] sixBallAutoMiddlePaths = new String[]{ "SixBallMidComplete" }; @@ -353,19 +352,22 @@ 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_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)); - return new SequentialCommandGroup( + // 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_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)); + return new SequentialCommandGroup( m_sixBallAuto0, + new InstantCommand(() -> resetOdometry(new Pose2d())), + //new Wait(m_robotDrive, 0.5, 0), m_sixBallAuto1 - /**new ParallelRaceGroup( - m_sixBallAuto0, - new RunIntake(m_robotIntake) - )**/); //ParallelRaceGroup(m_sixBallAuto0, new RunIntake(m_robotIntake)); + // /**new ParallelRaceGroup( + // m_sixBallAuto0, + // new RunIntake(m_robotIntake) + // )**/); //ParallelRaceGroup(m_sixBallAuto0, new RunIntake(m_robotIntake)); + ); } catch (Exception e) { System.err.println("ERROR"); } diff --git a/src/main/java/frc4388/robot/commands/auto/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java index 6bbedf3..2b5a4e0 100644 --- a/src/main/java/frc4388/robot/commands/auto/Wait.java +++ b/src/main/java/frc4388/robot/commands/auto/Wait.java @@ -8,6 +8,7 @@ package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Wait extends CommandBase { From c5a34f37083b38377b86e226f16353e3a0a63a4a Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 10 Feb 2021 17:44:09 -0700 Subject: [PATCH 9/9] Tried fixing Path sequencing Issue Never got to test, problem with robot not enabling. --- src/main/java/frc4388/robot/RobotContainer.java | 3 +-- .../robot/commands/auto/SixBallAutoMiddle.java | 11 ++++++----- .../robot/commands/intake/RunExtenderOutIn.java | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cb582c9..ca94ecd 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -360,8 +360,7 @@ public class RobotContainer { // return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); return new SequentialCommandGroup( m_sixBallAuto0, - new InstantCommand(() -> resetOdometry(new Pose2d())), - //new Wait(m_robotDrive, 0.5, 0), + new InstantCommand(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d()))), m_sixBallAuto1 // /**new ParallelRaceGroup( // m_sixBallAuto0, diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index 17b544c..23097c8 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -16,10 +16,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.RobotContainer; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.commands.intake.RunIntake; import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; +import frc4388.robot.RobotContainer; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -31,10 +31,11 @@ public class SixBallAutoMiddle extends SequentialCommandGroup { public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); - Intake m_intake = new Intake(); + RobotContainer m_robotContainer = new RobotContainer(); addCommands( - paths[0] + paths[0], + new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d(0, 0, new Rotation2d()))) ); } } diff --git a/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java index c222ad6..57e1db9 100644 --- a/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java @@ -18,8 +18,8 @@ import frc4388.robot.subsystems.Intake; public class RunExtenderOutIn extends CommandBase { private Intake m_intake; - CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); - CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); + //CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); + //CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); CANDigitalInput m_extenderForwardLimit; CANDigitalInput m_extenderReverseLimit;