Merge branch 'master' into covid-shooter-revert

PathWeaver/Groups/SequentialTest

@@ -0,0 +1,2 @@
+Seq1
+Seq2

PathWeaver/Paths/Barrel

@@ -0,0 +1,12 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.1943219738452617,-2.286,1.0,0.0,true,
+3.1673831089351285,-2.3571291631772207,0.24740578496424614,-0.04948115699284905,true,
+4.237413128905495,-2.7839041422405457,0.4391452683115382,-0.7793282226373774,true,
+3.736416414352895,-3.55086207562971,-1.1442517554596408,0.20410977259550345,true,
+3.581787798750241,-2.6354606712619977,0.6679956194034653,0.6679956194034657,true,
+6.476435482831926,-1.7571701346389224,0.5752184500418727,0.8844756812471817,true,
+6.117697094633767,-0.7737321394060425,-1.168992333956064,-0.012370289248212263,true,
+5.802254718804354,-1.95509476261032,0.6432550409070421,-1.2184734909489143,true,
+7.020728209753269,-3.3096414352895698,1.6019524576434971,-0.8164390903820133,true,
+8.239201700702182,-3.0746059395735355,0.2102949172196098,0.5009967145525991,true,
+7.608316949043354,-2.286,-1.7503959286220452,0.73,true,

PathWeaver/Paths/BarrelManyWaypoints

@@ -0,0 +1,30 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.2831,-2.134,0.28851579591573784,0.011906332538813569,true,
+2.3262034400566898,-2.1715748244540354,0.7043757677782395,-0.022896780061941242,false,
+3.396233460027056,-2.2025005475745667,0.30307208658120244,-0.012370289248212263,true,
+4.070414224054628,-2.48701720028345,0.2861568691130597,-0.28731844423844977,false,
+4.336375442891193,-2.9694584809637306,0.0865920247374854,-0.377293822070476,true,
+4.21267255040907,-3.47045519551633,-0.20556149326155532,-0.19696961676596483,false,
+3.754971848225213,-3.5941580879984536,-0.4762561360561741,-0.012370289248212263,true,
+3.3653077369065247,-3.3653077369065247,-0.13443796040407763,0.23133984130054164,false,
+3.383863170778844,-2.882866456226244,0.19173948334729118,0.3711086774463692,true,
+3.724046125104682,-2.548868646524512,0.3196032483111817,0.2004660051639724,false,
+4.398226889132254,-2.3880548862977515,0.4329601236874323,0.018555433872318616,true,
+5.424960896733878,-2.2519817045674158,0.6465000032013642,0.14021955272022218,false,
+6.3403623011015915,-1.9798353411067444,0.24740578496424614,0.22266520646782206,true,
+6.822803581781872,-1.540690072795207,-0.061851446241062646,0.3587383881981576,true,
+6.643434387682792,-1.126285382980094,-0.2783315080847766,0.1237028924821233,true,
+6.0929565161373445,-1.008767635122077,-0.5875887392900854,-0.024740578496424526,true,
+5.6909221155704435,-1.2747288539586417,-0.1592675774531093,-0.23351174607207098,false,
+5.579589512336532,-1.6829483991496488,0.012370289248213595,-0.3216275204535204,true,
+5.777514140307929,-2.221055981446885,0.17025498901011377,-0.34473677929021274,false,
+6.0929565161373445,-2.715867551375378,0.24896066707734657,-0.28730368548164703,false,
+6.519731495200669,-3.0869762288217477,0.3278126650776283,-0.1979246279713971,false,
+7.070209366746118,-3.3900483154029506,0.4267749790633246,-0.11133260323391081,true,
+7.781500998518326,-3.47045519551633,0.385943708764414,0.13418037797408539,false,
+8.251571989950396,-3.0684207949494295,0.006185144624108574,0.402034400566901,true,
+8.214461122205757,-2.6230903820137854,-0.13854051276931562,0.27031752154848193,false,
+7.738204986149584,-2.3200182954325834,-0.5752184500418736,0.061851446241061314,true,
+6.934136185015782,-2.313833150808477,-0.5009967145525982,0.0061851446241063535,true,
+5.6909221155704435,-2.3014628615602657,-0.6123293177865108,0.0,true,
+0.7984727179024672,-2.134,-0.5319224376731302,0.018091477162919478,true,

