From c933508ef05484791894c2b85757fb41b0191e93 Mon Sep 17 00:00:00 2001 From: Matthew Perlman Date: Thu, 18 Apr 2024 18:36:08 -0400 Subject: [PATCH] fixed gyro yaw stuff, adjustments to disrupt autos, timeouts for all goal based commands, amp with intake is now on press not hold, some other auto adjustments/additions --- .pathplanner/settings.json | 6 +- src/main/deploy/pathplanner/autos/4Bot.auto | 2 +- .../deploy/pathplanner/paths/1BotTaxi.path | 4 +- .../deploy/pathplanner/paths/1TopTaxi.path | 6 +- .../pathplanner/paths/3BotCenter.1.path | 4 +- .../pathplanner/paths/3BotCenter.2.path | 4 +- .../pathplanner/paths/3BotCenter.3.path | 12 +- .../pathplanner/paths/3BotCenter.4.path | 12 +- .../pathplanner/paths/3BotCenterAlt.1.path | 2 +- .../pathplanner/paths/3BotCenterAlt.3.path | 2 +- .../pathplanner/paths/3BotCenterAlt.4.path | 6 +- src/main/deploy/pathplanner/paths/3Mid.1.path | 2 +- src/main/deploy/pathplanner/paths/3Mid.2.path | 2 +- src/main/deploy/pathplanner/paths/3Mid.3.path | 2 +- .../pathplanner/paths/3TopCenter.1.path | 32 ++++-- .../pathplanner/paths/3TopCenter.2.path | 30 +++-- .../pathplanner/paths/3TopCenter.3.path | 22 +++- .../pathplanner/paths/3TopCenter.4.path | 26 ++++- src/main/deploy/pathplanner/paths/4Bot.1.path | 4 +- .../paths/{TopChaos.path => 4Bot.2.path} | 2 +- src/main/deploy/pathplanner/paths/4Bot.3.path | 28 ++--- src/main/deploy/pathplanner/paths/4Bot.4.path | 2 +- src/main/deploy/pathplanner/paths/4Bot.5.path | 2 +- src/main/deploy/pathplanner/paths/4Mid.5.path | 2 +- src/main/deploy/pathplanner/paths/4Top.3.path | 10 +- src/main/deploy/pathplanner/paths/4Top.4.path | 16 +-- src/main/deploy/pathplanner/paths/4Top.5.path | 8 +- src/main/deploy/pathplanner/paths/4Top.6.path | 12 +- .../deploy/pathplanner/paths/4TopAlt.3.path | 2 +- .../deploy/pathplanner/paths/4TopAlt.4.path | 4 +- .../deploy/pathplanner/paths/4TopAlt.5.path | 12 +- .../deploy/pathplanner/paths/4TopAlt.6.path | 4 +- .../deploy/pathplanner/paths/AmpAlign.path | 52 +++++++++ .../deploy/pathplanner/paths/BotDisrupt.path | 81 +++++++++++-- src/main/deploy/pathplanner/paths/Bruh.path | 2 +- .../deploy/pathplanner/paths/New Path.path | 2 +- .../deploy/pathplanner/paths/TopDisrupt.path | 107 ++++++++++++++---- .../java/org/robolancers321/Constants.java | 2 +- .../org/robolancers321/RobotContainer.java | 23 ++-- .../commands/ScoreAmpIntake.java | 14 ++- .../commands/ScoreSpeakerFixedAuto.java | 2 +- .../subsystems/drivetrain/Drivetrain.java | 7 ++ .../subsystems/drivetrain/SwerveModule.java | 5 + .../subsystems/intake/Retractor.java | 2 +- .../subsystems/launcher/Flywheel.java | 2 +- .../subsystems/launcher/Pivot.java | 2 +- 46 files changed, 421 insertions(+), 166 deletions(-) rename src/main/deploy/pathplanner/paths/{TopChaos.path => 4Bot.2.path} (96%) create mode 100644 src/main/deploy/pathplanner/paths/AmpAlign.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index b6c178d..ddfa954 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,21 +4,21 @@ "holonomicMode": true, "pathFolders": [ "3BotCenter", + "3BotCenterAlt", "3Mid", "3TopCenter", "4Bot", "4Mid", "4Top", - "4TopAlt", "BotDisrupt", "BotTaxi", - "3BotCenterAlt", + "4TopAlt", "TopDisrupt", "TopTaxi" ], "autoFolders": [], "defaultMaxVel": 4.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 4.0, "defaultMaxAngVel": 270.0, "defaultMaxAngAccel": 360.0, "maxModuleSpeed": 4.0 diff --git a/src/main/deploy/pathplanner/autos/4Bot.auto b/src/main/deploy/pathplanner/autos/4Bot.auto index 3995140..3d5f78a 100644 --- a/src/main/deploy/pathplanner/autos/4Bot.auto +++ b/src/main/deploy/pathplanner/autos/4Bot.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "TopChaos" + "pathName": "4Bot.2" } }, { diff --git a/src/main/deploy/pathplanner/paths/1BotTaxi.path b/src/main/deploy/pathplanner/paths/1BotTaxi.path index c0ddca7..d7b454c 100644 --- a/src/main/deploy/pathplanner/paths/1BotTaxi.path +++ b/src/main/deploy/pathplanner/paths/1BotTaxi.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 0.7383447478405972, - "y": 2.0783638253329397 + "x": 1.0949554723536934, + "y": 1.0026621579960722 }, "isLocked": false, "linkedName": "BotStart" diff --git a/src/main/deploy/pathplanner/paths/1TopTaxi.path b/src/main/deploy/pathplanner/paths/1TopTaxi.path index 8ed657d..8ff0af6 100644 --- a/src/main/deploy/pathplanner/paths/1TopTaxi.path +++ b/src/main/deploy/pathplanner/paths/1TopTaxi.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 2.168910700160914, + "x": 1.9409516681628791, "y": 7.705730447124233 }, "prevControl": { - "x": 1.3752333551014027, + "x": 1.1472743231033677, "y": 7.689442794925981 }, "nextControl": { - "x": 3.081821080793479, + "x": 2.8538620487954445, "y": 7.724464970492544 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/3BotCenter.1.path b/src/main/deploy/pathplanner/paths/3BotCenter.1.path index d80effe..81e12ad 100644 --- a/src/main/deploy/pathplanner/paths/3BotCenter.1.path +++ b/src/main/deploy/pathplanner/paths/3BotCenter.1.path @@ -48,14 +48,14 @@ { "waypointRelativePos": 1, "rotationDegrees": 0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/3BotCenter.2.path b/src/main/deploy/pathplanner/paths/3BotCenter.2.path index a9a70bb..11b05c5 100644 --- a/src/main/deploy/pathplanner/paths/3BotCenter.2.path +++ b/src/main/deploy/pathplanner/paths/3BotCenter.2.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -67,5 +67,5 @@ "reversed": false, "folder": "3BotCenter", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3BotCenter.3.path b/src/main/deploy/pathplanner/paths/3BotCenter.3.path index 8c5bf60..ad10f63 100644 --- a/src/main/deploy/pathplanner/paths/3BotCenter.3.path +++ b/src/main/deploy/pathplanner/paths/3BotCenter.3.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.848666424864946, - "y": 2.5529623669803687 + "x": 7.885445770891543, + "y": 2.476602645512984 }, "prevControl": { - "x": 7.296610270823963, - "y": 2.5122137229899386 + "x": 7.33338961685056, + "y": 2.435854001522554 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "3BotCenter", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3BotCenter.4.path b/src/main/deploy/pathplanner/paths/3BotCenter.4.path index 29deca4..4ff94a0 100644 --- a/src/main/deploy/pathplanner/paths/3BotCenter.4.path +++ b/src/main/deploy/pathplanner/paths/3BotCenter.4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.848666424864946, - "y": 2.5529623669803687 + "x": 7.885445770891543, + "y": 2.476602645512984 }, "prevControl": null, "nextControl": { - "x": 7.039577114378632, - "y": 2.0497482836291248 + "x": 7.07635646040523, + "y": 1.9733885621617402 }, "isLocked": false, "linkedName": "3BotCenter.3End" @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -61,5 +61,5 @@ "reversed": false, "folder": "3BotCenter", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3BotCenterAlt.1.path b/src/main/deploy/pathplanner/paths/3BotCenterAlt.1.path index 77fd1a6..6781484 100644 --- a/src/main/deploy/pathplanner/paths/3BotCenterAlt.1.path +++ b/src/main/deploy/pathplanner/paths/3BotCenterAlt.1.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/3BotCenterAlt.3.path b/src/main/deploy/pathplanner/paths/3BotCenterAlt.3.path index 0247846..f0a7c75 100644 --- a/src/main/deploy/pathplanner/paths/3BotCenterAlt.3.path +++ b/src/main/deploy/pathplanner/paths/3BotCenterAlt.3.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/3BotCenterAlt.4.path b/src/main/deploy/pathplanner/paths/3BotCenterAlt.4.path index 1767923..37ca1cb 100644 --- a/src/main/deploy/pathplanner/paths/3BotCenterAlt.4.path +++ b/src/main/deploy/pathplanner/paths/3BotCenterAlt.4.path @@ -48,14 +48,14 @@ { "waypointRelativePos": 1.0, "rotationDegrees": 0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -67,5 +67,5 @@ "reversed": false, "folder": "3BotCenterAlt", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3Mid.1.path b/src/main/deploy/pathplanner/paths/3Mid.1.path index 769ce3d..aa8b9f3 100644 --- a/src/main/deploy/pathplanner/paths/3Mid.1.path +++ b/src/main/deploy/pathplanner/paths/3Mid.1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/3Mid.2.path b/src/main/deploy/pathplanner/paths/3Mid.2.path index 4bb0368..2a08310 100644 --- a/src/main/deploy/pathplanner/paths/3Mid.2.path +++ b/src/main/deploy/pathplanner/paths/3Mid.2.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/3Mid.3.path b/src/main/deploy/pathplanner/paths/3Mid.3.path index 3cacf21..02dc72d 100644 --- a/src/main/deploy/pathplanner/paths/3Mid.3.path +++ b/src/main/deploy/pathplanner/paths/3Mid.3.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/3TopCenter.1.path b/src/main/deploy/pathplanner/paths/3TopCenter.1.path index 6f7bd9f..edd2fc0 100644 --- a/src/main/deploy/pathplanner/paths/3TopCenter.1.path +++ b/src/main/deploy/pathplanner/paths/3TopCenter.1.path @@ -16,16 +16,32 @@ }, { "anchor": { - "x": 2.2415544831089678, - "y": 7.725816220863211 + "x": 1.8469520908507473, + "y": 7.739298532032179 }, "prevControl": { - "x": 1.8370098278658107, - "y": 7.6863484496199765 + "x": 1.4406046908225594, + "y": 7.729507028417042 }, "nextControl": { - "x": 3.180717852181861, - "y": 7.817441915406908 + "x": 2.713837081618185, + "y": 7.7601873269904305 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.1760527286957885, + "y": 7.739298532032179 + }, + "prevControl": { + "x": 3.3822785202822323, + "y": 7.728854134553053 + }, + "nextControl": { + "x": 5.141906451839974, + "y": 7.752007133652497 }, "isLocked": false, "linkedName": null @@ -54,7 +70,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 @@ -70,5 +86,5 @@ "rotation": 60.59, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3TopCenter.2.path b/src/main/deploy/pathplanner/paths/3TopCenter.2.path index 2980af9..2e74ca3 100644 --- a/src/main/deploy/pathplanner/paths/3TopCenter.2.path +++ b/src/main/deploy/pathplanner/paths/3TopCenter.2.path @@ -16,16 +16,32 @@ }, { "anchor": { - "x": 2.843437994568298, - "y": 7.6863484496199765 + "x": 4.88627175727634, + "y": 7.791520519427808 }, "prevControl": { - "x": 3.8582130601048834, - "y": 7.6528021664617425 + "x": 5.37558711858268, + "y": 7.795343711817274 }, "nextControl": { - "x": 1.6495379144604458, - "y": 7.725816220863211 + "x": 4.39695639597, + "y": 7.787697327038341 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7765034664929384, + "y": 7.791520519427808 + }, + "prevControl": { + "x": 3.524231941789387, + "y": 7.750471024527878 + }, + "nextControl": { + "x": 1.5837472720204562, + "y": 7.857001560928128 }, "isLocked": false, "linkedName": null @@ -46,7 +62,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0, + "waypointRelativePos": 2.0, "rotationDegrees": 0.0, "rotateFast": true } diff --git a/src/main/deploy/pathplanner/paths/3TopCenter.3.path b/src/main/deploy/pathplanner/paths/3TopCenter.3.path index 29809b4..a5ed464 100644 --- a/src/main/deploy/pathplanner/paths/3TopCenter.3.path +++ b/src/main/deploy/pathplanner/paths/3TopCenter.3.path @@ -16,20 +16,36 @@ }, { "anchor": { - "x": 2.8237041089466803, + "x": 2.212506002620149, "y": 7.715949278052403 }, "prevControl": { - "x": 2.0639495125144114, + "x": 1.45275140618788, "y": 7.715949278052403 }, "nextControl": { - "x": 4.00349508386258, + "x": 3.3922969775360485, "y": 7.715949278052403 }, "isLocked": false, "linkedName": null }, + { + "anchor": { + "x": 4.374496280799177, + "y": 7.715949278052403 + }, + "prevControl": { + "x": 3.7585213006870717, + "y": 7.733478963047839 + }, + "nextControl": { + "x": 5.022048924504974, + "y": 7.697520942115676 + }, + "isLocked": false, + "linkedName": null + }, { "anchor": { "x": 7.915046599323969, diff --git a/src/main/deploy/pathplanner/paths/3TopCenter.4.path b/src/main/deploy/pathplanner/paths/3TopCenter.4.path index 26eda36..2a9b0ec 100644 --- a/src/main/deploy/pathplanner/paths/3TopCenter.4.path +++ b/src/main/deploy/pathplanner/paths/3TopCenter.4.path @@ -14,18 +14,34 @@ "isLocked": false, "linkedName": "3TopCenter.3End" }, + { + "anchor": { + "x": 4.572939832902567, + "y": 7.73568316367402 + }, + "prevControl": { + "x": 5.314492053920494, + "y": 7.718409737073927 + }, + "nextControl": { + "x": 3.967318271118982, + "y": 7.749790274809825 + }, + "isLocked": false, + "linkedName": null + }, { "anchor": { "x": 2.912506594243959, "y": 7.73568316367402 }, "prevControl": { - "x": 4.11627361716262, - "y": 7.755417049295637 + "x": 3.935831586675896, + "y": 7.728854134553053 }, "nextControl": { - "x": 1.7826026597933275, - "y": 7.717160148355157 + "x": 1.7824760040550014, + "y": 7.743224279023548 }, "isLocked": false, "linkedName": null @@ -46,7 +62,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0, + "waypointRelativePos": 2.0, "rotationDegrees": 0.0, "rotateFast": true } diff --git a/src/main/deploy/pathplanner/paths/4Bot.1.path b/src/main/deploy/pathplanner/paths/4Bot.1.path index 521ffeb..b1fffc6 100644 --- a/src/main/deploy/pathplanner/paths/4Bot.1.path +++ b/src/main/deploy/pathplanner/paths/4Bot.1.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/TopChaos.path b/src/main/deploy/pathplanner/paths/4Bot.2.path similarity index 96% rename from src/main/deploy/pathplanner/paths/TopChaos.path rename to src/main/deploy/pathplanner/paths/4Bot.2.path index bf8879c..730c0c8 100644 --- a/src/main/deploy/pathplanner/paths/TopChaos.path +++ b/src/main/deploy/pathplanner/paths/4Bot.2.path @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4Bot.3.path b/src/main/deploy/pathplanner/paths/4Bot.3.path index 24e7b5e..9882e7d 100644 --- a/src/main/deploy/pathplanner/paths/4Bot.3.path +++ b/src/main/deploy/pathplanner/paths/4Bot.3.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 1.7004772383296536, - "y": 1.7449146453214444 + "x": 1.867840885808999, + "y": 0.6371082462266705 }, "isLocked": false, "linkedName": "4Bot.2End" }, { "anchor": { - "x": 4.85069226914488, - "y": 1.0305536160287727 + "x": 5.241381271566616, + "y": 1.0026621579960722 }, "prevControl": { - "x": 3.531816388121289, - "y": 1.0027878080072234 + "x": 2.8182810564094427, + "y": 1.1697725176620835 }, "nextControl": { - "x": 5.963221740994124, - "y": 1.0539752891203358 + "x": 6.095855913998675, + "y": 0.9437328723111029 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 0.8080477216589235 }, "prevControl": { - "x": 7.1928595783011815, - "y": 0.8548910678420494 + "x": 6.870707278310231, + "y": 0.887773785725688 }, "nextControl": null, "isLocked": false, @@ -46,16 +46,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1, + "waypointRelativePos": 1.0, "rotationDegrees": 0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -70,5 +70,5 @@ "rotation": -57.44999650780657, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4Bot.4.path b/src/main/deploy/pathplanner/paths/4Bot.4.path index 9941715..da63010 100644 --- a/src/main/deploy/pathplanner/paths/4Bot.4.path +++ b/src/main/deploy/pathplanner/paths/4Bot.4.path @@ -70,5 +70,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4Bot.5.path b/src/main/deploy/pathplanner/paths/4Bot.5.path index e050c3c..c521baa 100644 --- a/src/main/deploy/pathplanner/paths/4Bot.5.path +++ b/src/main/deploy/pathplanner/paths/4Bot.5.path @@ -60,7 +60,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/4Mid.5.path b/src/main/deploy/pathplanner/paths/4Mid.5.path index eeb3673..ea746a3 100644 --- a/src/main/deploy/pathplanner/paths/4Mid.5.path +++ b/src/main/deploy/pathplanner/paths/4Mid.5.path @@ -48,5 +48,5 @@ "rotation": 0.0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4Top.3.path b/src/main/deploy/pathplanner/paths/4Top.3.path index 625531a..254fe82 100644 --- a/src/main/deploy/pathplanner/paths/4Top.3.path +++ b/src/main/deploy/pathplanner/paths/4Top.3.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.403268186323488, - "y": 6.790539831491163 + "x": 1.564953358914352, + "y": 7.770631724469556 }, "isLocked": false, "linkedName": "4Top.2End" @@ -20,8 +20,8 @@ "y": 7.419940993728142 }, "prevControl": { - "x": 6.780348176080967, - "y": 7.301537679998438 + "x": 7.142261612767501, + "y": 7.41552221017928 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/4Top.4.path b/src/main/deploy/pathplanner/paths/4Top.4.path index 51382f5..0b3fdc6 100644 --- a/src/main/deploy/pathplanner/paths/4Top.4.path +++ b/src/main/deploy/pathplanner/paths/4Top.4.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 4.027471131865342, - "y": 7.3114046228092455 + "x": 3.862720804322017, + "y": 7.419940993728142 }, "prevControl": { - "x": 5.7566528594595665, - "y": 7.463725552451105 + "x": 5.591902531916242, + "y": 7.572261923370001 }, "nextControl": { - "x": 2.2982894042711175, - "y": 7.1590836931673865 + "x": 2.1335390767277924, + "y": 7.267620064086283 }, "isLocked": false, "linkedName": null @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -67,5 +67,5 @@ "reversed": false, "folder": "4Top", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4Top.5.path b/src/main/deploy/pathplanner/paths/4Top.5.path index 05f9d8b..523688c 100644 --- a/src/main/deploy/pathplanner/paths/4Top.5.path +++ b/src/main/deploy/pathplanner/paths/4Top.5.path @@ -20,8 +20,8 @@ "y": 5.785153253616065 }, "prevControl": { - "x": 6.560474404828979, - "y": 5.855418272890754 + "x": 6.557375353937133, + "y": 6.078639332848802 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 270.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/4Top.6.path b/src/main/deploy/pathplanner/paths/4Top.6.path index fd88be2..af7f07e 100644 --- a/src/main/deploy/pathplanner/paths/4Top.6.path +++ b/src/main/deploy/pathplanner/paths/4Top.6.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.227992087938526, - "y": 7.026501927468906 + "x": 7.392927152266519, + "y": 7.352855825304526 }, "isLocked": false, "linkedName": "4Top.5End" @@ -20,8 +20,8 @@ "y": 6.604911811820771 }, "prevControl": { - "x": 1.7707422576043426, - "y": 7.670597937486888 + "x": 2.1602840152245197, + "y": 7.948186481614694 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -54,5 +54,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4TopAlt.3.path b/src/main/deploy/pathplanner/paths/4TopAlt.3.path index 88122d5..13dc45d 100644 --- a/src/main/deploy/pathplanner/paths/4TopAlt.3.path +++ b/src/main/deploy/pathplanner/paths/4TopAlt.3.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/4TopAlt.4.path b/src/main/deploy/pathplanner/paths/4TopAlt.4.path index ac55bff..0d71f1b 100644 --- a/src/main/deploy/pathplanner/paths/4TopAlt.4.path +++ b/src/main/deploy/pathplanner/paths/4TopAlt.4.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -67,5 +67,5 @@ "reversed": false, "folder": "4TopAlt", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4TopAlt.5.path b/src/main/deploy/pathplanner/paths/4TopAlt.5.path index 47bf7c1..6505127 100644 --- a/src/main/deploy/pathplanner/paths/4TopAlt.5.path +++ b/src/main/deploy/pathplanner/paths/4TopAlt.5.path @@ -36,8 +36,8 @@ "y": 4.371055665188254 }, "prevControl": { - "x": 7.707840800296987, - "y": 4.963072233836776 + "x": 7.549593114453406, + "y": 5.065532777375987 }, "nextControl": null, "isLocked": false, @@ -46,23 +46,23 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0, + "waypointRelativePos": 0.55, "rotationDegrees": 0, - "rotateFast": true + "rotateFast": false } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0.0, "rotation": -51.00900595749459, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": "4TopAlt", diff --git a/src/main/deploy/pathplanner/paths/4TopAlt.6.path b/src/main/deploy/pathplanner/paths/4TopAlt.6.path index 5677f39..1afaf84 100644 --- a/src/main/deploy/pathplanner/paths/4TopAlt.6.path +++ b/src/main/deploy/pathplanner/paths/4TopAlt.6.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, @@ -70,5 +70,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AmpAlign.path b/src/main/deploy/pathplanner/paths/AmpAlign.path new file mode 100644 index 0000000..b396010 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AmpAlign.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.83, + "y": 6.89 + }, + "prevControl": null, + "nextControl": { + "x": 1.83, + "y": 6.99 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.83, + "y": 7.625004858550643 + }, + "prevControl": { + "x": 1.83, + "y": 7.525004858550643 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 90.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BotDisrupt.path b/src/main/deploy/pathplanner/paths/BotDisrupt.path index 68a4cc5..f310b75 100644 --- a/src/main/deploy/pathplanner/paths/BotDisrupt.path +++ b/src/main/deploy/pathplanner/paths/BotDisrupt.path @@ -64,12 +64,76 @@ }, { "anchor": { - "x": 6.977687032297143, - "y": 6.81805748226881 + "x": 7.507815524536903, + "y": 7.1 }, "prevControl": { - "x": 7.638772200621325, - "y": 6.837791367890429 + "x": 7.494947962968767, + "y": 7.5310633125325825 + }, + "nextControl": { + "x": 7.528704319495153, + "y": 6.400225368898574 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.507815524536903, + "y": 1.0754967663781487 + }, + "prevControl": { + "x": 7.489537828948432, + "y": 2.1871895839358344 + }, + "nextControl": { + "x": 7.526093220125373, + "y": -0.036196051179535615 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.496371565580283, + "y": 6.799302758910862 + }, + "prevControl": { + "x": 7.505999994506352, + "y": 4.7375911910499084 + }, + "nextControl": { + "x": 7.486743136654214, + "y": 8.861014326771818 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.507815524536903, + "y": 1.0754967663781487 + }, + "prevControl": { + "x": 7.505862299813023, + "y": 2.958765543169226 + }, + "nextControl": { + "x": 7.5097687492607825, + "y": -0.8077720104129291 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.507815524536903, + "y": 7.164856670680264 + }, + "prevControl": { + "x": 7.486926729578653, + "y": 8.18868379760639 }, "nextControl": null, "isLocked": false, @@ -79,12 +143,7 @@ "rotationTargets": [ { "waypointRelativePos": 2.0, - "rotationDegrees": 141.34, - "rotateFast": true - }, - { - "waypointRelativePos": 3, - "rotationDegrees": 141.34, + "rotationDegrees": -131.16420231185015, "rotateFast": false } ], @@ -98,7 +157,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0, + "rotation": -130.0, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Bruh.path b/src/main/deploy/pathplanner/paths/Bruh.path index 49a416e..ef95a43 100644 --- a/src/main/deploy/pathplanner/paths/Bruh.path +++ b/src/main/deploy/pathplanner/paths/Bruh.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 648a285..1385373 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/TopDisrupt.path b/src/main/deploy/pathplanner/paths/TopDisrupt.path index 204833d..382723d 100644 --- a/src/main/deploy/pathplanner/paths/TopDisrupt.path +++ b/src/main/deploy/pathplanner/paths/TopDisrupt.path @@ -16,16 +16,32 @@ }, { "anchor": { - "x": 2.7250346808327657, - "y": 7.725816220862211 + "x": 2.2333947975784003, + "y": 7.770631724469556 }, "prevControl": { - "x": 1.531896950757774, - "y": 7.682031350033771 + "x": 1.0396092146603575, + "y": 7.75137711829346 }, "nextControl": { - "x": 3.8005314472109144, - "y": 7.765283992105446 + "x": 2.880947441284196, + "y": 7.781076121948681 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.061164356425405, + "y": 7.770631724469556 + }, + "prevControl": { + "x": 3.309167737928352, + "y": 7.770631724469556 + }, + "nextControl": { + "x": 4.980508676613902, + "y": 7.770631724469556 }, "isLocked": false, "linkedName": null @@ -49,27 +65,75 @@ { "anchor": { "x": 8.595865653275746, - "y": 1.1248314804321928 + "y": 1.0131065554751972 }, "prevControl": { "x": 8.595865653269769, - "y": 2.2397960180535748 + "y": 2.1280710930965796 }, "nextControl": { "x": 8.595865653279503, - "y": 0.42407011533044847 + "y": 0.3123451903734529 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.727147871598543, + "y": 2.0888794958251493 + }, + "prevControl": { + "x": 7.719507214798679, + "y": 1.389759398637559 + }, + "nextControl": { + "x": 7.748036666556794, + "y": 4.00020423450516 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.727147871598543, + "y": 7.478188595054036 + }, + "prevControl": { + "x": 7.717030561388807, + "y": 7.5776754787831 + }, + "nextControl": { + "x": 7.9151470262228, + "y": 5.629530241248778 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.9678200894863345, - "y": 0.8880248529727832 + "x": 7.727147871598543, + "y": 1.1488837227038329 }, "prevControl": { - "x": 7.6585060862429435, - "y": 0.779488482053888 + "x": 7.7167034741194165, + "y": 1.9531023285965181 + }, + "nextControl": { + "x": 7.737338372475927, + "y": 0.36421515514538605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.727147871598543, + "y": 6.945524323618622 + }, + "prevControl": { + "x": 7.737592269077669, + "y": 9.03440381944376 }, "nextControl": null, "isLocked": false, @@ -78,32 +142,27 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1, + "waypointRelativePos": 1.9500000000000002, "rotationDegrees": 0.0, "rotateFast": true }, { - "waypointRelativePos": 2.0, - "rotationDegrees": -126.87, + "waypointRelativePos": 3.0, + "rotationDegrees": -30.0, "rotateFast": true - }, - { - "waypointRelativePos": 3, - "rotationDegrees": -126.87, - "rotateFast": false } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 360.0 }, "goalEndState": { "velocity": 0, - "rotation": 89.02897806892084, + "rotation": -30.0, "rotateFast": true }, "reversed": false, @@ -112,5 +171,5 @@ "rotation": 59.16216805531273, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/org/robolancers321/Constants.java b/src/main/java/org/robolancers321/Constants.java index 3e22d48..101be10 100644 --- a/src/main/java/org/robolancers321/Constants.java +++ b/src/main/java/org/robolancers321/Constants.java @@ -44,7 +44,7 @@ public static final class DrivetrainConstants { public static final double kMaxTeleopRotationPercent = 1.0; public static final PathConstraints kAutoConstraints = - new PathConstraints(4.0, 3.0, 270 * Math.PI / 180, 360 * Math.PI / 180); + new PathConstraints(4.0, 4.0, 270 * Math.PI / 180, 360 * Math.PI / 180); public static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 3019476..864e53b 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -157,8 +157,7 @@ private void configureLEDs() { LED.registerSignal( 6, () -> - this.retractor.atGoalTimed(0.6) - && this.retractor.getGoal() == RetractorConstants.RetractorSetpoint.kAmp.angle, + this.retractor.getGoal() == RetractorConstants.RetractorSetpoint.kAmp.angle, LED.solid(Section.FULL, new Color(20, 0, 50))); } @@ -244,9 +243,8 @@ private void configureDriverController() { .whileTrue(new OuttakeNote().unless(() -> climbing)); new Trigger(this.driverController::getAButton) - .onTrue( - // new IntakeSource()); - this.drivetrain.turnToAngle(90.0).unless(() -> climbing)); + .whileTrue(this.drivetrain.alignToAmp().unless(() -> climbing)); + new Trigger(this.driverController::getBButton) .whileTrue(new AutoPickupNote().unless(() -> climbing)); @@ -345,13 +343,14 @@ private void configureManipulatorController() { new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.5) .and(() -> !climbing) - .whileTrue(new ScoreAmpIntake().unless(() -> climbing)); - new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.5) - .and(() -> !climbing) - .onFalse( - new ParallelDeadlineGroup( - new WaitCommand(0.4), this.sucker.ampShot(), Commands.idle(this.retractor)) - .unless(() -> climbing)); + .onTrue(new ScoreAmpIntake().unless(() -> climbing)); + + // new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.5) + // .and(() -> !climbing) + // .onFalse( + // new ParallelDeadlineGroup( + // new WaitCommand(0.4), this.sucker.ampShot(), Commands.idle(this.retractor)) + // .unless(() -> climbing)); new Trigger(() -> this.manipulatorController.getLeftY() < -0.8) .onTrue(this.pivot.aimAtAmp().unless(() -> climbing)); diff --git a/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java b/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java index b0bae71..c742951 100644 --- a/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java +++ b/src/main/java/org/robolancers321/commands/ScoreAmpIntake.java @@ -2,11 +2,15 @@ package org.robolancers321.commands; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + import org.robolancers321.subsystems.intake.Retractor; import org.robolancers321.subsystems.intake.Sucker; -public class ScoreAmpIntake extends SequentialCommandGroup { +public class ScoreAmpIntake extends ParallelRaceGroup { private Retractor retractor; private Sucker sucker; @@ -14,6 +18,12 @@ public ScoreAmpIntake() { this.retractor = Retractor.getInstance(); this.sucker = Sucker.getInstance(); - this.addCommands(this.retractor.moveToAmp(), Commands.idle()); + this.addCommands( + new WaitCommand(3.0), + this.retractor.moveToAmp().until(() -> this.retractor.atGoalTimed(0.5)).andThen(this.sucker.ampShot().withTimeout(0.4)) + + // new ParallelDeadlineGroup( + // new WaitCommand(0.4), this.sucker.ampShot(), Commands.idle(this.retractor)) + ); } } diff --git a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java index 1f95ec1..5a97a60 100644 --- a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java +++ b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java @@ -31,7 +31,7 @@ public ScoreSpeakerFixedAuto() { new ParallelCommandGroup( this.flywheel.revSpeaker(), this.retractor.moveToSpeaker(), - this.pivot.aimAtSpeakerFixed()), + this.pivot.aimAtSpeakerFixed()).withTimeout(3.0), new ParallelDeadlineGroup( (new WaitUntilCommand(this.indexer::exitBeamBroken) .andThen(new WaitUntilCommand(this.indexer::exitBeamNotBroken)) diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 59d1995..bb26276 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -24,6 +24,8 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Optional; import java.util.function.BooleanSupplier; @@ -637,4 +639,9 @@ public Command pathfindToTrap() { return AutoBuilder.pathfindToPose( this.getClosestTrapPosition().pose, DrivetrainConstants.kAutoConstraints); } + + // TODO: distance filter, same as trap + public Command alignToAmp(){ + return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile("AmpAlign"), DrivetrainConstants.kAutoConstraints); + } } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index c2082d2..92b78c6 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -314,6 +314,11 @@ protected void tune() { @Override public void periodic() { SmartDashboard.putNumber(this.id + " ref angle", this.commandedState.angle.getDegrees()); + SmartDashboard.putNumber(this.id + " strator current draw", driveMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber(this.id + " supply current draw", driveMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber(this.id + " torque current draw", driveMotor.getTorqueCurrent().getValueAsDouble()); + + // this.driveController.setReference( // this.commandedState.speedMetersPerSecond, ControlType.kVelocity); diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 0867a92..140a204 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -248,7 +248,7 @@ public Command moveToAngle(DoubleSupplier angleDegSupplier) { angleDegSupplier.getAsDouble(), RetractorConstants.kMinAngle, RetractorConstants.kMaxAngle))) - .until(this::atGoal)); + .until(this::atGoal)).withTimeout(4.0); } public Command moveToAngle(double angleDeg) { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 7c8dd7e..0eb6272 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -173,7 +173,7 @@ public Command revSpeaker() { () -> { this.goalRPM = FlywheelConstants.FlywheelSetpoint.kSpeaker.rpm; }) - .alongWith(new WaitUntilCommand(this::isRevved)); + .alongWith(new WaitUntilCommand(this::isRevved).withTimeout(4.0)); } public Command revTrap() { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 02eb1c3..d6c1ade 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -221,7 +221,7 @@ public Command moveToAngle(DoubleSupplier angleDegSupplier) { angleDegSupplier.getAsDouble(), PivotConstants.kMinAngle, PivotConstants.kMaxAngle))) - .until(this::atGoal)); + .until(this::atGoal)).withTimeout(4.0); } public Command moveToAngle(double angleDeg) {