diff --git a/build.gradle b/build.gradle index c6d0407d..ade2c7e7 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" id "com.peterabeles.gversion" version "1.10" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" } java { diff --git a/src/main/deploy/pathplanner/autos/Non Proc 3P Back.auto b/src/main/deploy/pathplanner/autos/Non Proc 3P Back.auto new file mode 100644 index 00000000..370a3ff3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Non Proc 3P Back.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Non Proc 3P Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Non Proc 3P Part 2" + } + }, + { + "type": "path", + "data": { + "pathName": "Non Proc 3P Part 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Non Proc 3P Part 4" + } + }, + { + "type": "path", + "data": { + "pathName": "Non Proc 3P Part 5" + } + }, + { + "type": "path", + "data": { + "pathName": "Non Proc 3P Part 6" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3p Part 4.path b/src/main/deploy/pathplanner/paths/3p Part 4.path index ec99b8da..0ffeef17 100644 --- a/src/main/deploy/pathplanner/paths/3p Part 4.path +++ b/src/main/deploy/pathplanner/paths/3p Part 4.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.4623933308999684, - "y": 3.9350675594736853 + "x": 2.733196721311475, + "y": 3.191854508196721 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.1268442622950818, - "y": 0.9022028688524597 + "x": 1.0549180327868852, + "y": 0.8542520491803282 }, "prevControl": { - "x": 1.2801788743579254, - "y": 1.0996579187841775 + "x": 1.2082526448497288, + "y": 1.0517070991120465 }, "nextControl": null, "isLocked": false, @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0.0, + "velocity": 0.5, "rotation": -130.31410016049722 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 1.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 1.path new file mode 100644 index 00000000..b7dc0e8e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.480327868852459, + "y": 6.744 + }, + "prevControl": null, + "nextControl": { + "x": 6.693459821961271, + "y": 6.435926031716894 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.149692622950819, + "y": 6.151 + }, + "prevControl": { + "x": 6.803183415463562, + "y": 6.404541372953622 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": -131.65526786557322 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -157.16643596899323 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 2.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 2.path new file mode 100644 index 00000000..e3acbfb9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.399, + "y": 5.223 + }, + "prevControl": null, + "nextControl": { + "x": 6.169325791052848, + "y": 4.82632093567127 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3306352459016395, + "y": 7.397 + }, + "prevControl": { + "x": 1.6882833267044668, + "y": 7.0725257002517985 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 131.83680398463187 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -123.01789886636891 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 3.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 3.path new file mode 100644 index 00000000..b78eb4b9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.331, + "y": 7.397 + }, + "prevControl": null, + "nextControl": { + "x": 2.4849779316121507, + "y": 6.168069226669158 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2230000000000003, + "y": 4.327249999999999 + }, + "prevControl": { + "x": 2.1614054490342824, + "y": 3.8237130356587317 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 1.3342198686546403 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 126.77960024038128 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 4.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 4.path new file mode 100644 index 00000000..07a04a26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.0568647540983607, + "y": 3.825 + }, + "prevControl": null, + "nextControl": { + "x": 3.096406281985313, + "y": 4.729376248025407 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.331, + "y": 7.397 + }, + "prevControl": { + "x": 1.2128566705124273, + "y": 7.617322839709348 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 127.06544922742414 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 5.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 5.path new file mode 100644 index 00000000..9e4abb02 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 5.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.331, + "y": 7.397 + }, + "prevControl": null, + "nextControl": { + "x": 2.0793784072979005, + "y": 6.739865555901801 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.29125, + "y": 4.278499999999999 + }, + "prevControl": { + "x": 2.080705009415983, + "y": 5.0195092615652666 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": -4.667500514107807 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 129.4692757880314 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 6.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 6.path new file mode 100644 index 00000000..2b10d35f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 6.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.104815573770492, + "y": 4.161 + }, + "prevControl": null, + "nextControl": { + "x": 3.587813386309917, + "y": 4.126091988307253 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.331, + "y": 7.397 + }, + "prevControl": { + "x": 1.316664468999613, + "y": 7.147411353321236 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 128.49168164709494 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path b/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path index cd3e314c..bc3ef8fb 100644 --- a/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path +++ b/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.0190673805973045, - "y": 1.2928065821314694 + "x": 2.03227239650616, + "y": 1.3255122229552796 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.2177254098360653, - "y": 3.4495901639344266 + "x": 2.3376024590163933, + "y": 3.623411885245902 }, "prevControl": { - "x": 2.1842176784272214, - "y": 2.7096986638772265 + "x": 2.053961680497683, + "y": 2.8685853299533908 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 2.0, - "rotation": 4.398705354995537 + "rotation": 3.012787504183176 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Proc Part 5 Back.path b/src/main/deploy/pathplanner/paths/Proc Part 5 Back.path index afe9a3f2..3453c071 100644 --- a/src/main/deploy/pathplanner/paths/Proc Part 5 Back.path +++ b/src/main/deploy/pathplanner/paths/Proc Part 5 Back.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.833800523907986, - "y": 1.6156801872723725 + "x": 2.197116034701116, + "y": 2.2487172421985746 }, "isLocked": false, "linkedName": "Proc2Part4" }, { "anchor": { - "x": 2.361577868852459, - "y": 3.539497950819672 + "x": 2.338, + "y": 3.623 }, "prevControl": { - "x": 2.0624564447553917, - "y": 2.68204192524031 + "x": 1.9168277457475669, + "y": 2.6698318198866833 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 2.0, - "rotation": 4.76364169072624 + "rotation": 5.079607860014388 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Proc Two Piece Part 2 test.path b/src/main/deploy/pathplanner/paths/Proc Two Piece Part 2 test.path index 4af753c0..815539c5 100644 --- a/src/main/deploy/pathplanner/paths/Proc Two Piece Part 2 test.path +++ b/src/main/deploy/pathplanner/paths/Proc Two Piece Part 2 test.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.3306352459016395, - "y": 0.6025102459016398 + "x": 1.282684426229508, + "y": 0.5545594262295083 }, "prevControl": { - "x": 2.049897540983606, - "y": 0.6504610655737716 + "x": 2.0019467213114748, + "y": 0.6025102459016399 }, "nextControl": null, "isLocked": false, @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0.0, + "velocity": 0.5, "rotation": -125.53767779197437 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Proc part 6 back.path b/src/main/deploy/pathplanner/paths/Proc part 6 back.path index 6b0bc61a..3bc60874 100644 --- a/src/main/deploy/pathplanner/paths/Proc part 6 back.path +++ b/src/main/deploy/pathplanner/paths/Proc part 6 back.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.5, "rotation": -128.4296930429381 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Proc2P +Algae Part 3.path b/src/main/deploy/pathplanner/paths/Proc2P +Algae Part 3.path index 4f85efed..35e54ba8 100644 --- a/src/main/deploy/pathplanner/paths/Proc2P +Algae Part 3.path +++ b/src/main/deploy/pathplanner/paths/Proc2P +Algae Part 3.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.3785860655737705, - "y": 0.5545594262295083 + "x": 1.3186475409836067, + "y": 0.6624487704918033 }, "prevControl": null, "nextControl": { - "x": 2.051945679569937, - "y": 1.2141556064827757 + "x": 1.992007154979773, + "y": 1.3220449507450707 }, "isLocked": false, - "linkedName": "Proc2PPart2" + "linkedName": null }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Proc2P Part 1.path b/src/main/deploy/pathplanner/paths/Proc2P Part 1.path index f6805921..d8371bc5 100644 --- a/src/main/deploy/pathplanner/paths/Proc2P Part 1.path +++ b/src/main/deploy/pathplanner/paths/Proc2P Part 1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.480327868852459, - "y": 1.2558401639344265 + "x": 7.22858606557377, + "y": 1.4896004098360651 }, "prevControl": null, "nextControl": { - "x": 6.640835853648995, - "y": 1.5065202897254946 + "x": 6.292462720624658, + "y": 2.234853917631673 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.149692622950819, - "y": 1.8492315573770486 + "x": 5.730122950819672, + "y": 2.292776639344262 }, "prevControl": { - "x": 6.831750722385158, - "y": 1.540147562370991 + "x": 6.252487509980028, + "y": 2.0414527288053086 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 2.0, - "rotation": 130.76360520094124 + "rotation": 119.89890183861452 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 161.56505117707798 + "rotation": 140.19442890773485 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc2P Part 2.path b/src/main/deploy/pathplanner/paths/Proc2P Part 2.path index 274eecb1..0bf82dbf 100644 --- a/src/main/deploy/pathplanner/paths/Proc2P Part 2.path +++ b/src/main/deploy/pathplanner/paths/Proc2P Part 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.149692622950819, - "y": 1.8492315573770486 + "x": 5.730122950819672, + "y": 2.292776639344262 }, "prevControl": null, "nextControl": { - "x": 5.412853020546758, - "y": 1.5188407674951923 + "x": 4.993283348415611, + "y": 1.9623858494624056 }, "isLocked": false, "linkedName": "Proc2P" }, { "anchor": { - "x": 1.3785860655737705, - "y": 0.5545594262295083 + "x": 1.3186475409836067, + "y": 0.6624487704918033 }, "prevControl": { - "x": 2.1265346794474533, - "y": 1.0840398460083853 + "x": 2.0665961548572898, + "y": 1.191929190270681 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 2.0, - "rotation": 130.76360520094124 + "rotation": 119.89890183861452 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc2P Part 3.path b/src/main/deploy/pathplanner/paths/Proc2P Part 3.path index 0d8dda76..31042fa6 100644 --- a/src/main/deploy/pathplanner/paths/Proc2P Part 3.path +++ b/src/main/deploy/pathplanner/paths/Proc2P Part 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3785860655737705, - "y": 0.5545594262295083 + "x": 1.3186475409836067, + "y": 0.6624487704918033 }, "prevControl": null, "nextControl": { - "x": 1.364611735806812, - "y": 0.30495029557206554 + "x": 1.3046732112166481, + "y": 0.41283963983436056 }, "isLocked": false, "linkedName": "Proc2PPart2" diff --git a/src/main/java/team1403/robot/Constants.java b/src/main/java/team1403/robot/Constants.java index d0e5b532..cac47e86 100644 --- a/src/main/java/team1403/robot/Constants.java +++ b/src/main/java/team1403/robot/Constants.java @@ -31,6 +31,7 @@ public class Constants { public static class RioPorts { private static final int kTBD = 0; + public static final int kServoPort = 0; public static final int kAlgaeIntakePhotogateID = kTBD; } @@ -92,7 +93,7 @@ public static class Vision { //top-to-bottom disp = 17.82426 inches //public static final Rotation3d kCameraRotation = new Rotation3d(Math.PI, Units.degreesToRadians(-25), Math.PI); public static final Rotation3d kLimelightRotation = new Rotation3d(Units.degreesToRadians(0), 0, Units.degreesToRadians(0)); - public static final Translation3d kCameraOffset = new Translation3d(Units.inchesToMeters(3.213) - 0.08,Units.inchesToMeters(-0.495) + 0.02,Units.inchesToMeters(17.396+0.5)); + public static final Translation3d kCameraOffset = new Translation3d(Units.inchesToMeters(3.213) - 0.08,Units.inchesToMeters(-0.495) + 0.02,Units.inchesToMeters(17.396)); //public static final Transform3d kCameraTransfrom = new Transform3d(kCameraOffset, kCameraRotation); public static final Transform3d kLimelightTransform = new Transform3d(kCameraOffset, kLimelightRotation); @@ -148,6 +149,8 @@ public static class Setpoints { public static final double Source = 1; public static final double Current = 1; public static final double Min = 1; + public static final double Max = 61; + public static final double Barge = 60; } } @@ -160,6 +163,7 @@ public static class Setpoints{ public static final double Source = - 12 / 360.; public static final double Current = 0.23 * 360.; public static final double Drive = -64 / 360.; + public static final double Barge = -75 / 360; } public static final double WristKS = 0; @@ -180,6 +184,12 @@ public static class Setpoints{ public static class Climber { public static final double upSpeed = 1; public static final double downSpeed = -1; + + public static final double upPosition = 13; + public static final double downPosition = -77; + + public static final double ratchetDisengage = 0.125; + public static final double ratchetEngage = 0; } public static class AlgaeIntake { @@ -203,4 +213,9 @@ public static class AlgaeWrist { public static final double Ki = 0.0; public static final double Kd = 0.0; } + + public static class LED { + public static final double speed = 0.7; + public static final int kLedCount = -1; //Placeholder + } } \ No newline at end of file diff --git a/src/main/java/team1403/robot/RobotContainer.java b/src/main/java/team1403/robot/RobotContainer.java index 22e2057f..46df7a56 100644 --- a/src/main/java/team1403/robot/RobotContainer.java +++ b/src/main/java/team1403/robot/RobotContainer.java @@ -60,6 +60,7 @@ import team1403.robot.subsystems.Blackbox.ReefScoreLevel; import team1403.robot.subsystems.Blackbox.ReefSelect; import team1403.robot.subsystems.Blackbox.State; +import team1403.robot.subsystems.LEDSubsystem.LEDConfig; import team1403.robot.subsystems.ClimberSubsystem; import team1403.robot.subsystems.CoralIntakeSubsystem; import team1403.robot.subsystems.ElevatorSubsystem; @@ -215,14 +216,15 @@ private void configureBindings() { //new Trigger(() -> true).whileTrue(m_stateMachine); RobotModeTriggers.disabled().negate() - .and(() -> Blackbox.robotState != Blackbox.State.ManualElevator).whileTrue(m_stateMachine); + .and(() -> Blackbox.robotState != Blackbox.State.ManualElevator + && Blackbox.robotState != Blackbox.State.MoveElevator).whileTrue(m_stateMachine); RobotModeTriggers.disabled().negate() .and(() -> Blackbox.robotState == Blackbox.State.ManualElevator).whileTrue( Commands.run(() -> { m_elevator.moveToSetpoint(m_elevator.getSetpoint() - - MathUtil.applyDeadband(m_operatorController.getRightY(), 0.05) * Constants.kLoopTime * 32); + MathUtil.applyDeadband(m_operatorController.getRightY(), 0.05) * Constants.kLoopTime * 50); m_wrist.moveToSetpoint(m_wrist.getSetpoint() + - MathUtil.applyDeadband(m_operatorController.getLeftY(), 0.05) * Constants.kLoopTime / 7.0); + MathUtil.applyDeadband(m_operatorController.getLeftY(), 0.05) * Constants.kLoopTime / 5.0); }, m_elevator, m_wrist)); //Logs elevator + wrist mechanism in advantage kit new CoralMechanism(m_wrist, m_elevator).ignoringDisable(true).schedule(); @@ -234,8 +236,8 @@ private void configureBindings() { () -> -m_driverController.getLeftX(), () -> -m_driverController.getLeftY(), () -> -m_driverController.getRightX(), - () -> m_driverController.getHID().getXButton(), - () -> m_driverController.getHID().getYButton(), + () -> m_driverController.getHID().getPOV() == 180, + () -> m_driverController.getHID().getPOV() == 0, () -> m_driverController.getHID().getAButton(), () -> m_driverController.getRightTriggerAxis(), () -> m_driverController.getLeftTriggerAxis())); @@ -265,8 +267,19 @@ private void configureBindings() { )); //there's really no other good buttons unfortunately - m_driverController.leftStick().whileTrue(new ClimberCommand(m_climber, Constants.Climber.upSpeed)); - m_driverController.rightStick().whileTrue(new ClimberCommand(m_climber, Constants.Climber.downSpeed)); + m_driverController.x().whileTrue( + Commands.sequence( + new InstantCommand(() -> m_climber.setServo(Constants.Climber.ratchetDisengage)), + Commands.waitSeconds(.2), + new ClimberCommand(m_climber, true) + ) + ); + m_driverController.y().whileTrue(new ClimberCommand(m_climber, false)); + // m_driverController.povDown().whileTrue(new ClimberCommand(m_climber, true)); + + // m_driverController.povUp().whileTrue(new InstantCommand(() -> m_climber.setServo(Constants.Climber.ratchetEngage))); + // // m_driverController.x().whileTrue(new InstantCommand(() -> m_climber.setServo(.5))); + // m_driverController.povDown().whileTrue(new InstantCommand(() -> m_climber.setServo(Constants.Climber.ratchetDisengage))); m_operatorController.b() .and(() -> Blackbox.robotState != State.ManualElevator) @@ -274,7 +287,7 @@ private void configureBindings() { m_operatorController.a() .and(() -> Blackbox.robotState != State.ManualElevator) .onTrue(Blackbox.reefScoreLevelCmd(Blackbox.ReefScoreLevel.L2)); - m_operatorController.x() + m_operatorController.x() .and(() -> Blackbox.robotState != State.ManualElevator) .onTrue(Blackbox.reefScoreLevelCmd(Blackbox.ReefScoreLevel.L3)); m_operatorController.y() @@ -297,36 +310,54 @@ private void configureBindings() { .and(() -> Blackbox.robotState == State.ManualElevator) .onTrue( Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L1), - new WristCommand(m_wrist, Constants.Wrist.Setpoints.L1) + new WristCommand(m_wrist, Constants.Wrist.Setpoints.L1), + Blackbox.robotStateCmd(State.ManualElevator) )); m_operatorController.a() .and(() -> Blackbox.robotState == State.ManualElevator) .onTrue( Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L2), - new WristCommand(m_wrist, Constants.Wrist.Setpoints.L2) + new WristCommand(m_wrist, Constants.Wrist.Setpoints.L2), + Blackbox.robotStateCmd(State.ManualElevator) )); m_operatorController.x() .and(() -> Blackbox.robotState == State.ManualElevator) .onTrue( Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L3), - new WristCommand(m_wrist, Constants.Wrist.Setpoints.L3) + new WristCommand(m_wrist, Constants.Wrist.Setpoints.L3), + Blackbox.robotStateCmd(State.ManualElevator) )); m_operatorController.y() .and(() -> Blackbox.robotState == State.ManualElevator) .onTrue( Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L4), - new WristCommand(m_wrist, Constants.Wrist.Setpoints.L4) + new WristCommand(m_wrist, Constants.Wrist.Setpoints.L4), + Blackbox.robotStateCmd(State.ManualElevator) )); m_operatorController.povRight() - .and(() -> Blackbox.robotState == State.ManualElevator) + //.and(() -> Blackbox.robotState == State.ManualElevator) .onTrue( Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L3Algae), new WristCommand(m_wrist, Constants.Wrist.Setpoints.Source) + //switch back to manual elevator intentionally omitted + )); + m_operatorController.povDown() + //.and(() -> Blackbox.robotState == State.ManualElevator) + .onTrue( + Commands.sequence(Blackbox.robotStateCmd(State.MoveElevator), + new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.Barge), + new WristCommand(m_wrist, Constants.Wrist.Setpoints.Barge) + //Blackbox.robotStateCmd(State.ManualElevator) )); /* m_operatorController.rightBumper() @@ -338,6 +369,7 @@ private void configureBindings() { m_coralIntake.setDefaultCommand(new DefaultIntakeCommand(m_coralIntake)); m_led.setDefaultCommand(new LightCommand(m_led)); + // m_climber.setDefaultCommand(new Command(() -> m_climber.setServo(0))); // m_algaeIntake.setDefaultCommand(new DefaultAlgaeIntakeCommand(m_algaeIntake)); // coral intake @@ -359,14 +391,18 @@ private void configureBindings() { new RepeatNTimes(Commands.sequence( new CoralIntakeSpeed(m_coralIntake, -Constants.CoralIntake.wiggle).withTimeout(0.3), new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.wiggle).withTimeout(0.4) //runs inward for longer to avoid piece falling out - ), 4))); + ), 1))); new Trigger(() -> Blackbox.robotState == State.placing && m_wrist.isAtSetpoint() && m_elevator.isAtSetpoint() && Blackbox.getCloseAlign(m_swerve.getPose()) && !Blackbox.isAligning()) - .debounce(0.1).onTrue(opVibrationCmd); + .debounce(0.1).onTrue( + Commands.parallel( + opVibrationCmd.asProxy(), + m_led.requestState(LEDConfig.Style.Strobe, LEDConfig.Color.Green).repeatedly() + .until(() -> !Blackbox.isCoralLoaded()))); NamedCommands.registerCommand("CoralScore", new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.release).withTimeout(0.5).asProxy()); @@ -377,11 +413,12 @@ private void configureBindings() { NamedCommands.registerCommand("WaitForPlace", Commands.waitUntil(() -> !Blackbox.isCoralLoaded()).withTimeout(0.1)); NamedCommands.registerCommand("WaitForCoral", + Commands.waitUntil(() -> Blackbox.isCoralLoaded())); NamedCommands.registerCommand("WaitForSetpoint", new WaitUntilDebounced(() -> m_wrist.isAtSetpoint() && m_elevator.isAtSetpoint(), 0.1).withTimeout(3)); - NamedCommands.registerCommand("ReefAlignL", getAlignCommand(Blackbox.ReefSelect.LEFT).withTimeout(2)); - NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT).withTimeout(2)); + NamedCommands.registerCommand("ReefAlignL", getAlignCommand(Blackbox.ReefSelect.LEFT).withTimeout(2.7)); + NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT).withTimeout(2.7)); NamedCommands.registerCommand("Loading", Blackbox.robotStateCmd(Blackbox.State.loading)); @@ -391,6 +428,7 @@ private void configureBindings() { (make sure robot is facing a tag to seed the position) */ m_autoChooser.addOption("THREE PIECE BACK FINAL PROCESSOR SIDE", AutoHelper.getThreePieceBackProc(m_swerve)); m_autoChooser.addOption("MOVE AUTO ANYWHERE", AutoHelper.getMoveAuto(m_swerve)); + m_autoChooser.addOption("THREE PIECE BACK FINAL NON PROCESSOR SIDE UNTESTED", AutoHelper.getThreePieceBackNonProc(m_swerve)); m_autoChooser.addOption("ONE PIECE CENTER UNTESTED", AutoHelper.getOnePCenter(m_swerve)); m_autoChooser.addOption("THREE PIECE SIDE PROCESSOR UNTESTED", AutoHelper.getThreePieceSideProc(m_swerve)); m_autoChooser.addOption("TWO PIECE PROCESSOR UNTESTED", AutoHelper.getTwoPieceProc(m_swerve)); diff --git a/src/main/java/team1403/robot/commands/AlignCommand.java b/src/main/java/team1403/robot/commands/AlignCommand.java index cf5ff660..a6dc5391 100644 --- a/src/main/java/team1403/robot/commands/AlignCommand.java +++ b/src/main/java/team1403/robot/commands/AlignCommand.java @@ -23,7 +23,7 @@ public class AlignCommand extends Command { //Variable for local copy of Pathplanner state private PathPlannerTrajectoryState m_state; //Variable for local copy of the maximum faliure treshold - private static final double kTreshM = 0.01; + private static final double kTreshM = 0.015; /** * Aligns to a certain target diff --git a/src/main/java/team1403/robot/commands/ClimberCommand.java b/src/main/java/team1403/robot/commands/ClimberCommand.java index 5e67ef7a..1d59d193 100644 --- a/src/main/java/team1403/robot/commands/ClimberCommand.java +++ b/src/main/java/team1403/robot/commands/ClimberCommand.java @@ -1,46 +1,74 @@ package team1403.robot.commands; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import team1403.robot.Constants; import team1403.robot.subsystems.ClimberSubsystem; public class ClimberCommand extends Command { //Variable for local instantce of Climber subsystem - public ClimberSubsystem m_climber; - //Variable for local copy of motor speed - public double m_speed; + private ClimberSubsystem m_climber; + //Variable for local copy of motor direction + private boolean isGoingDown; /** * Runs the climber * * @param climber the climber subsystem - * @param speed the speed at which to move the motor + * @param goingDown whether the climber is going down */ - public ClimberCommand(ClimberSubsystem climber, double speed) { + public ClimberCommand(ClimberSubsystem climber, boolean goingDown) { m_climber = climber; - m_speed = speed; - + isGoingDown = goingDown; + addRequirements(m_climber); } @Override - public void initialize() {} + public void initialize() { + if(isGoingDown){ + m_climber.setServo(Constants.Climber.ratchetDisengage); + Commands.waitSeconds(2); + } + // m_timer.reset(); + } + /* + * ratchet engaged -> the button is ubpressed (default setpoint physically), + * and the motor can only move upwards + * + * ratched disengaged -> the button is pressed in by the servo, + * motor can move in both directions + */ @Override public void execute() { - //Sets the speed of the climber motor - m_climber.setMotorSpeed(m_speed); + if(isGoingDown){ + m_climber.setServo(Constants.Climber.ratchetDisengage); + if(m_climber.getMotorPosition() > Constants.Climber.downPosition) + m_climber.setMotorSpeed(Constants.Climber.downSpeed); + } else { + m_climber.setServo(Constants.Climber.ratchetEngage); + if(m_climber.getMotorPosition() < Constants.Climber.upPosition) + m_climber.setMotorSpeed(Constants.Climber.upSpeed); + } } @Override public void end(boolean interrupted) { //Stops the climber motor when the command ends m_climber.stopMotors(); + m_climber.setServo(Constants.Climber.ratchetEngage); } @Override public boolean isFinished() { - return false; + if(isGoingDown) + return m_climber.getMotorPosition() <= Constants.Climber.downPosition; + else + return m_climber.getMotorPosition() >= Constants.Climber.upPosition; } } \ No newline at end of file diff --git a/src/main/java/team1403/robot/commands/CoralMechanism.java b/src/main/java/team1403/robot/commands/CoralMechanism.java index ef82d3f3..f712b160 100644 --- a/src/main/java/team1403/robot/commands/CoralMechanism.java +++ b/src/main/java/team1403/robot/commands/CoralMechanism.java @@ -55,7 +55,8 @@ public void execute() { //Updates the wrist angle in advantage scope by rotating the mechanism m_wristMech.setAngle(Rotation2d.fromRotations(m_wrist.getWristAngle() + 0.25)); //Logs the mechanism - Logger.recordOutput("CoralMechanism", m_mechanism); + if (Constants.DEBUG_MODE) + Logger.recordOutput("CoralMechanism", m_mechanism); } @Override diff --git a/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java b/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java index 8160710d..7de864e5 100644 --- a/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java +++ b/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java @@ -108,7 +108,7 @@ public void execute() { SmartDashboard.putBoolean("isFieldRelative", m_isFieldRelative); //if (Constants.DEBUG_MODE) SmartDashboard.putBoolean("Aimbot", m_aimbotSupplier.getAsBoolean()); - m_speedLimiter = 0.3 * (1.0 - m_snipingMode.getAsDouble() * 0.7) + (m_speedSupplier.getAsDouble() * 0.7); + m_speedLimiter = 0.3 * (1.0 - m_snipingMode.getAsDouble() * 0.7) + squareNum(m_speedSupplier.getAsDouble()) * 0.7; if (DriverStation.isAutonomousEnabled()) { m_drivetrainSubsystem.drive(new ChassisSpeeds()); diff --git a/src/main/java/team1403/robot/commands/LightCommand.java b/src/main/java/team1403/robot/commands/LightCommand.java index 89ace475..20dc0977 100644 --- a/src/main/java/team1403/robot/commands/LightCommand.java +++ b/src/main/java/team1403/robot/commands/LightCommand.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.Command; import team1403.robot.subsystems.Blackbox; import team1403.robot.subsystems.LEDSubsystem; +import team1403.robot.subsystems.LEDSubsystem.LEDConfig; public class LightCommand extends Command{ @@ -27,22 +28,23 @@ public void initialize() {} public void execute() { switch(Blackbox.robotState){ case loading: - m_LED.setLEDcolor(LEDSubsystem.Color.Yellow); + m_LED.setLEDcolor(LEDConfig.Color.Yellow); break; case driving: - m_LED.setLEDcolor(LEDSubsystem.Color.Green); + m_LED.setLEDcolor(LEDConfig.Color.Green); break; case aligning: - m_LED.setLEDcolor(LEDSubsystem.Color.Blue); + m_LED.setLEDcolor(LEDConfig.Color.Blue); break; case placing: - m_LED.setLEDcolor(LEDSubsystem.Color.Pink); + m_LED.setLEDcolor(LEDConfig.Color.Pink); break; case exiting: - m_LED.setLEDcolor(LEDSubsystem.Color.Red); + m_LED.setLEDcolor(LEDConfig.Color.Red); break; case ManualElevator: - m_LED.setLEDcolor(LEDSubsystem.Color.White); + case MoveElevator: + m_LED.setLEDcolor(LEDConfig.Color.White); break; } diff --git a/src/main/java/team1403/robot/commands/StateMachine.java b/src/main/java/team1403/robot/commands/StateMachine.java index b8f478aa..872e9fa8 100644 --- a/src/main/java/team1403/robot/commands/StateMachine.java +++ b/src/main/java/team1403/robot/commands/StateMachine.java @@ -46,8 +46,8 @@ public void execute() { if (Blackbox.isCoralLoaded()) Blackbox.robotState = State.driving; } break; - case driving: - if(Blackbox.reefLevel != Blackbox.reefLevel.drive) + case driving: + if(Blackbox.reefLevel != Blackbox.ReefScoreLevel.drive) m_wristSubsystem.moveToSetpoint(Constants.Wrist.Setpoints.Drive); if(Blackbox.isAligning() && Blackbox.reefLevel != Blackbox.ReefScoreLevel.drive) Blackbox.robotState = State.aligning; @@ -89,8 +89,14 @@ public void execute() { break; } case ManualElevator: + case MoveElevator: break; } + + /* + if(!Blackbox.isCoralLoaded()) { + Blackbox.robotState = State.loading; + } */ } @Override diff --git a/src/main/java/team1403/robot/commands/auto/AutoHelper.java b/src/main/java/team1403/robot/commands/auto/AutoHelper.java index 01f076aa..d8d65b61 100644 --- a/src/main/java/team1403/robot/commands/auto/AutoHelper.java +++ b/src/main/java/team1403/robot/commands/auto/AutoHelper.java @@ -138,6 +138,49 @@ public static Command getThreePieceBackProc(SwerveSubsystem m_swerve) { } } + public static Command getThreePieceBackNonProc(SwerveSubsystem m_swerve) { + try { + return Commands.sequence( + Commands.parallel( + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 1", m_swerve, true), + Commands.sequence( + Commands.waitSeconds(0.75), + NamedCommands.getCommand("CoralL4")) + ), + NamedCommands.getCommand("ReefAlignL"), + NamedCommands.getCommand("WaitForSetpoint"), + NamedCommands.getCommand("CoralScore"), + NamedCommands.getCommand("Loading"), + Commands.waitSeconds(0.1), + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 2", m_swerve), + NamedCommands.getCommand("WaitForCoral"), + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 3", m_swerve), + NamedCommands.getCommand("CoralL4"), + NamedCommands.getCommand("ReefAlignR"), + NamedCommands.getCommand("WaitForSetpoint"), + NamedCommands.getCommand("CoralScore"), + Commands.parallel( + NamedCommands.getCommand("Loading"), + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 4", m_swerve) + ), + NamedCommands.getCommand("WaitForCoral"), + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 5", m_swerve), + NamedCommands.getCommand("CoralL4"), + NamedCommands.getCommand("ReefAlignL"), + NamedCommands.getCommand("WaitForSetpoint"), + NamedCommands.getCommand("CoralScore"), + Commands.parallel( + NamedCommands.getCommand("Loading"), + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 6", m_swerve) + ) + + ); + } catch (Exception e) { + System.err.println("Could not load auto: " + e.getMessage()); + return Commands.none(); + } + } + public static Command testAutoAlign(SwerveSubsystem m_swerve){ try{ return Commands.sequence( diff --git a/src/main/java/team1403/robot/subsystems/Blackbox.java b/src/main/java/team1403/robot/subsystems/Blackbox.java index 8e6f03cc..3db13c2b 100644 --- a/src/main/java/team1403/robot/subsystems/Blackbox.java +++ b/src/main/java/team1403/robot/subsystems/Blackbox.java @@ -25,7 +25,8 @@ public enum State { aligning, placing, exiting, - ManualElevator + ManualElevator, + MoveElevator } public enum ReefSelect { diff --git a/src/main/java/team1403/robot/subsystems/ClimberSubsystem.java b/src/main/java/team1403/robot/subsystems/ClimberSubsystem.java index 5512e766..fbefbfc5 100644 --- a/src/main/java/team1403/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/ClimberSubsystem.java @@ -1,5 +1,6 @@ package team1403.robot.subsystems; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.util.datalog.DataLog; @@ -11,15 +12,17 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import team1403.robot.Constants; public class ClimberSubsystem extends SubsystemBase{ private SparkMax m_motor; + private Servo m_servo; public ClimberSubsystem(){ - m_motor = new SparkMax(Constants.CanBus.ClimberMotor, MotorType.kBrushless); + m_motor = new SparkMax(Constants.CanBus.ClimberMotor, MotorType.kBrushless); m_servo = new Servo(Constants.RioPorts.kServoPort); configMotors(); } @@ -39,9 +42,26 @@ public void stopMotors() { m_motor.set(0); } + public void setServo(double setpoint) { + m_servo.set(setpoint); + } + + public double getServo() { + return m_servo.get(); + } + + @AutoLogOutput + public double getMotorPosition() { + return m_motor.getEncoder().getPosition(); + } + + @AutoLogOutput + public double getMotorSpeed() { + return m_motor.get(); + } + @Override public void periodic() { - Logger.recordOutput("Climber/Right Speed", m_motor.get()); } } \ No newline at end of file diff --git a/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java b/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java index b2fb0ccc..898cbebf 100644 --- a/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java @@ -25,7 +25,7 @@ public class CoralIntakeSubsystem extends SubsystemBase { private SparkMax m_intakeMotor; private CANrange m_CANRange; private LinearFilter m_filter = LinearFilter.singlePoleIIR(Constants.kLoopTime * 10, Constants.kLoopTime); - private Debouncer m_debouncer = new Debouncer(0.4, DebounceType.kRising); + private Debouncer m_debouncer = new Debouncer(0.15, DebounceType.kRising); public CoralIntakeSubsystem() { m_intakeMotor = new SparkMax(Constants.CanBus.intakeMotorID, MotorType.kBrushless); diff --git a/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java b/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java index 61cfb2c7..6f3cc167 100644 --- a/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/ElevatorSubsystem.java @@ -109,7 +109,9 @@ public void moveToSetpoint(double setPoint) { public void safeMove(double target) { if(Blackbox.isElevatorSafe()) setpoint = target; - setpoint = Math.max(setpoint, Constants.Elevator.Setpoints.Min); + setpoint = MathUtil.clamp(setpoint, + Constants.Elevator.Setpoints.Min, + Constants.Elevator.Setpoints.Max); } public double getSetpoint() { diff --git a/src/main/java/team1403/robot/subsystems/LEDSubsystem.java b/src/main/java/team1403/robot/subsystems/LEDSubsystem.java index 6525e79c..9d6bee08 100644 --- a/src/main/java/team1403/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/LEDSubsystem.java @@ -1,112 +1,133 @@ package team1403.robot.subsystems; -import com.ctre.phoenix.led.CANdle; -import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.*; +import com.ctre.phoenix.led.CANdle.LEDStripType; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import team1403.robot.Constants; public class LEDSubsystem extends SubsystemBase { - private final CANdle m_candle; - - public enum Color { - Green, - Red, - Off, - Blue, - Grey, - Pink, - White, - Yellow, - Brown, - Orange, - Purple + private final int m_ledCount = Constants.LED.kLedCount; + private final double m_animSpeed = Constants.LED.speed; + + public enum LEDConfig { + ; //It just works don't question it + public enum Style { + Solid, + Rainbow, + Larson, + Strobe, + Upwards, + Downwards + } + + public enum Color { + Green(0, 255, 0), + Red(255, 0, 0), + Blue(0, 0, 255), + Grey(128, 128, 128), + Pink(255, 182, 193), + White(255, 255, 255), + Yellow(255, 255, 0), + Brown(139, 69, 19), + Orange(255, 165, 0), + Purple(128, 0, 128), + Off(0,0,0); + + private final int red; + private final int green; + private final int blue; + + Color(int red, int green, int blue) { + this.red = red; + this.green = green; + this.blue = blue; + } + + public int getRed() { + return red; + } + + public int getGreen() { + return green; + } + + public int getBlue() { + return blue; + } + } } public LEDSubsystem() { m_candle = new CANdle(Constants.CanBus.kCandleID); CANdleConfiguration config = new CANdleConfiguration(); + config.stripType = LEDStripType.RGB; + config.brightnessScalar = 1; m_candle.configAllSettings(config); + m_candle.setLEDs(0, 0, 0); } - public void setLEDcolor(Color color) { - switch(color) { - case Green: - m_candle.setLEDs(0, 255, 0); - break; - case Red: - m_candle.setLEDs(255, 0, 0); - break; - case Off: + public void setLEDcolor(LEDConfig.Color color) { + setLEDcolor(LEDConfig.Style.Solid, color); + } + + + public void setLEDcolor(LEDConfig.Style style, LEDConfig.Color primary) { + if (primary == null) primary = LEDConfig.Color.Off; + + int P_red = primary.getRed(); + int P_green = primary.getGreen(); + int P_blue = primary.getBlue(); + + switch(style) { + case Solid: + m_candle.setLEDs(P_red, P_green, P_blue); + break; + + case Rainbow: + m_candle.animate(new RainbowAnimation(1.0, m_animSpeed, m_ledCount)); + break; + + case Larson: + m_candle.animate(new LarsonAnimation(P_red, P_green, P_blue, 0, + m_animSpeed, + m_ledCount, + LarsonAnimation.BounceMode.Center, 7)); + break; + + case Strobe: + m_candle.animate(new StrobeAnimation(P_red, P_green, P_blue, 0, + m_animSpeed, m_ledCount)); + break; + case Upwards: + m_candle.animate(new ColorFlowAnimation(P_red, P_green, P_blue, 0, + m_animSpeed, + m_ledCount, + ColorFlowAnimation.Direction.Forward)); + break; + + case Downwards: + m_candle.animate(new ColorFlowAnimation(P_red, P_green, P_blue, 0, + m_animSpeed, + m_ledCount, + ColorFlowAnimation.Direction.Backward)); + break; + + default: m_candle.setLEDs(0, 0, 0); break; - case Blue: - m_candle.setLEDs(0, 0, 255); - break; - case Grey: - m_candle.setLEDs(128, 128, 128); - break; - case Pink: - m_candle.setLEDs(255, 182, 193); - break; - case White: - m_candle.setLEDs(255, 255, 255); - break; - case Yellow: - m_candle.setLEDs(255, 255, 0); - break; - case Brown: - m_candle.setLEDs(139, 69, 19); - break; - case Orange: - m_candle.setLEDs(255, 165, 0); - break; - case Purple: - m_candle.setLEDs(128, 0, 128); - break; } } - public void setLEDcolor(Color color, int startIndex, int count) { - switch(color) { - case Green: - m_candle.setLEDs(0, 255, 0, 0, startIndex, count); // RGB for Green - break; - case Red: - m_candle.setLEDs(255, 0, 0, 0, startIndex, count); // RGB for Red - break; - case Off: - m_candle.setLEDs(0, 0, 0, 0, startIndex, count); // RGB for Black (off) - break; - case Blue: - m_candle.setLEDs(0, 0, 255, 0, startIndex, count); // RGB for Blue - break; - case Grey: - m_candle.setLEDs(128, 128, 128, 0, startIndex, count); // RGB for Grey - break; - case Pink: - m_candle.setLEDs(255, 182, 193, 0, startIndex, count); // RGB for Pink - break; - case White: - m_candle.setLEDs(255, 255, 255, 0, startIndex, count); // RGB for White - break; - case Yellow: - m_candle.setLEDs(255, 255, 0, 0, startIndex, count); // RGB for Yellow - break; - case Brown: - m_candle.setLEDs(139, 69, 19, 0, startIndex, count); // RGB for Brown - break; - case Orange: - m_candle.setLEDs(255, 165, 0, 0, startIndex, count); // RGB for Orange - break; - case Purple: - m_candle.setLEDs(128, 0, 128, 0, startIndex, count); // RGB for Purple - break; - } + public void setBrightness(double brightness) { + m_candle.configBrightnessScalar(brightness); + } + + public Command requestState(LEDConfig.Style style, LEDConfig.Color color) { + return runOnce(() -> setLEDcolor(style, color)); } - - @Override public void periodic() {} diff --git a/src/main/java/team1403/robot/subsystems/WristSubsystem.java b/src/main/java/team1403/robot/subsystems/WristSubsystem.java index 4d51a0b4..6ee186ec 100644 --- a/src/main/java/team1403/robot/subsystems/WristSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/WristSubsystem.java @@ -112,6 +112,11 @@ public boolean isElevatorSafe() { return getWristAngleDeg() < 0; } + @AutoLogOutput + public double wristInaccuracy() { + return getWristAngleDeg() - getWristSetpointDeg(); + } + public void stop() { m_wristMotor.set(0); } diff --git a/src/main/java/team1403/robot/swerve/TunerConstants.java b/src/main/java/team1403/robot/swerve/TunerConstants.java index bd7a12e7..7dfe5883 100644 --- a/src/main/java/team1403/robot/swerve/TunerConstants.java +++ b/src/main/java/team1403/robot/swerve/TunerConstants.java @@ -88,7 +88,7 @@ public class TunerConstants { public static final double kDriveGearRatio = 6.746031746031747; private static final double kSteerGearRatio = 21.428571428571427; - public static final Distance kWheelRadius = Inches.of(1.916674214478960775); + public static final Distance kWheelRadius = Inches.of(1.925); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 03df051a..bef4a151 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.1.0", + "version": "4.1.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.1.0" + "version": "4.1.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.1.0", + "version": "4.1.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6-replay-25.3.1.json b/vendordeps/Phoenix6-replay-25.3.2.json similarity index 92% rename from vendordeps/Phoenix6-replay-25.3.1.json rename to vendordeps/Phoenix6-replay-25.3.2.json index 45516a40..37d21edf 100644 --- a/vendordeps/Phoenix6-replay-25.3.1.json +++ b/vendordeps/Phoenix6-replay-25.3.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-replay-25.3.1.json", + "fileName": "Phoenix6-replay-25.3.2.json", "name": "CTRE-Phoenix (v6) Replay", - "version": "25.3.1", + "version": "25.3.2", "frcYear": "2025", "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.1" + "version": "25.3.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,7 +48,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "api-cpp-replay", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -62,7 +62,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +76,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -118,7 +118,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -132,7 +132,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -146,7 +146,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -160,7 +160,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -174,7 +174,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -202,7 +202,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -216,7 +216,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -230,7 +230,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -246,7 +246,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -259,7 +259,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "wpiapi-cpp-replay", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_Phoenix6_WPIReplay", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_PhoenixTools_Replay", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -448,7 +448,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -464,7 +464,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -480,7 +480,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.3.2", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true,