PathWeaver/Paths/BarrelStart

@@ -0,0 +1,14 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.1943219738452617,-2.286,1.0,0.0,true,
+3.396233460027056,-2.2025005475745667,1.5833970237711785,0.02474057849642497,true,
+4.540485215486696,-3.006569348708368,0.15462861560265395,-1.1071408877150035,true,
+3.451899761644012,-3.60034323262256,-0.7607727887650584,0.7051064871481025,true,
+3.5941580879984536,-2.4808320556593437,1.2184734909489143,0.8226242350061197,true,
+6.105326805385555,-2.208685692198673,1.5215455775301168,0.4824412806802809,true,
+6.971247052760418,-1.2623585647104294,-0.3216275204535206,1.7070999162533012,true,
+6.154807962378405,-0.6933252592926624,-0.7793282226373774,-0.17318404947497257,true,
+5.523923210719577,-1.4602831926818267,0.42058983443921694,-0.9030311151194992,true,
+6.612508664562261,-3.303456290665464,1.7689513624943638,-0.7174767763963148,true,
+8.239201700702182,-3.4766403401404364,0.9648825613605609,0.8968459704953937,true,
+8.381460027056624,-2.2334262706950976,-0.5442927269213422,0.2474057849642466,true,
+5.598144946208851,-2.12,-5.115114604135798,0.06803659086516767,true,

PathWeaver/Paths/Bounce

@@ -0,0 +1,11 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.2685437093345355,-2.2829074276879466,3.048,0.0,true,
+2.258166849191522,-0.6129183791792823,0.11133260323391081,-0.38347896669458226,true,
+2.9818287702119433,-3.1735682535592344,0.5566630161695545,-1.4473238420408423,true,
+4.385856599884042,-3.3405671584101007,0.6556253301552539,1.3731021065515683,true,
+4.596151517103652,-0.8726944533917412,0.20415183442002244,0.00717066557576255,false,
+4.998185917670552,-3.3158265799136757,0.8164390903820147,-1.4163981189203128,true,
+6.408398891966757,-3.4766403401404364,0.9958082844810932,0.8288093796302256,true,
+6.87228473877472,-0.829398441022998,0.12370289248212352,-0.40821954519100667,true,
+7.311430007086258,-1.8561324486246213,0.4858385107411982,-0.5239684308130039,false,
+8.3567194485602,-2.375684597049539,0.6246996070347217,-0.25359092958835294,true,

PathWeaver/Paths/Bounce1

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.2190625523416865,-2.295277716936159,2.4431321265219355,-0.012370289248212707,true,
+2.295277716936159,-0.6685846807962377,-0.061851446241061314,0.5442927269213425,true,

PathWeaver/Paths/Bounce2

@@ -0,0 +1,5 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+2.295,-0.7613618501578302,-0.2474057849642466,-2.133874895316627,true,
+3.804453005218064,-3.7920827159698507,1.6328781807640258,0.25359092958835294,true,
+4.521929781614379,-2.9447179024673065,-0.04948115699285083,0.7422173548927398,true,
+4.521929781614379,-0.68095497004445,-0.04329601236874314,0.377293822070476,true,

PathWeaver/Paths/Bounce3

@@ -0,0 +1,5 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+4.589966372479545,-0.68095497004445,0.03711086774463812,-0.8349945242543321,true,
+5.622885524705275,-3.68075011273594,2.770944791599561,0.15462861560265395,true,
+6.692915544675642,-2.882866456226244,0.12988803710623031,0.9339568382400305,true,
+6.8537293049024015,-0.6933252592926624,-0.06803659086516767,0.5937738839141918,true,

PathWeaver/Paths/Bounce4

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+6.884655028022934,-0.8912498872640597,0.2721463634606698,-2.752389357727243,true,
+8.344349159311989,-2.369499452425433,0.7236619210204207,-0.18555433872318483,true,

PathWeaver/Paths/Galactic Search ABLUE

@@ -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,

