From afff5226c14d3e8343659fb16c74307fcc826c8a Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 18:24:58 -0700 Subject: [PATCH] Almost finished 6 ball auto, just a little too wide --- PathWeaver/Paths/FirstPath0 | 2 +- PathWeaver/Paths/FirstPath1 | 2 +- PathWeaver/pathweaver.json | 4 ++-- src/main/deploy/paths/FirstPath0.wpilib.json | 2 +- src/main/deploy/paths/FirstPath1.wpilib.json | 2 +- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 6 +++--- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++++-- .../frc4388/robot/commands/auto/SixBallAutoMiddle.java | 1 - src/main/java/frc4388/robot/subsystems/Drive.java | 4 ++-- 10 files changed, 18 insertions(+), 15 deletions(-) diff --git a/PathWeaver/Paths/FirstPath0 b/PathWeaver/Paths/FirstPath0 index 43523ef..18dd1ec 100644 --- a/PathWeaver/Paths/FirstPath0 +++ b/PathWeaver/Paths/FirstPath0 @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name -2.923987849862624,-2.381155324331042,3.134,0.0,true, +3.2,-2.4,0.2,2.5,true, 5.006107200045366,-0.7154598441848491,2.0,0.0,true, diff --git a/PathWeaver/Paths/FirstPath1 b/PathWeaver/Paths/FirstPath1 index 83f52cc..4ae0aad 100644 --- a/PathWeaver/Paths/FirstPath1 +++ b/PathWeaver/Paths/FirstPath1 @@ -1,3 +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, +7.2,-0.7154598441848491,1.0,0.0,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index cae9a01..ddd52e0 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -1,8 +1,8 @@ { "lengthUnit": "Meter", "exportUnit": "Always Meters", - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 1.7, + "maxAcceleration": 2.7, "wheelBase": 0.648, "gameName": "Infinite Recharge", "outputDir": ".." diff --git a/src/main/deploy/paths/FirstPath0.wpilib.json b/src/main/deploy/paths/FirstPath0.wpilib.json index d4ab98b..9697957 100644 --- a/src/main/deploy/paths/FirstPath0.wpilib.json +++ b/src/main/deploy/paths/FirstPath0.wpilib.json @@ -1 +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 +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.241057734845558,"velocity":0.6508558840830065,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3413462238092895,"velocity":0.9216348042850815,"acceleration":2.7000000000000015,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.41891023510025166,"velocity":1.1310576347706796,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.485033699119182,"velocity":1.3095909876217915,"acceleration":1.198557825708184,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5460162839325096,"velocity":1.3826821418817183,"acceleration":-0.09664254151193248,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.6067366321916278,"velocity":1.3768139731044675,"acceleration":0.056078716638971786,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.6689099696895163,"velocity":1.3803005740805108,"acceleration":0.1547804484445089,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.732304190496085,"velocity":1.3901127600057417,"acceleration":0.2103982966969226,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7967364279166413,"velocity":1.4036691930113985,"acceleration":0.23280173311870972,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.8620547624805587,"velocity":1.4188754145023061,"acceleration":0.23088864679733498,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.9281194841468721,"velocity":1.4341290086888838,"acceleration":0.21222177317200774,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.9947872927323935,"velocity":1.4482773692403952,"acceleration":0.18271830275439588,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":1.061900352517623,"velocity":1.4605401536170066,"acceleration":0.14658686074019905,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":1.1292807138826122,"velocity":1.4704172292650406,"acceleration":0.10649570641795872,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":1.1967296497350939,"velocity":1.4776002513357902,"acceleration":0.06387559089571403,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.2640309657840656,"velocity":1.4818991626664775,"acceleration":0.019269616300747743,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.3309571960078612,"velocity":1.4831888054433455,"acceleration":-0.027311892934865002,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.3972776197336674,"velocity":1.4813774691311514,"acceleration":-0.07602490623349939,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.4627671006194463,"velocity":1.4763986374875295,"acceleration":-0.12676857025603222,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.5272147641598541,"velocity":1.4682286993241702,"acceleration":-0.17863556576701026,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.590431452927045,"velocity":1.456935950360326,"acceleration":-0.2291597090436514,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.6522547165397012,"velocity":1.4427685492587208,"acceleration":-0.2733089917486314,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.7125498410614521,"velocity":1.4262893495683229,"acceleration":-0.3022406193756772,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.771205213726376,"velocity":1.408561313404365,"acceleration":-0.301932659771223,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.8281203979112544,"velocity":1.3913767604620557,"acceleration":-0.25183985881265725,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.883186076418791,"velocity":1.3775090277612945,"acceleration":-0.853135730581628,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.9370285392585973,"velocity":1.3315740988901423,"acceleration":-2.699999999999998,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.992654531738804,"velocity":1.1813839191935844,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":2.0538463235767033,"velocity":1.016166081231257,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":2.124517065276345,"velocity":0.825355078642223,"acceleration":-2.7,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":2.4302041314401315,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-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 index 357c1aa..1fcfcfc 100644 --- a/src/main/deploy/paths/FirstPath1.wpilib.json +++ b/src/main/deploy/paths/FirstPath1.wpilib.json @@ -1 +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 +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26560616204123155,"velocity":0.7171366375113252,"acceleration":2.700000000000001,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3755418080019105,"velocity":1.0139628816051585,"acceleration":2.7000000000000015,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.45975256131083786,"velocity":1.2413319155392624,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5305335413392231,"velocity":1.4324405616159026,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5926137419852601,"velocity":1.6000571033602022,"acceleration":1.7616233327671682,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6492510855336531,"velocity":1.6998307692610013,"acceleration":-1.618642537235751E-4,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.703708212041964,"velocity":1.699821954598859,"acceleration":-9.558060076856542E-5,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7575219555253974,"velocity":1.6998168110489273,"acceleration":-3.565667757010362E-5,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.8105612321047558,"velocity":1.6998149198445438,"acceleration":2.0620985976386642E-5,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.8626915928733692,"velocity":1.6998159948239822,"acceleration":7.612797407786101E-5,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.9137773666638485,"velocity":1.699819883880445,"acceleration":1.3403797148642016E-4,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.9636838028938515,"velocity":1.6998265732379214,"acceleration":1.9801974397020216E-4,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":1.0122792145971,"velocity":1.699836196088905,"acceleration":2.725041863281043E-4,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":1.059437121565497,"velocity":1.6998490468159724,"acceleration":3.630343580963836E-4,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":1.105038393643873,"velocity":1.6998656016445097,"acceleration":4.7670999504396565E-4,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":1.148973394189874,"velocity":1.6998865458984023,"acceleration":6.227242300883394E-4,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.1911441237674736,"velocity":1.6999128066335107,"acceleration":8.129491144905005E-4,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.231466364211058,"velocity":1.6999455865631736,"acceleration":0.0010624275510308508,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.269871823331935,"velocity":1.6999863895810536,"acceleration":-1.1176149536288589,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.3067580684990234,"velocity":1.6587617703990953,"acceleration":-2.7,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.3796242838165373,"velocity":1.4620229890418077,"acceleration":-2.7,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.4531813456172147,"velocity":1.2634189221799788,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.528567885890812,"velocity":1.059875263441266,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.6090377433938088,"velocity":0.8426066481831752,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.7044707307238756,"velocity":0.5849375823919951,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.9211142797579477,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"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 146225f..9a194ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,7 +70,7 @@ public final class Constants { public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; - public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.16, 0.0, 0.0, 0.058, 0, 1.0); + public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); /* Trajectory Constants */ public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 7ad3d4f..084e7b8 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -80,7 +80,7 @@ public class Robot extends TimedRobot { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(false); + m_robotContainer.setDriveGearState(true); m_robotContainer.resetOdometry(new Pose2d()); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); @@ -93,7 +93,7 @@ public class Robot extends TimedRobot { // schedule the autonomous command (example) if (m_autonomousCommand != null) { - //m_autonomousCommand.schedule(); + m_autonomousCommand.schedule(); SmartDashboard.putString("Is Auto Start?", "YEA"); } } @@ -108,7 +108,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(false); + m_robotContainer.setDriveGearState(true); m_robotContainer.shiftClimberRachet(false); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3f4e391..184e156 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -259,9 +259,13 @@ public class RobotContainer { //RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. - return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + try { + return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + } catch (Exception e) { + System.err.println("ERROR"); + } - //return new InstantCommand(); + return new InstantCommand(); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index f47951f..c1e9f38 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -29,7 +29,6 @@ public class SixBallAutoMiddle extends SequentialCommandGroup { // super(new FooCommand(), new BarCommand()); addCommands( - new Wait(drive, 0, 1), 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 d545e64..fe96ca2 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -342,7 +342,7 @@ public class Drive extends SubsystemBase { * using the Differential Drive class to manage the two inputs */ public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); + m_driveTrain.arcadeDrive(-move, steer); m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); } @@ -573,7 +573,7 @@ public class Drive extends SubsystemBase { @Override public void reset() { // TODO Auto-generated method stub - resetGyroYaw(); + resetGyroYaw(0); } @Override