From 05c0ef4047cbb46238a3653729e7d539916cffd6 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 12:41:37 -0700 Subject: [PATCH] Created 6 Ball Auto with PathWeaver Co-Authored-By: Keenan D. Buckley Co-Authored-By: kyrarivera --- PathWeaver/Groups/FirstPathGroup | 2 + PathWeaver/Paths/FirstPath0 | 3 + PathWeaver/Paths/FirstPath1 | 3 + PathWeaver/output/FirstPath.wpilib.json | 1 + PathWeaver/pathweaver.json | 9 +++ src/main/deploy/paths/FirstPath0.wpilib.json | 1 + src/main/deploy/paths/FirstPath1.wpilib.json | 1 + src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 71 ++++++++++++++----- .../commands/auto/SixBallAutoMiddle.java | 35 +++++++++ .../java/frc4388/robot/subsystems/Drive.java | 17 ++--- 12 files changed, 122 insertions(+), 26 deletions(-) create mode 100644 PathWeaver/Groups/FirstPathGroup create mode 100644 PathWeaver/Paths/FirstPath0 create mode 100644 PathWeaver/Paths/FirstPath1 create mode 100644 PathWeaver/output/FirstPath.wpilib.json create mode 100644 PathWeaver/pathweaver.json create mode 100644 src/main/deploy/paths/FirstPath0.wpilib.json create mode 100644 src/main/deploy/paths/FirstPath1.wpilib.json create mode 100644 src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java diff --git a/PathWeaver/Groups/FirstPathGroup b/PathWeaver/Groups/FirstPathGroup new file mode 100644 index 0000000..58e567a --- /dev/null +++ b/PathWeaver/Groups/FirstPathGroup @@ -0,0 +1,2 @@ +FirstPath0 +FirstPath1 diff --git a/PathWeaver/Paths/FirstPath0 b/PathWeaver/Paths/FirstPath0 new file mode 100644 index 0000000..43523ef --- /dev/null +++ b/PathWeaver/Paths/FirstPath0 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +2.923987849862624,-2.381155324331042,3.134,0.0,true, +5.006107200045366,-0.7154598441848491,2.0,0.0,true, diff --git a/PathWeaver/Paths/FirstPath1 b/PathWeaver/Paths/FirstPath1 new file mode 100644 index 0000000..83f52cc --- /dev/null +++ b/PathWeaver/Paths/FirstPath1 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +5.006,-0.715,3.048,0.0,true, +7.635526493704714,-0.7154598441848491,1.0,0.0,true, diff --git a/PathWeaver/output/FirstPath.wpilib.json b/PathWeaver/output/FirstPath.wpilib.json new file mode 100644 index 0000000..758ca33 --- /dev/null +++ b/PathWeaver/output/FirstPath.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.3,"y":-2.416848798905604},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.211173470810985,"velocity":0.6335204124329551,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.366889570140447,"y":-2.4163605362457954},"rotation":{"radians":0.021427560503521688}},"curvature":0.6111933780057722},{"time":0.2999081850801547,"velocity":0.8997245552404642,"acceleration":2.9999999999999982,"pose":{"translation":{"x":3.4348387255942074,"y":-2.413127483495524},"rotation":{"radians":0.07833904937585699}},"curvature":1.0234441896058954},{"time":0.3699333515595466,"velocity":1.1098000546786397,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.5047142961833893,"y":-2.4048968129411943},"rotation":{"radians":0.15838369767185723}},"curvature":1.213822601378998},{"time":0.40145286132680885,"velocity":1.2043585839804265,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.5405910165378556,"y":-2.398342148588244},"rotation":{"radians":0.20314475328020926}},"curvature":1.2329656733386238},{"time":0.4315074877648594,"velocity":1.2945224632945782,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.577185748691339,"y":-2.389919658328446},"rotation":{"radians":0.24914964974226336}},"curvature":1.2110821004593237},{"time":0.46050401407792596,"velocity":1.3815120422337779,"acceleration":3.0,"pose":{"translation":{"x":3.6145536688933833,"y":-2.3794835720596605},"rotation":{"radians":0.29518340315729685}},"curvature":1.1575611604229465},{"time":0.4887377357194707,"velocity":1.4662132071584122,"acceleration":3.0000000000000044,"pose":{"translation":{"x":3.6527393941094592,"y":-2.366915117615063},"rotation":{"radians":0.3402532850169521}},"curvature":1.0820536553272881},{"time":0.5437179276413854,"velocity":1.6311537829241565,"acceleration":3.0,"pose":{"translation":{"x":3.7316925948840147,"y":-2.335034255723885},"rotation":{"radians":0.42468100827986427}},"curvature":0.8990051032472685},{"time":0.5975937989543301,"velocity":1.7927813968629904,"acceleration":2.999999999999998,"pose":{"translation":{"x":3.814207972162949,"y":-2.2938241072957206},"rotation":{"radians":0.4988354340363108}},"curvature":0.7135359234210717},{"time":0.6509282653574082,"velocity":1.9527847960722249,"acceleration":3.0000000000000053,"pose":{"translation":{"x":3.9003076130426964,"y":-2.2431916794422597},"rotation":{"radians":0.5616623820417155}},"curvature":0.5512407842873545},{"time":0.7039320341670622,"velocity":2.111796102501187,"acceleration":2.9999999999999942,"pose":{"translation":{"x":3.989887277814993,"y":-2.183367954498983},"rotation":{"radians":0.6135829743337091}},"curvature":0.4192974357240935},{"time":0.7565960373881516,"velocity":2.269788112164455,"acceleration":2.9999999999999973,"pose":{"translation":{"x":4.08273060721369,"y":-2.1148718927780776},"rotation":{"radians":0.6556696605154494}},"curvature":0.3156321132771837},{"time":0.8087781515227375,"velocity":2.4263344545682126,"acceleration":2.9999999999999964,"pose":{"translation":{"x":4.178523329661565,"y":-2.038474435321345},"rotation":{"radians":0.689150621724}},"curvature":0.23494375911219692},{"time":0.8346228721652991,"velocity":2.503868616495897,"acceleration":3.0,"pose":{"translation":{"x":4.227404226828149,"y":-1.997613644550836},"rotation":{"radians":0.703026083367203}},"curvature":0.20143603346697642},{"time":0.8602616455497234,"velocity":2.58078493664917,"acceleration":2.9999999999999933,"pose":{"translation":{"x":4.276867468517136,"y":-1.9551625066531164},"rotation":{"radians":0.7151623578596862}},"curvature":0.1716130584911985},{"time":0.8856616408535245,"velocity":2.6569849225605733,"acceleration":2.999999999999997,"pose":{"translation":{"x":4.326852559608481,"y":-1.9112722288145225},"rotation":{"radians":0.7256708125595506}},"curvature":0.14489219858770688},{"time":0.9107892607468286,"velocity":2.7323677822404857,"acceleration":2.9999999999999933,"pose":{"translation":{"x":4.377295549321474,"y":-1.8661030175331623},"rotation":{"radians":0.7346488446533966}},"curvature":0.12074979352801006},{"time":0.9356106857771136,"velocity":2.8068320573313406,"acceleration":3.0000000000000098,"pose":{"translation":{"x":4.4281294751912,"y":-1.8198229537049446},"rotation":{"radians":0.7421799396015097}},"curvature":0.0987167163871896},{"time":0.9600923145879835,"velocity":2.8802769437639504,"acceleration":2.614799069105123,"pose":{"translation":{"x":4.479284807045011,"y":-1.7726068677096063},"rotation":{"radians":0.7483339599437678}},"curvature":0.07837160771189397},{"time":0.9842391546760743,"velocity":2.9434160787481205,"acceleration":0.7091827420070252,"pose":{"translation":{"x":4.5306898909789775,"y":-1.7246352144967432},"rotation":{"radians":0.753167533852469}},"curvature":0.05933300885315723},{"time":1.008233944163923,"velocity":2.9604327693509944,"acceleration":0.6929390292542418,"pose":{"translation":{"x":4.582271393334359,"y":-1.6760929486718354},"rotation":{"radians":0.7567244542336702}},"curvature":0.04125109123041828},{"time":1.0322061707397836,"velocity":2.977044060763534,"acceleration":0.6902603500969828,"pose":{"translation":{"x":4.633954744674066,"y":-1.627168399582279},"rotation":{"radians":0.7590360284526021}},"curvature":0.023799333454241605},{"time":1.0560961165417506,"velocity":2.9935343431165977,"acceleration":-0.15360934586446268,"pose":{"translation":{"x":4.685664583759118,"y":-1.5780521464034132},"rotation":{"radians":0.7601213391661734}},"curvature":0.006666277661887551},{"time":1.079922908603498,"velocity":2.989874325173944,"acceleration":-0.7013269525645326,"pose":{"translation":{"x":4.737325201525113,"y":-1.5289358932245467},"rotation":{"radians":0.7599873909288635}},"curvature":-0.010452640946589037},{"time":1.1037564741134904,"velocity":2.973159203306074,"acceleration":-2.462603005612013,"pose":{"translation":{"x":4.7888609850586885,"y":-1.480011344134991},"rotation":{"radians":0.7586291273016804}},"curvature":-0.02786327923933318},{"time":1.127758375594418,"velocity":2.9140520485787382,"acceleration":-3.0,"pose":{"translation":{"x":4.840196861573979,"y":-1.4314690783100834},"rotation":{"radians":0.7560293108474947}},"curvature":-0.045881117204540606},{"time":1.1521061171128049,"velocity":2.841008824023578,"acceleration":-3.000000000000006,"pose":{"translation":{"x":4.891258742389087,"y":-1.3834974250972196},"rotation":{"radians":0.7521582651136997}},"curvature":-0.06483987946511054},{"time":1.176818513738355,"velocity":2.766871634146927,"acceleration":-3.0000000000000067,"pose":{"translation":{"x":4.941973966902539,"y":-1.3362813391018817},"rotation":{"radians":0.7469734848845259}},"curvature":-0.08510057564153622},{"time":1.201861468016625,"velocity":2.691742771312117,"acceleration":-3.0,"pose":{"translation":{"x":4.992271746569754,"y":-1.290001275273664},"rotation":{"radians":0.7404191301220512}},"curvature":-0.10706103507590262},{"time":1.2272000754281833,"velocity":2.6157269490774424,"acceleration":-3.0000000000000067,"pose":{"translation":{"x":5.042083608879501,"y":-1.2448320639923036},"rotation":{"radians":0.7324254318684337}},"curvature":-0.13116590516731727},{"time":1.2527989511809319,"velocity":2.5389303218191968,"acceleration":-2.9999999999999933,"pose":{"translation":{"x":5.091343841330369,"y":-1.2009417861537095},"rotation":{"radians":0.7229080571771316}},"curvature":-0.15791689397528239},{"time":1.2786226365851732,"velocity":2.461459265606473,"acceleration":-2.999999999999993,"pose":{"translation":{"x":5.139989935407221,"y":-1.1584906482559907},"rotation":{"radians":0.7117675077743372}},"curvature":-0.1878827213264359},{"time":1.3046361067471872,"velocity":2.383418855120431,"acceleration":-3.0,"pose":{"translation":{"x":5.187963030557664,"y":-1.1176298574854808},"rotation":{"radians":0.698888667392153}},"curvature":-0.2217077309734497},{"time":1.3570967645000946,"velocity":2.226036881861709,"acceleration":-3.000000000000004,"pose":{"translation":{"x":5.2816756855422335,"y":-1.0412324000287478},"rotation":{"radians":0.6673773445352108}},"curvature":-0.3039170857471932},{"time":1.4099383576443232,"velocity":2.0675121024290233,"acceleration":-2.999999999999998,"pose":{"translation":{"x":5.3721007522253466,"y":-0.9727363383078425},"rotation":{"radians":0.6271532154617565}},"curvature":-0.41122091053998006},{"time":1.4629853653237144,"velocity":1.90837107939085,"acceleration":-3.000000000000002,"pose":{"translation":{"x":5.458943958445762,"y":-0.9129126133645666},"rotation":{"radians":0.5768350522658812}},"curvature":-0.5506698994005365},{"time":1.5161869847789742,"velocity":1.7487662210250707,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":5.542012021186539,"y":-0.8622801855111053},"rotation":{"radians":0.5151039437421481}},"curvature":-0.7268715379569509},{"time":1.569704317704183,"velocity":1.5882142222494449,"acceleration":-3.0000000000000027,"pose":{"translation":{"x":5.62122685382185,"y":-0.82107003708294},"rotation":{"radians":0.44119370579756156}},"curvature":-0.9353726275622822},{"time":1.6240447272199892,"velocity":1.425192993702026,"acceleration":-3.0,"pose":{"translation":{"x":5.696639773363788,"y":-0.7891891751917628},"rotation":{"radians":0.355773742439762}},"curvature":-1.1512466772492118},{"time":1.65183493577584,"velocity":1.3418223680344739,"acceleration":-3.0,"pose":{"translation":{"x":5.732975432412914,"y":-0.7766207207471636},"rotation":{"radians":0.30965392012596843}},"curvature":-1.2454525068745395},{"time":1.6802952251020455,"velocity":1.2564415000558569,"acceleration":-3.0,"pose":{"translation":{"x":5.7684457077091915,"y":-0.7661846344783774},"rotation":{"radians":0.2621942336526418}},"curvature":-1.317284482724688},{"time":1.709712366919219,"velocity":1.1681900746043365,"acceleration":-2.999999999999997,"pose":{"translation":{"x":5.803099735316894,"y":-0.7577621442185807},"rotation":{"radians":0.21442500493297934}},"curvature":-1.3548895419721148},{"time":1.7404828856629597,"velocity":1.0758785183731143,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":5.8369974028864435,"y":-0.7512074798656316},"rotation":{"radians":0.16764067155308857}},"curvature":-1.3463080501042284},{"time":1.8086103280430197,"velocity":0.8714961912329341,"acceleration":-3.0000000000000004,"pose":{"translation":{"x":5.902819630302291,"y":-0.7429768093113003},"rotation":{"radians":0.08329065457988592}},"curvature":-1.152025179001855},{"time":1.8946607845548429,"velocity":0.6133448216974647,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":5.966623393988655,"y":-0.7397437565610252},"rotation":{"radians":0.022845873177659935}},"curvature":-0.6940883889283442},{"time":2.099109058453998,"velocity":0.0,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":6.029320137849446,"y":-0.7392554939012195},"rotation":{"radians":7.105427357601138E-15}},"curvature":2.019483917365906E-28}] \ No newline at end of file diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json new file mode 100644 index 0000000..cae9a01 --- /dev/null +++ b/PathWeaver/pathweaver.json @@ -0,0 +1,9 @@ +{ + "lengthUnit": "Meter", + "exportUnit": "Always Meters", + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "wheelBase": 0.648, + "gameName": "Infinite Recharge", + "outputDir": ".." +} \ No newline at end of file diff --git a/src/main/deploy/paths/FirstPath0.wpilib.json b/src/main/deploy/paths/FirstPath0.wpilib.json new file mode 100644 index 0000000..d4ab98b --- /dev/null +++ b/src/main/deploy/paths/FirstPath0.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":1.0,"pose":{"translation":{"x":2.923987849862624,"y":-2.381155324331042},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.44218437689184154,"velocity":0.44218437689184154,"acceleration":1.0000000000000002,"pose":{"translation":{"x":3.0217501594011638,"y":-2.3806705245269772},"rotation":{"radians":0.0146897503609294}},"curvature":0.29333528903824513},{"time":0.6238359459597442,"velocity":0.6238359459597442,"acceleration":0.9999999999999997,"pose":{"translation":{"x":3.1185190613423117,"y":-2.3774604012288347},"rotation":{"radians":0.055801104897449175}},"curvature":0.5518028905787875},{"time":0.7614104213728297,"velocity":0.7614104213728297,"acceleration":-0.020312238777485957,"pose":{"translation":{"x":3.2134552378254417,"y":-2.369288104224536},"rotation":{"radians":0.11962328720251568}},"curvature":0.7835455155676119},{"time":0.8845575709677203,"velocity":0.7589090270654915,"acceleration":-0.23670869805843414,"pose":{"translation":{"x":3.305878010330035,"y":-2.3544171705664882},"rotation":{"radians":0.2025244128102492}},"curvature":0.9804968904285762},{"time":0.9461205222432847,"velocity":0.7443365410204178,"acceleration":-0.1799104932363984,"pose":{"translation":{"x":3.3509749755370053,"y":-2.3440550990939353},"rotation":{"radians":0.24979596445375538}},"curvature":1.0601182481988838},{"time":1.0083957614112635,"velocity":0.733132572025292,"acceleration":-0.1225833359284373,"pose":{"translation":{"x":3.3952549352656796,"y":-2.3315757826241224},"rotation":{"radians":0.3000956802504631}},"curvature":1.1234869825536697},{"time":1.0712457417484191,"velocity":0.7254282117725268,"acceleration":-0.06551043105781508,"pose":{"translation":{"x":3.4386727888015693,"y":-2.316886980870757},"rotation":{"radians":0.35269632003371687}},"curvature":1.1681980064641713},{"time":1.134532145637512,"velocity":0.7212822921736534,"acceleration":-0.009888307104318211,"pose":{"translation":{"x":3.4811913995620687,"y":-2.2999210261364276},"rotation":{"radians":0.40682014847091763}},"curvature":1.1926534843629537},{"time":1.1981195784423337,"velocity":0.7206535201101041,"acceleration":0.04287858621452664,"pose":{"translation":{"x":3.5227812699586405,"y":-2.2806337063767472},"rotation":{"radians":0.4616677692247436}},"curvature":1.1963869868499444},{"time":1.261880918792892,"velocity":0.7233875162394793,"acceleration":0.09137033478935387,"pose":{"translation":{"x":3.5634202162590025,"y":-2.2590031482644917},"rotation":{"radians":0.5164531175977884}},"curvature":1.1802003969697121},{"time":1.325702955244089,"velocity":0.7292189570769635,"acceleration":0.1343245138273121,"pose":{"translation":{"x":3.603093043449319,"y":-2.2350287002537437},"rotation":{"radians":0.5704391899518312}},"curvature":1.146080956245348},{"time":1.3894909282971848,"velocity":0.7377872455453502,"acceleration":0.17077914994055526,"pose":{"translation":{"x":3.6417912200963847,"y":-2.208729815644034},"rotation":{"radians":0.6229690813832267}},"curvature":1.0969268305279805},{"time":1.4531709546778435,"velocity":0.7486624663188313,"acceleration":0.20017252997962934,"pose":{"translation":{"x":3.679512553209812,"y":-2.180144935644483},"rotation":{"radians":0.6734882066924126}},"curvature":1.0361587010764104},{"time":1.5166898957207804,"velocity":0.7613772134490229,"acceleration":0.2223882977051323,"pose":{"translation":{"x":3.716260863104222,"y":-2.149330372437944},"rotation":{"radians":0.7215557075396957}},"curvature":0.9673130071901889},{"time":1.580012832524956,"velocity":0.7754594935705932,"acceleration":0.2377416301716882,"pose":{"translation":{"x":3.7520456582614283,"y":-2.1163591922451417},"rotation":{"radians":0.7668452706153216}},"curvature":0.8936975562975177},{"time":1.6431187496388067,"velocity":0.7904623971787195,"acceleration":0.24894039216309485,"pose":{"translation":{"x":3.7868818101926265,"y":-2.081320098388818},"rotation":{"radians":0.8091372708920906}},"curvature":0.8181552957739961},{"time":1.7686011530089851,"velocity":0.8217000358832593,"acceleration":0.24906305234095572,"pose":{"translation":{"x":3.853792534741811,"y":-2.0054644668714947},"rotation":{"radians":0.8842979035064634}},"curvature":0.6697194927501393},{"time":1.893100387385028,"velocity":0.8527081952110687,"acceleration":0.23936369079091954,"pose":{"translation":{"x":3.917206914192091,"y":-1.9227434029455939},"rotation":{"radians":0.946833282093064}},"curvature":0.5331300183596569},{"time":2.0164165423541625,"velocity":0.8822256051986257,"acceleration":0.22786218090886912,"pose":{"translation":{"x":3.977403825633883,"y":-1.834315541408051},"rotation":{"radians":0.9972277324197186}},"curvature":0.4120275090416596},{"time":2.138166470714266,"velocity":0.9099678094002575,"acceleration":0.22097608388849158,"pose":{"translation":{"x":4.034716741397703,"y":-1.7414824848456552},"rotation":{"radians":1.0362305174205781}},"curvature":0.30537028739932087},{"time":2.2577476094083764,"velocity":0.9363923811358086,"acceleration":0.22412680138865984,"pose":{"translation":{"x":4.089523324644169,"y":-1.645653061687585},"rotation":{"radians":1.0645726139607177}},"curvature":0.20965549834045422},{"time":2.374341246522086,"velocity":0.9625241400843745,"acceleration":0.24325526789596533,"pose":{"translation":{"x":4.142235024953995,"y":-1.548307584257946},"rotation":{"radians":1.082797067361366}},"curvature":0.12016969703985506},{"time":2.4869389468596177,"velocity":0.9899141238444503,"acceleration":-0.08989432371689705,"pose":{"translation":{"x":4.193286673917996,"y":-1.4509621068283067},"rotation":{"radians":1.0911600272745865}},"curvature":0.03144641201074882},{"time":2.5966005204711453,"velocity":0.9800561708469113,"acceleration":-0.29645758100115066,"pose":{"translation":{"x":4.243126080727084,"y":-1.3551326836702366},"rotation":{"radians":1.0895668164055365}},"curvature":-0.06280765335835946},{"time":2.705539868235784,"velocity":0.9477602753327634,"acceleration":-0.3342144599476055,"pose":{"translation":{"x":4.292203627762266,"y":-1.2622996271078404},"rotation":{"radians":1.0775204885631444}},"curvature":-0.1701207808611152},{"time":2.814165712334998,"velocity":0.9114559475107917,"acceleration":-0.3895828982839913,"pose":{"translation":{"x":4.340961866184649,"y":-1.173871765570298},"rotation":{"radians":1.0540743696689703}},"curvature":-0.2998324969707626},{"time":2.922061882096109,"velocity":0.8694214449815167,"acceleration":-0.4558401339381142,"pose":{"translation":{"x":4.389825111525432,"y":-1.0911507016443966},"rotation":{"radians":1.0177978738880868}},"curvature":-0.4635499088098712},{"time":3.0291652313372652,"velocity":0.8205994399182074,"acceleration":-0.5175273987277358,"pose":{"translation":{"x":4.4391890392759095,"y":-1.0152950701270744},"rotation":{"radians":0.9667945512215511}},"curvature":-0.6747572633079081},{"time":3.135775528406369,"velocity":0.7654256901984429,"acceleration":-0.5484700400579469,"pose":{"translation":{"x":4.489410280477473,"y":-0.9472847960779487},"rotation":{"radians":0.8988641812251724}},"curvature":-0.945872071723171},{"time":3.2424654074278623,"velocity":0.706909487977747,"acceleration":-0.5348283872397173,"pose":{"translation":{"x":4.540796017311605,"y":-0.8878853528718582},"rotation":{"radians":0.8119795371767505}},"curvature":-1.2796551201137232},{"time":3.2960757092532202,"velocity":0.6782371767130563,"acceleration":-0.49969792815057806,"pose":{"translation":{"x":4.567005484389693,"y":-0.8615864682621477},"rotation":{"radians":0.7610796746282562}},"curvature":-1.464229870757777},{"time":3.3499292350563303,"velocity":0.6513266814456385,"acceleration":-0.4429951720848838,"pose":{"translation":{"x":4.59359357868988,"y":-0.8376120202514001},"rotation":{"radians":0.7053027710759296}},"curvature":-1.6522464815533484},{"time":3.4040244578100665,"velocity":0.6273627589328771,"acceleration":-0.3650514227347253,"pose":{"translation":{"x":4.620580157279175,"y":-0.8159814621391459},"rotation":{"radians":0.6449987874949148}},"curvature":-1.833253448963883},{"time":3.458291373425353,"velocity":0.6075525441800915,"acceleration":-0.2675424030750641,"pose":{"translation":{"x":4.6479800358439665,"y":-0.7966941423794647},"rotation":{"radians":0.5808072835890076}},"curvature":-1.9936672001360811},{"time":3.512577503974265,"velocity":0.593028702359389,"acceleration":-0.1528367269212617,"pose":{"translation":{"x":4.675802663552211,"y":-0.7797281876451341},"rotation":{"radians":0.5136887003097897}},"curvature":-2.1180834030120494},{"time":3.566644387560178,"velocity":0.5847652968372852,"acceleration":-0.02318286706797017,"pose":{"translation":{"x":4.704051797915623,"y":-0.7650393858917703},"rotation":{"radians":0.44491642185653585}},"curvature":-2.1916290124257998},{"time":3.620177917379769,"velocity":0.5835242361317984,"acceleration":0.12007141974380939,"pose":{"translation":{"x":4.732725179651859,"y":-0.7525600694219574},"rotation":{"radians":0.37601968000317076}},"curvature":-2.202854559744868},{"time":3.6728118750569414,"velocity":0.589844070156832,"acceleration":0.2771969926585082,"pose":{"translation":{"x":4.761814207546703,"y":-0.7421979979494044},"rotation":{"radians":0.3086811644403933}},"curvature":-2.1461830808554048},{"time":3.7241590649993936,"velocity":0.604077356790345,"acceleration":0.10247113166467756,"pose":{"translation":{"x":4.79130361331626,"y":-0.7338352416630802},"rotation":{"radians":0.24460958247996245}},"curvature":-2.022892354696512},{"time":3.7745471252102867,"velocity":0.6092406783425431,"acceleration":-1.0,"pose":{"translation":{"x":4.821171136469141,"y":-0.7273270642913574},"rotation":{"radians":0.18541805256103536}},"curvature":-1.8401738034507031},{"time":3.8270328664625684,"velocity":0.5567549370902614,"acceleration":-1.0000000000000013,"pose":{"translation":{"x":4.85138719916865,"y":-0.7225008061661495},"rotation":{"radians":0.13253790889217945}},"curvature":-1.6086657324414335},{"time":3.8852342566258984,"velocity":0.49855354692693143,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":4.881914581094971,"y":-0.7191547672870588},"rotation":{"radians":0.08718511374843187}},"curvature":-1.3394321525761819},{"time":4.030532016951451,"velocity":0.35325578660137885,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":4.943714258106309,"y":-0.7159446439889163},"rotation":{"radians":0.023010588392320376}},"curvature":-0.7193516920501322},{"time":4.3837878035528295,"velocity":0.0,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":5.00610720004536,"y":-0.7154598441848514},"rotation":{"radians":-3.552713678800552E-15}},"curvature":-3.5527136788007045E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/FirstPath1.wpilib.json b/src/main/deploy/paths/FirstPath1.wpilib.json new file mode 100644 index 0000000..357c1aa --- /dev/null +++ b/src/main/deploy/paths/FirstPath1.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":0.9999999999999999,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43672580581865883,"velocity":0.4367258058186588,"acceleration":0.9999999999999996,"pose":{"translation":{"x":5.101364714733885,"y":-0.715000133837411},"rotation":{"radians":-4.133406269459032E-6}},"curvature":-8.340395092369047E-5},{"time":0.6186417897382253,"velocity":0.6186417897382251,"acceleration":1.000000000000001,"pose":{"translation":{"x":5.197358832001073,"y":-0.7150010200477351},"rotation":{"radians":-1.5340737547402928E-5}},"curvature":-1.4656459700474635E-4},{"time":0.7595467927096973,"velocity":0.7595467927096974,"acceleration":1.0,"pose":{"translation":{"x":5.294455665127399,"y":-0.7150032761523467},"rotation":{"radians":-3.1829811880428774E-5}},"curvature":-1.8983396025576592E-4},{"time":0.8797386662062437,"velocity":0.8797386662062439,"acceleration":0.9999999999999994,"pose":{"translation":{"x":5.392970060293234,"y":-0.7150073815320199},"rotation":{"radians":-5.1940694850243696E-5}},"curvature":-2.1577333197717033E-4},{"time":0.9869867172733406,"velocity":0.9869867172733408,"acceleration":0.12636200956149105,"pose":{"translation":{"x":5.493071389722454,"y":-0.7150136872941142},"rotation":{"radians":-7.424773047402172E-5}},"curvature":-2.2788163887374496E-4},{"time":1.0893819317414866,"velocity":0.9999255823430155,"acceleration":1.6865081478813383E-5,"pose":{"translation":{"x":5.594796544871399,"y":-0.7150224261397599},"rotation":{"radians":-9.759499776855064E-5}},"curvature":-2.297012203219806E-4},{"time":1.19265591257446,"velocity":0.9999273240671169,"acceleration":3.1333529513589754E-5,"pose":{"translation":{"x":5.698062929617832,"y":-0.7150337222310442},"rotation":{"radians":-1.2108746283101436E-4}},"curvature":-2.243247378336802E-4},{"time":1.2972818696190205,"velocity":0.9999306023676299,"acceleration":4.001965252959674E-5,"pose":{"translation":{"x":5.802681453449902,"y":-0.7150476010581972},"rotation":{"radians":-1.440580917623151E-4}},"curvature":-2.1420508869061022E-4},{"time":1.4029770535527222,"velocity":0.999934832252165,"acceleration":4.492623868769467E-5,"pose":{"translation":{"x":5.908369524655109,"y":-0.715063999306777},"rotation":{"radians":-1.6602563430818663E-4}},"curvature":-2.0114813255280375E-4},{"time":1.5093782536804359,"velocity":0.9999396124578785,"acceleration":4.760827159068493E-5,"pose":{"translation":{"x":6.01476404350926,"y":-0.7150827747248557},"rotation":{"radians":-1.8665207023750203E-4}},"curvature":-1.863925586327476E-4},{"time":1.6160547787367876,"velocity":0.9999446911428558,"acceleration":4.9242040270331036E-5,"pose":{"translation":{"x":6.12143439546543,"y":-0.7151037159902053},"rotation":{"radians":-2.0570427731088185E-4}},"curvature":-1.7071579130564232E-4},{"time":1.7225214395294832,"velocity":0.999949933778454,"acceleration":5.0734682359676405E-5,"pose":{"translation":{"x":6.22789544434293,"y":-0.7151265525774831},"rotation":{"radians":-2.2302157264049555E-4}},"curvature":-1.5453311203111758E-4},{"time":1.8282515334495566,"velocity":0.9999552979611849,"acceleration":5.284205054641302E-5,"pose":{"translation":{"x":6.333620525516261,"y":-0.7151509646254184},"rotation":{"radians":-2.384891161487098E-4}},"curvature":-1.3797542338492307E-4},{"time":1.932689830603459,"velocity":0.9999608166949621,"acceleration":5.6287027733918696E-5,"pose":{"translation":{"x":6.438054439104082,"y":-0.7151765928039965},"rotation":{"radians":-2.52016346465048E-4}},"curvature":-1.2094086552319517E-4},{"time":2.0352655612061343,"velocity":0.9999665903779553,"acceleration":6.188490438000034E-5,"pose":{"translation":{"x":6.540626443158164,"y":-0.7152030481816463},"rotation":{"radians":-2.6351930547014786E-4}},"curvature":-1.0311956260767918E-4},{"time":2.135405403849388,"velocity":0.9999727875225419,"acceleration":7.069238955792907E-5,"pose":{"translation":{"x":6.640763246852358,"y":-0.7152299220924245},"rotation":{"radians":-2.729056617250116E-4}},"curvature":-8.399141357141933E-5},{"time":2.2325464742950283,"velocity":0.999979654656936,"acceleration":8.420632541569917E-5,"pose":{"translation":{"x":6.737902003671551,"y":-0.7152567960032029},"rotation":{"radians":-2.8006132532287797E-4}},"curvature":-6.279554631299584E-5},{"time":2.3261493144579335,"velocity":0.9999875366081545,"acceleration":1.0464975058692262E-4,"pose":{"translation":{"x":6.831503304600633,"y":-0.7152832513808527},"rotation":{"radians":-2.848376935983519E-4}},"curvature":-3.8467738220686515E-5},{"time":2.4157108813665107,"velocity":0.9999969092037937,"acceleration":-6.272661685415307E-5,"pose":{"translation":{"x":6.9210641713134535,"y":-0.7153088795594308},"rotation":{"radians":-2.870387793125379E-4}},"curvature":-9.539523948400935E-6},{"time":2.5007782528032245,"velocity":0.9999915732153788,"acceleration":-1.8157570256514238E-4,"pose":{"translation":{"x":7.006131049361786,"y":-0.715333291607366},"rotation":{"radians":-2.8640782227810663E-4}},"curvature":2.6008813680473154E-5},{"time":2.580961267446303,"velocity":0.9999770139281612,"acceleration":-0.34334358581599045,"pose":{"translation":{"x":7.086312801364286,"y":-0.715356128194644},"rotation":{"radians":-2.826136513720897E-4}},"curvature":7.094629694583373E-5},{"time":2.656934801320935,"velocity":0.9738919883805325,"acceleration":-1.0000000000000009,"pose":{"translation":{"x":7.161293700195457,"y":-0.7153770694599936},"rotation":{"radians":-2.7523841740668075E-4}},"curvature":1.290294163432336E-4},{"time":2.731182328814193,"velocity":0.8996444608872743,"acceleration":-1.0000000000000009,"pose":{"translation":{"x":7.230846422174607,"y":-0.7153958448780722},"rotation":{"radians":-2.6377107859051235E-4}},"curvature":2.0521978632845688E-4},{"time":2.805379689250417,"velocity":0.8254471004510505,"acceleration":-1.0,"pose":{"translation":{"x":7.294845040254814,"y":-0.715412243126652},"rotation":{"radians":-2.4761650980063257E-4}},"curvature":3.0569017364415825E-4},{"time":2.9536779192175344,"velocity":0.6771488704839332,"acceleration":-1.0000000000000002,"pose":{"translation":{"x":7.4062611988333185,"y":-0.7154374180450894},"rotation":{"radians":-1.9878917382344983E-4}},"curvature":6.041665401664144E-4},{"time":3.1045757838606516,"velocity":0.526251005840816,"acceleration":-0.9999999999999997,"pose":{"translation":{"x":7.497056433411484,"y":-0.7154524626528294},"rotation":{"radians":-1.264307214018874E-4}},"curvature":0.0010074652170135845},{"time":3.2721414624042318,"velocity":0.358685327297236,"acceleration":-1.0,"pose":{"translation":{"x":7.571198911703647,"y":-0.7154588241371141},"rotation":{"radians":-4.363729332648743E-5}},"curvature":0.001105121415441531},{"time":3.6308267897014677,"velocity":0.0,"acceleration":-1.0,"pose":{"translation":{"x":7.635526493704722,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0d2305f..8d00465 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -73,7 +73,7 @@ public final class Constants { public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.0518, 0, 1.0); /* Trajectory Constants */ - public static final double MAX_SPEED_METERS_PER_SECOND = 1; + public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; public static final double TRACK_WIDTH_METERS = 0.648; public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 9e827dd..82c0c98 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,6 +10,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -78,7 +79,7 @@ public class Robot extends TimedRobot { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(false); - m_robotContainer.resetOdometry(); + m_robotContainer.resetOdometry(new Pose2d()); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); /* diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6b92ba8..3ebd83c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,26 +7,33 @@ package frc4388.robot; +import java.nio.file.Path; import java.util.List; import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; 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.RamseteCommand; 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.OIConstants; +import frc4388.robot.commands.auto.SixBallAutoMiddle; import frc4388.robot.commands.auto.Wait; import frc4388.robot.commands.climber.DisengageRachet; import frc4388.robot.commands.climber.RunClimberWithTriggers; @@ -233,10 +240,19 @@ public class RobotContainer { public Command getAutonomousCommand() { // Create config for trajectory TrajectoryConfig config = getTrajectoryConfig(); - Trajectory trajectory = getTrajectory(config); - RamseteCommand ramseteCommand = getRamseteCommand(trajectory); + //Trajectory trajectory = getTrajectory(config); + + String trajectoryJSON0 = "paths/FirstPath0.wpilib.json"; + String trajectoryJSON1 = "paths/FirstPath1.wpilib.json"; + + String[] sixBallAutoMiddlePaths = new String[]{ + "FirstPath0", + "FirstPath1" + }; + // Run path following command, then stop at the end. - return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //SequentialCommandGroup ramseteCommands = new SequentialCommandGroup(ramseteCommand0, ramseteCommand1); + return new SixBallAutoMiddle(buildPaths(sixBallAutoMiddlePaths)).andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); @@ -262,25 +278,19 @@ public class RobotContainer { Trajectory getTrajectory(TrajectoryConfig config) { Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), + new Pose2d(2.9, -2.4, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path List.of( - new Translation2d(1, -1), - new Translation2d(2, 0.0), - new Translation2d(1, 1), - new Translation2d(0, 0), - new Translation2d(1, -1), - new Translation2d(2, 0.0), - new Translation2d(1, 1) + new Translation2d(4.1, -1.7) ), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(0, 0, new Rotation2d(0)), + new Pose2d(5.1, -0.7, new Rotation2d(0)), // Pass config config); return exampleTrajectory; } - RamseteCommand getRamseteCommand(Trajectory trajectory) { + public RamseteCommand getRamseteCommand(Trajectory trajectory) { RamseteCommand ramseteCommand = new RamseteCommand( trajectory, m_robotDrive::getPose, @@ -292,6 +302,36 @@ public class RobotContainer { return ramseteCommand; } + public RamseteCommand[] buildPaths(String[] paths) { + RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; + Trajectory initialTrajectory; + + try { + if (true) { + String path = paths[0]; + String trajectoryJSON = "paths/" + path + ".wpilib.json"; + Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + initialTrajectory = trajectory; + RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + ramseteCommands[0] = ramseteCommand; + } + + for(int i = 1; i < paths.length; i++) { + String path = paths[i]; + String trajectoryJSON = "paths/" + path + ".wpilib.json"; + Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + ramseteCommands[i] = ramseteCommand; + } + } catch (Exception e) { + DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); + } + + return ramseteCommands; + } + /** * Sets Motors to a NeutralMode. * @param mode NeutralMode to set motors to @@ -311,9 +351,8 @@ public class RobotContainer { /** * */ - public void resetOdometry() { - m_robotDrive.resetGyroAngles(); - m_robotDrive.setOdometry(new Pose2d()); + public void resetOdometry(Pose2d pose) { + m_robotDrive.setOdometry(pose); } /** diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java new file mode 100644 index 0000000..f89f6ac --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 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.RamseteCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.RobotContainer; + +// 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 SixBallAutoMiddle extends SequentialCommandGroup { + /** + * Creates a new SixBallAutoMiddle. + */ + public SixBallAutoMiddle(RamseteCommand[] paths) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands( + paths[0], + paths[1] + ); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 609b44e..4cb2ab5 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -105,7 +105,7 @@ public class Drive extends SubsystemBase { m_leftBackMotor.configFactoryDefault(); m_rightBackMotor.configFactoryDefault(); m_pigeon.configFactoryDefault(); - resetGyroYaw(); + resetGyroYaw(0); /* Config Open Loop Ramp so we don't make sudden output changes */ @@ -497,24 +497,25 @@ public class Drive extends SubsystemBase { */ public void setOdometry(Pose2d pose) { resetEncoders(); + resetGyroYaw(pose.getRotation().getDegrees()); m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); } /** * Resets the yaw of the pigeon */ - public void resetGyroYaw() { - m_pigeon.setYaw(0); - m_pigeon.setAccumZAngle(0); - resetGyroAngles(); + public void resetGyroYaw(double angle) { + m_pigeon.setYaw(angle); + m_pigeon.setAccumZAngle(angle); + resetGyroAngles(angle); } /** * Add docs here */ - public void resetGyroAngles() { - m_lastAngleYaw = 0; - m_currentAngleYaw = 0; + public void resetGyroAngles(double angle) { + m_lastAngleYaw = angle; + m_currentAngleYaw = angle; } /**