diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..2b9c849 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,19 @@ +{ + "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + } + }, + "NetworkTables Info": { + "Connections": { + "open": true + }, + "visible": true + } +} diff --git a/.gitignore b/.gitignore index 5528d4f..4d15a70 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,4 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ +src/main/java/frc/robot/BuildConstants.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 58c4307..837ebc7 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -11,9 +11,9 @@ "autoFolders": [ "TEST" ], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 4.0, + "defaultMaxAccel": 2.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5 + "maxModuleSpeed": 5.0 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index c73a804..462fb1d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { diff --git a/src/main/deploy/pathplanner/autos/Btmalt.auto b/src/main/deploy/pathplanner/autos/Btmalt.auto index 302dad8..e72ebf7 100644 --- a/src/main/deploy/pathplanner/autos/Btmalt.auto +++ b/src/main/deploy/pathplanner/autos/Btmalt.auto @@ -52,6 +52,12 @@ "data": { "pathName": "MB-TO-SHOOT" } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Mid4B.auto b/src/main/deploy/pathplanner/autos/Mid4B.auto new file mode 100644 index 0000000..e7965f1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid4B.auto @@ -0,0 +1,85 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.171904793952038, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "TO-1" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid4Lime.auto b/src/main/deploy/pathplanner/autos/Mid4Lime.auto new file mode 100644 index 0000000..880137f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid4Lime.auto @@ -0,0 +1,100 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.171904793952038, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TO-2" + } + }, + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TO-3" + } + }, + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TO-1" + } + }, + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "TargetSwerve" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto deleted file mode 100644 index c4472d3..0000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.160210639509598, - "y": 5.531335051274091 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SmartLaunchMid" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopMain.auto b/src/main/deploy/pathplanner/autos/TopMain.auto index 55266dd..a305c48 100644 --- a/src/main/deploy/pathplanner/autos/TopMain.auto +++ b/src/main/deploy/pathplanner/autos/TopMain.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "T-SHOOT" + } + }, { "type": "named", "data": { @@ -18,9 +24,15 @@ } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.0 + "pathName": "T-TO-1" + } + }, + { + "type": "path", + "data": { + "pathName": "T-SHOOT" } }, { @@ -32,19 +44,19 @@ { "type": "path", "data": { - "pathName": "T-SHOOT" + "pathName": "T-TO-M1" } }, { "type": "path", "data": { - "pathName": "T-TO-2" + "pathName": "T-SHOOT" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "T-SHOOT" + "name": "LaunchASAP" } } ] diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto index 9a003af..aaac7df 100644 --- a/src/main/deploy/pathplanner/autos/test.auto +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -16,12 +16,6 @@ "data": { "pathName": "M-SHOOT" } - }, - { - "type": "named", - "data": { - "name": "LaunchASAP" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path index 6d42fb5..57b73b1 100644 --- a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": { - "x": 1.1952931028369176, - "y": 4.139730672623739 + "x": 1.2654580294915565, + "y": 4.397002070357417 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TAXI.path b/src/main/deploy/pathplanner/paths/BTM-TAXI.path index 6ac425c..2227f38 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TAXI.path +++ b/src/main/deploy/pathplanner/paths/BTM-TAXI.path @@ -44,8 +44,8 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-2.path b/src/main/deploy/pathplanner/paths/BTM-TO-2.path index 642b0a9..bcb016c 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-2.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.9029392417759186, - "y": 4.256672217048139 + "x": 1.9731041684305581, + "y": 4.513943614781817 }, "isLocked": false, "linkedName": "BTM SHOOT" @@ -43,8 +43,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-3.path b/src/main/deploy/pathplanner/paths/BTM-TO-3.path index 72d8c43..c669d19 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-3.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.1602106395095981, - "y": 4.256672217048139 + "x": 1.2303755661642377, + "y": 4.513943614781817 }, "isLocked": false, "linkedName": "BTM SHOOT" }, { "anchor": { - "x": 2.4699559370628714, - "y": 4.128036518181299 + "x": 2.70383902591167, + "y": 4.1046482092964185 }, "prevControl": { - "x": 1.6981417438618356, - "y": 4.139730672623739 + "x": 1.9320248327106342, + "y": 4.116342363738859 }, "nextControl": null, "isLocked": false, @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path index 2ba3da8..3f22626 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.9029392417759181, - "y": 4.256672217048139 + "x": 0.6807503073695607, + "y": 1.6605699308264719 }, "isLocked": false, "linkedName": "BTM SHOOT" @@ -68,8 +68,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path index 5c38c08..ba27366 100644 --- a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": null, "nextControl": { - "x": 1.3940937283583967, - "y": 2.1049477996391905 + "x": 0.3533139829812422, + "y": 0.9238382009527565 }, "isLocked": false, "linkedName": "BTM SHOOT" @@ -20,8 +20,8 @@ "y": 0.7835083476434771 }, "prevControl": { - "x": 5.440271165442615, - "y": 0.573013567679558 + "x": 4.972504987745018, + "y": 0.1754123166365992 }, "nextControl": null, "isLocked": false, @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/M-SHOOT.path b/src/main/deploy/pathplanner/paths/M-SHOOT.path index 4c9740b..e748879 100644 --- a/src/main/deploy/pathplanner/paths/M-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/M-SHOOT.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.183598948394478, - "y": 5.5430292057165325 + "x": 1.6045885083223155, + "y": 5.507946742389212 }, "prevControl": null, "nextControl": { - "x": 1.2435065695868026, - "y": 5.546357406893884 + "x": 1.4993411183403562, + "y": 5.507946742389212 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.5928943538798759, - "y": 5.5430292057165325 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": { - "x": 1.5333576812770562, - "y": 5.535587121641179 + "x": 1.4408703461281562, + "y": 5.543029205716531 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/M-TO-1.path b/src/main/deploy/pathplanner/paths/M-TO-1.path index bb9a10b..a575a60 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-1.path +++ b/src/main/deploy/pathplanner/paths/M-TO-1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.5928943538798759, - "y": 5.5430292057165325 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": null, "nextControl": { - "x": 1.5812001994374358, - "y": 6.303149244475128 + "x": 1.3005404928188768, + "y": 6.291455090032686 }, "isLocked": false, "linkedName": "MID SHOOT" }, { "anchor": { - "x": 2.5752033270448305, - "y": 6.841080348827365 + "x": 2.9377221147604686, + "y": 6.993104356579084 }, "prevControl": { - "x": 1.5578118905525564, - "y": 6.794303731057606 + "x": 1.9203306782681946, + "y": 6.946327738809325 }, "nextControl": null, "isLocked": false, @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/M-TO-2.path b/src/main/deploy/pathplanner/paths/M-TO-2.path index 5d3fc47..ecc7bb4 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-2.path +++ b/src/main/deploy/pathplanner/paths/M-TO-2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.5928943538798759, - "y": 5.5430292057165325 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": null, "nextControl": { - "x": 1.663059280534515, - "y": 5.55472336015897 + "x": 1.3823995739159562, + "y": 5.543029205716529 }, "isLocked": false, "linkedName": "MID SHOOT" }, { "anchor": { - "x": 2.5284267092750707, - "y": 5.556066607628711 + "x": 2.855863033663389, + "y": 5.5430292057165325 }, "prevControl": { - "x": 1.943718987153074, - "y": 5.55472336015897 + "x": 2.271155311541392, + "y": 5.541685958246791 }, "nextControl": null, "isLocked": false, @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/M-TO-3.path b/src/main/deploy/pathplanner/paths/M-TO-3.path index 75e82f4..527a15d 100644 --- a/src/main/deploy/pathplanner/paths/M-TO-3.path +++ b/src/main/deploy/pathplanner/paths/M-TO-3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.5928943538798759, - "y": 5.5430292057165325 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": null, "nextControl": { - "x": 1.3005404928188773, - "y": 4.420390379242298 + "x": 1.0198807862003183, + "y": 4.408696224799857 }, "isLocked": false, "linkedName": "MID SHOOT" }, { "anchor": { - "x": 2.61028579037215, - "y": 4.326837143702777 + "x": 2.69214487146923, + "y": 4.268366371490578 }, "prevControl": { - "x": 2.493344245947751, - "y": 4.736132549188175 + "x": 2.575203327044831, + "y": 4.6776617769759765 }, "nextControl": null, "isLocked": false, @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path index 7000475..323d269 100644 --- a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.9029392417759192, - "y": 4.256672217048139 + "x": 0.973104168430559, + "y": 4.513943614781817 }, "prevControl": { - "x": 1.0315749406427586, - "y": 2.432384124027509 + "x": 1.1017398672973977, + "y": 2.6896555217611873 }, "nextControl": null, "isLocked": false, @@ -44,14 +44,14 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -59.264512298079936, + "rotation": -47.489552921999234, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path index 803b271..c98ea25 100644 --- a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path +++ b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.5812001994374358, - "y": 5.55472336015897 + "x": 1.2771521839339972, + "y": 5.531335051274091 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.5928943538798759, - "y": 5.5430292057165325 + "x": 1.3122346472613169, + "y": 5.531335051274091 }, "prevControl": { - "x": 1.3005404928188773, - "y": 5.5430292057165325 + "x": 1.0198807862003183, + "y": 5.531335051274091 }, "nextControl": null, "isLocked": false, @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-SHOOT.path b/src/main/deploy/pathplanner/paths/T-SHOOT.path index a6efb21..7464326 100644 --- a/src/main/deploy/pathplanner/paths/T-SHOOT.path +++ b/src/main/deploy/pathplanner/paths/T-SHOOT.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6396709716496354, - "y": 6.689056341075646 + "x": 1.3239288017037572, + "y": 6.759221267730286 }, "prevControl": null, "nextControl": { - "x": 1.008186631757879, - "y": 6.607197259978566 + "x": 1.0198807862003185, + "y": 6.595503105536126 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": { - "x": 1.2420697206066778, - "y": 6.747527113287845 + "x": 1.195293102836918, + "y": 6.618891414421005 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-1.path b/src/main/deploy/pathplanner/paths/T-TO-1.path index 7fbf13c..e17bcbd 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.3707054194735169, - "y": 6.817692039942485 + "x": 1.3239288017037574, + "y": 6.689056341075645 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-2.path b/src/main/deploy/pathplanner/paths/T-TO-2.path index 960ca4a..640a7f9 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.6162826627647553, - "y": 6.303149244475128 + "x": 1.5695060449949958, + "y": 6.174513545608288 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-3.path b/src/main/deploy/pathplanner/paths/T-TO-3.path index 8463bc3..9c6511d 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 2.844168879220949, - "y": 6.221290163378049 + "x": 2.79739226145119, + "y": 6.092654464511209 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M1.path b/src/main/deploy/pathplanner/paths/T-TO-M1.path index 6001ca6..1e5fca4 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M1.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M1.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.9889153137819637, - "y": 6.199087072848821 + "x": 2.61028579037215, + "y": 6.630585568863446 }, "isLocked": false, "linkedName": "TOP SHOOT" }, { "anchor": { - "x": 2.917573402714373, - "y": 6.286398158381903 + "x": 3.0429695047424277, + "y": 6.560420642208806 }, "prevControl": { - "x": 2.5200511662284586, - "y": 6.304887564730085 + "x": 2.6472040803080072, + "y": 6.518761123847288 }, "nextControl": { - "x": 3.2793314510542197, - "y": 6.269572202645167 + "x": 3.9317252423678632, + "y": 6.653973877748326 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.9545143705672015, - "y": 7.226987445427883 + "x": 8.57430455601652, + "y": 7.402399762064483 }, "prevControl": { - "x": 7.483034508688559, - "y": 6.690794661330605 + "x": 6.375803520837811, + "y": 7.566117924258642 }, "nextControl": null, "isLocked": false, @@ -77,14 +77,14 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 27.050597007086274, + "rotation": -3.57633437499725, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M2.path b/src/main/deploy/pathplanner/paths/T-TO-M2.path index 5dfa5e0..313520a 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M2.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.9889153137819633, - "y": 6.199087072848821 + "x": 1.9421386960122038, + "y": 6.070451373981981 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -77,8 +77,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/T-TO-M3.path b/src/main/deploy/pathplanner/paths/T-TO-M3.path index dd71b42..d7f453b 100644 --- a/src/main/deploy/pathplanner/paths/T-TO-M3.path +++ b/src/main/deploy/pathplanner/paths/T-TO-M3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8795509328910394, - "y": 6.689056341075646 + "x": 0.8327743151212799, + "y": 6.560420642208806 }, "prevControl": null, "nextControl": { - "x": 1.9889153137819628, - "y": 6.199087072848821 + "x": 1.9421386960122033, + "y": 6.070451373981981 }, "isLocked": false, "linkedName": "TOP SHOOT" @@ -75,8 +75,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/TO-1.path b/src/main/deploy/pathplanner/paths/TO-1.path new file mode 100644 index 0000000..d7a4833 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TO-1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.037272222692593, + "y": 6.0692661556263285 + }, + "prevControl": null, + "nextControl": { + "x": 2.329626083753592, + "y": 6.256372626705367 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.832474724778509, + "y": 6.876162812154685 + }, + "prevControl": { + "x": 2.4582617826204305, + "y": 6.607197259978565 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 57.99461679191646, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 35.537677791974396, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TO-2.path b/src/main/deploy/pathplanner/paths/TO-2.path new file mode 100644 index 0000000..d88b0fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TO-2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9904956049228335, + "y": 5.566417514601411 + }, + "prevControl": null, + "nextControl": { + "x": 2.1900526364941553, + "y": 5.5797213167061654 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.59859163592971, + "y": 5.566417514601411 + }, + "prevControl": { + "x": 2.1776020760018726, + "y": 5.589805823486292 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 4.3987053549955135, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TO-3.path b/src/main/deploy/pathplanner/paths/TO-3.path new file mode 100644 index 0000000..36da75c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TO-3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9554131415955136, + "y": 4.934933174709655 + }, + "prevControl": null, + "nextControl": { + "x": 2.247767002656512, + "y": 4.572414386994017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.68045071702679, + "y": 4.198201444835939 + }, + "prevControl": { + "x": 2.4231793192931113, + "y": 4.373613761472538 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -34.50852298766841, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": -35.537677791974374, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index df70e97..579cecb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,13 +47,13 @@ public static final class Swerve { public static final SensorDirectionValue cancoderInvert = chosenModule.cancoderInvert; /* Swerve Current Limiting */ - public static final int angleCurrentLimit = 15; - public static final int angleCurrentThreshold = 30; + public static final int angleCurrentLimit = 12; + public static final int angleCurrentThreshold = 25; public static final double angleCurrentThresholdTime = 0.1; public static final boolean angleEnableCurrentLimit = true; - public static final int driveCurrentLimit = 20; - public static final int driveCurrentThreshold = 40; + public static final int driveCurrentLimit = 30; + public static final int driveCurrentThreshold = 45; public static final double driveCurrentThresholdTime = 0.1; public static final boolean driveEnableCurrentLimit = true; @@ -68,11 +68,13 @@ public static final class Swerve { public static final double angleKD = chosenModule.angleKD; /* Drive Motor PID Values */ - public static final double driveKP = 0.0; //TODO: This must be tuned to specific robot + public static final double driveKP = 0.8; //TODO: This must be tuned to specific robot public static final double driveKI = 0.0; - public static final double driveKD = 0.0; + public static final double driveKD = 0.3; public static final double driveKF = 0.0; + + /* Drive Motor Characterization Values From SYSID */ public static final double driveKS = 0.32; //TODO: This must be tuned to specific robot public static final double driveKV = 1.51; @@ -80,9 +82,12 @@ public static final class Swerve { /* Swerve Profiling Values */ /** Meters per Second */ - public static final double maxSpeed = 4.5; //TODOx: This must be tuned to specific robot DONE + public static final double maxSpeed = 5.5; //TODOx: This must be tuned to specific robot DONE /** Radians per Second */ - public static final double maxAngularVelocity = 10.0; //TODOx: This must be tuned to specific robot DONE + public static final double maxAngularVelocity = 7.0; //TODOx: This must be tuned to specific robot DONE + + /** Rotation control dampening */ + public static final double rotationDampening = 0.75; /* Neutral Modes */ public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast; @@ -91,8 +96,8 @@ public static final class Swerve { /* Module Specific Constants */ /* Front Left Module - Module 0 */ public static final class Mod0 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 3; - public static final int angleMotorID = 4; + public static final int driveMotorID = 5; + public static final int angleMotorID = 6; public static final int canCoderID = 18; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(11.25); public static final SwerveModuleConstants constants = @@ -101,8 +106,8 @@ public static final class Mod0 { //TODOx: This must be tuned to specific robot D /* Front Right Module - Module 1 */ public static final class Mod1 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 1; - public static final int angleMotorID = 2; + public static final int driveMotorID = 3; + public static final int angleMotorID = 4; public static final int canCoderID = 17; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-93.25); public static final SwerveModuleConstants constants = @@ -111,9 +116,9 @@ public static final class Mod1 { //TODOx: This must be tuned to specific robot D /* Back Left Module - Module 2 */ public static final class Mod2 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 5; - public static final int angleMotorID = 6; - public static final int canCoderID = 16; + public static final int driveMotorID = 7; + public static final int angleMotorID = 8; + public static final int canCoderID = 19; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(-42.45); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); @@ -121,9 +126,9 @@ public static final class Mod2 { //TODOx: This must be tuned to specific robot D /* Back Right Module - Module 3 */ public static final class Mod3 { //TODOx: This must be tuned to specific robot DONE - public static final int driveMotorID = 7; - public static final int angleMotorID = 8; - public static final int canCoderID = 19; + public static final int driveMotorID = 1; + public static final int angleMotorID = 2; + public static final int canCoderID = 16; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(55.89); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); @@ -175,6 +180,7 @@ public static final class Launcher { public static final double P = 0.08; public static final double I = 0.05; public static final double D = 0.00; + public static final double FF = 0.000156; @@ -187,6 +193,22 @@ public static final class Launcher { public static final double OutPoint = 1000; public static final double InPoint = -1000; + + } + + public static final class TargetSwerve { + // public static final double P = 0.05; + // public static final double I = 0; + // public static final double D = 0.001; + public static final double P = 0.06; + public static final double I = 0; + public static final double D = 0.002; + + public static final double clamp = 0.75; + + public static final double SmartLaunchTime = 0.5; + + } // holds constants for mode system @@ -213,16 +235,18 @@ public static final class Colors { public static final Color IA = Color.kPink; // Intake Amp Color public static final Color N = Color.kYellow; // Note Color public static final Color LS = Color.kGreen; // Launcher Spinup Color - public static final Color L = Color.kAqua; // Color Color - public static final Color DIS = Color.kPurple; //Disable Color + public static final Color LT = Color.kPurple; // Launcher Targeting Color + public static final Color LK = Color.kGreen; // Launcher Lock Color + public static final Color L = Color.kAqua; // Launcher Launching Color + // public static final Color DIS = Color.kPurple;//Disable Color } } public static final class AutoConstants { //TODO: The below constants are used in the example auto, and must be tuned to specific robot - public static final double kMaxSpeedMetersPerSecond = 1; - public static final double kMaxAccelerationMetersPerSecondSquared = 0.5; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxSpeedMetersPerSecond = 1.2; + public static final double kMaxAccelerationMetersPerSecondSquared = 0.8; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI*1.5; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI*1.5; public static final double kPXController = 1; public static final double kPYController = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe3c941..abe96c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,44 +1,47 @@ package frc.robot; import org.opencv.core.Mat; -import org.opencv.core.Point; -import org.opencv.core.MatOfPoint; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; -import com.revrobotics.CANSparkMax; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.CvSink; import edu.wpi.first.cscore.CvSource; import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - -import java.util.ArrayList; -import java.util.List; - -import frc.robot.autos.*; -import frc.robot.commands.*; -import frc.robot.subsystems.*; +import frc.robot.commands.IntakeAmp; +import frc.robot.commands.IntakeDefault; +import frc.robot.commands.IntakeFix; +import frc.robot.commands.IntakeRun; +import frc.robot.commands.LaunchASAP; +import frc.robot.commands.LaunchControled; +import frc.robot.commands.TargetSwerve; +import frc.robot.commands.TargetSwervePlus; +import frc.robot.commands.TargetSwerveAuto; +import frc.robot.commands.TeleopSwerve; +import frc.robot.subsystems.Band; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Swerve; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -85,7 +88,7 @@ public class RobotContainer { private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault(); /* Subsystems */ - private final LEDS theLEDs = new LEDS(0,60); + private final LEDS theLEDs = new LEDS(9,176); private final Band theBand = new Band(); @@ -105,6 +108,9 @@ public class RobotContainer { private final SendableChooser autoChooser; private final SendableChooser musiChooser; + private Field2d m_field = new Field2d(); + private Field2d m_LLfield = new Field2d(); + private Thread m_visionThread; @@ -130,10 +136,17 @@ public RobotContainer() { intake.setDefaultCommand(new IntakeDefault(intake,LIM)); + NamedCommands.registerCommand("RunIntake", new IntakeRun(intake, LIM, new JoystickButton(secondary, 3), theLEDs)); NamedCommands.registerCommand("EndIntake", new IntakeDefault(intake, LIM)); NamedCommands.registerCommand("LaunchASAP", new LaunchASAP(intake,launcher,theLEDs)); - + NamedCommands.registerCommand("TargetSwerve", + new TargetSwerveAuto( + s_Swerve, + theLEDs, + launcher, + intake + )); // Configure the button bindings configureButtonBindings(); @@ -145,13 +158,14 @@ public RobotContainer() { musiChooser = theBand.Buildchoser(); SmartDashboard.putData("Music Choser",musiChooser); + SmartDashboard.putNumber("SmartLaunch",0); secondary.getPOV(); // theLEDs.SetFull(255, 60, 0); theLEDs.setMode("D"); - + m_visionThread = new Thread( () -> { @@ -194,6 +208,15 @@ public RobotContainer() { }); m_visionThread.setDaemon(true); m_visionThread.start(); + + + + + SmartDashboard.putNumber("Xpid", 0); + SmartDashboard.putNumber("Ypid", 0); + SmartDashboard.putBoolean("Xshoot", false); + SmartDashboard.putBoolean("Yshoot", false); + SmartDashboard.putBoolean("Shoot", false); } /** @@ -208,9 +231,33 @@ private void configureButtonBindings() { // new JoystickButton(secondary, 4).onTrue(new LaunchASAP(intake,launcher,theLEDs)); new JoystickButton(secondary, 4).onTrue(new LaunchControled(intake,launcher,theLEDs,new JoystickButton(secondary, 4),new JoystickButton(secondary, 6))); new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); + new JoystickButton(driver, 5).onTrue(new IntakeRun(intake, LIM, new JoystickButton(driver, 5),theLEDs)); new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),new JoystickButton(secondary, 6),theLEDs)); new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); - new JoystickButton(driver, 3).onTrue(smartLaunch()); + new JoystickButton(driver, 4).onTrue( + new TargetSwerve( + s_Swerve, + () -> -driver.getRawAxis(strafeAxis), + () -> -driver.getRawAxis(rotationAxis), + theLEDs, + launcher, + intake, + new JoystickButton(driver, 4), + new JoystickButton(secondary, 6), + new JoystickButton(driver, 6) + )); + new JoystickButton(driver, 3).onTrue( + new TargetSwervePlus( + s_Swerve, + () -> -driver.getRawAxis(strafeAxis), + () -> -driver.getRawAxis(rotationAxis), + theLEDs, + launcher, + intake, + new JoystickButton(driver, 4), + new JoystickButton(secondary, 6) + )); + // new JoystickButton(driver, 3).onTrue(smartLaunch()); } public void updateInfo() { @@ -228,13 +275,27 @@ public void updateInfo() { SmartDashboard.putBoolean("LS", theLEDs.getMode() == "LS"); SmartDashboard.putBoolean("L", theLEDs.getMode() == "L"); + Pose2d pose = s_Swerve.getPose(); + m_field.setRobotPose(pose); + + // NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); + NetworkTable LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + double[] LLpose = LLtbl.getEntry("botpose").getDoubleArray(new double[6]); + + + m_LLfield.setRobotPose(new Pose2d(LLpose[0], LLpose[1], new Rotation2d(LLpose[3],LLpose[4]))); + SmartDashboard.putData("intake", intake); SmartDashboard.putData("Launcher", launcher); + SmartDashboard.putData("Swerve", s_Swerve); SmartDashboard.putData("Gyro",s_Swerve.gyro); - SmartDashboard.putBoolean("ControlorA", new JoystickButton(secondary, 1).getAsBoolean()); - SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); - SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); + SmartDashboard.putData("BotPose", m_field); + SmartDashboard.putData("BotPoseLL", m_LLfield); + // SmartDashboard.putBoolean("ControlorA", new JoystickButton(secondary, 1).getAsBoolean()); + // SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); + // SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); + // SmartDashboard.putData(PDP); // System.out.println("Radio"); // System.out.println(PDP.getCurrent(15)); // System.out.println("RIO"); @@ -242,7 +303,11 @@ public void updateInfo() { // System.out.println("Total"); // System.out.println(PDP.getTotalCurrent()); - intake.updateData(); + + // NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); + // NetworkTable LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + + // intake.updateData(); double throtleVal = (driver.getRawAxis(throttleAxis)+1)/2; SmartDashboard.putNumber("Throttle", throtleVal); @@ -289,7 +354,7 @@ public void teleopPeriodic() { } public void autonomousPeriodic() { - // theLEDs.rainbow(); + theLEDs.rainbow(1); } public void testInit() { @@ -297,7 +362,7 @@ public void testInit() { } public void testPeriodic() { - theLEDs.rainbow(); + theLEDs.rainbow(1); } public void disabledInit () { diff --git a/src/main/java/frc/robot/commands/IntakeDefault.java b/src/main/java/frc/robot/commands/IntakeDefault.java index a870301..7a2a542 100644 --- a/src/main/java/frc/robot/commands/IntakeDefault.java +++ b/src/main/java/frc/robot/commands/IntakeDefault.java @@ -14,7 +14,7 @@ public class IntakeDefault extends Command { private Boolean first; /** - * Run intake untill note is obtained or manualy disabled + * Run intake until note is obtained or manualy disabled * * @param s_Intake Intake object */ diff --git a/src/main/java/frc/robot/commands/LaunchControled.java b/src/main/java/frc/robot/commands/LaunchControled.java index e7c19e5..07b56b4 100644 --- a/src/main/java/frc/robot/commands/LaunchControled.java +++ b/src/main/java/frc/robot/commands/LaunchControled.java @@ -14,6 +14,7 @@ public class LaunchControled extends Command{ private Launcher s_Launcher; private Timer timer; private boolean launch; + private boolean bHold; private LEDS theLEDs; private JoystickButton overrideButton; private JoystickButton launchButton; @@ -25,6 +26,7 @@ public LaunchControled(Intake s_Intake, Launcher s_Launcher, LEDS theLEDs, Joyst this.timer = new Timer(); this.launch = false; this.theLEDs = theLEDs; + this.bHold = true; this.overrideButton = overrideButton; this.launchButton = launchButton; @@ -37,6 +39,7 @@ public void initialize() { timer = new Timer(); // timer.start(); launch = false; + bHold = true; s_Intake.lightPull(false); s_Launcher.startLaunch(); System.out.println("Start LaunchASAP"); @@ -47,7 +50,7 @@ public void initialize() { @Override public void execute() { - + if (bHold) bHold = overrideButton.getAsBoolean(); if (launchButton.getAsBoolean()) { timer.start(); launch = true; @@ -69,6 +72,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return (timer.get() >= (Constants.Launcher.LaunchStopTime) && launch); + return (timer.get() >= (Constants.Launcher.LaunchStopTime) && launch) || (overrideButton.getAsBoolean() && !bHold); } } diff --git a/src/main/java/frc/robot/commands/SmartShoot.java b/src/main/java/frc/robot/commands/SmartShoot.java deleted file mode 100644 index ef16016..0000000 --- a/src/main/java/frc/robot/commands/SmartShoot.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.networktables.PubSub; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import java.util.function.BooleanSupplier; - -import frc.robot.Constants; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.LEDS; - -public class SmartShoot extends Command { - - - /** - * Run intake untill note is obtained or manualy disabled - * - * @param s_Intake Intake object - */ - public SmartShoot() { - } - - @Override - public void initialize() { - - } - - @Override - public void execute() { - - } - - @Override - public void end(boolean interrupted) { - - } - - @Override - public boolean isFinished() { - return false; - } - -} diff --git a/src/main/java/frc/robot/commands/TargetSwerve.java b/src/main/java/frc/robot/commands/TargetSwerve.java new file mode 100644 index 0000000..9ce835a --- /dev/null +++ b/src/main/java/frc/robot/commands/TargetSwerve.java @@ -0,0 +1,183 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.Timer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Intake; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.Swerve; + + +public class TargetSwerve extends Command { + private Swerve s_Swerve; + private Launcher s_Launcher; + private Intake s_Intake; + private DoubleSupplier rotationSup; + + private JoystickButton overrideButton; + private JoystickButton launchButton; + private JoystickButton altLaunch; + + private double swerve_X_speed; + private double swerve_Y_speed; + + private boolean shoot; + private boolean spin; + private boolean end; + private boolean launching; + private Timer timer; + private Timer timerL; + + + + private Boolean bHold; + private NetworkTable LLtbl; + + private PIDController Xpid; + private PIDController Ypid; + + + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TargetSwerve(Swerve s_Swerve, + DoubleSupplier strafeSup, + DoubleSupplier rotationSup, + LEDS led, + Launcher s_Launcher, + Intake s_Intake, + JoystickButton overrideButton, + JoystickButton launchButton, + JoystickButton altLaunch) { + this.s_Swerve = s_Swerve; + this.s_Launcher = s_Launcher; + this.s_Intake = s_Intake; + addRequirements(s_Swerve); + addRequirements(s_Launcher); + addRequirements(s_Intake); + + this.rotationSup = rotationSup; + this.overrideButton = overrideButton; + this.launchButton = launchButton; + this.altLaunch = altLaunch; + this.led = led; + } + + + @Override + public void initialize() { + this.bHold = true; + + Xpid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + Ypid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + + Xpid.setTolerance(3); + Ypid.setTolerance(3); + + Xpid.setSetpoint(0); + Ypid.setSetpoint(0); + + timer = new Timer(); + timerL = new Timer(); + shoot = false; + spin = false; + end = false; + launching = false; + + led.setMode("LT"); + System.out.println("set LT"); + + s_Launcher.startLaunch(); + s_Intake.lightPull(false); + + LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + /* Get Values, Deadband*/ + double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); + double throtleVal = 0.3; + + + // Get Targert X and Y + double X_from_camera = LLtbl.getValue("tx").getDouble(); + double Y_from_camera = LLtbl.getValue("ty").getDouble(); + // Calculate PID for movement + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + + // If target reached set shoot to true + shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); + // if target reached zero motion + if(Xpid.atSetpoint()) swerve_X_speed = 0; + if(Ypid.atSetpoint()) swerve_Y_speed = 0; + + // set led modes + if(shoot && !end && !launching) { + if(led.getMode()!="LK"){ + led.setMode("LK"); + System.out.println("set LK"); + } + } else if (!end && led.getMode()!="LT" && !launching) { + led.setMode("LT"); + System.out.println("set LT"); + } + + SmartDashboard.putNumber("SmartLaunch", timerL.get()); + if((launchButton.getAsBoolean() && !end) || (altLaunch.getAsBoolean() && !end)) { + s_Intake.pushIntake(false); + end = true; + timer.start(); + led.setMode("L"); + System.out.println("set L"); + launching = true; + } + + SmartDashboard.putNumber("Xpid", swerve_X_speed); + SmartDashboard.putNumber("Ypid", swerve_Y_speed); + SmartDashboard.putBoolean("Xshoot", Xpid.atSetpoint()); + SmartDashboard.putBoolean("Yshoot", Ypid.atSetpoint()); + SmartDashboard.putBoolean("Shoot", shoot); + + /* Drive */ + s_Swerve.drive( + new Translation2d(-swerve_Y_speed * throtleVal, -swerve_X_speed * throtleVal).times(Constants.Swerve.maxSpeed), + (rotationVal * throtleVal) * Constants.Swerve.maxAngularVelocity, + false, + false + ); + } + + @Override + public void end(boolean interrupted) { + s_Intake.pushIntake(true); + s_Launcher.endLaunch(); + led.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + + return (overrideButton.getAsBoolean() && !bHold) || (end && timer.get() >= Constants.Launcher.LaunchStopTime); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TargetSwerveAuto.java b/src/main/java/frc/robot/commands/TargetSwerveAuto.java new file mode 100644 index 0000000..06fcd17 --- /dev/null +++ b/src/main/java/frc/robot/commands/TargetSwerveAuto.java @@ -0,0 +1,159 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.Timer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Intake; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.Swerve; + + +public class TargetSwerveAuto extends Command { + private Swerve s_Swerve; + private Launcher s_Launcher; + private Intake s_Intake; + + private double swerve_X_speed; + private double swerve_Y_speed; + + private boolean shoot; + private boolean end; + private boolean launching; + private Timer timer; + private Timer timerL; + + private NetworkTable LLtbl; + + private PIDController Xpid; + private PIDController Ypid; + + + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param translationSup forward and back suplier, typicaly a controlor axis + * @param strafeSup left and right suplier, typicaly a controlor axis + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TargetSwerveAuto(Swerve s_Swerve, LEDS led, Launcher s_Launcher, Intake s_Intake) { + this.s_Swerve = s_Swerve; + this.s_Launcher = s_Launcher; + this.s_Intake = s_Intake; + addRequirements(s_Swerve); + addRequirements(s_Launcher); + addRequirements(s_Intake); + + this.led = led; + } + + + @Override + public void initialize() { + + Xpid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + Ypid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + + Xpid.setTolerance(3); + Ypid.setTolerance(3); + + Xpid.setSetpoint(0); + Ypid.setSetpoint(0); + + timer = new Timer(); + timerL = new Timer(); + shoot = false; + end = false; + launching = false; + + led.setMode("LT"); + System.out.println("set LT"); + + s_Launcher.startLaunch(); + s_Intake.lightPull(false); + + LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + } + + @Override + public void execute() { + + // Get Targert X and Y + double X_from_camera = LLtbl.getValue("tx").getDouble(); + double Y_from_camera = LLtbl.getValue("ty").getDouble(); + // Calculate PID for movement + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + + // If target reached set shoot to true + shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); + // if target reached zero motion + if(Xpid.atSetpoint()) swerve_X_speed = 0; + if(Ypid.atSetpoint()) swerve_Y_speed = 0; + + // set led modes + if(shoot && !end && !launching) { + if(led.getMode()!="LK"){ + led.setMode("LK"); + System.out.println("set LK"); + timerL.reset(); + timerL.start(); + } + } else if (!end && led.getMode()!="LT" && !launching) { + led.setMode("LT"); + System.out.println("set LT"); + } + + SmartDashboard.putNumber("SmartLaunch", timerL.get()); + if(timerL.hasElapsed(Constants.TargetSwerve.SmartLaunchTime) && !launching) { + s_Intake.pushIntake(false); + end = true; + timer.start(); + led.setMode("L"); + System.out.println("set L"); + launching = true; + } + + + SmartDashboard.putNumber("Xpid", swerve_X_speed); + SmartDashboard.putNumber("Ypid", swerve_Y_speed); + SmartDashboard.putBoolean("Xshoot", Xpid.atSetpoint()); + SmartDashboard.putBoolean("Yshoot", Ypid.atSetpoint()); + SmartDashboard.putBoolean("Shoot", shoot); + + /* Drive */ + s_Swerve.drive( + new Translation2d(-swerve_Y_speed * 0.5, -swerve_X_speed * 0.5).times(Constants.Swerve.maxSpeed), + 0 * Constants.Swerve.maxAngularVelocity, + false, + false + ); + } + + @Override + public void end(boolean interrupted) { + s_Intake.pushIntake(true); + s_Launcher.endLaunch(); + led.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + return end && timer.get() >= Constants.Launcher.LaunchStopTime; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TargetSwervePlus.java b/src/main/java/frc/robot/commands/TargetSwervePlus.java new file mode 100644 index 0000000..a4e1f21 --- /dev/null +++ b/src/main/java/frc/robot/commands/TargetSwervePlus.java @@ -0,0 +1,178 @@ +package frc.robot.commands; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.Timer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; +import frc.robot.subsystems.Intake; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.subsystems.Swerve; + + +public class TargetSwervePlus extends Command { + private Swerve s_Swerve; + private Launcher s_Launcher; + private Intake s_Intake; + private DoubleSupplier rotationSup; + + private JoystickButton overrideButton; + private JoystickButton launchButton; + + + private double swerve_X_speed; + private double swerve_Y_speed; + + private boolean shoot; + private boolean spin; + private boolean end; + private boolean launching; + private Timer timer; + private Timer timerL; + + + + private Boolean bHold; + private NetworkTable LLtbl; + + private PIDController Xpid; + private PIDController Ypid; + + + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param translationSup forward and back suplier, typicaly a controlor axis + * @param strafeSup left and right suplier, typicaly a controlor axis + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TargetSwervePlus(Swerve s_Swerve, DoubleSupplier strafeSup, DoubleSupplier rotationSup, LEDS led, Launcher s_Launcher, Intake s_Intake, JoystickButton overrideButton, JoystickButton launchButton) { + this.s_Swerve = s_Swerve; + this.s_Launcher = s_Launcher; + this.s_Intake = s_Intake; + addRequirements(s_Swerve); + addRequirements(s_Launcher); + addRequirements(s_Intake); + + this.rotationSup = rotationSup; + this.overrideButton = overrideButton; + this.launchButton = launchButton; + this.led = led; + } + + + @Override + public void initialize() { + this.bHold = true; + + Xpid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + Ypid = new PIDController(Constants.TargetSwerve.P, Constants.TargetSwerve.I, Constants.TargetSwerve.D); + + Xpid.setTolerance(3); + Ypid.setTolerance(3); + + Xpid.setSetpoint(0); + Ypid.setSetpoint(0); + + timer = new Timer(); + timerL = new Timer(); + shoot = false; + spin = false; + end = false; + launching = false; + + led.setMode("LT"); + System.out.println("set LT"); + + s_Launcher.startLaunch(); + s_Intake.lightPull(false); + + LLtbl = NetworkTableInstance.getDefault().getTable("limelight"); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + /* Get Values, Deadband*/ + double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); + double throtleVal = 0.3; + + + // Get Targert X and Y + double X_from_camera = LLtbl.getValue("tx").getDouble(); + double Y_from_camera = LLtbl.getValue("ty").getDouble(); + // Calculate PID for movement + swerve_X_speed = MathUtil.clamp(Xpid.calculate(X_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + swerve_Y_speed = MathUtil.clamp(Ypid.calculate(Y_from_camera), -Constants.TargetSwerve.clamp,Constants.TargetSwerve.clamp); + + // If target reached set shoot to true + shoot = Xpid.atSetpoint() && Ypid.atSetpoint(); + // if target reached zero motion + if(Xpid.atSetpoint()) swerve_X_speed = 0; + if(Ypid.atSetpoint()) swerve_Y_speed = 0; + + // set led modes + if(shoot && !end && !launching) { + if(led.getMode()!="LK"){ + led.setMode("LK"); + System.out.println("set LK"); + timerL.reset(); + timerL.start(); + } + } else if (!end && led.getMode()!="LT" && !launching) { + led.setMode("LT"); + System.out.println("set LT"); + } + + SmartDashboard.putNumber("SmartLaunch", timerL.get()); + if((launchButton.getAsBoolean() && !end) || (timerL.hasElapsed(Constants.TargetSwerve.SmartLaunchTime) && !launching)) { + s_Intake.pushIntake(false); + end = true; + timer.start(); + led.setMode("L"); + System.out.println("set L"); + launching = true; + } + + SmartDashboard.putNumber("Xpid", swerve_X_speed); + SmartDashboard.putNumber("Ypid", swerve_Y_speed); + SmartDashboard.putBoolean("Xshoot", Xpid.atSetpoint()); + SmartDashboard.putBoolean("Yshoot", Ypid.atSetpoint()); + SmartDashboard.putBoolean("Shoot", shoot); + + /* Drive */ + s_Swerve.drive( + new Translation2d(-swerve_Y_speed * throtleVal, -swerve_X_speed * throtleVal).times(Constants.Swerve.maxSpeed), + (rotationVal * throtleVal) * Constants.Swerve.maxAngularVelocity, + false, + false + ); + } + + @Override + public void end(boolean interrupted) { + s_Intake.pushIntake(true); + s_Launcher.endLaunch(); + led.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + + return (overrideButton.getAsBoolean() && !bHold) || (end && timer.get() >= Constants.Launcher.LaunchStopTime); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 8833ac2..769fc63 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -8,6 +8,8 @@ import frc.robot.Constants; import java.util.function.IntSupplier; +import java.sql.Time; +import java.util.concurrent.TimeUnit; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; @@ -76,14 +78,21 @@ public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID, Xbox } public void startIntake() { - m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); - m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); // barPID.setReference(0.3, CANSparkMax.ControlType.kDutyCycle); m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, false); m_barMotor.set(Constants.Intake.acuateVel); System.out.println("Start"); + // wait(1000); + + try { + TimeUnit.MILLISECONDS.sleep(500); + } catch(InterruptedException e){ + System.out.println("interuption, sleeping"); + } + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); } public void endIntake() { diff --git a/src/main/java/frc/robot/subsystems/LEDS.java b/src/main/java/frc/robot/subsystems/LEDS.java index a68592c..0436990 100644 --- a/src/main/java/frc/robot/subsystems/LEDS.java +++ b/src/main/java/frc/robot/subsystems/LEDS.java @@ -142,6 +142,14 @@ public void updateMode() { SetFull(Constants.Mode.Colors.LS); System.out.println("LS"); break; + case "LT": + SetFull(Constants.Mode.Colors.LT); + System.out.println("LT"); + break; + case "LK": + SetFull(Constants.Mode.Colors.LK); + System.out.println("LK"); + break; case "L": SetFull(Constants.Mode.Colors.L); System.out.println("L"); @@ -153,7 +161,7 @@ public void updateMode() { } public void setMode(String modeID) { - String[] vModes = {"D","IO","IR","IF","IA","N","LS","L"}; + String[] vModes = {"D","IO","IR","IF","IA","N","LS","L","LT","LK"}; if (Arrays.asList(vModes).contains(modeID)) { mode = modeID; } @@ -183,8 +191,7 @@ public void rainbow() { m_led.setData(m_ledBuffer); } - - + /** * Rainbow function, called perodicly, shifts a rainbow through the strip * diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 7997519..2baa06e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,50 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/* +NetworkTableInstance.getDefault().getTable("limelight").getEntry("priorityid").setNumber(7); +int tag_id = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tid").getNumber(); +double X_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[0]); +double Y_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); +double X_rotation_from_camera = NetworkTableInstance.getDefault().getTable("limelight").getEntry("targetpose_robotspace").getDoubleArray(new double[1]); +if (X_from_camera <= 0.5) { + if (X_from_camera >= 2) { + swerve_X_speed = 1; + } else { + swerve_X_speed = 1.5 / X_from_camera - 1; + } +} else if (X_from_camera >= -0.5) { + if (X_from_camera <= -2) { + swerve_X_speed = -1; + } else { + swerve_X_speed = 1.5 / X_from_camera + 1; + } +} else { + shoot_x = true; +} + +if (Y_from_camera <= 0.5) { + if (Y_from_camera >= 2) { + swerve_Y_speed = 1; + } else { + swerve_Y_speed = 1.5 / Y_from_camera - 1; + } +} else if (Y_from_camera >= -0.5) { + if (Y_from_camera <= -2) { + swerve_Y_speed = -1; + } else { + swerve_Y_speed = 1.5 / Y_from_camera + 1; + } +} else { + shoot_y = true; +} + +if (shoot_y && shoot_x) { + shoot +} +*/ + public class Swerve extends SubsystemBase { // Delcare variables public SwerveDriveOdometry swerveOdometry; @@ -200,6 +244,9 @@ public void periodic(){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } + + // put module states to logger + } } \ No newline at end of file