PathWeaver/Paths/Seq1

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.6396523867809052,-2.313833150808477,0.6989213425239973,-0.0061851446241063535,true,
+3.1117168073181727,-2.3200182954325834,0.6494401855311471,0.0,true,

PathWeaver/Paths/Seq2

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+2.957088191715518,-2.3571291631772207,3.048,0.0,true,
+4.6270772402241835,-2.3571291631772207,0.6822312697287889,-0.01423565032532359,true,

PathWeaver/Paths/Slalom

@@ -0,0 +1,11 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.2,-3.85,2.084900856793146,0.0,true,
+2.8890516008503506,-2.536498357276299,1.3978426850479937,0.1484434709785476,true,
+6.278510854860529,-2.9199773239708815,1.107140887715004,-1.9050245442246987,true,
+7.98561077111383,-3.649824389615409,1.1628071893319598,0.45151555755975004,true,
+8.282497713070926,-2.7282378406235903,-0.6370698962829353,0.797883656509695,true,
+7.360911164079107,-2.8395704438575016,-0.36492353282226375,-0.7731430780132702,true,
+6.569212652193518,-3.625083811118984,-0.9958082844810923,-0.5752184500418731,true,
+4.107525091799265,-4.039488500934097,-2.0287274367068218,-0.14225832635444213,true,
+2.412795464794176,-2.9756436255878373,-0.5628481607936611,1.230843780197127,true,
+0.8232132963988917,-2.3262034400566898,-1.1380666108355342,-0.043296012368743586,true,

PathWeaver/Paths/SlalomClose

@@ -0,0 +1,12 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.2,-3.55,3.847667074663401,0.07954480448366974,true,
+2.790089286864652,-2.54,1.861728531855955,0.1519451137022485,true,
+5.171369967145526,-2.369499452425433,2.3503549571603415,0.024740578496424526,true,
+7.020728209753269,-3.4890106293886487,0.8411796688784383,-0.7978836565096956,true,
+8.171165109837014,-3.458084906268118,0.6061441731624058,0.5195521484249181,true,
+8.301053146943245,-2.6911269728789535,-0.5319224376731313,0.9648825613605616,true,
+7.32380029633447,-2.752978419120015,-0.4329601236874323,-0.5071818591767059,true,
+6.457880048959607,-3.5632323648779227,-0.6803659086516785,-0.2597760742124593,true,
+4.658002963344714,-3.6683798234877276,-2.968869419570958,-0.12988803710622943,true,
+2.5612389357727237,-3.0374950718288987,-0.40203440056690054,1.1813626232042773,true,
+0.7984727179024672,-2.3,-0.7669579333891645,-0.02474057849642497,false,

PathWeaver/pathweaver.json

