diff --git a/logs/FRC_TBD_0b09622a29ea64b8.wpilog b/logs/FRC_TBD_0b09622a29ea64b8.wpilog new file mode 100644 index 00000000..3f983fd8 Binary files /dev/null and b/logs/FRC_TBD_0b09622a29ea64b8.wpilog differ diff --git a/logs/FRC_TBD_25d35e33ade850f9.wpilog b/logs/FRC_TBD_25d35e33ade850f9.wpilog deleted file mode 100644 index 536a67e3..00000000 Binary files a/logs/FRC_TBD_25d35e33ade850f9.wpilog and /dev/null differ diff --git a/src/main/deploy/pathplanner/autos/BCA BLUE.auto b/src/main/deploy/pathplanner/autos/BCA BLUE.auto index 24d9090d..e38c6155 100644 --- a/src/main/deploy/pathplanner/autos/BCA BLUE.auto +++ b/src/main/deploy/pathplanner/autos/BCA BLUE.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Blue Center to C" + "pathName": "Blue 90 Deg B Shoot to C" } }, { @@ -33,12 +33,6 @@ "data": { "pathName": "Blue Center to A" } - }, - { - "type": "path", - "data": { - "pathName": "Blue A to Center" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/BCA RED.auto b/src/main/deploy/pathplanner/autos/BCA RED.auto index 9f95d67a..a83277ff 100644 --- a/src/main/deploy/pathplanner/autos/BCA RED.auto +++ b/src/main/deploy/pathplanner/autos/BCA RED.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Red Center to C" + "pathName": "Red 90 Deg B Shoot to C" } }, { @@ -33,12 +33,6 @@ "data": { "pathName": "Red Center to A" } - }, - { - "type": "path", - "data": { - "pathName": "Red A to Center" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path b/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path index 77a92caf..0406c942 100644 --- a/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.8887518932445275, - "y": 4.32662701436514 + "x": 1.843395339506453, + "y": 4.143491955138672 }, "isLocked": false, "linkedName": "Blue BCA B Shoot Pose" }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 1.5581750644122865, - "y": 4.3785396044875196 + "x": 2.048144690184057, + "y": 4.114036770794276 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue A to Center.path b/src/main/deploy/pathplanner/paths/Blue A to Center.path index 18646e91..56138fbc 100644 --- a/src/main/deploy/pathplanner/paths/Blue A to Center.path +++ b/src/main/deploy/pathplanner/paths/Blue A to Center.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.101440276954628, - "y": 7.133434209888363 + "x": 3.3013338050485626, + "y": 7.2749805200131235 }, "prevControl": null, "nextControl": { - "x": 2.9106477302293583, - "y": 6.9718848238973115 + "x": 3.110541258323293, + "y": 7.113431134022072 }, "isLocked": false, "linkedName": "Blue BCA A" diff --git a/src/main/deploy/pathplanner/paths/Blue A to D.path b/src/main/deploy/pathplanner/paths/Blue A to D.path index 8038c033..078edc50 100644 --- a/src/main/deploy/pathplanner/paths/Blue A to D.path +++ b/src/main/deploy/pathplanner/paths/Blue A to D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.954672602983599, - "y": 7.029418819812343 + "x": 3.2143199684464845, + "y": 7.113452071956227 }, "prevControl": null, "nextControl": { - "x": 8.522793824407191, - "y": 7.437045490202704 + "x": 8.786290798324133, + "y": 7.476231693120632 }, "isLocked": false, "linkedName": "Blue ADEF A" @@ -20,8 +20,8 @@ "y": 7.429163844438801 }, "prevControl": { - "x": 3.438956606648067, - "y": 7.060221406032563 + "x": 3.2657732528510195, + "y": 7.0862756529426 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path index 2c3f5073..602ea2d8 100644 --- a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.101440276954628, - "y": 7.133434209888363 + "x": 3.3013338050485626, + "y": 7.2749805200131235 }, "prevControl": null, "nextControl": { - "x": 2.8769730857742335, - "y": 7.243500008770597 + "x": 3.076866613868168, + "y": 7.385046318895357 }, "isLocked": false, "linkedName": "Blue BCA A" }, { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": { - "x": 2.8749498626139145, - "y": 5.658868892078977 + "x": 2.8650467626737863, + "y": 5.7664977519795535 }, "nextControl": { - "x": 3.3250501373860857, - "y": 5.441131107921023 + "x": 3.3151470374459575, + "y": 5.548759967821599 }, "isLocked": false, "linkedName": "Blue BCA B" }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 2.3975266746789083, - "y": 4.484961972566135 + "x": 2.887496300450679, + "y": 4.220459138872892 }, "nextControl": { - "x": 2.850128781571133, - "y": 4.272473542677072 + "x": 3.3400984073429036, + "y": 4.007970708983828 }, "isLocked": false, "linkedName": "Blue BCA C" @@ -96,12 +96,12 @@ }, { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": { - "x": 2.1642311849658524, - "y": 6.346855151347819 + "x": 2.05831973891677, + "y": 6.392846740793796 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Amp to A.path b/src/main/deploy/pathplanner/paths/Blue Amp to A.path index 2b873f2c..8d952245 100644 --- a/src/main/deploy/pathplanner/paths/Blue Amp to A.path +++ b/src/main/deploy/pathplanner/paths/Blue Amp to A.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.954672602983599, - "y": 7.029418819812343 + "x": 3.2143199684464845, + "y": 7.113452071956227 }, "prevControl": { - "x": 1.5093951667101788, - "y": 6.588627448229724 + "x": 1.7690425321730643, + "y": 6.6726607003736085 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue B to C.path b/src/main/deploy/pathplanner/paths/Blue B to C.path index dafe6f9a..5ae1beb1 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to C.path +++ b/src/main/deploy/pathplanner/paths/Blue B to C.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.6938714628917357, - "y": 5.631284339205584 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 2.0322297871158246, - "y": 5.373357077845108 + "x": 1.7912352437800116, + "y": 5.494793717191601 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA B" }, { "anchor": { - "x": 2.8299207212253714, - "y": 4.58 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 2.3071646664126666, - "y": 4.837775838126081 + "x": 2.395957238900271, + "y": 4.417946095006697 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA C" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Blue B to Center.path b/src/main/deploy/pathplanner/paths/Blue B to Center.path index 8e828743..2e56bcde 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to Center.path +++ b/src/main/deploy/pathplanner/paths/Blue B to Center.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 2.85, - "y": 5.55 + "x": 2.8433488902081527, + "y": 5.617436576633564 }, "isLocked": false, "linkedName": "Blue BCA B" }, { "anchor": { - "x": 1.9507925278871394, - "y": 5.55 + "x": 2.138713570581903, + "y": 5.514320436192957 }, "prevControl": { - "x": 2.200792527887139, - "y": 5.55 + "x": 2.3857289673852144, + "y": 5.552835287122212 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue B to D.path b/src/main/deploy/pathplanner/paths/Blue B to D.path index ba11d6d7..b3752ee9 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to D.path +++ b/src/main/deploy/pathplanner/paths/Blue B to D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 4.09653456185508, - "y": 5.917887071242202 + "x": 4.086631461914951, + "y": 6.025515931142778 }, "isLocked": false, "linkedName": "Blue BCA B" diff --git a/src/main/deploy/pathplanner/paths/Blue B to F.path b/src/main/deploy/pathplanner/paths/Blue B to F.path index 36dbdbb7..23bf1ad3 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to F.path +++ b/src/main/deploy/pathplanner/paths/Blue B to F.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 5.044398825776819, - "y": 4.782298508049397 + "x": 5.03449572583669, + "y": 4.889927367949974 }, "isLocked": false, "linkedName": "Blue BCA B" diff --git a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path index 94d21d45..eea1e153 100644 --- a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path +++ b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": null, "nextControl": { - "x": 2.1014736029324417, - "y": 4.3270624847953885 + "x": 2.5914432287042124, + "y": 4.062559651102145 }, "isLocked": false, "linkedName": "Blue BCA C" }, { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": { - "x": 2.4487602821359506, - "y": 6.028702057404115 + "x": 2.3428488360868682, + "y": 6.074693646850092 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Center to A.path b/src/main/deploy/pathplanner/paths/Blue Center to A.path index 4b24808f..727b3008 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to A.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": null, "nextControl": { - "x": 2.8901573716465885, - "y": 6.887841058048188 + "x": 3.176206283465587, + "y": 7.200482584899087 }, "isLocked": false, "linkedName": "Blue BCA C Shoot Pose" }, { "anchor": { - "x": 3.101440276954628, - "y": 7.133434209888363 + "x": 3.3013338050485626, + "y": 7.2749805200131235 }, "prevControl": { - "x": 2.6374369114478973, - "y": 6.684084632617781 + "x": 2.616694679797016, + "y": 6.77157523067328 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Center to B.path b/src/main/deploy/pathplanner/paths/Blue Center to B.path index b22a760e..02cf07e7 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to B.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to B.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.359711113629195, - "y": 5.530775383617674 + "x": 2.267677335262537, + "y": 5.592946872771572 }, "isLocked": false, "linkedName": "Blue BCA Starting Pose" }, { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": { - "x": 2.393853468522427, - "y": 5.551255318985296 + "x": 2.1856013617617, + "y": 5.5787055761019975 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Blue Center to C.path b/src/main/deploy/pathplanner/paths/Blue Center to C.path index cbc05c12..45d871ce 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to C.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to C.path @@ -8,32 +8,27 @@ }, "prevControl": null, "nextControl": { - "x": 2.0924424154876817, - "y": 5.333196225699311 + "x": 2.28067836224651, + "y": 5.0083681416224035 }, "isLocked": false, "linkedName": "Blue BCA B Shoot Pose" }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 1.758976899669116, - "y": 5.811554088964557 + "x": 2.53011686068628, + "y": 5.129772001745606 }, "nextControl": null, "isLocked": false, "linkedName": "Blue BCA C" } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -52.33367496345772 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], diff --git a/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path b/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path index a8da4d1c..f454a004 100644 --- a/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path +++ b/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": { - "x": 3.9604601919295908, - "y": 5.356198902739307 + "x": 3.8545487458805083, + "y": 5.402190492185284 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path b/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path index be2ff57e..dc8244ee 100644 --- a/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 2.315502052772566, - "y": 4.530128331558909 + "x": 2.8054716785443365, + "y": 4.265625497865665 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path index 0c2f87e9..0b9d594b 100644 --- a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path @@ -69,7 +69,7 @@ }, "prevControl": { "x": 7.862682673242908, - "y": 0.976362387364147 + "y": 0.9763623873641469 }, "nextControl": { "x": 7.414861727999737, @@ -112,16 +112,16 @@ }, { "anchor": { - "x": 2.954672602983599, - "y": 7.029418819812343 + "x": 3.2143199684464845, + "y": 7.113452071956227 }, "prevControl": { - "x": 3.9529715982039386, - "y": 7.541832769355557 + "x": 4.212618963666824, + "y": 7.625866021499442 }, "nextControl": { - "x": 1.956373607763259, - "y": 6.517004870269129 + "x": 2.2160209732261444, + "y": 6.601038122413013 }, "isLocked": false, "linkedName": "Blue ADEF A" diff --git a/src/main/deploy/pathplanner/paths/Red B to C.path b/src/main/deploy/pathplanner/paths/Red B to C.path index 748ef4ff..44769eb8 100644 --- a/src/main/deploy/pathplanner/paths/Red B to C.path +++ b/src/main/deploy/pathplanner/paths/Red B to C.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.6938714628917357, - "y": 5.631284339205584 + "x": 3.1, + "y": 5.57 }, "prevControl": null, "nextControl": { - "x": 2.0322297871158246, - "y": 5.373357077845108 + "x": 1.8919980957521443, + "y": 5.607295441711419 }, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA B" }, { "anchor": { - "x": 2.8299207212253714, - "y": 4.58 + "x": 2.6088286579474946, + "y": 4.298674642736191 }, "prevControl": { - "x": 2.0304029772866095, - "y": 5.007284560700074 + "x": 1.6421097406918816, + "y": 4.538467469792007 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA C" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path index 867bdb6b..2d8c2412 100644 --- a/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path +++ b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Red BCA C Shoot Pose" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Red Center to B.path b/src/main/deploy/pathplanner/paths/Red Center to B.path index 2c905a8d..e9ba4363 100644 --- a/src/main/deploy/pathplanner/paths/Red Center to B.path +++ b/src/main/deploy/pathplanner/paths/Red Center to B.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -48,5 +48,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index cb66e368..9e6a03fe 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -298,10 +298,10 @@ public void configureAutons() { AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, "Mobility"); // BCA - //AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, - //"Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center"); - //AutonConfig BCA_RED = new AutonConfig("4 BCA", FourPieceBCA::new, - //"Red Center to B", "Red B to Center", "Red Center to C", "Red C to Shoot Before A", "Red Center to A", "Red A to Center"); + AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, + "Blue Center to B", "Blue B to Center","Blue 90 Deg B Shoot to C", "Blue C to Shoot Before A", "Blue Center to A"); + AutonConfig BCA_RED = new AutonConfig("4 BCA", FourPieceBCA::new, + "Red Center to B", "Red B to Center", "Red 90 Deg B Shoot to C", "Red C to Shoot Before A", "Red Center to A"); // HGF AutonConfig HGF_BLUE = new AutonConfig("4 HGF", FourPieceHGF::new, @@ -377,8 +377,8 @@ public void configureAutons() { MOBILITY_BLUE.registerBlue(autonChooser); MOBILITY_RED.registerRed(autonChooser); - //BCA_BLUE.registerDefaultBlue(autonChooser); - //BCA_RED.registerDefaultRed(autonChooser); + BCA_BLUE.registerDefaultBlue(autonChooser); + BCA_RED.registerDefaultRed(autonChooser); New_BCA_Blue.registerDefaultBlue(autonChooser); New_BCA_Red.registerDefaultRed(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index 576303e3..cfdc89e1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -9,7 +9,6 @@ import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; -import com.stuypulse.robot.constants.Settings.Alignment.Shoot; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -39,8 +38,7 @@ public FourPieceBCA(PathPlannerPath... paths) { // Drive to A + Shoot A new IntakeSetAcquire(), - SwerveDrive.getInstance().followPathCommand(paths[4]), - new FollowPathThenShoot(paths[5], true), + new FollowPathThenShoot(paths[4], true), new ArmToFeed() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java index 51b3509a..e08d1cd6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java @@ -9,7 +9,6 @@ import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; -import com.stuypulse.robot.constants.Settings.Alignment.Shoot; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java index 2c33f898..5ff36818 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java @@ -37,7 +37,7 @@ public SwerveDriveAlignToSpeaker() { .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kD) + controller = new AnglePIDController(Settings.Swerve.Motion.THETA.kP, Settings.Swerve.Motion.THETA.kI, Settings.Swerve.Motion.THETA.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index d69e511e..75ac1535 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; @@ -38,7 +39,7 @@ public SwerveDriveDriveAligned(Gamepad driver) { new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), new VLowPassFilter(Drive.RC.get())); - controller = new AnglePIDController(Assist.kP, Assist.kI, Assist.kD) + controller = new AnglePIDController(Motion.THETA.kP, Motion.THETA.kI, Motion.THETA.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java index 28dbc8d8..a2abbc2f 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java @@ -4,8 +4,11 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; public class SwerveDriveDriveAlignedSpeaker extends SwerveDriveDriveAligned{ @@ -13,17 +16,33 @@ public SwerveDriveDriveAlignedSpeaker(Gamepad driver) { super(driver); } + private Pose2d getTargetSpeakerPose() { + Pose2d speakerPose = Field.getAllianceSpeakerPose().transformBy(new Transform2d(-Units.inchesToMeters(1.5), 0, new Rotation2d())); + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + + double angleFromSpeakerBaseToRobot = Units.radiansToDegrees(Math.atan((speakerPose.getY() - robotPose.getY())/(speakerPose.getX() - robotPose.getX()))); + + // aim at the side of the speaker if youre on the side + // if (angleFromSpeakerBaseToRobot > 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + // if (angleFromSpeakerBaseToRobot < 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, -Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + return speakerPose; + } + @Override protected Rotation2d getTargetAngle() { Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); - Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + Translation2d speakerPose = getTargetSpeakerPose().getTranslation(); return currentPose.minus(speakerPose).getAngle(); } @Override protected double getDistanceToTarget() { Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); - Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + Translation2d speakerPose = getTargetSpeakerPose().getTranslation(); return currentPose.getDistance(speakerPose); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 3bc76a29..5de7d52a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -27,7 +27,7 @@ public interface Limelight { new CameraConfig( "tower-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)), + new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(3.333797 - 1.4375), Units.inchesToMeters(23.929362)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-15), Units.degreesToRadians(180)) ), "11", @@ -37,7 +37,7 @@ public interface Limelight { new CameraConfig( "plate-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)), + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591 - 1.4375), Units.inchesToMeters(19.216471)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-10), Units.degreesToRadians(0)) ), "96", diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 13d3b51c..56809d2f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -30,6 +30,8 @@ public interface Field { double SPEAKER_MAX_HEIGHT = 2.11; // represents the top of the speaker opening double SPEAKER_MIN_HEIGHT = 1.98; // represents the bottom of the speaker opening + double SPEAKER_OPENING_WIDTH = Units.inchesToMeters(41); + public static Pose3d transformToOppositeAlliance(Pose3d pose) { Pose3d rotated = pose.rotateBy(new Rotation3d(0, 0, Math.PI)); @@ -155,12 +157,7 @@ public static AprilTag getTag(int id) { public static Pose2d getAllianceSpeakerPose() { return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER) - .getLocation().toPose2d().transformBy(new Transform2d(SPEAKER_OPENING_X, 0, new Rotation2d())); - } - - public static Pose2d getSpeakerPathFindPose() { - return getAllianceSpeakerPose().transformBy( - new Transform2d(0, Units.inchesToMeters(200), new Rotation2d())); + .getLocation().toPose2d(); } /*** AMP ***/ diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 90c24f94..d88b602d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -39,18 +39,18 @@ public interface Arm { SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 750); SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 700); - SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85); - SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25); + double MAX_ANGLE = 85; + double MIN_ANGLE = -90 + 12.25; SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5); SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 49); - SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", MIN_ANGLE.get()); + SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", MIN_ANGLE); SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", -50); - SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 90); - SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get()); + SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 85); + SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 0); + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE + 0); SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 4); SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -33); @@ -85,7 +85,7 @@ public interface Intake { double INTAKE_FEED_SPEED = 0.4; - double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 20; + double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE + 20; double ARM_SPEED_THRESHOLD_TO_FEED = 1.75; // degrees per second double INTAKE_SHOOT_SPEED = 0.9; @@ -107,16 +107,13 @@ public interface Shooter { double TARGET_RPM_THRESHOLD = 250; double MAX_WAIT_TO_REACH_TARGET = 2.0; - ShooterSpeeds SPEAKER = new ShooterSpeeds( - new SmartNumber("Shooter/Speaker RPM", 5500), - new SmartNumber("Shooter/Speaker RPM differential", 500) - ); + // ShooterSpeeds SPEAKER = new ShooterSpeeds( + // new SmartNumber("Shooter/Speaker RPM", 5500), + // new SmartNumber("Shooter/Speaker RPM differential", 500) + // ); - // TODO: Find velocity - double SPEAKER_SHOT_VELOCITY = 10.0; // m/s - - SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.0); - SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.0); + double HAS_NOTE_FALLING_DEBOUNCE = 0.0; + double HAS_NOTE_RISING_DEBOUNCE = 0.0; // left runs faster than right public interface LEFT { @@ -168,15 +165,8 @@ public interface Swerve { double SPEED_AT_12_VOLTS = 5.21; public interface Assist { - SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); - double AMP_WALL_SCORE_DISTANCE = (Settings.LENGTH / 2) + Units.inchesToMeters(2.5); - // angle PID - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 5.0); - SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); - SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.2); - double ANGLE_DERIV_RC = 0.05; double REDUCED_FF_DIST = 0.75; } @@ -217,9 +207,9 @@ public interface Drive { SmartNumber kI = new SmartNumber("Swerve/Turn/PID/kI", Robot.isReal() ? 0.0 : 0.0); SmartNumber kD = new SmartNumber("Swerve/Turn/PID/kD", Robot.isReal() ? 0.0 : 0.1); - SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", Robot.isReal() ? 0.31007 : Simulation.DRIVE_FRICTION_VOLTAGE); - SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", Robot.isReal() ? 1.62153 : 0.25); - SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", Robot.isReal() ? 0.0048373 : 0.01); + SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", Robot.isReal() ? 0.099001 : Simulation.DRIVE_FRICTION_VOLTAGE); + SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", Robot.isReal() ? 0.11964 : 0.25); + SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", Robot.isReal() ? 0.030009 : 0.01); double L2 = ((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)); // 6.74607175 double L3 = ((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 6.12244898 @@ -293,20 +283,6 @@ public interface Rotation { SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Alignment/Rotation/Omega Deadband", 0.05); } - - public interface Shoot { - public interface Translation { - SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); - SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.7); - } - - public interface Rotation { - SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); - SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); - } - } } public interface LED { diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 3f0e3ac2..7284fa62 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -1,11 +1,11 @@ package com.stuypulse.robot.subsystems.arm; -import org.ejml.dense.row.mult.SubmatrixOps_FDRM; - +import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; - -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -15,22 +15,16 @@ import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; -import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; 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.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; @@ -113,22 +107,48 @@ private double getTargetDegrees() { case FEED: return Settings.Arm.FEED_ANGLE.get(); case STOW: - return Settings.Arm.MIN_ANGLE.get(); + return Settings.Arm.MIN_ANGLE; case PRE_CLIMB: return Settings.Arm.PRE_CLIMB_ANGLE.get(); case CLIMBING: return Settings.Arm.POST_CLIMB_ANGLE.get(); default: - return Settings.Arm.MIN_ANGLE.get(); + return Settings.Arm.MIN_ANGLE; } } private double getSpeakerAngleElin() { try { - Pose2d speakerPose = Field.getAllianceSpeakerPose(); - double distanceToSpeaker = Units.metersToInches(SwerveDrive.getInstance().getPose().minus(speakerPose).getTranslation().getNorm()) - Units.metersToInches(Settings.WIDTH / 2); - SmartDashboard.putNumber("harry", distanceToSpeaker); - return SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); + Pose2d speakerPose = Field.getAllianceSpeakerPose().transformBy(new Transform2d(Field.SPEAKER_OPENING_X - Units.inchesToMeters(1.5), 0, new Rotation2d())); + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + + double angleFromSpeakerToRobot = Units.radiansToDegrees(Math.atan((speakerPose.getY() - robotPose.getY())/(speakerPose.getX() - robotPose.getX()))); + + // aim at the side of the speaker if youre on the side + // if (angleFromSpeakerToRobot > 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + // if (angleFromSpeakerToRobot < 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, -Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + + double distanceToSpeaker = Units.metersToInches(SwerveDrive.getInstance().getPose().minus(speakerPose).getTranslation().getNorm()) - Units.metersToInches(Settings.LENGTH / 2); + + double targetAngle = SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); + + if (distanceToSpeaker > 120) { + targetAngle += (distanceToSpeaker - 120) * (1.34 / 80) * (1.2); + } + + SmartDashboard.putNumber("Distance to speaker", distanceToSpeaker); + SmartDashboard.putNumber("Angle to speaker", angleFromSpeakerToRobot); + + // if the robot is more than 30 degrees off to the side from the perspective of the speaker + // this is intended to help with shooting from the sides + if (Math.abs(angleFromSpeakerToRobot) > 30) { + targetAngle += 2; + } + return targetAngle; } catch (Exception e) { e.printStackTrace(); @@ -304,7 +324,7 @@ public void periodic() { super.periodic(); if (bumpSwitchTriggered.get()) { - armEncoder.setPosition(Settings.Arm.MIN_ANGLE.get()/360); + armEncoder.setPosition(Settings.Arm.MIN_ANGLE/360); if (state == State.RESETTING) { state = State.FEED; } @@ -312,15 +332,15 @@ public void periodic() { if (state == State.RESETTING) { setVoltage(-1.5); - controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); + controller.update(Settings.Arm.MIN_ANGLE, Settings.Arm.MIN_ANGLE); } - else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTriggered.get()) { + else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE && bumpSwitchTriggered.get() && state != State.CLIMBING) { setVoltage(0); - controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); + controller.update(Settings.Arm.MIN_ANGLE, Settings.Arm.MIN_ANGLE); } else { - controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); - if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && getDegrees() < Settings.Arm.MAX_ANGLE.get()) { + controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE, Settings.Arm.MAX_ANGLE), getDegrees()); + if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && getDegrees() < Settings.Arm.MAX_ANGLE) { setVoltage(controller.getOutput() + 0.31); } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 8f0eb833..8cb12012 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -4,6 +4,7 @@ import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.shooter.Shooter.FeederState; +import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,12 +31,17 @@ public enum State { } private State state; + protected final StopWatch feedingTimer; protected Intake() { this.state = State.STOP; + feedingTimer = new StopWatch(); } public void setState(State state) { + if (state == State.FEEDING && this.state != State.FEEDING) { + feedingTimer.reset(); + } this.state = state; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 6582df43..9b25d72e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -6,6 +6,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -63,9 +64,19 @@ private void stop() { } private void feed() { - intakeMotor.set(Settings.Intake.INTAKE_FEED_SPEED); - funnelMotorLeft.stopMotor(); - funnelMotorRight.stopMotor(); + if (feedingTimer.getTime() < 0.75) { + intakeMotor.set(Settings.Intake.INTAKE_FEED_SPEED); + funnelMotorLeft.stopMotor(); + funnelMotorRight.stopMotor(); + } + else if (feedingTimer.getTime() > 1.5) { + feedingTimer.reset(); + } + else { + intakeMotor.set(1.0); + funnelMotorLeft.stopMotor(); + funnelMotorRight.stopMotor(); + } } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index af4ed8ed..3310b767 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -17,10 +17,12 @@ import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterLowFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; @@ -83,8 +85,8 @@ protected ShooterImpl() { Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor); Motors.Shooter.FEEDER_MOTOR.configure(feederMotor); - leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.SPEAKER.getLeftRPM()); - rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.SPEAKER.getRightRPM()); + leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", getSpeakerShotSpeeds().getLeftRPM()); + rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", getSpeakerShotSpeeds().getRightRPM()); } private double getLeftShooterRPM() { @@ -138,7 +140,7 @@ private void setFlywheelsBasedOnState() { double manualFerryDistance = Units.metersToInches(Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose())); switch (getFlywheelState()) { case SPEAKER: - setTargetSpeeds(Settings.Shooter.SPEAKER); + setTargetSpeeds(getSpeakerShotSpeeds()); break; case LOW_FERRY: setTargetSpeeds(getLowFerrySpeeds()); @@ -175,6 +177,22 @@ private void setFlywheelsBasedOnState() { } } + private ShooterSpeeds getSpeakerShotSpeeds() { + Pose2d speakerPose = Field.getAllianceSpeakerPose(); + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + double distanceToSpeaker = robotPose.minus(speakerPose).getTranslation().getNorm() - Settings.LENGTH / 2; + // return new ShooterSpeeds( + // 4000 + SLMath.clamp(distanceToSpeaker - 1.5, 0, Double.MAX_VALUE) * 600, + // 500 + // ); + if (distanceToSpeaker <= 1.5) { + return new ShooterSpeeds(4000, 500); + } + else { + return new ShooterSpeeds(5500, 500); + } + } + @Override public boolean hasNote() { return hasNote.get(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index bfa84f33..1a1e1b06 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -174,7 +174,7 @@ public void drive(Vector2D velocity, double rotation) { twistVel.dy / Settings.DT, twistVel.dtheta / Settings.DT )); - }/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + } private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); diff --git a/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java b/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java index f0cdc24d..f2b7ae90 100644 --- a/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java +++ b/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java @@ -9,6 +9,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.math.Angle; @@ -84,7 +85,7 @@ public FollowPathPointSpeakerCommand( this.shouldFlipPath = shouldFlipPath; - headingController = new AnglePIDController(Assist.kP, Assist.kI, Assist.kD) + headingController = new AnglePIDController(Motion.THETA.kP, Motion.THETA.kI, Motion.THETA.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); diff --git a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java index c8c92d5a..3ef236d6 100644 --- a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java @@ -10,67 +10,142 @@ public class SpeakerAngleElinInterpolation { // distance (angle), angle (radians) private static final double[][] distanceAndAngle = { - {45.1854245,-0.57751}, - {47.5, -0.60548}, - {50, -0.63458}, - {52.5, -0.66255}, - {55, -0.68943}, - {57.5,-0.71524}, - {60, -0.74002}, - {62.5, -0.76382}, - {65, -0.78668}, - {67.5, -0.80862}, - {70, -0.82969}, - {72.5, -0.84993}, - {75, -0.86938}, - {77.5, -0.88806}, - {80, -0.90602}, - {82.5, -0.92328}, - {85, -0.93989}, - {87.5, -0.95587}, - {90, -0.97125}, - {92.5, -0.98606}, - {95, -1.00032}, - {97.5, -1.01407}, - {100, -1.02732}, - {102.5, -1.0401}, - {105, -1.05243}, - {107.5, -1.06434}, - {110, -1.07583}, - {112.5, -1.08693}, - {115, -1.09767}, - {117.5, -1.10804}, - {120, -1.11808}, - {122.5, -1.12779}, - {125, -1.13719}, - {127.5, -1.14629}, - {130, -1.15512}, - {132.5, -1.16366}, - {135, -1.17195}, - {137.5, -1.17999}, - {140, -1.18779}, - {142.5, -1.19536}, - {145, -1.20271}, - {147.5, -1.20986}, - {150, -1.21679}, - {152.5, -1.22354}, - {155, -1.2301}, - {157.5, -1.23647}, - {160, -1.24268}, - {162.5, -1.24872}, - {165, -1.2546}, - {167.5, -1.26033}, - {170, -1.26591}, - {172.5, -1.27134}, - {175, -1.27664}, - {177.5, -1.28181}, - {180, -1.28685}, + // {45.1854245,-0.57751}, + // {47.5, -0.60548}, + // {50, -0.63458}, + // {52.5, -0.66255}, + // {55, -0.68943}, + // {57.5,-0.71524}, + // {60, -0.74002}, + // {62.5, -0.76382}, + // {65, -0.78668}, + // {67.5, -0.80862}, + // {70, -0.82969}, + // {72.5, -0.84993}, + // {75, -0.86938}, + // {77.5, -0.88806}, + // {80, -0.90602}, + // {82.5, -0.92328}, + // {85, -0.93989}, + // {87.5, -0.95587}, + // {90, -0.97125}, + // {92.5, -0.98606}, + // {95, -1.00032}, + // {97.5, -1.01407}, + // {100, -1.02732}, + // {102.5, -1.0401}, + // {105, -1.05243}, + // {107.5, -1.06434}, + // {110, -1.07583}, + // {112.5, -1.08693}, + // {115, -1.09767}, + // {117.5, -1.10804}, + // {120, -1.11808}, + // {122.5, -1.12779}, + // {125, -1.13719}, + // {127.5, -1.14629}, + // {130, -1.15512}, + // {132.5, -1.16366}, + // {135, -1.17195}, + // {137.5, -1.17999}, + // {140, -1.18779}, + // {142.5, -1.19536}, + // {145, -1.20271}, + // {147.5, -1.20986}, + // {150, -1.21679}, + // {152.5, -1.22354}, + // {155, -1.2301}, + // {157.5, -1.23647}, + // {160, -1.24268}, + // {162.5, -1.24872}, + // {165, -1.2546}, + // {167.5, -1.26033}, + // {170, -1.26591}, + // {172.5, -1.27134}, + // {175, -1.27664}, + // {177.5, -1.28181}, + // {180, -1.28685}, + {45, -0.53055}, + {47.5, -0.56248}, + {50, -0.59322}, + {52.5, -0.62279}, + {55, -0.65122}, + {57.5, -0.67854}, + {60, -0.70478}, + {62.5, -0.72998}, + {65, -0.75418}, + {67.5, -0.77742}, + {70, -0.79973}, + {72.5, -0.82116}, + {75, -0.84173}, + {77.5, -0.8615}, + {80, -0.88049}, + {82.5, -0.89874}, + {85, -0.91629}, + {87.5, -0.93316}, + {90, -0.94939}, + {92.5, -0.96502}, + {95, -0.98005}, + {97.5, -0.99454}, + {100, -1.00849}, + {102.5, -1.02195}, + {105, -1.03492}, + {107.5, -1.04743}, + {110, -1.05951}, + {112.5, -1.07117}, + {115, -1.08243}, + {117.5, -1.09331}, + {120, -1.10383}, + {122.5, -1.114}, + {125, -1.12385}, + {127.5, -1.13337}, + {130, -1.1426}, + {132.5, -1.15153}, + {135, -1.16019}, + {137.5, -1.16858}, + {140, -1.17672}, + {142.5, -1.18462}, + {145, -1.19229}, + {147.5, -1.19973}, + {150, -1.20695}, + {152.5, -1.21397}, + {155, -1.2208}, + {157.5, -1.22743}, + {160, -1.23388}, + {162.5, -1.24016}, + {165, -1.24626}, + {167.5, -1.25221}, + {170, -1.258}, + {172.5, -1.26364}, + {175, -1.26913}, + {177.5, -1.27449}, + {180, -1.27971}, + {182.5, -1.2848}, + {185, -1.28976}, + {187.5, -1.29461}, + {190, -1.29934}, + {192.5, -1.30396}, + {195, -1.30846}, + {197.5, -1.31287}, + {200, -1.31717}, + {202.5, -1.32137}, + {205, -1.32548}, + {207.5, -1.3295}, + {210, -1.33343}, + {212.5, -1.33727}, + {215, -1.34103}, + {217.5, -1.34471}, + {220, -1.34831}, + {222.5, -1.35184}, + {225, -1.35529}, + {227.5, -1.35867}, + {230, -1.36198}, }; static { interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap(); for (double[] data: distanceAndAngle) { - interpolatingDoubleTreeMap.put(data[0], data[1]); + interpolatingDoubleTreeMap.put(data[0] - 1.4375, data[1]); } }