Merge pull request #61 from Team4388/GalacticSearch

Galactic search

PathWeaver/Groups/GalacticSearch

@@ -0,0 +1,4 @@
+GSC_ARED
+GSC_ABLUE
+GSC_BRED
+GSC_BBLUE

PathWeaver/Paths/Bounce4

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

PathWeaver/Paths/GSC_ABLUE

@@ -0,0 +1,6 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.269,-2.313833150808477,1.336,0.031,true,
+4.794076145075049,-3.3281968691618884,1.2803249371899765,1.1937329124524894,true,
+5.5362934999677895,-2.060242221220125,0.9586974167364559,0.6556253301552535,true,
+6.909395606519357,-2.276722283063841,1.7442107839979393,0.04948115699284994,true,
+8.901012175481542,-2.208685692198673,0.5690333054177668,0.012370289248212374,true,

PathWeaver/Paths/GSC_ARED

@@ -1,3 +1,5 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Name
-0.0,0.0,10.0,0.0,true,
+1.4479129034336145,-1.621096952908587,1.2617695033176577,-1.0638448753462597,true,
-10.0,-10.0,0.0,-10.0,true,
+3.649824389615409,-2.8890516008503506,2.220466920054112,0.6308847516588285,true,
+5.715662694066867,-0.7242509824131932,2.4369469818978278,-0.03092572312053088,true,
+9.024715067963667,-1.392246601816659,0.6494401855311462,-0.4700709914320682,true,

PathWeaver/Paths/GSC_ARED_save

@@ -0,0 +1,5 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.4479129034336145,-1.621096952908587,1.2617695033176577,-1.0638448753462597,true,
+3.649824389615409,-2.8890516008503506,2.220466920054112,0.6308847516588285,true,
+5.715662694066867,-0.860324164143529,2.671982477613862,-0.049481156992849384,true,
+8.64742124589319,-1.540690072795207,0.4948115699284923,-0.265961218836565,true,

PathWeaver/Paths/GSC_BBLUE

@@ -0,0 +1,6 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.269,-2.2890925723120525,3.0,0.0,true,
+4.608521806351864,-3.0374950718288987,2.0,0.0,true,
+6.062030793016813,-1.695318688397861,2.5,0.006,true,
+7.701094118404947,-2.9385327578431997,2.5,0.0,true,
+8.758753849127102,-2.9323476132190938,0.5690333054177668,0.0,true,

PathWeaver/Paths/GSC_BRED

@@ -1,3 +1,6 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Name
-0.0,0.0,10.0,0.0,true,
+2.208685692198673,-0.8974350318881658,0.3896641113186885,-0.6185144624106166,true,
-10.0,-10.0,0.0,-10.0,true,
+2.957088191715518,-2.4808320556593437,1.0267340076016236,-1.3174358049346133,true,
+4.0765993686787345,-3.3220117245377825,1.2926952264381883,0.2783315080847779,true,
+5.864106165045416,-2.24579655994331,1.4473238420408423,0.29070179733298995,true,
+8.795864716871737,-1.8252067255040902,0.5442927269213431,-0.6679956194034657,true,

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