@@ -1,7 +1,7 @@
 {
   "lengthUnit": "Meter",
   "exportUnit": "Always Meters",
-  "maxVelocity": 1.7,
+  "maxVelocity": 2.6,
   "maxAcceleration": 2.7,
   "wheelBase": 0.648,
   "gameName": "Galactic Search A",

src/main/deploy/paths/Seq1.wpilib.json

@@ -0,0 +1 @@
+[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":1.6396523867809052,"y":-2.313833150808477},"rotation":{"radians":-0.008849326516257987}},"curvature":0.0},{"time":0.2722820970374773,"velocity":0.7351616620011887,"acceleration":2.7000000000000006,"pose":{"translation":{"x":1.739734776700425,"y":-2.3146446112802495},"rotation":{"radians":-0.007149647015655322}},"curvature":0.01725893438810635},{"time":0.35451476526736425,"velocity":0.9571898662218835,"acceleration":2.7,"pose":{"translation":{"x":1.8093165329850407,"y":-2.31510673816049},"rotation":{"radians":-0.006214537421300872}},"curvature":0.010262108695263363},{"time":0.4360203613484434,"velocity":1.1772549756407973,"acceleration":2.700000000000001,"pose":{"translation":{"x":1.8962996475995293,"y":-2.315615003996086},"rotation":{"radians":-0.005532904856197329}},"curvature":0.005996263181397757},{"time":0.5169500582728787,"velocity":1.3957651573367724,"acceleration":2.7000000000000015,"pose":{"translation":{"x":2.000415072981678,"y":-2.3161634275928744},"rotation":{"radians":-0.005039840497117272}},"curvature":0.0037792530506272166},{"time":0.5960164852085316,"velocity":1.6092445100630355,"acceleration":2.6999999999999944,"pose":{"translation":{"x":2.1192113689257215,"y":-2.316738666286958},"rotation":{"radians":-0.0046665773718276736}},"curvature":0.0026525886672486274},{"time":0.634352557101396,"velocity":1.7127519041737693,"acceleration":2.699999999999999,"pose":{"translation":{"x":2.18288684602207,"y":-2.317030681391432},"rotation":{"radians":-0.004508805721396696}},"curvature":0.0023250237048777172},{"time":0.6716263517870102,"velocity":1.8133911498249278,"acceleration":2.700000000000002,"pose":{"translation":{"x":2.248602565810843,"y":-2.317322139445587},"rotation":{"radians":-0.004363819082991022}},"curvature":0.0021040995089735234},{"time":0.7076518105628782,"velocity":1.9106598885197714,"acceleration":2.08330049831677,"pose":{"translation":{"x":2.315682270679263,"y":-2.31761024782456},"rotation":{"radians":-0.004227674108169131}},"curvature":0.0019683194092841986},{"time":0.7424426834355309,"velocity":1.983139731312244,"acceleration":-2.699999999999999,"pose":{"translation":{"x":2.3834160278296714,"y":-2.317892151968046},"rotation":{"radians":-0.0040968776842529794}},"curvature":0.0019052546752226036},{"time":0.7773927718968935,"velocity":1.888774492466565,"acceleration":-2.7000000000000037,"pose":{"translation":{"x":2.4510773500054253,"y":-2.318165001739701},"rotation":{"radians":-0.0039681904771730085}},"curvature":0.001909770538576822},{"time":0.8137373594388484,"velocity":1.7906441061032867,"acceleration":-2.699999999999997,"pose":{"translation":{"x":2.517940316216782,"y":-2.318426017786541},"rotation":{"radians":-0.0038384445909229676}},"curvature":0.0019833496346190354},{"time":0.851300180838236,"velocity":1.6892244883249403,"acceleration":-2.6999999999999953,"pose":{"translation":{"x":2.583296692466793,"y":-2.3186725578983505},"rotation":{"radians":-0.003704364009272917}},"curvature":0.002134423240000436},{"time":0.8898901860409195,"velocity":1.585031474277695,"acceleration":-2.700000000000001,"pose":{"translation":{"x":2.6464730524771953,"y":-2.318902183367081},"rotation":{"radians":-0.003562375965213496}},"curvature":0.002379784285935144},{"time":0.9693308968593537,"velocity":1.3705415550679225,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":2.7638687816148764,"y":-2.319302351210369},"rotation":{"radians":-0.003237626203522703}},"curvature":0.0032804752312573255},{"time":1.0503847372304065,"velocity":1.1516961860660802,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":2.8660868353612363,"y":-2.3196136033526837},"rotation":{"radians":-0.0028212080559704344}},"curvature":0.005135608410691401},{"time":1.1316234025131842,"velocity":0.9323517898025805,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":2.950739195403628,"y":-2.3198305248667666},"rotation":{"radians":-0.002252114531954038}},"curvature":0.008854161979101095},{"time":1.2129544945128348,"velocity":0.7127578414035238,"acceleration":-2.7,"pose":{"translation":{"x":3.0176383566138476,"y":-2.319957327362707},"rotation":{"radians":-0.001466708977573865}},"curvature":0.015315329026373479},{"time":1.4769388802178436,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.111716807318168,"y":-2.320018295432578},"rotation":{"radians":2.803592695601656E-14}},"curvature":1.0107951236368945E-13}]

src/main/deploy/paths/Seq2.wpilib.json

