Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-08 16:28:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #61 from Team4388/GalacticSearch

Browse Source 
Galactic search
This commit is contained in:
Evan Goertzen
2021-04-22 17:44:43 -06:00
committed by GitHub
parent e914c511a8 1327ceb2e0
commit 8a3db9133f
50 changed files with 533 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
PathWeaver/Groups/GalacticSearch
+4
View File
@@ -0,0 +1,4 @@
GSC_ARED
GSC_ABLUE
GSC_BRED
GSC_BBLUE
PathWeaver/Paths/Bounce4
+2 -2
View File
@@ -1,3 +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,
0.0,0.0,10.0,0.0,true,
10.0,-10.0,0.0,-10.0,true,
PathWeaver/Paths/GSC_ABLUE
+6
View File
@@ -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
+4 -2
View File
@@ -1,3 +1,5 @@
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,
1.4479129034336145,-1.621096952908587,1.2617695033176577,-1.0638448753462597,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
+5
View File
@@ -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
+6
View File
@@ -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
+5 -2
View File
@@ -1,3 +1,6 @@
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,
2.208685692198673,-0.8974350318881658,0.3896641113186885,-0.6185144624106166,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,
PathWeaver/pathweaver.json
+1 -1
View File
@@ -6,4 +6,4 @@
"wheelBase": 0.648,
"gameName": "Galactic Search A",
"outputDir": ".."
}
}
src/main/deploy/paths/Barrel.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/BarrelManyWaypoints.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/BarrelStart.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Bounce.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Bounce1.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Bounce2.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Bounce3.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Bounce4.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/DriveOffLineBackward.wpilib.json
+1 -1
View File
@@ -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
View File
@@ -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/EightBallMid0.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMid1.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMid2.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/FiveBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/GSC_ABLUE.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/GSC_ARED.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/GSC_ARED_save.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/GSC_BBLUE.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/GSC_BRED.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Galactic Search ABLUE.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Galactic Search ARED.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Galactic Search BBLUE.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Galactic Search BRED.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Seq1.wpilib.json
+1 -1
View File
@@ -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
View File
@@ -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/deploy/paths/SixBallMid0.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/SixBallMid1.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/SixBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Slalom.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/TenBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/Constants.java
+14
View File
@@ -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
+5
View File
@@ -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
+40 -54
View File
@@ -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.
@@ -210,7 +205,7 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
@@ -269,7 +264,7 @@ public class RobotContainer {
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
//.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
@@ -314,7 +309,6 @@ public class RobotContainer {
/* Button Fox */
// Storage Manual
new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH)
@@ -377,7 +371,7 @@ public class RobotContainer {
};
m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
String[] barrelStart = new String[]{
"Barrel"
};
@@ -392,13 +386,13 @@ public class RobotContainer {
};
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"
};
@@ -416,7 +410,7 @@ public class RobotContainer {
};
m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
String[] fiveBallAutoMiddlePaths = new String[]{
"FiveBallMidComplete"
};
@@ -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[] sequentialTestPaths = new String[]{
"Seq1",
"Seq2"
String[] galacticSearchPaths = new String[]{
"GSC_ARED",
"GSC_ABLUE",
"GSC_BRED",
"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,
@@ -517,12 +503,12 @@ public class RobotContainer {
String path = paths[0];
String trajectoryJSON = "paths/" + path + ".wpilib.json";
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
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();
@@ -565,7 +551,7 @@ public class RobotContainer {
}
/**
*
*
*/
public void shiftClimberRachet(boolean state) {
m_robotClimber.shiftServo(state);
src/main/java/frc4388/robot/commands/auto/Bounce.java
+1 -9
View File
@@ -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)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
paths[3],
new InstantCommand(() -> drive.switchReversed(false)),
new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d()))
paths[3]
);
}
}
src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java
+65
View File
@@ -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
+51
View File
@@ -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
+163
View File
@@ -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
+27
View File
@@ -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
+1
View File
@@ -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
+1
View File
@@ -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
+101 -1
View File
@@ -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() {
}
@@ -22,12 +26,108 @@ public class LimeLight extends SubsystemBase {
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
}
public void limeOn(){
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
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