@@ -1 +1 @@
-[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
+[{"time":0.0,"velocity":0.0,"acceleration":4.0,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.25,"velocity":1.0,"acceleration":4.000000000000002,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.35355339059327373,"velocity":1.4142135623730951,"acceleration":-4.000000000000002,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45710678118654746,"velocity":1.0,"acceleration":-4.0,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7071067811865475,"velocity":0.0,"acceleration":-4.0,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]

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

@@ -1 +1 @@
-[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
+[{"time":0.0,"velocity":0.0,"acceleration":4.0,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.25,"velocity":1.0,"acceleration":4.000000000000002,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.35355339059327373,"velocity":1.4142135623730951,"acceleration":-4.000000000000002,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45710678118654746,"velocity":1.0,"acceleration":-4.0,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7071067811865475,"velocity":0.0,"acceleration":-4.0,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]

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

@@ -1 +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}]
+[{"time":0.0,"velocity":0.0,"acceleration":4.0,"pose":{"translation":{"x":1.6396523867809052,"y":-2.313833150808477},"rotation":{"radians":-0.008849326516257987}},"curvature":0.0},{"time":0.22370256982835532,"velocity":0.8948102793134213,"acceleration":4.000000000000002,"pose":{"translation":{"x":1.739734776700425,"y":-2.3146446112802495},"rotation":{"radians":-0.007149647015655322}},"curvature":0.01725893438810635},{"time":0.29126360085837655,"velocity":1.1650544034335064,"acceleration":3.9999999999999987,"pose":{"translation":{"x":1.8093165329850407,"y":-2.31510673816049},"rotation":{"radians":-0.006214537421300872}},"curvature":0.010262108695263363},{"time":0.3582272811631442,"velocity":1.4329091246525767,"acceleration":4.0,"pose":{"translation":{"x":1.8962996475995293,"y":-2.315615003996086},"rotation":{"radians":-0.005532904856197329}},"curvature":0.005996263181397757},{"time":0.4247178120294986,"velocity":1.6988712481179944,"acceleration":4.000000000000002,"pose":{"translation":{"x":2.000415072981678,"y":-2.3161634275928744},"rotation":{"radians":-0.005039840497117272}},"curvature":0.0037792530506272166},{"time":0.48967751039048535,"velocity":1.9587100415619416,"acceleration":3.9999999999999893,"pose":{"translation":{"x":2.1192113689257215,"y":-2.316738666286958},"rotation":{"radians":-0.0046665773718276736}},"curvature":0.0026525886672486274},{"time":0.5211738074032778,"velocity":2.084695229613111,"acceleration":3.9999999999999933,"pose":{"translation":{"x":2.18288684602207,"y":-2.317030681391432},"rotation":{"radians":-0.004508805721396696}},"curvature":0.0023250237048777172},{"time":0.5517973546329684,"velocity":2.2071894185318737,"acceleration":4.0,"pose":{"translation":{"x":2.248602565810843,"y":-2.317322139445587},"rotation":{"radians":-0.004363819082991022}},"curvature":0.0021040995089735234},{"time":0.5813952892569915,"velocity":2.325581157027966,"acceleration":3.0863711086174437,"pose":{"translation":{"x":2.315682270679263,"y":-2.31761024782456},"rotation":{"radians":-0.004227674108169131}},"curvature":0.0019683194092841986},{"time":0.609978908058461,"velocity":2.413800812276556,"acceleration":-3.9999999999999933,"pose":{"translation":{"x":2.3834160278296714,"y":-2.317892151968046},"rotation":{"radians":-0.0040968776842529794}},"curvature":0.0019052546752226036},{"time":0.6386933358140949,"velocity":2.2989431012540202,"acceleration":-4.0,"pose":{"translation":{"x":2.4510773500054253,"y":-2.318165001739701},"rotation":{"radians":-0.0039681904771730085}},"curvature":0.001909770538576822},{"time":0.6685534614740198,"velocity":2.1795025986143206,"acceleration":-3.9999999999999933,"pose":{"translation":{"x":2.517940316216782,"y":-2.318426017786541},"rotation":{"radians":-0.0038384445909229676}},"curvature":0.0019833496346190354},{"time":0.6994144683799933,"velocity":2.0560585709904267,"acceleration":-3.9999999999999964,"pose":{"translation":{"x":2.583296692466793,"y":-2.3186725578983505},"rotation":{"radians":-0.003704364009272917}},"curvature":0.002134423240000436},{"time":0.7311193928956206,"velocity":1.929238872927918,"acceleration":-4.0,"pose":{"translation":{"x":2.6464730524771953,"y":-2.318902183367081},"rotation":{"radians":-0.003562375965213496}},"curvature":0.002379784285935144},{"time":0.7963865968448722,"velocity":1.6681700571309113,"acceleration":-4.000000000000001,"pose":{"translation":{"x":2.7638687816148764,"y":-2.319302351210369},"rotation":{"radians":-0.003237626203522703}},"curvature":0.0032804752312573255},{"time":0.862979121960345,"velocity":1.4017999566690202,"acceleration":-4.000000000000001,"pose":{"translation":{"x":2.8660868353612363,"y":-2.3196136033526837},"rotation":{"radians":-0.0028212080559704344}},"curvature":0.005135608410691401},{"time":0.9297234962358286,"velocity":1.1348224595670855,"acceleration":-4.0,"pose":{"translation":{"x":2.950739195403628,"y":-2.3198305248667666},"rotation":{"radians":-0.002252114531954038}},"curvature":0.008854161979101095},{"time":0.9965438068079335,"velocity":0.8675412172786658,"acceleration":-4.0,"pose":{"translation":{"x":3.0176383566138476,"y":-2.319957327362707},"rotation":{"radians":-0.001466708977573865}},"curvature":0.015315329026373479},{"time":1.2134291111276,"velocity":0.0,"acceleration":-4.0,"pose":{"translation":{"x":3.111716807318168,"y":-2.320018295432578},"rotation":{"radians":2.803592695601656E-14}},"curvature":1.0107951236368945E-13}]

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

@@ -1 +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}]
+[{"time":0.0,"velocity":0.0,"acceleration":4.000000000000001,"pose":{"translation":{"x":2.957088191715518,"y":-2.3571291631772207},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.21808500530414088,"velocity":0.8723400212165637,"acceleration":4.000000000000002,"pose":{"translation":{"x":3.0522103307783257,"y":-2.3571275191873893},"rotation":{"radians":5.101157578230443E-5}},"curvature":0.001038792914185728},{"time":0.3078210770047905,"velocity":1.2312843080191624,"acceleration":4.0,"pose":{"translation":{"x":3.146595821982689,"y":-2.357116740977926},"rotation":{"radians":1.9099520049927404E-4}},"curvature":0.0019054639517066963},{"time":0.3758336533995869,"velocity":1.503334613598348,"acceleration":4.0,"pose":{"translation":{"x":3.2395900571900658,"y":-2.357089632328051},"rotation":{"radians":4.035864972296716E-4}},"curvature":0.002652082514494386},{"time":0.43216660310526034,"velocity":1.7286664124210418,"acceleration":4.000000000000003,"pose":{"translation":{"x":3.3306241198087982,"y":-2.357040972350315},"rotation":{"radians":6.754485548111559E-4}},"curvature":0.0033104753166963616},{"time":0.4806882847981965,"velocity":1.9227531391927866,"acceleration":4.000000000000011,"pose":{"translation":{"x":3.419210597831801,"y":-2.3569673627586405},"rotation":{"radians":9.950959992412528E-4}},"curvature":0.0038981243771801294},{"time":0.5233790748522218,"velocity":2.093516299408888,"acceleration":4.0,"pose":{"translation":{"x":3.5049393968742493,"y":-2.356867075136363},"rotation":{"radians":0.0013520244174984744}},"curvature":0.0044210302444997965},{"time":0.561420326630057,"velocity":2.245681306520229,"acceleration":4.000000000000005,"pose":{"translation":{"x":3.587473553211266,"y":-2.3567398982042733},"rotation":{"radians":0.0017360184754233884}},"curvature":0.004874609692377478},{"time":0.5955909702838039,"velocity":2.3823638811352166,"acceleration":3.9999999999999942,"pose":{"translation":{"x":3.6665450468156116,"y":-2.3565869850886587},"rotation":{"radians":0.002136559645089136}},"curvature":0.005243133825564394},{"time":0.6264435255500808,"velocity":2.505774102200324,"acceleration":1.6029015279156391,"pose":{"translation":{"x":3.7419506143953716,"y":-2.3564107005893433},"rotation":{"radians":0.0025422828689825997}},"curvature":0.0054979122679133},{"time":0.6547599636139544,"velocity":2.5511625640380355,"acceleration":-4.0,"pose":{"translation":{"x":3.813547562431645,"y":-2.3562144684477317},"rotation":{"radians":0.0029404502277413256}},"curvature":0.005594270462982216},{"time":0.6818741541688004,"velocity":2.4427058018186516,"acceleration":-4.0,"pose":{"translation":{"x":3.881249580216232,"y":-2.3560026186148484},"rotation":{"radians":0.003316424361517827}},"curvature":0.005467307107409776},{"time":0.7085651194152542,"velocity":2.335941940832836,"acceleration":-3.999999999999996,"pose":{"translation":{"x":3.9450225528893235,"y":-2.3557802345193797},"rotation":{"radians":0.00365313891813889}},"curvature":0.005026461101145579},{"time":0.7604702537843829,"velocity":2.1283214033563214,"acceleration":-3.999999999999998,"pose":{"translation":{"x":4.06088076092986,"y":-2.355327048251995},"rotation":{"radians":0.004125323787162938}},"curvature":0.0026737820186648843},{"time":0.8101789654223723,"velocity":1.9294865568043638,"acceleration":-4.0,"pose":{"translation":{"x":4.161734080074728,"y":-2.354904842813889},"rotation":{"radians":0.004154108750296397}},"curvature":-0.0029414739347258},{"time":0.8576157563080513,"velocity":1.739739393261648,"acceleration":-3.999999999999997,"pose":{"translation":{"x":4.24876157183169,"y":-2.3545658321706227},"rotation":{"radians":0.0034781161359367808}},"curvature":-0.01397812424405661},{"time":0.9029851741082922,"velocity":1.5582617220606847,"acceleration":-4.0,"pose":{"translation":{"x":4.323575482900814,"y":-2.35435962366233},"rotation":{"radians":0.0018016415428211323}},"curvature":-0.032741911322928474},{"time":0.9902889053471235,"velocity":1.2090467971053591,"acceleration":-4.000000000000001,"pose":{"translation":{"x":4.444373578973541,"y":-2.354501684748035},"rotation":{"radians":-0.005323585518118219}},"curvature":-0.0899279066882791},{"time":1.0845913856635665,"velocity":0.8318368758395872,"acceleration":-3.9999999999999996,"pose":{"translation":{"x":4.5405986995686,"y":-2.3554900302219766},"rotation":{"radians":-0.015505320196648706}},"curvature":-0.10506063032999254},{"time":1.2925506046234634,"velocity":0.0,"acceleration":-3.9999999999999996,"pose":{"translation":{"x":4.627077240224189,"y":-2.3571291631772207},"rotation":{"radians":-0.02086328407244092}},"curvature":2.0691998138625314E-15}]

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

@@ -220,6 +220,20 @@ public final class Constants {
         public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
         public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;
         public static final double GRAV = 385.83;
+
+            //Galactic Search
+        public static final double searchError = 2;
+        /*public static final double bothCloseVisibleY = -17.69;
+        public static final double closeLeftVisibleY = -12.57;
+        public static final double closeRightVisibleY = -11.35;
+        public static final double farLeftVisibleX = 3.58;
+        public static final double farRightVisibleX = 7.04;*/
+
+        public static final double[] aRed = {1.6, -11.7};
+        public static final double[] bRed = {2.5, -5.5};
+        public static final double[] aBlue = {9.9, 9.0};
+        public static final double[] bBlue = {5.5, 13.3};
+
         }
 
     public static final class OIConstants {

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

@@ -76,6 +76,7 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledPeriodic() {
     m_robotContainer.resetOdometry(new Pose2d());
+    m_robotContainer.idenPath();
   }
 
   /**
@@ -83,6 +84,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
+    //m_robotContainer.buildAutos();
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
@@ -106,6 +108,9 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.schedule();
       SmartDashboard.putString("Is Auto Start?", "YEA");
     }
+    else{
+
+    }
   }
 
   /**

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

@@ -8,7 +8,6 @@
 package frc4388.robot;
 
 import java.nio.file.Path;
-import java.util.List;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
@@ -18,43 +17,37 @@ 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.Transform2d;
-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.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.DriveOffLineBackward;
+import frc4388.robot.commands.auto.DriveOffLineForward;
+import frc4388.robot.commands.auto.EightBallAutoMiddle;
+import frc4388.robot.commands.auto.FiveBallAutoMiddle;
+import frc4388.robot.commands.auto.GalacticSearch;
+import frc4388.robot.commands.auto.IdentifyPath;
+import frc4388.robot.commands.auto.SequentialTest;
+import frc4388.robot.commands.auto.SixBallAutoMiddle;
+import frc4388.robot.commands.auto.Slalom;
+import frc4388.robot.commands.auto.TenBallAutoMiddle;
 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;
 import frc4388.robot.commands.drive.SkipSong;
@@ -117,6 +110,8 @@ public class RobotContainer {
     public final LimeLight m_robotLime = new LimeLight();
 
     /* Controllers */
+
+    public boolean isGS = false;
     private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private static XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
     private static XboxController m_buttonFox = new XboxController(OIConstants. BUTTON_FOX_ID);
@@ -141,7 +136,6 @@ public class RobotContainer {
 
     TenBallAutoMiddle m_tenBallAutoMiddle;
 
-
     Slalom m_slalom;
 
     Barrel m_barrel;
@@ -154,8 +148,9 @@ public class RobotContainer {
 
     SequentialTest m_sequentialTest;
 
-    public static boolean m_isShooterManual = false;
+    GalacticSearch m_galacticSearch;
 
+    public static boolean m_isShooterManual = false;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -314,7 +309,6 @@ public class RobotContainer {
 
 
 
-
         /* Button Fox */
         // Storage Manual
         new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH)
@@ -427,18 +421,24 @@ public class RobotContainer {
             "SixBallMidComplete",
             "TenBallMidComplete"
         };
+        m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
+                m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
 
-        //m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
+        String[] galacticSearchPaths = new String[]{
-    
+              "GSC_ARED",
-        String[] sequentialTestPaths = new String[]{
+              "GSC_ABLUE",
-            "Seq1",
+              "GSC_BRED",
-            "Seq2"
+              "GSC_BBLUE"
         };
 
-        m_sequentialTest = new SequentialTest(this, buildPaths(sequentialTestPaths));
+        m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
 
     }
 
+    public void idenPath()
+    {
+        m_robotLime.identifyPath();
+    }
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
      *
@@ -449,21 +449,21 @@ public class RobotContainer {
         //TrajectoryConfig config = getTrajectoryConfig();
         //Trajectory trajectory = getTrajectory(config);
         //RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
-
         // Run path following command, then stop at the end.
         try {
             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_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_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_bounce.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
+            return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
+
 
         } catch (Exception e) {
             System.err.println("ERROR");
@@ -471,6 +471,7 @@ public class RobotContainer {
 
         return new InstantCommand();
     }
+
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
             DriveConstants.MAX_SPEED_METERS_PER_SECOND,
@@ -479,21 +480,6 @@ public class RobotContainer {
             .setKinematics(DriveConstants.kDriveKinematics);
     }
 
-    Trajectory getTrajectory(TrajectoryConfig config) {
-        Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-            // Start at the origin facing the +X direction
-            new Pose2d(2.9, -2.4, new Rotation2d(0)),
-            // Pass through these two interior waypoints, making an 's' curve path
-            List.of(
-                new Translation2d(4.1, -1.7)
-            ),
-            // End 3 meters straight ahead of where we started, facing forward
-            new Pose2d(5.1, -0.7, new Rotation2d(0)),
-            // Pass config
-            config);
-        return exampleTrajectory;
-    }
-
     public RamseteCommand getRamseteCommand(Trajectory trajectory) {
         RamseteCommand ramseteCommand = new RamseteCommand(
             trajectory,
@@ -521,8 +507,8 @@ public class RobotContainer {
                 SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString());
 
                 Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
-                initialTrajectory = trajectory;
 
+                initialTrajectory = trajectory;
                 RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
                 ramseteCommands[0] = ramseteCommand;
                 times[0] = initialTrajectory.getTotalTimeSeconds();

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

@@ -27,17 +27,9 @@ public class Bounce extends SequentialCommandGroup {
     // 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)),
+      paths[3]
-      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/ExecuteCommand.java

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandBase;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import frc4388.robot.subsystems.LimeLight;
+
+public class ExecuteCommand extends CommandBase {
+  /**
+   * Creates a new ExecuteCommand.
+   */
+  RamseteCommand[] m_paths;
+  LimeLight m_limeLight;
+  public ExecuteCommand(LimeLight limeLight, RamseteCommand[] paths) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_limeLight = limeLight;
+    m_paths = paths;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    String gsPath = m_limeLight.galacticSearchPath;
+    switch (gsPath)
+    {
+      case "A_RED":
+        new RunPath(m_paths[0]);
+        break;
+      case "A_BLUE":
+        new RunPath(m_paths[1]);
+        break;
+      case "B_RED":
+        new RunPath(m_paths[2]);
+        break;
+      case "B_BLUE":
+        new RunPath(m_paths[3]);
+        break;
+      case "test":
+        new RunPath(m_paths[0]);
+        break;
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.commands.intake.RunIntake;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.LimeLight;
+
+// 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 GalacticSearch extends SequentialCommandGroup {
+  /**
+   * Creates a new GalacticSearch.
+   */
+  public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) {
+    // addCommands(
+    //   new IdentifyPath(m_limeLight),
+    //   new ExecuteCommand(m_limeLight, paths)
+    // );
+   if (m_limeLight.galacticSearchPath == "A_RED")
+    {
+      addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
+    }
+    else if (m_limeLight.galacticSearchPath == "A_BLUE")
+    {
+      addCommands(new ParallelCommandGroup(paths[1], new RunIntake(m_intake)));
+    }
+    else if (m_limeLight.galacticSearchPath == "B_RED")
+    {
+      addCommands(new ParallelCommandGroup(paths[2], new RunIntake(m_intake)));
+    }
+    else if (m_limeLight.galacticSearchPath == "B_BLUE")
+    {
+      addCommands(new ParallelCommandGroup(paths[3], new RunIntake(m_intake)));
+    }
+    else if (m_limeLight.galacticSearchPath == "test")
+    {
+      addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake)));
+    }
+  }
+}

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

@@ -0,0 +1,163 @@
+/*----------------------------------------------------------------------------*/
+/* 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 javax.lang.model.util.ElementScanner6;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.LimeLight;
+
+public class IdentifyPath extends CommandBase {
+
+  LimeLight m_limeLight;
+  double xAngle;
+  double yAngle;
+  double target;
+  public String path;
+  boolean closeVisible;
+  boolean finished;
+  boolean pathFound;
+
+  public IdentifyPath(LimeLight limeLight) {
+    m_limeLight = limeLight;
+    addRequirements(m_limeLight);
+    m_limeLight.limeOff();
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    path = "";
+    closeVisible = false;
+    pathFound = false;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    target = m_limeLight.getV();
+    xAngle = m_limeLight.getX();
+    yAngle = m_limeLight.getY();
+    //m_limeLight.limeOn();
+    //     //Identify which of four paths
+    //   m_limeLight.changePipeline(1);//Dual Targetting Lowest
+    //   if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS
+    //   {
+    //     closeVisible = true;
+    //   }
+    //   else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS
+    //   {
+    //     closeVisible = false;
+    //   }
+
+    //   if (closeVisible)
+    //   {
+    //      m_limeLight.changePipeline(2); //Dual Targetting Highest
+    //     if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH
+    //     {
+    //       path = "A_BLUE";
+    //     }
+    //     if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH
+    //     {
+    //       path = "B_BLUE";
+    //     }
+    //   }
+
+    //   else{
+    //     m_limeLight.changePipeline(1); //Dual Targetting Lowest
+    //     if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH
+    //     {
+    //       path = "A_RED";
+    //     }
+    //     else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH
+    //     {
+    //       path = "B_RED";
+    //     }
+    //   }
+
+    SmartDashboard.putBoolean("PathFound", pathFound);
+    //   System.out.println("If you see this message a bunch of times in a row, IdentifyPath.java is stuck trying to find the path for GalacticSearch");
+    // System.out.println(path);
+
+
+    m_limeLight.changePipeline(1);
+    if (withinCoords(VisionConstants.aBlue))
+    {
+      pathFound = true;
+      path = "A_BLUE";
+    }
+
+    m_limeLight.changePipeline(2);
+    if (withinCoords(VisionConstants.bBlue))
+    {
+      pathFound = true;
+      path = "B_BLUE";
+    }
+
+    m_limeLight.changePipeline(1);
+    if (withinCoords(VisionConstants.aRed))
+    {
+      pathFound = true;
+      path = "A_RED";
+    }
+
+    m_limeLight.changePipeline(1);
+    if (withinCoords(VisionConstants.bRed))
+    {
+      pathFound = true;
+      path = "B_RED";
+    }
+
+    path = "test";
+  }
+
+  public boolean withinError(double angle, double goal)
+  {
+    if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError))
+    {
+      return true;
+    }
+    else
+    {
+      return false;
+    }
+  }
+
+  public boolean withinCoords(double[] coords)
+  {
+    if (withinError(xAngle, coords[0]) && withinError(yAngle, coords[1]))
+    {
+      return true;
+    }
+    else
+    {
+      return false;
+    }
+  }
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (path != "")
+    {
+      SmartDashboard.putString("GalacticSearchPath", path);
+      m_limeLight.galacticSearchPath = path;
+      //m_limeLight.limeOff();
+      return true;
+    }
+    return false;
+  }
+}

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

src/main/java/frc4388/robot/commands/shooter/TrackTarget.java

@@ -59,6 +59,7 @@ public class TrackTarget extends CommandBase {
     // Vision Processing Mode
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -540,6 +540,7 @@ public class Drive extends SubsystemBase {
     m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
   }
 
+
   /**
    * Resets the yaw of the pigeon
    */

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

@@ -8,12 +8,16 @@
 package frc4388.robot.subsystems;
 
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.VisionConstants;
 
 public class LimeLight extends SubsystemBase {
   /**
    * Creates a new LimeLight.
    */
+  public String galacticSearchPath;
+
   public LimeLight() {
 
   }
@@ -28,6 +32,102 @@ public class LimeLight extends SubsystemBase {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
+  public void changePipeline(int pipelineId)
+  {
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
+  }
+
+  public double getV()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
+  }
+  public double getX()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
+  }
+  public double getY()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
+  }
+
+  int i = 0;
+  boolean onceThrough = false;
+  boolean pathFound = false;
+  public void identifyPath(){
+
+    if (!onceThrough)
+    {
+      changePipeline(2);
+      if (withinCoords(VisionConstants.aBlue))
+      {
+        pathFound = true;
+        galacticSearchPath = "A_BLUE";
+      }
+
+      changePipeline(4);
+      if (withinCoords(VisionConstants.bBlue))
+      {
+        pathFound = true;
+        galacticSearchPath = "B_BLUE";
+      }
+
+      changePipeline(1);
+      if (withinCoords(VisionConstants.aRed))
+      {
+        pathFound = true;
+        galacticSearchPath = "A_RED";
+      }
+
+      changePipeline(3);
+      if (withinCoords(VisionConstants.bRed))
+      {
+        pathFound = true;
+        galacticSearchPath = "B_RED";
+      }
+
+      if (pathFound == false)
+      {
+        galacticSearchPath = "PathNotFound";
+      }
+      SmartDashboard.putString("GalacticSearchPath", galacticSearchPath);
+      onceThrough = true;
+    }
+    else{
+      i++;
+      SmartDashboard.putNumber("Counter", i);
+    }
+    if (i >= 50)
+    {
+      i=0;
+      pathFound = false;
+      onceThrough = false;
+    }
+  }
+
+public boolean withinError(double angle, double goal)
+{
+  if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError))
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+public boolean withinCoords(double[] coords)
+{
+  if (withinError(getX(), coords[0]) && withinError(getY(), coords[1]))
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run