diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..8fc6a4a 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -16,6 +16,8 @@ "name": "WPILib roboRIO Debug", "request": "launch", "desktop": false, + } + ] -} +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..9af17c2 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,62 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "type": "gradle", + "id": "c:\\Users\\uahs\\FRC\\FRC2023buildFRC2023", + "script": "build", + "description": "Assembles and tests this project.", + "group": { + "kind": "build", + "isDefault": true + }, + "project": "FRC2023", + "buildFile": "C:\\Users\\uahs\\FRC\\FRC2023\\build.gradle", + "rootProject": "FRC2023", + "projectFolder": "c:\\Users\\uahs\\FRC\\FRC2023", + "workspaceFolder": "c:\\Users\\uahs\\FRC\\FRC2023", + "args": "", + "javaDebug": false, + "problemMatcher": [ + "$gradle" + ], + "label": "gradle: build" + }, + { + "type": "gradle", + "id": "c:\\Users\\uahs\\FRC\\FRC2023deployFRC2023", + "script": "deploy", + "description": "Deploy all artifacts on all targets", + "group": "deployutils", + "project": "FRC2023", + "buildFile": "C:\\Users\\uahs\\FRC\\FRC2023\\build.gradle", + "rootProject": "FRC2023", + "projectFolder": "c:\\Users\\uahs\\FRC\\FRC2023", + "workspaceFolder": "c:\\Users\\uahs\\FRC\\FRC2023", + "args": "", + "javaDebug": false, + "problemMatcher": [ + "$gradle" + ], + "label": "gradle: deploy" + }, + { + "type": "gradle", + "id": "c:\\Users\\uahs\\FRC\\FRC2023simulateJavaFRC2023", + "script": "simulateJava", + "description": "", + "group": "other", + "project": "FRC2023", + "buildFile": "C:\\Users\\uahs\\FRC\\FRC2023\\build.gradle", + "rootProject": "FRC2023", + "projectFolder": "c:\\Users\\uahs\\FRC\\FRC2023", + "workspaceFolder": "c:\\Users\\uahs\\FRC\\FRC2023", + "args": "", + "javaDebug": false, + "problemMatcher": [ + "$gradle" + ], + "label": "gradle: simulateJava" + } + ] +} \ No newline at end of file diff --git a/FRC_20230322_235755.wpilog b/FRC_20230322_235755.wpilog new file mode 100644 index 0000000..3b05a6e Binary files /dev/null and b/FRC_20230322_235755.wpilog differ diff --git a/FRC_20230323_000335.wpilog b/FRC_20230323_000335.wpilog new file mode 100644 index 0000000..4b39558 Binary files /dev/null and b/FRC_20230323_000335.wpilog differ diff --git a/FRC_20230323_000749.wpilog b/FRC_20230323_000749.wpilog new file mode 100644 index 0000000..6340ac3 Binary files /dev/null and b/FRC_20230323_000749.wpilog differ diff --git a/FRC_20230323_012104.wpilog b/FRC_20230323_012104.wpilog new file mode 100644 index 0000000..2be84ff Binary files /dev/null and b/FRC_20230323_012104.wpilog differ diff --git a/FRC_20230323_012830.wpilog b/FRC_20230323_012830.wpilog new file mode 100644 index 0000000..9281adf Binary files /dev/null and b/FRC_20230323_012830.wpilog differ diff --git a/FRC_20230323_013221.wpilog b/FRC_20230323_013221.wpilog new file mode 100644 index 0000000..45a752c Binary files /dev/null and b/FRC_20230323_013221.wpilog differ diff --git a/PathWeaver/Autos/Simple b/PathWeaver/Autos/Simple new file mode 100644 index 0000000..e69de29 diff --git a/PathWeaver/Paths/Unnamed.path b/PathWeaver/Paths/Unnamed.path new file mode 100644 index 0000000..e788503 --- /dev/null +++ b/PathWeaver/Paths/Unnamed.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +1.756467530029566,-3.1428612868388037,2.00341992,0.0,true,false, +4.838755874880599,-3.2752294979673753,0.5105631000673485,-0.01890974444693949,true,false, diff --git a/PathWeaver/output/Unnamed.wpilib.json b/PathWeaver/output/Unnamed.wpilib.json new file mode 100644 index 0000000..44d8f39 --- /dev/null +++ b/PathWeaver/output/Unnamed.wpilib.json @@ -0,0 +1,602 @@ +[ +{ +"acceleration": 5.0, +"curvature": 0.0, +"pose": { +"rotation": { +"radians": 0.0 +}, +"translation": { +"x": 1.756467530029566, +"y": 4.870818393161196 +} +}, +"time": 0.0, +"velocity": 0.0 +}, +{ +"acceleration": 5.0, +"curvature": -0.04952397800223459, +"pose": { +"rotation": { +"radians": -0.0016755165712417123 +}, +"translation": { +"x": 1.819560807699957, +"y": 4.870782051235034 +} +}, +"time": 0.15886256719179875, +"velocity": 0.7943128359589937 +}, +{ +"acceleration": 5.000000000000002, +"curvature": -0.07498285143005556, +"pose": { +"rotation": { +"radians": -0.005919055387292468 +}, +"translation": { +"x": 1.8853771558683816, +"y": 4.87054126871536 +} +}, +"time": 0.2270771470168166, +"velocity": 1.135385735084083 +}, +{ +"acceleration": 4.9999999999999964, +"curvature": -0.07900094356114654, +"pose": { +"rotation": { +"radians": -0.011467262061689673 +}, +"translation": { +"x": 1.956120028570464, +"y": 4.869927848312823 +} +}, +"time": 0.2825990862818267, +"velocity": 1.4129954314091335 +}, +{ +"acceleration": 5.000000000000001, +"curvature": -0.07110097553183925, +"pose": { +"rotation": { +"radians": -0.01731456015495924 +}, +"translation": { +"x": 2.033461631538293, +"y": 4.868810733220677 +} +}, +"time": 0.33286951150256083, +"velocity": 1.6643475575128042 +}, +{ +"acceleration": 5.000000000000002, +"curvature": -0.05916975446809144, +"pose": { +"rotation": { +"radians": -0.02286104241311815 +}, +"translation": { +"x": 2.1185820888214235, +"y": 4.867093369680655 +} +}, +"time": 0.38060113464535494, +"velocity": 1.9030056732267748 +}, +{ +"acceleration": 5.000000000000002, +"curvature": -0.047429610061624626, +"pose": { +"rotation": { +"radians": -0.027833944367337847 +}, +"translation": { +"x": 2.212208609407877, +"y": 4.864711069548845 +} +}, +"time": 0.4269894066084668, +"velocity": 2.134947033042334 +}, +{ +"acceleration": 4.999999999999997, +"curvature": -0.03747197550875067, +"pose": { +"rotation": { +"radians": -0.03215982845174818 +}, +"translation": { +"x": 2.314654653845145, +"y": 4.861628372861564 +} +}, +"time": 0.47256419582393006, +"velocity": 2.3628209791196504 +}, +{ +"acceleration": 5.000000000000003, +"curvature": -0.029567170225579607, +"pose": { +"rotation": { +"radians": -0.035865481483146715 +}, +"translation": { +"x": 2.4258591008611896, +"y": 4.857836410401236 +} +}, +"time": 0.5175176817342713, +"velocity": 2.5875884086713565 +}, +{ +"acceleration": 5.000000000000005, +"curvature": -0.023476451789962688, +"pose": { +"rotation": { +"radians": -0.03901841997693912 +}, +"translation": { +"x": 2.5454254139854453, +"y": 4.853350266262263 +} +}, +"time": 0.561858281476831, +"velocity": 2.8092914073841553 +}, +{ +"acceleration": 5.0000000000000036, +"curvature": -0.020997417674310206, +"pose": { +"rotation": { +"radians": -0.04041190394752767 +}, +"translation": { +"x": 2.60813827625839, +"y": 4.8508574905204735 +} +}, +"time": 0.583771944030965, +"velocity": 2.918859720154825 +}, +{ +"acceleration": 5.00000000000002, +"curvature": -0.018835361772300725, +"pose": { +"rotation": { +"radians": -0.04169613753461251 +}, +"translation": { +"x": 2.6726608081698195, +"y": 4.848206340416898 +} +}, +"time": 0.6054919262076718, +"velocity": 3.0274596310383592 +}, +{ +"acceleration": 5.00000000000001, +"curvature": -0.016949029737338354, +"pose": { +"rotation": { +"radians": -0.042880093483323105 +}, +"translation": { +"x": 2.7388665485222425, +"y": 4.845404263849944 +} +}, +"time": 0.6269979881310558, +"velocity": 3.1349899406552795 +}, +{ +"acceleration": 5.0000000000000036, +"curvature": -0.015301432593146988, +"pose": { +"rotation": { +"radians": -0.043972136288279944 +}, +"translation": { +"x": 2.8066154164096973, +"y": 4.842459711281124 +} +}, +"time": 0.6482681604657038, +"velocity": 3.2413408023285197 +}, +{ +"acceleration": 5.0000000000000036, +"curvature": -0.013859933153125142, +"pose": { +"rotation": { +"radians": -0.04497997576971995 +}, +"translation": { +"x": 2.875754935174657, +"y": 4.839382053315232 +} +}, +"time": 0.6692793149560983, +"velocity": 3.3463965747804925 +}, +{ +"acceleration": 4.999999999999997, +"curvature": -0.012596034464373773, +"pose": { +"rotation": { +"radians": -0.04591065851230018 +}, +"translation": { +"x": 2.9461214563649385, +"y": 4.836181498280528 +} +}, +"time": 0.6900076156889032, +"velocity": 3.4500380784445173 +}, +{ +"acceleration": 5.000000000000009, +"curvature": -0.011485017958834939, +"pose": { +"rotation": { +"radians": -0.046770583063665834 +}, +"translation": { +"x": 3.0175413836906078, +"y": 4.8328690098089275 +} +}, +"time": 0.7104288782660162, +"velocity": 3.552144391330082 +}, +{ +"acceleration": 5.000000000000019, +"curvature": -0.010505520489558523, +"pose": { +"rotation": { +"radians": -0.04756552914576065 +}, +"translation": { +"x": 3.089832396980884, +"y": 4.829456224416175 +} +}, +"time": 0.7305188575826859, +"velocity": 3.6525942879134305 +}, +{ +"acceleration": 5.000000000000018, +"curvature": -0.00963910174098629, +"pose": { +"rotation": { +"radians": -0.048300694232534185 +}, +"translation": { +"x": 3.1628046761410493, +"y": 4.8259553690820365 +} +}, +"time": 0.7502534801299624, +"velocity": 3.7512674006498137 +}, +{ +"acceleration": 3.394132407212726, +"curvature": -0.008869829692576539, +"pose": { +"rotation": { +"radians": -0.048980733025688915 +}, +"translation": { +"x": 3.236262125109354, +"y": 4.822379178830481 +} +}, +"time": 0.7696090331832102, +"velocity": 3.848045165916053 +}, +{ +"acceleration": -5.000000000000012, +"curvature": -0.00818389705576209, +"pose": { +"rotation": { +"radians": -0.04960979686933106 +}, +"translation": { +"x": 3.310003595813919, +"y": 4.818740814309857 +} +}, +"time": 0.7886360421084686, +"velocity": 3.9126253535215985 +}, +{ +"acceleration": -5.000000000000012, +"curvature": -0.007569272618031433, +"pose": { +"rotation": { +"radians": -0.0501915711733772 +}, +"translation": { +"x": 3.38382411212965, +"y": 4.815053779373088 +} +}, +"time": 0.8077605153467686, +"velocity": 3.817002987330098 +}, +{ +"acceleration": -5.000000000000006, +"curvature": -0.007015386026771952, +"pose": { +"rotation": { +"radians": -0.05072930960142203 +}, +"translation": { +"x": 3.4575160938351353, +"y": 4.811331838657848 +} +}, +"time": 0.8273425147208368, +"velocity": 3.719092990459757 +}, +{ +"acceleration": -5.000000000000006, +"curvature": -0.006512841278363326, +"pose": { +"rotation": { +"radians": -0.05122586421942116 +}, +"translation": { +"x": 3.530870580569559, +"y": 4.807588935166745 +} +}, +"time": 0.8473613195229344, +"velocity": 3.618998966449269 +}, +{ +"acceleration": -5.000000000000006, +"curvature": -0.0060531520758653435, +"pose": { +"rotation": { +"radians": -0.05168371106562971 +}, +"translation": { +"x": 3.603678455789602, +"y": 4.803839107847508 +} +}, +"time": 0.8677946430737515, +"velocity": 3.516832348695183 +}, +{ +"acceleration": -4.9999999999999964, +"curvature": -0.005628490613561627, +"pose": { +"rotation": { +"radians": -0.05210497073667856 +}, +"translation": { +"x": 3.6757316707263517, +"y": 4.800096409173171 +} +}, +"time": 0.8886186239006003, +"velocity": 3.4127124445609387 +}, +{ +"acceleration": -5.0000000000000036, +"curvature": -0.005231439741817692, +"pose": { +"rotation": { +"radians": -0.05249142362014349 +}, +"translation": { +"x": 3.746824468342208, +"y": 4.796374822722252 +} +}, +"time": 0.9098078085622812, +"velocity": 3.3067665212525346 +}, +{ +"acceleration": -5.0, +"curvature": -0.004854736437507258, +"pose": { +"rotation": { +"radians": -0.05284451935880415 +}, +"translation": { +"x": 3.8167546072877867, +"y": 4.792688180758941 +} +}, +"time": 0.9313351260996824, +"velocity": 3.199129933565528 +}, +{ +"acceleration": -4.9999999999999805, +"curvature": -0.004490991620216842, +"pose": { +"rotation": { +"radians": -0.05316538001446973 +}, +"translation": { +"x": 3.8853245858588306, +"y": 4.789050081813282 +} +}, +"time": 0.953171854129836, +"velocity": 3.0899462934147603 +}, +{ +"acceleration": -4.999999999999993, +"curvature": -0.0041323670867580405, +"pose": { +"rotation": { +"radians": -0.053454796209848915 +}, +"translation": { +"x": 3.9523428659531117, +"y": 4.785473808261355 +} +}, +"time": 0.9752875766910373, +"velocity": 2.979367680608754 +}, +{ +"acceleration": -5.0, +"curvature": -0.0037701839695964004, +"pose": { +"rotation": { +"radians": -0.05371321525765029 +}, +"translation": { +"x": 4.017625097027338, +"y": 4.7819722439054635 +} +}, +"time": 0.9976501341138051, +"velocity": 2.867554893494915 +}, +{ +"acceleration": -5.0, +"curvature": -0.0033944276172928633, +"pose": { +"rotation": { +"radians": -0.05394071992094672 +}, +"translation": { +"x": 4.080995340054061, +"y": 4.778557791554313 +} +}, +"time": 1.020225565475619, +"velocity": 2.7546777366858457 +}, +{ +"acceleration": -5.0, +"curvature": -0.002551346114063216, +"pose": { +"rotation": { +"radians": -0.05430128599909566 +}, +"translation": { +"x": 4.201345507175866, +"y": 4.77203693461419 +} +}, +"time": 1.0658698115182164, +"velocity": 2.526456506472859 +}, +{ +"acceleration": -4.999999999999996, +"curvature": -0.0014652077657104244, +"pose": { +"rotation": { +"radians": -0.05452826224444222 +}, +"translation": { +"x": 4.312200595778248, +"y": 4.765997708085713 +} +}, +"time": 1.1119100803795317, +"velocity": 2.2962551621662826 +}, +{ +"acceleration": -4.999999999999997, +"curvature": 9.928198215151619e-05, +"pose": { +"rotation": { +"radians": -0.05460373963303463 +}, +"translation": { +"x": 4.412580751816422, +"y": 4.760513611847832 +} +}, +"time": 1.158003067749405, +"velocity": 2.0657902253169156 +}, +{ +"acceleration": -4.999999999999997, +"curvature": 0.00257442957399391, +"pose": { +"rotation": { +"radians": -0.0544958374877657 +}, +"translation": { +"x": 4.501758205362103, +"y": 4.7556425375795985 +} +}, +"time": 1.2037711094159596, +"velocity": 1.836950016984144 +}, +{ +"acceleration": -4.999999999999998, +"curvature": 0.00680135396626293, +"pose": { +"rotation": { +"radians": -0.05415136104541591 +}, +"translation": { +"x": 4.579296437224504, +"y": 4.751424131326035 +} +}, +"time": 1.2488037707952988, +"velocity": 1.6117867100874474 +}, +{ +"acceleration": -5.000000000000002, +"curvature": 0.014492448070612654, +"pose": { +"rotation": { +"radians": -0.05348353008341699 +}, +"translation": { +"x": 4.645089345571336, +"y": 4.747877156064014 +} +}, +"time": 1.2926671702991206, +"velocity": 1.392469712568339 +}, +{ +"acceleration": -4.999999999999999, +"curvature": 0.05832182743896331, +"pose": { +"rotation": { +"radians": -0.05054236862620969 +}, +"translation": { +"x": 4.742901870907644, +"y": 4.742752310476593 +} +}, +"time": 1.3752524609677619, +"velocity": 0.9795432592251321 +}, +{ +"acceleration": -4.999999999999999, +"curvature": 1.1686718506781632e-13, +"pose": { +"rotation": { +"radians": -0.037020115873909694 +}, +"translation": { +"x": 4.8387558748806, +"y": 4.7384501820326275 +} +}, +"time": 1.5711611128127883, +"velocity": 0.0 +} +] \ No newline at end of file diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index 1e801ee..1cc4740 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -2,8 +2,8 @@ "lengthUnit": "Meter", "exportUnit": "Always Meters", "maxVelocity": 5.0, - "maxAcceleration": 6.0, - "trackWidth": 1.0, - "gameName": "Rapid React", + "maxAcceleration": 5.0, + "trackWidth": 0.5843, + "gameName": "Charged Up", "outputDir": "" } \ No newline at end of file diff --git a/final-final-final.json b/final-final-final.json index ae0155b..8ddd056 100644 --- a/final-final-final.json +++ b/final-final-final.json @@ -196,7 +196,7 @@ "autoPopulate": true, "autoPopulatePrefix": "SmartDashboard/", "widgetPane": { - "gridSize": 8.0, + "gridSize": 32.0, "showGrid": true, "hgap": 16.0, "vgap": 16.0, diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..8d5e760 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,14 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, + "Keyboard 1 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -18,12 +28,16 @@ } ], "axisCount": 3, - "buttonCount": 4, + "buttonCount": 8, "buttonKeys": [ 90, 88, 67, - 86 + 86, + -1, + -1, + -1, + -1 ], "povConfig": [ { @@ -50,13 +64,17 @@ "incKey": 75 } ], - "axisCount": 2, - "buttonCount": 4, + "axisCount": 6, + "buttonCount": 8, "buttonKeys": [ 77, 44, 46, - 47 + 47, + -1, + -1, + -1, + -1 ], "povCount": 0 }, @@ -88,5 +106,14 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + {}, + { + "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" + } ] } diff --git a/simgui.json b/simgui.json index 4937c92..0f1daaf 100644 --- a/simgui.json +++ b/simgui.json @@ -1,10 +1,18 @@ { "HALProvider": { "Other Devices": { + "SPARK MAX [2]": { + "header": { + "open": true + } + }, "SPARK MAX [3]": { "header": { "open": true } + }, + "window": { + "visible": false } }, "RoboRIO": { @@ -13,9 +21,6 @@ }, "RoboRIO Input": { "open": true - }, - "window": { - "visible": true } } }, @@ -26,6 +31,7 @@ "/LiveWindow/Tank/Drive": "DifferentialDrive", "/LiveWindow/Ungrouped/DigitalInput[1]": "Digital Input", "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", + "/Shuffleboard/Driver/Gyro": "Gyro", "/Shuffleboard/Motors/All": "Motor Controller", "/Shuffleboard/Motors/Drive": "DifferentialDrive", "/Shuffleboard/Motors/Left": "Motor Controller", @@ -41,7 +47,11 @@ "/Shuffleboard/Subsystems/DriveCommand": "Command", "/Shuffleboard/Subsystems/FineDriveCommand": "Command", "/Shuffleboard/Subsystems/IncreaseMaxSpeedCommand": "Command", + "/Shuffleboard/Subsystems/Lineup Controller": "PIDController", + "/Shuffleboard/Subsystems/LineupCommand": "Command", + "/Shuffleboard/Subsystems/LineupPID": "Subsystem", "/Shuffleboard/Subsystems/MoveArmYCommand": "Command", + "/Shuffleboard/Subsystems/OpenClawCommand": "Command", "/Shuffleboard/Subsystems/PDP": "Subsystem", "/Shuffleboard/Subsystems/PID": "Subsystem", "/Shuffleboard/Subsystems/PlaceConeSecondLevelCommand": "Command", @@ -49,6 +59,7 @@ "/Shuffleboard/Subsystems/SwitchIdleModeCommmand": "Command", "/Shuffleboard/Subsystems/Tank": "Subsystem", "/Shuffleboard/Subsystems/Turret": "Subsystem", + "/SmartDashboard/Field": "Field2d", "/SmartDashboard/Scheduler": "Scheduler" }, "windows": { @@ -57,6 +68,14 @@ "visible": true } }, + "/SmartDashboard/Field": { + "Robot": { + "style": "Track" + }, + "window": { + "visible": true + } + }, "/SmartDashboard/Scheduler": { "window": { "visible": true @@ -70,11 +89,34 @@ "open": true }, "Shuffleboard": { + "Subsystems": { + "Lineup Controller": { + "open": true + }, + "open": true + }, "open": true }, "SmartDashboard": { "open": true } } + }, + "NetworkTables Info": { + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, + "Server": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + } } } diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..99c6348 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,357 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import java.util.List; + +/** + * Contains various field dimensions and useful reference points. Dimensions are + * in meters, and sets + * of corners start in the lower left moving clockwise. + * + *

+ * All translations and poses are stored with the origin at the rightmost point + * on the BLUE + * ALLIANCE wall. Use the {@link #allianceFlip(Translation2d)} and + * {@link #allianceFlip(Pose2d)} + * methods to flip these values based on the current alliance color. + */ +public final class FieldConstants { + public static final boolean isWPIField = false; // Red alliance + + public static final double fieldLength = Units.inchesToMeters(651.25); + public static final double fieldWidth = Units.inchesToMeters(315.5) + + (isWPIField ? Units.inchesToMeters(3.0) : 0.0); + public static final double tapeWidth = Units.inchesToMeters(2.0); + + // Dimensions for community and charging station, including the tape. + public static final class Community { + // Region dimensions + public static final double innerX = 0.0; + public static final double midX = Units.inchesToMeters(132.375); // Tape to the left of charging station + public static final double outerX = Units.inchesToMeters(193.25); // Tape to the right of charging station + public static final double leftY = Units.feetToMeters(18.0); + public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth; + public static final double rightY = 0.0; + public static final Translation2d[] regionCorners = new Translation2d[] { + new Translation2d(innerX, rightY), + new Translation2d(innerX, leftY), + new Translation2d(midX, leftY), + new Translation2d(midX, midY), + new Translation2d(outerX, midY), + new Translation2d(outerX, rightY), + }; + + // Charging station dimensions + public static final double chargingStationLength = Units.inchesToMeters(76.125); + public static final double chargingStationWidth = Units.inchesToMeters(97.25); + public static final double chargingStationOuterX = outerX - tapeWidth; + public static final double chargingStationInnerX = chargingStationOuterX - chargingStationLength; + public static final double chargingStationLeftY = midY - tapeWidth; + public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth; + public static final Translation2d[] chargingStationCorners = new Translation2d[] { + new Translation2d(chargingStationInnerX, chargingStationRightY), + new Translation2d(chargingStationInnerX, chargingStationLeftY), + new Translation2d(chargingStationOuterX, chargingStationRightY), + new Translation2d(chargingStationOuterX, chargingStationLeftY) + }; + + // Cable bump + public static final double cableBumpInnerX = innerX + Grids.outerX + Units.inchesToMeters(95.25); + public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7); + public static final Translation2d[] cableBumpCorners = new Translation2d[] { + new Translation2d(cableBumpInnerX, 0.0), + new Translation2d(cableBumpInnerX, chargingStationRightY), + new Translation2d(cableBumpOuterX, 0.0), + new Translation2d(cableBumpOuterX, chargingStationRightY) + }; + } + + // Dimensions for grids and nodes + public static final class Grids { + // X layout + public static final double outerX = Units.inchesToMeters(54.25); + public static final double lowX = outerX - (Units.inchesToMeters(14.25) / 2.0); // Centered when under cube + // nodes + public static final double midX = outerX - Units.inchesToMeters(22.75); + public static final double highX = outerX - Units.inchesToMeters(39.75); + + // Y layout + public static final int nodeRowCount = 9; + public static final double[] nodeY = isWPIField + ? new double[] { + Units.inchesToMeters(20.19 + 22.0 * 0), + Units.inchesToMeters(20.19 + 22.0 * 1), + Units.inchesToMeters(20.19 + 22.0 * 2), + Units.inchesToMeters(20.19 + 22.0 * 3), + Units.inchesToMeters(20.19 + 22.0 * 4), + Units.inchesToMeters(20.19 + 22.0 * 5), + Units.inchesToMeters(20.19 + 22.0 * 6), + Units.inchesToMeters(20.19 + 22.0 * 7), + Units.inchesToMeters(20.19 + 22.0 * 8 + 2.5) + } + : new double[] { + Units.inchesToMeters(20.19 + 22.0 * 0), + Units.inchesToMeters(20.19 + 22.0 * 1), + Units.inchesToMeters(20.19 + 22.0 * 2), + Units.inchesToMeters(20.19 + 22.0 * 3), + Units.inchesToMeters(20.19 + 22.0 * 4), + Units.inchesToMeters(20.19 + 22.0 * 5), + Units.inchesToMeters(20.19 + 22.0 * 6), + Units.inchesToMeters(20.19 + 22.0 * 7), + Units.inchesToMeters(20.19 + 22.0 * 8) + }; + + // Z layout + public static final double cubeEdgeHigh = Units.inchesToMeters(3.0); + public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh; + public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh; + public static final double highConeZ = Units.inchesToMeters(46.0); + public static final double midConeZ = Units.inchesToMeters(34.0); + + // Translations (all nodes in the same column/row have the same X/Y coordinate) + public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] low3dTranslations = new Translation3d[nodeRowCount]; + public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount]; + public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount]; + + static { + for (int i = 0; i < nodeRowCount; i++) { + boolean isCube = i == 1 || i == 4 || i == 7; + lowTranslations[i] = new Translation2d(lowX, nodeY[i]); + low3dTranslations[i] = new Translation3d(lowX, nodeY[i], 0.0); + midTranslations[i] = new Translation2d(midX, nodeY[i]); + mid3dTranslations[i] = new Translation3d(midX, nodeY[i], isCube ? midCubeZ : midConeZ); + highTranslations[i] = new Translation2d(highX, nodeY[i]); + high3dTranslations[i] = new Translation3d(highX, nodeY[i], isCube ? highCubeZ : highConeZ); + } + } + + // Complex low layout (shifted to account for cube vs cone rows and wide edge + // nodes) + public static final double complexLowXCones = outerX - Units.inchesToMeters(16.0) / 2.0; // Centered X under + // cone nodes + public static final double complexLowXCubes = lowX; // Centered X under cube nodes + public static final double complexLowOuterYOffset = nodeY[0] + - (Units.inchesToMeters(3.0) + (Units.inchesToMeters(25.75) / 2.0)); + + public static final Translation2d[] complexLowTranslations = new Translation2d[] { + new Translation2d(complexLowXCones, nodeY[0] - complexLowOuterYOffset), + new Translation2d(complexLowXCubes, nodeY[1]), + new Translation2d(complexLowXCones, nodeY[2]), + new Translation2d(complexLowXCones, nodeY[3]), + new Translation2d(complexLowXCubes, nodeY[4]), + new Translation2d(complexLowXCones, nodeY[5]), + new Translation2d(complexLowXCones, nodeY[6]), + new Translation2d(complexLowXCubes, nodeY[7]), + new Translation2d(complexLowXCones, nodeY[8] + complexLowOuterYOffset), + }; + + public static final Translation3d[] complexLow3dTranslations = new Translation3d[] { + new Translation3d(complexLowXCones, nodeY[0] - complexLowOuterYOffset, 0.0), + new Translation3d(complexLowXCubes, nodeY[1], 0.0), + new Translation3d(complexLowXCones, nodeY[2], 0.0), + new Translation3d(complexLowXCones, nodeY[3], 0.0), + new Translation3d(complexLowXCubes, nodeY[4], 0.0), + new Translation3d(complexLowXCones, nodeY[5], 0.0), + new Translation3d(complexLowXCones, nodeY[6], 0.0), + new Translation3d(complexLowXCubes, nodeY[7], 0.0), + new Translation3d(complexLowXCones, nodeY[8] + complexLowOuterYOffset, 0.0), + }; + } + + // Dimensions for loading zone and substations, including the tape + public static final class LoadingZone { + // Region dimensions + public static final double width = Units.inchesToMeters(99.0); + public static final double innerX = FieldConstants.fieldLength; + public static final double midX = fieldLength - Units.inchesToMeters(132.25); + public static final double outerX = fieldLength - Units.inchesToMeters(264.25); + public static final double leftY = FieldConstants.fieldWidth; + public static final double midY = leftY - Units.inchesToMeters(50.5); + public static final double rightY = leftY - width; + public static final Translation2d[] regionCorners = new Translation2d[] { + new Translation2d( + midX, rightY), // Start at lower left next to border with opponent community + new Translation2d(midX, midY), + new Translation2d(outerX, midY), + new Translation2d(outerX, leftY), + new Translation2d(innerX, leftY), + new Translation2d(innerX, rightY), + }; + + // Double substation dimensions + public static final double doubleSubstationLength = Units.inchesToMeters(14.0); + public static final double doubleSubstationX = innerX - doubleSubstationLength; + public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375); + public static final double doubleSubstationCenterY = fieldWidth - Units.inchesToMeters(49.76); + + // Single substation dimensions + public static final double singleSubstationWidth = Units.inchesToMeters(22.75); + public static final double singleSubstationLeftX = FieldConstants.fieldLength - doubleSubstationLength + - Units.inchesToMeters(88.77); + public static final double singleSubstationCenterX = singleSubstationLeftX + (singleSubstationWidth / 2.0); + public static final double singleSubstationRightX = singleSubstationLeftX + singleSubstationWidth; + public static final Translation2d singleSubstationTranslation = new Translation2d(singleSubstationCenterX, + leftY); + + public static final double singleSubstationHeight = Units.inchesToMeters(18.0); + public static final double singleSubstationLowZ = Units.inchesToMeters(27.125); + public static final double singleSubstationCenterZ = singleSubstationLowZ + (singleSubstationHeight / 2.0); + public static final double singleSubstationHighZ = singleSubstationLowZ + singleSubstationHeight; + } + + // Locations of staged game pieces + public static final class StagingLocations { + public static final double centerOffsetX = Units.inchesToMeters(47.36); + public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36); + public static final double firstY = Units.inchesToMeters(36.19); + public static final double separationY = Units.inchesToMeters(48.0); + public static final Translation2d[] translations = new Translation2d[4]; + + static { + for (int i = 0; i < translations.length; i++) { + translations[i] = new Translation2d(positionX, firstY + (i * separationY)); + } + } + } + + // AprilTag constants + public static final double aprilTagWidth = Units.inchesToMeters(6.0); + public static final AprilTagFieldLayout aprilTags = isWPIField + ? new AprilTagFieldLayout( + List.of( + new AprilTag( + 1, + new Pose3d( + Units.inchesToMeters(610.125), + Units.inchesToMeters(43.5), + Units.inchesToMeters(19.25), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 2, + new Pose3d( + Units.inchesToMeters(610.375), + Units.inchesToMeters(109.5), + Units.inchesToMeters(19.25), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 3, + new Pose3d( + Units.inchesToMeters(610.0), + Units.inchesToMeters(176.0), + Units.inchesToMeters(19.25), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 4, + new Pose3d( + Units.inchesToMeters(635.375), + Units.inchesToMeters(272.0), + Units.inchesToMeters(27.25), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 5, + new Pose3d( + Units.inchesToMeters(14.25), + LoadingZone.doubleSubstationCenterY, + Units.inchesToMeters(27.38), + new Rotation3d())), + new AprilTag( + 6, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[7], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 7, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[4], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 8, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[1], + Units.inchesToMeters(18.22), + new Rotation3d()))), + fieldLength, + fieldWidth) + : new AprilTagFieldLayout( + List.of( + new AprilTag( + 1, + new Pose3d( + Units.inchesToMeters(610.77), + Grids.nodeY[1], + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 2, + new Pose3d( + Units.inchesToMeters(610.77), + Grids.nodeY[4], + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 3, + new Pose3d( + Units.inchesToMeters(610.77), + Grids.nodeY[7], + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 4, + new Pose3d( + Units.inchesToMeters(636.96), + LoadingZone.doubleSubstationCenterY, + Units.inchesToMeters(27.38), + new Rotation3d(0.0, 0.0, Math.PI))), + new AprilTag( + 5, + new Pose3d( + Units.inchesToMeters(14.25), + LoadingZone.doubleSubstationCenterY, + Units.inchesToMeters(27.38), + new Rotation3d())), + new AprilTag( + 6, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[7], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 7, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[4], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 8, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[1], + Units.inchesToMeters(18.22), + new Rotation3d()))), + fieldLength, + fieldWidth); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 04a6bc4..3bb2f33 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ public class RobotContainer { private final Turret m_Turret = new Turret(); private final PID m_PID = new PID(); private final PDP m_PDP = new PDP(); + private final LineupPID m_LineupPID = new LineupPID(); // INSTANTIATES ALL COMMANDS private final OpenClawCommand m_OpenClawCommand = new OpenClawCommand(m_claw); @@ -47,6 +48,7 @@ public class RobotContainer { private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand = new DecreaseMaxSpeedCommand(m_Tank); private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank); + private final LineupCommand m_LineupCommand = new LineupCommand(m_LineupPID); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -66,6 +68,10 @@ public RobotContainer() { tab.add(m_IncreaseMaxSpeedCommand); tab.add(m_MoveArmYCommand); tab.add(m_PDP); + tab.add(m_OpenClawCommand); + tab.add(m_LineupPID); + tab.add(m_LineupCommand); + tab.add(m_PID); tab.add(m_PlaceConeSecondLevelCommand); @@ -86,6 +92,8 @@ private void configureButtonBindings() { .whileTrue(m_IncreaseMaxSpeedCommand); new JoystickButton(m_driverController, XboxController.Button.kBack.value) .whileTrue(m_DecreaseMaxSpeedCommand); + new JoystickButton(m_driverController, XboxController.Button.kA.value) + .whileTrue(m_LineupCommand); new JoystickButton(m_armController2, XboxController.Button.kX.value) .whileTrue(m_CloseClawCommand); diff --git a/src/main/java/frc/robot/commands/LineupCommand.java b/src/main/java/frc/robot/commands/LineupCommand.java new file mode 100644 index 0000000..746bb5e --- /dev/null +++ b/src/main/java/frc/robot/commands/LineupCommand.java @@ -0,0 +1,36 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.LineupPID; +import frc.robot.subsystems.Tank; + +public class LineupCommand extends CommandBase { + private LineupPID m_lineupPID; + private double m_output; + + public LineupCommand(LineupPID lineupPID) { + m_lineupPID = lineupPID; + addRequirements(m_lineupPID); + } + + @Override + public void initialize() { + m_lineupPID.setSetpoint(); + } + + @Override + public void execute() { + m_output = m_lineupPID.m_output; + Tank.arcadeDrive(0, m_output); + } + + @Override + public void end(boolean interrupted) { + m_lineupPID.disable(); + } + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LineupPID.java b/src/main/java/frc/robot/subsystems/LineupPID.java new file mode 100644 index 0000000..8b3b402 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LineupPID.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import frc.robot.subsystems.Vision; +import frc.robot.subsystems.Tank; + +public class LineupPID extends PIDSubsystem { + private static double kP = 0.0; + private static double kI = 0.0; + private static double kD = 0.0; + private PIDController m_controller; + public double m_output; + public double m_setpoint; + + public LineupPID() { + super(new PIDController(kP, kI, kD)); + addChild("Lineup Controller", getController()); + // add that sendable to the shuffleboard + Shuffleboard.getTab("Subsystems").add(getController()); + m_controller = getController(); + + } + + @Override + public void useOutput(double output, double setpoint) { + // Use the output here + m_output = output; + m_setpoint = setpoint; + } + + @Override + public double getMeasurement() { + // Return the distance from the best retroreflective tape pole (determined by the Limelight itself, configured in the Limelight's web interface) to the center of the robot + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0.0); + + + } + + // Set the PID controller's setpoint to the distance from the best retroreflective tape pole (determined by the Limelight itself, configured in the Limelight's web interface) to the center of the robot + public void setSetpoint() { + m_controller.setSetpoint(getMeasurement()); + } + +} diff --git a/src/main/java/frc/robot/subsystems/PID.java b/src/main/java/frc/robot/subsystems/PID.java index 9d9fef9..b8a9e91 100644 --- a/src/main/java/frc/robot/subsystems/PID.java +++ b/src/main/java/frc/robot/subsystems/PID.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class PID extends SubsystemBase { - public AHRS gyro; + private final double kP = 0.18; private final double kI = 0.025; private final double kD = 0.3; @@ -21,6 +21,8 @@ public class PID extends SubsystemBase { public ShuffleboardTab tab; public ShuffleboardTab dash; + public AHRS gyro = Tank.gyro; + private float initPitch; public PIDController pidController; @@ -28,9 +30,7 @@ public class PID extends SubsystemBase { public PID() { toleranceDeg = 0.5; iLimit = 2.0; - gyro = new AHRS(SPI.Port.kMXP); - addChild("Gyro", gyro); - Shuffleboard.getTab("Driver").add("Gyro", gyro); + initPitch = gyro.getPitch(); pidController = new PIDController(P, I, D); diff --git a/src/main/java/frc/robot/subsystems/Tank.java b/src/main/java/frc/robot/subsystems/Tank.java index 731f498..ac258f7 100644 --- a/src/main/java/frc/robot/subsystems/Tank.java +++ b/src/main/java/frc/robot/subsystems/Tank.java @@ -1,16 +1,23 @@ package frc.robot.subsystems; +import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.shuffleboard.*; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Interfaces.*; import frc.robot.RobotContainer; +import edu.wpi.first.wpilibj.SPI; public class Tank extends SubsystemBase { public CANSparkMax leftFront; @@ -22,7 +29,7 @@ public class Tank extends SubsystemBase { public MotorControllerGroup right; public MotorControllerGroup all; - public DifferentialDrive drive; + public static DifferentialDrive drive; public boolean brakeMode; private GenericEntry stallWidget; @@ -55,9 +62,21 @@ public class Tank extends SubsystemBase { private GenericEntry driveWidget; + private DifferentialDriveOdometry odometry; + + public static AHRS gyro; + + private Field2d field2d; + private int initialCurrentLimit = 50; + /** */ public Tank() { + + gyro = new AHRS(SPI.Port.kMXP); + addChild("Gyro", gyro); + Shuffleboard.getTab("Driver").add("Gyro", gyro); + motorsTab = Shuffleboard.getTab("Motors"); leftFront = new CANSparkMax(Constants.CanConstants.kLeftFrontMotorPort, MotorType.kBrushless); @@ -136,39 +155,46 @@ public Tank() { rightFront.setSmartCurrentLimit(40, 60); rightRear.setSmartCurrentLimit(40, 60); - tankLayout = - Shuffleboard.getTab("Config") - .getLayout("Tank", BuiltInLayouts.kList) - .withSize(1, 4) - .withPosition(0, 0); + tankLayout = Shuffleboard.getTab("Config") + .getLayout("Tank", BuiltInLayouts.kList) + .withSize(1, 4) + .withPosition(0, 0); // create a new slider widget for the current limits - stallWidget = - tankLayout.add("Stall Limit", 40).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); - freeWidget = - tankLayout.add("Free Limit", 40).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); + stallWidget = tankLayout.add("Stall Limit", 40).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); + freeWidget = tankLayout.add("Free Limit", 40).withWidget(BuiltInWidgets.kNumberSlider).getEntry(); // create a new boolean box widget for the idle mode idleModeWidget = tankLayout.add("Braking Mode", false).withWidget(BuiltInWidgets.kBooleanBox); // create a new button widget for the idle mode switch - idleModeSwitch = - tankLayout - .add("Switch Idle Mode", false) - .withWidget(BuiltInWidgets.kToggleButton) - .getEntry(); + idleModeSwitch = tankLayout + .add("Switch Idle Mode", false) + .withWidget(BuiltInWidgets.kToggleButton) + .getEntry(); // create a new slider widget for the ramp rate - rampRateWidget = - tankLayout - .add("Ramp Rate", Constants.CanConstants.kRampRate) - .withWidget(BuiltInWidgets.kNumberSlider) - .getEntry(); + rampRateWidget = tankLayout + .add("Ramp Rate", Constants.CanConstants.kRampRate) + .withWidget(BuiltInWidgets.kNumberSlider) + .getEntry(); - // create a new slider widget for the max speed (don't call getEntry() yet, it can be changed + // create a new slider widget for the max speed (don't call getEntry() yet, it + // can be changed // by button inputs) maxSpeedWidget = tankLayout.add("Max Speed", maxSpeed).withWidget(BuiltInWidgets.kNumberSlider); maxSpeedEntry = maxSpeedWidget.getEntry(); + + // implement odometry + odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(gyro.getAngle()), + Units.inchesToMeters( + (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10) * Math.PI), + Units.inchesToMeters( + (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10) * Math.PI)); + + field2d = new Field2d(); + SmartDashboard.putData(field2d); + } @Override @@ -202,6 +228,14 @@ public void periodic() { maxSpeedEntry.setDouble(maxSpeed); // i hate my life + + odometry.update(Rotation2d.fromDegrees(gyro.getAngle()), + Units.inchesToMeters( + (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10) * Math.PI), + Units.inchesToMeters( + (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10) * Math.PI)); + + field2d.setRobotPose(odometry.getPoseMeters()); } public double getMaxSpeed() { @@ -211,10 +245,10 @@ public double getMaxSpeed() { /** * Drives the robot using arcade drive. * - * @param speed The forward/backward speed. + * @param speed The forward/backward speed. * @param rotation The rotation speed. */ - public void arcadeDrive(double speed, double rotation) { + public static void arcadeDrive(double speed, double rotation) { try { drive.arcadeDrive( -speed * Math.pow(Math.abs(speed), 0.5), rotation * Math.pow(Math.abs(rotation), 0.5)); @@ -295,15 +329,13 @@ public void setAllMotors(double speed) { } public double getLeftDistance() { - double numRotations = - (leftFront.getEncoder().getPosition() + leftRear.getEncoder().getPosition()) / 2; + double numRotations = (leftFront.getEncoder().getPosition() + leftRear.getEncoder().getPosition()) / 2; return -numRotations * Constants.AutoConstants.encoderFactor; // This is flipped to make forward positive } public double getRightDistance() { - double numRotations = - (rightFront.getEncoder().getPosition() + rightRear.getEncoder().getPosition()) / 2; + double numRotations = (rightFront.getEncoder().getPosition() + rightRear.getEncoder().getPosition()) / 2; return -numRotations * Constants.AutoConstants.encoderFactor; // This is flipped to make forward positive } diff --git a/src/main/java/frc/robot/util/limelight/LEDMode.java b/src/main/java/frc/robot/util/limelight/LEDMode.java new file mode 100644 index 0000000..4b760f1 --- /dev/null +++ b/src/main/java/frc/robot/util/limelight/LEDMode.java @@ -0,0 +1,28 @@ +package frc.robot.util.limelight; + +import java.util.Objects; + +public enum LEDMode { + PIPELINE(0), + OFF(1), + BLINK(2), + ON(3); + private int value; + + LEDMode(int value) { + this.value = value; + } + + public int getValue() { + return value; + } + + public static LEDMode fromValue(int value) { + for (LEDMode ledMode : LEDMode.values()) { + if (Objects.equals(ledMode.value, value)) { + return ledMode; + } + } + return null; + } +} diff --git a/src/main/java/frc/robot/util/limelight/Limelight.java b/src/main/java/frc/robot/util/limelight/Limelight.java new file mode 100644 index 0000000..43bf32a --- /dev/null +++ b/src/main/java/frc/robot/util/limelight/Limelight.java @@ -0,0 +1,82 @@ +package frc.robot.util.limelight; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +import java.util.Arrays; +import java.util.List; +import java.util.Objects; +import java.util.Optional; + +public class Limelight { + private NetworkTable table; + private NetworkTableEntry tv, tx, ty, ta, ts, tl, ledMode, pipeline, stream; + private List knownPipelines; + private Target lastTarget = null; + + + public Limelight(Pipeline... knownPipelines) { + this("limelight", knownPipelines); + } + + public Limelight(String tableName, Pipeline... knownPipelines) { + this.table = NetworkTableInstance.getDefault().getTable(tableName); + this.tv = table.getEntry("tv"); + this.tx = table.getEntry("tx"); + this.ty = table.getEntry("ty"); + this.ta = table.getEntry("ta"); + this.ts = table.getEntry("ts"); + this.tl = table.getEntry("tl"); + this.pipeline = table.getEntry("pipeline"); + this.ledMode = table.getEntry("ledMode"); + this.stream = table.getEntry("stream"); + this.knownPipelines = Arrays.asList(knownPipelines); + } + + public Optional getTarget() { + boolean exists = tv.getDouble(0.0) > 0.0; + if (!exists) { + return Optional.empty(); + } + return Optional.of(lastTarget = new Target( + tx.getDouble(0.0), + ty.getDouble(0.0), + ta.getDouble(0.0), + ts.getDouble(0.0), + tl.getDouble(0.0))); + } + + public Optional getLastTarget() { + return Optional.ofNullable(lastTarget); + } + + public LEDMode getLEDMode() { + return LEDMode.fromValue(this.ledMode.getNumber(0.0).intValue()); + } + + public void setLEDMode(LEDMode ledMode) { + this.ledMode.setNumber(ledMode.getValue()); + } + + public int getPipelineId() { + return this.pipeline.getNumber(0.0).intValue(); + } + + public Optional getPipeline() { + int id = getPipelineId(); + return knownPipelines.stream().filter(pipeline -> Objects.equals(pipeline.getId(), id)).findFirst(); + } + + public void setPipeline(Pipeline pipeline) { + this.pipeline.setNumber(pipeline.getId()); + } + + public StreamMode getStreamMode() { + return StreamMode.fromValue(this.stream.getNumber(0.0).intValue()); + } + + public void setStreamMode(StreamMode streamMode) { + this.stream.setNumber(streamMode.getValue()); + } +} diff --git a/src/main/java/frc/robot/util/limelight/Pipeline.java b/src/main/java/frc/robot/util/limelight/Pipeline.java new file mode 100644 index 0000000..73cf4c0 --- /dev/null +++ b/src/main/java/frc/robot/util/limelight/Pipeline.java @@ -0,0 +1,19 @@ +package frc.robot.util.limelight; + +public class Pipeline { + private String name; + private int id; + + public Pipeline(String name, int id) { + this.name = name; + this.id = id; + } + + public String getName() { + return name; + } + + public int getId() { + return id; + } +} diff --git a/src/main/java/frc/robot/util/limelight/StreamMode.java b/src/main/java/frc/robot/util/limelight/StreamMode.java new file mode 100644 index 0000000..a99dbbd --- /dev/null +++ b/src/main/java/frc/robot/util/limelight/StreamMode.java @@ -0,0 +1,27 @@ +package frc.robot.util.limelight; + +import java.util.Objects; + +public enum StreamMode { + STANDARD(0), + PIP_MAIN(1), + PIP_SECONDARY(2); + private int value; + + StreamMode(int value) { + this.value = value; + } + + public int getValue() { + return value; + } + + public static StreamMode fromValue(int value) { + for (StreamMode streamMode : StreamMode.values()) { + if (Objects.equals(streamMode.value, value)) { + return streamMode; + } + } + return null; + } +} diff --git a/src/main/java/frc/robot/util/limelight/Target.java b/src/main/java/frc/robot/util/limelight/Target.java new file mode 100644 index 0000000..592318b --- /dev/null +++ b/src/main/java/frc/robot/util/limelight/Target.java @@ -0,0 +1,33 @@ +package frc.robot.util.limelight; + +public class Target { + private double x, y, area, skew, latency; + + public Target(double x, double y, double area, double skew, double latency) { + this.x = x; + this.y = y; + this.area = area; + this.skew = skew; + this.latency = latency; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getArea() { + return area; + } + + public double getSkew() { + return skew; + } + + public double getLatency() { + return latency; + } +}