@@ -0,0 +1 @@
+[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":2.957088191715518,"y":-2.3571291631772207},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26544461524158175,"velocity":0.7167004611522707,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.0522103307783257,"y":-2.3571275191873893},"rotation":{"radians":5.101157578230443E-5}},"curvature":0.001038792914185728},{"time":0.3746678834467968,"velocity":1.0116032853063515,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.146595821982689,"y":-2.357116740977926},"rotation":{"radians":1.9099520049927404E-4}},"curvature":0.0019054639517066963},{"time":0.4574501551922932,"velocity":1.2351154190191918,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2395900571900658,"y":-2.357089632328051},"rotation":{"radians":4.035864972296716E-4}},"curvature":0.002652082514494386},{"time":0.5260164380469629,"velocity":1.4202443827267999,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.3306241198087982,"y":-2.357040972350315},"rotation":{"radians":6.754485548111559E-4}},"curvature":0.0033104753166963616},{"time":0.5850751482498664,"velocity":1.5797029002746394,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.419210597831801,"y":-2.3569673627586405},"rotation":{"radians":9.950959992412528E-4}},"curvature":0.0038981243771801294},{"time":0.6370367231616594,"velocity":1.7199991525364804,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.5049393968742493,"y":-2.356867075136363},"rotation":{"radians":0.0013520244174984744}},"curvature":0.0044210302444997965},{"time":0.6833390603048902,"velocity":1.8450154628232036,"acceleration":2.7,"pose":{"translation":{"x":3.587473553211266,"y":-2.3567398982042733},"rotation":{"radians":0.0017360184754233884}},"curvature":0.004874609692377478},{"time":0.7249302432685079,"velocity":1.957311656824971,"acceleration":2.6999999999999926,"pose":{"translation":{"x":3.6665450468156116,"y":-2.3565869850886587},"rotation":{"radians":0.002136559645089136}},"curvature":0.005243133825564394},{"time":0.7624827776596516,"velocity":2.0587034996810587,"acceleration":1.0819585313430768,"pose":{"translation":{"x":3.7419506143953716,"y":-2.3564107005893433},"rotation":{"radians":0.0025422828689825997}},"curvature":0.0054979122679133},{"time":0.7969484484947217,"velocity":2.095993926279525,"acceleration":-2.699999999999992,"pose":{"translation":{"x":3.813547562431645,"y":-2.3562144684477317},"rotation":{"radians":0.0029404502277413256}},"curvature":0.005594270462982216},{"time":0.8299507902622387,"velocity":2.0068876035072294,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":3.881249580216232,"y":-2.3560026186148484},"rotation":{"radians":0.003316424361517827}},"curvature":0.005467307107409776},{"time":0.8624379985890592,"velocity":1.919172141024814,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":3.9450225528893235,"y":-2.3557802345193797},"rotation":{"radians":0.00365313891813889}},"curvature":0.005026461101145579},{"time":0.9256149162430781,"velocity":1.748594463358963,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":4.06088076092986,"y":-2.355327048251995},"rotation":{"radians":0.004125323787162938}},"curvature":0.0026737820186648843},{"time":0.9861184332845145,"velocity":1.5852349673470847,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":4.161734080074728,"y":-2.354904842813889},"rotation":{"radians":0.004154108750296397}},"curvature":-0.0029414739347258},{"time":1.0438566564483853,"velocity":1.4293417648046338,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":4.24876157183169,"y":-2.3545658321706227},"rotation":{"radians":0.0034781161359367808}},"curvature":-0.01397812424405661},{"time":1.099078553226315,"velocity":1.2802426435042238,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":4.323575482900814,"y":-2.35435962366233},"rotation":{"radians":0.0018016415428211323}},"curvature":-0.032741911322928474},{"time":1.2053412709015958,"velocity":0.9933333057809655,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":4.444373578973541,"y":-2.354501684748035},"rotation":{"radians":-0.005323585518118219}},"curvature":-0.0899279066882791},{"time":1.3201225946749353,"velocity":0.6834237315929491,"acceleration":-2.7,"pose":{"translation":{"x":4.5405986995686,"y":-2.3554900302219766},"rotation":{"radians":-0.015505320196648706}},"curvature":-0.10506063032999254},{"time":1.5732424952649164,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":4.627077240224189,"y":-2.3571291631772207},"rotation":{"radians":-0.02086328407244092}},"curvature":2.0691998138625314E-15}]

src/main/java/frc4388/robot/Constants.java

@@ -36,7 +36,7 @@ public final class Constants {
         public static final boolean isRightMotorInverted = true;
         public static final boolean isLeftMotorInverted = false;
         public static final boolean isRightArcadeInverted = false;
-        public static final boolean isAuxPIDInverted = true;
+        public static final boolean isAuxPIDInverted = false;
 
         /* Drive Configuration */
         public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config

src/main/java/frc4388/robot/Robot.java

@@ -26,6 +26,9 @@ public class Robot extends TimedRobot {
   Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
+  double m_initialTime;
+  double m_currentTime;
+  double m_deltaTime;
 
   /**
    * This function is run when the robot is first started up and should be
@@ -67,10 +70,12 @@ public class Robot extends TimedRobot {
     /* Builds Autos */
     m_robotContainer.buildAutos();
     SmartDashboard.putString("Is Auto Start?", "NAH");
+    m_robotContainer.m_robotLime.limeOff();
   }
 
   @Override
   public void disabledPeriodic() {
+    m_robotContainer.resetOdometry(new Pose2d());
   }
 
   /**
@@ -78,12 +83,12 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
-    
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
+    m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
     m_robotContainer.setDriveGearState(true);
-    m_robotContainer.resetOdometry(new Pose2d());
+
+    m_initialTime = System.currentTimeMillis();
 
     //m_robotContainer.resetGyroYawRobotContainer(0);
 
@@ -108,6 +113,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousPeriodic() {
+    m_currentTime = System.currentTimeMillis();
+    m_deltaTime = m_currentTime - m_initialTime;
+    SmartDashboard.putNumber("Auto Path Time", (m_deltaTime)/1000);
   }
 
   @Override

src/main/java/frc4388/robot/RobotContainer.java

@@ -38,14 +38,22 @@ import frc4388.robot.commands.auto.DriveOffLineBackward;
 import frc4388.robot.commands.auto.DriveOffLineForward;
 import frc4388.robot.commands.auto.EightBallAutoMiddle;
 import frc4388.robot.commands.auto.FiveBallAutoMiddle;
+import frc4388.robot.commands.auto.SequentialTest;
 import frc4388.robot.commands.auto.SixBallAutoMiddle;
+import frc4388.robot.commands.auto.Slalom;
+import frc4388.robot.commands.auto.TankDriveVelocity;
 import frc4388.robot.commands.auto.TenBallAutoMiddle;
 import frc4388.robot.commands.InterruptSubystem;
 import frc4388.robot.commands.auto.AutoPath1FromCenter;
+import frc4388.robot.commands.auto.Barrel;
+import frc4388.robot.commands.auto.BarrelMany;
+import frc4388.robot.commands.auto.BarrelStart;
+import frc4388.robot.commands.auto.Bounce;
 import frc4388.robot.commands.auto.Wait;
 import frc4388.robot.commands.climber.DisengageRachet;
 import frc4388.robot.commands.climber.RunClimberWithTriggers;
 import frc4388.robot.commands.climber.RunLevelerWithJoystick;
+import frc4388.robot.commands.drive.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.drive.DriveStraightToPositionMM;
 import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.PlaySongDrive;
@@ -106,7 +114,7 @@ public class RobotContainer {
     /* Cameras */
     private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
     private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
-    private final LimeLight m_robotLime = new LimeLight();
+    public final LimeLight m_robotLime = new LimeLight();
 
     /* Controllers */
     private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -119,6 +127,10 @@ public class RobotContainer {
 
     SixBallAutoMiddle m_sixBallAutoMiddle;
 
+    SixBallAutoMiddle m_sixBallAutoMiddle0;
+
+    SixBallAutoMiddle m_sixBallAutoMiddle1;
+
     EightBallAutoMiddle m_eightBallAutoMiddle;
 
     DriveOffLineForward m_driveOffLineForward;
@@ -129,8 +141,21 @@ public class RobotContainer {
 
     TenBallAutoMiddle m_tenBallAutoMiddle;
 
+    Slalom m_slalom;
+
+    Barrel m_barrel;
+
+    BarrelStart m_barrelStart;
+
+    BarrelMany m_barrelMany;
+
+    Bounce m_bounce;
+
+    SequentialTest m_sequentialTest;
+
     public static boolean m_isShooterManual = false;
 
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -171,9 +196,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));
-        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
+        m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
     /**
@@ -190,12 +214,14 @@ public class RobotContainer {
             .whenReleased(new InterruptSubystem(m_robotShooter))
             .whenReleased(new InterruptSubystem(m_robotShooterHood));
 
+
         // B driver test button
-        /*new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
+        new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new TurnDegrees(m_robotDrive, 90));*/
+            .whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
+
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new Wait(m_robotDrive, 0, 0));
+            .whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -326,6 +352,51 @@ public class RobotContainer {
 
         m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
 
+        String[] sixBallAutoMiddle0Paths = new String[]{
+            "SixBallMid0"
+        };
+
+        m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
+
+        String[] sixBallAutoMiddle1Paths = new String[]{
+            "SixBallMid1"
+        };
+
+        m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
+
+        String[] slalom = new String[]{
+            "Slalom"
+        };
+
+        m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
+
+        String[] barrel = new String[]{
+            "BarrelStart"
+        };
+
+        m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
+        
+        String[] barrelStart = new String[]{
+            "Barrel"
+        };
+
+        m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
+
+        String[] bounce = new String[]{
+            "Bounce1",
+            "Bounce2",
+            "Bounce3",
+            "Bounce4"
+        };
+
+        m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
+        
+        String[] barrelMany = new String[]{
+            "BarrelManyWaypoints"
+        };
+
+        m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
+        
         String[] eightBallAutoMiddlePaths = new String[]{
             "EightBallMidComplete"
         };
@@ -336,7 +407,7 @@ public class RobotContainer {
             "DriveOffLineForward"
         };
 
-        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
+        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
 
         String[] driveOffLineBackwardPaths = new String[]{
             "DriveOffLineBackward"
@@ -355,8 +426,14 @@ public class RobotContainer {
             "TenBallMidComplete"
         };
 
-        m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
+        //m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
-                m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
+    
+        String[] sequentialTestPaths = new String[]{
+            "Seq1",
+            "Seq2"
+        };
+
+        m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths));
     }
 
     /**
@@ -375,12 +452,15 @@ public class RobotContainer {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
 
             //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_sixBallAutoMiddle1.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_driveOffLinfeBackward.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 m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-
+            //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_bounce.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
         } catch (Exception e) {
             System.err.println("ERROR");
         }
@@ -506,6 +586,7 @@ public class RobotContainer {
         return m_driverXbox;
     }
 
+
     /**
      * Used for analog inputs like triggers and axises.
      * @return The IHandController interface for the Operator Controller.

src/main/java/frc4388/robot/commands/auto/Barrel.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 Barrel extends SequentialCommandGroup {
+  /**
+   * Creates a new Barrel.
+   */
+  public Barrel(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+   addCommands(
+     paths[0]
+   );
+  }
+}

src/main/java/frc4388/robot/commands/auto/BarrelMany.java

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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 BarrelMany extends SequentialCommandGroup {
+  /**
+   * Creates a new BarrelMany.
+   */
+  public BarrelMany(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    addCommands(
+      paths[0]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/BarrelStart.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+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 BarrelStart extends SequentialCommandGroup {
+  /**
+   * Creates a new BarrelStart.
+   */
+  public BarrelStart(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      //new Wait(drive, 0.01, 1),
+      new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa
+    );
+    
+  }
+}

src/main/java/frc4388/robot/commands/auto/Bounce.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj.geometry.Pose2d;
+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 frc4388.robot.RobotContainer;
+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 Bounce extends SequentialCommandGroup {
+  /**
+   * Creates a new Bounce.
+   */
+  public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[1],
+      new InstantCommand(() -> drive.switchReversed(false)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[2],
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[3],
+      new InstantCommand(() -> drive.switchReversed(false)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d()))
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java

@@ -7,8 +7,11 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+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.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup {
   /**
    * Creates a new DriveOffLineForward.
    */
-  public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
+  public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
 
     addCommands(
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
       paths[0]
     );
   }

src/main/java/frc4388/robot/commands/auto/SequentialTest.java

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj.geometry.Pose2d;
+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.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 SequentialTest extends SequentialCommandGroup {
+  /**
+   * Creates a new SequentialTest.
+   */
+  public SequentialTest(RobotContainer m_robotContainer, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d())),
+      paths[1]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/Slalom.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 Slalom extends SequentialCommandGroup {
+  /**
+   * Creates a new Slalom.
+   */
+  public Slalom(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java

@@ -15,10 +15,10 @@ public class TankDriveVelocity extends CommandBase {
   double m_leftTargetVel;
   double m_rightTargetVel;
 
-  double m_targetTime;
+  long m_targetTime;
-  double m_firstTimeSec;
+  long m_firstTime;
-  double m_currentTimeSec;
+  long m_currentTime;
-  double m_diffSec;
+  long m_diffTime;
 
   /**
    * Creates a new TankDriveVelocity.
@@ -28,24 +28,24 @@ public class TankDriveVelocity extends CommandBase {
     m_drive = subsystem;
     m_leftTargetVel = leftTargetVel;
     m_rightTargetVel = rightTargetVel;
-    m_targetTime = targetTime;
+    m_targetTime = (long) (targetTime * 1000);
     addRequirements(subsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_firstTimeSec = (System.currentTimeMillis() / 1000);
+    m_firstTime = System.currentTimeMillis();
-    m_diffSec = 0;
+    m_diffTime = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_currentTimeSec = (System.currentTimeMillis() / 1000);
+    m_currentTime = System.currentTimeMillis();
-    m_diffSec = m_currentTimeSec - m_firstTimeSec;
+    m_diffTime = m_currentTime - m_firstTime;
     
-    if (m_diffSec < m_targetTime) {
+    if (m_diffTime < m_targetTime) {
       m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
     }
 
@@ -59,7 +59,7 @@ public class TankDriveVelocity extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_diffSec >= m_targetTime) {
+    if (m_diffTime >= m_targetTime) {
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java

@@ -29,7 +29,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    m_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java

@@ -46,7 +46,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
                   new Wait(m_drive, 0, 0),
                   new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
-                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+                  new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
   }
 
   public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {

src/main/java/frc4388/robot/commands/drive/TurnDegrees.java

@@ -38,6 +38,7 @@ public class TurnDegrees extends CommandBase {
   public void initialize() {
     m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
     m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    SmartDashboard.putNumber("Current Yaw Ticks", m_currentYawInTicks);
     m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
 
     i = 0;
@@ -48,10 +49,10 @@ public class TurnDegrees extends CommandBase {
   public void execute() {
     m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
 
-    m_drive.runTurningPID(m_targetAngleTicksOut);
+    m_drive.runTurningPID(m_targetAngleTicksIn);
 
-    //SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
+    SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
-    //SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
+    SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
 
     i++;
   }

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -46,6 +46,7 @@ public class Drive extends SubsystemBase {
   public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
   public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
   public static GyroBase m_pigeonGyro;
+  public boolean m_isReversed;
 
   /* Drive objects to manage Drive Train */
   public DifferentialDrive m_driveTrain;
@@ -339,9 +340,26 @@ public class Drive extends SubsystemBase {
     m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
     m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
 
-    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
+    updateOdometry(m_isReversed);
+  }
+
+  public void updateOdometry(boolean reversed){
+    if (reversed){
+      m_odometry.update(Rotation2d.fromDegrees( -getHeading()-180),
+                                              -inchesToMeters(getDistanceInches(m_rightFrontMotor)),
+                                              inchesToMeters(getDistanceInches(m_leftFrontMotor)));
+    }
+    else
+    {
+      m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               inchesToMeters(getDistanceInches(m_leftFrontMotor)),
                                               -inchesToMeters(getDistanceInches(m_rightFrontMotor)));
+    }
+  }
+
+  public void switchReversed(boolean reversed)
+  {
+    m_isReversed = reversed;
   }
 
   /**
@@ -401,7 +419,7 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
 
-    m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
+    m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); 
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
@@ -426,7 +444,7 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.follow(m_rightFrontMotor);
 
     m_driveTrain.feedWatchdog();
-
+  
   }
 
   /**

src/main/java/frc4388/robot/subsystems/LimeLight.java

@@ -20,7 +20,7 @@ public class LimeLight extends SubsystemBase {
 
   public void limeOff(){
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
   }
   
   public void limeOn(){