From aab6de95ff62905268b247f5917153cdfbdf0b6b Mon Sep 17 00:00:00 2001 From: Ryan Elbert Date: Fri, 7 Mar 2025 14:26:57 -0800 Subject: [PATCH 1/6] added a few basic autos because there weren't any --- .../pathplanner/autos/Center Start.auto | 38 +++++++++++++ .../autos/Drive Station 1-Wall Leave.auto | 19 +++++++ .../autos/Drive Station 3-Wall Leave.auto | 19 +++++++ .../paths/Drive Station 1-Wall Leave.path | 54 +++++++++++++++++++ .../paths/Drive Station 3-Wall Leave.path | 54 +++++++++++++++++++ .../org/ironriders/core/RobotCommands.java | 23 ++++++++ ...rLib.json => PathplannerLib-2025.2.5.json} | 8 +-- 7 files changed, 211 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Center Start.auto create mode 100644 src/main/deploy/pathplanner/autos/Drive Station 1-Wall Leave.auto create mode 100644 src/main/deploy/pathplanner/autos/Drive Station 3-Wall Leave.auto create mode 100644 src/main/deploy/pathplanner/paths/Drive Station 1-Wall Leave.path create mode 100644 src/main/deploy/pathplanner/paths/Drive Station 3-Wall Leave.path rename vendordeps/{PathplannerLib.json => PathplannerLib-2025.2.5.json} (87%) diff --git a/src/main/deploy/pathplanner/autos/Center Start.auto b/src/main/deploy/pathplanner/autos/Center Start.auto new file mode 100644 index 0000000..dbe8b4f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Start.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Start - Blue Front Reef" + } + }, + { + "type": "named", + "data": { + "name": "Prepare to Score Coral L4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score Coral L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Drive Station 1-Wall Leave.auto b/src/main/deploy/pathplanner/autos/Drive Station 1-Wall Leave.auto new file mode 100644 index 0000000..98b3f35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Drive Station 1-Wall Leave.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Drive Station 1-Wall Leave" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Drive Station 3-Wall Leave.auto b/src/main/deploy/pathplanner/autos/Drive Station 3-Wall Leave.auto new file mode 100644 index 0000000..7e89621 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Drive Station 3-Wall Leave.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Drive Station 3-Wall Leave" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Drive Station 1-Wall Leave.path b/src/main/deploy/pathplanner/paths/Drive Station 1-Wall Leave.path new file mode 100644 index 0000000..7975880 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Drive Station 1-Wall Leave.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.369005681818181, + "y": 7.4103551136363635 + }, + "prevControl": null, + "nextControl": { + "x": 8.05682504398939, + "y": 7.411896466486474 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.547045454545454, + "y": 7.4103551136363635 + }, + "prevControl": { + "x": 4.286535325255353, + "y": 7.401947491517235 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Drive Station 3-Wall Leave.path b/src/main/deploy/pathplanner/paths/Drive Station 3-Wall Leave.path new file mode 100644 index 0000000..784ef00 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Drive Station 3-Wall Leave.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.428835227272726, + "y": 0.7493323863636363 + }, + "prevControl": null, + "nextControl": { + "x": 6.440250246110068, + "y": 0.7687047006621895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.557017045454546, + "y": 0.7493323863636363 + }, + "prevControl": { + "x": 5.172929444322903, + "y": 0.7667118730651692 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/org/ironriders/core/RobotCommands.java b/src/main/java/org/ironriders/core/RobotCommands.java index 0879977..70bf5b5 100644 --- a/src/main/java/org/ironriders/core/RobotCommands.java +++ b/src/main/java/org/ironriders/core/RobotCommands.java @@ -74,6 +74,16 @@ public RobotCommands( NamedCommands.registerCommand("Score Coral L4", this.scoreCoral(ElevatorConstants.Level.L4)); + + NamedCommands.registerCommand("Prepare to Score Coral L1", + this.prepareToScoreCoral(ElevatorConstants.Level.L1)); + NamedCommands.registerCommand("Prepare to Score Coral L2", + this.prepareToScoreCoral(ElevatorConstants.Level.L2)); + NamedCommands.registerCommand("Prepare to Score Coral L3", + this.prepareToScoreCoral(ElevatorConstants.Level.L3)); + NamedCommands.registerCommand("Prepare to Score Coral L4", + this.prepareToScoreCoral(ElevatorConstants.Level.L4)); + NamedCommands.registerCommand("Climber Down", climbCommands.set(ClimbConstants.State.DOWN)); NamedCommands.registerCommand("Climber Up", climbCommands.set(ClimbConstants.State.UP)); @@ -136,6 +146,19 @@ public Command toggleClimber() { // TODO } + public Command prepareToScoreCoral(ElevatorConstants.Level level) { + return Commands.sequence( + elevatorCommands.set(level), + coralWristCommands.set(switch (level) { + case L1, L2, L3 -> CoralWristConstants.State.L1toL3; + case L4 -> CoralWristConstants.State.L4; + default -> { + throw new IllegalArgumentException( + "Cannot score coral to level: " + level); + } + })); + } + public Command scoreCoral(ElevatorConstants.Level level) { return Commands.sequence( elevatorCommands.set(level), diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.2.5.json similarity index 87% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.2.5.json index bfc052b..b2a7265 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.2.5.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.2.5.json", "name": "PathplannerLib", - "version": "2025.2.4", + "version": "2025.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.4" + "version": "2025.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.4", + "version": "2025.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From e383542b3afa3c82844cb40633910f2927a1a20f Mon Sep 17 00:00:00 2001 From: saturncorgi Date: Fri, 7 Mar 2025 15:55:43 -0800 Subject: [PATCH 2/6] ljlsdf --- ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat | Bin 2048 -> 2048 bytes simgui-window.json | 42 +++++++------- simgui.json | 1 + .../pathplanner/autos/leave start blue 1.auto | 19 ++++++ .../pathplanner/autos/leave start blue 2.auto | 19 ++++++ .../pathplanner/autos/leave start blue 3.auto | 19 ++++++ .../pathplanner/autos/leave start red 1.auto | 19 ++++++ .../pathplanner/autos/leave start red 2.auto | 19 ++++++ .../pathplanner/autos/leave start red 3.auto | 19 ++++++ .../paths/ leave start blue 2.path | 54 ++++++++++++++++++ .../Blue Back Left Reef - Blue Left Dock.path | 2 +- .../Blue Back Reef - Blue Left Human.path | 2 +- .../Blue Back Reef - Blue Right Human.path | 2 +- ...ue Back Right Reef - Blue Right Human.path | 2 +- ...lue Front Left Reef - Blue Left Human.path | 2 +- ...e Front Right Reef - Blue Right Human.path | 2 +- ...Blue Left Human - Blue Back Left Reef.path | 2 +- .../Blue Left Human - Blue Back Reef.path | 2 +- ...lue Left Start - Blue Front Left Reef.path | 2 +- .../Blue Right Human - Blue Back Reef.path | 2 +- ...ue Right Human - Blue Back Right Reef.path | 2 +- ...e Right Start - Blue Front Right Reef.path | 2 +- .../Blue Start - Blue Front Left Reef.path | 2 +- .../paths/Blue Start - Blue Front Reef.path | 2 +- .../Blue Start - Blue Front Right Reef.path | 2 +- .../pathplanner/paths/leave start blue 1.path | 54 ++++++++++++++++++ .../pathplanner/paths/leave start blue 3.path | 54 ++++++++++++++++++ .../pathplanner/paths/leave start red 1.path | 54 ++++++++++++++++++ .../pathplanner/paths/leave start red 2.path | 54 ++++++++++++++++++ .../pathplanner/paths/leave start red 3.path | 54 ++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 11 +++- vendordeps/PathplannerLib.json | 6 +- 35 files changed, 487 insertions(+), 41 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/leave start blue 1.auto create mode 100644 src/main/deploy/pathplanner/autos/leave start blue 2.auto create mode 100644 src/main/deploy/pathplanner/autos/leave start blue 3.auto create mode 100644 src/main/deploy/pathplanner/autos/leave start red 1.auto create mode 100644 src/main/deploy/pathplanner/autos/leave start red 2.auto create mode 100644 src/main/deploy/pathplanner/autos/leave start red 3.auto create mode 100644 src/main/deploy/pathplanner/paths/ leave start blue 2.path create mode 100644 src/main/deploy/pathplanner/paths/leave start blue 1.path create mode 100644 src/main/deploy/pathplanner/paths/leave start blue 3.path create mode 100644 src/main/deploy/pathplanner/paths/leave start red 1.path create mode 100644 src/main/deploy/pathplanner/paths/leave start red 2.path create mode 100644 src/main/deploy/pathplanner/paths/leave start red 3.path diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat index c0c75516fb4af6730a827bec7d43cc7b8c37d714..ce9fb31abae0b869778ecd0ac9200b787af8a827 100644 GIT binary patch delta 76 zcmZn=Xb|9d%&_m(*)wNsCtqYtpQyk(vBZbt5yJswp~)tUK3u06w*9JSVqjoknkZ<- Tc8cM^pE4$h7Vga!Oea_Y-%K7G delta 76 zcmZn=Xb|9d&ams%*)wNsCtqYtpQyk(vBZbt8N(iAp~)tUK5T~>p8knonkZ<+b%f!@ TuM{Q*1_maG7WT~+Oea_Y??N6M diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat index f9b352eb646bfaff02f85fdbe4725f880bf0a6db..4ba7a1445c099d801ad5e03d48cc7e39691b0708 100644 GIT binary patch delta 65 zcmZn=Xb|9d&anH{*)wNsCtqYtpQyk(vBZb*#YD#D$u5jGT&Eef|EgnRU|?XHC}_rZ Sis9g&QYMHx?#&iVCs+Yl1{h)h delta 65 zcmZn=Xb@n0Iq_ig#1`yq;L_sUABMdix TB{MNFFfc)+**9A-onQq3DM1@s diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat index 1000d8f188aa7c58dea9e465d97651a98c130ceb..99700fb15505ee639c5df1d87fc36172b49006f6 100644 GIT binary patch delta 71 zcmZn=Xb@m|$q@8)@HBt_RJYuh!87Ki0v@Lvp?ZX69uifjxgN( TmCVGzz`z8NX5VbVbb=KC6rUSg diff --git a/simgui-window.json b/simgui-window.json index eaa0b4f..c32dc16 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -6,49 +6,49 @@ "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "583", - "maximized": "0", + "height": "450", + "maximized": "1", "style": "0", "userScale": "2", - "width": "944", - "xpos": "974", - "ypos": "495" + "width": "959", + "xpos": "0", + "ypos": "575" } }, "Window": { "###/FMSInfo": { "Collapsed": "0", - "Pos": "89,544", - "Size": "181,167" + "Pos": "3,360", + "Size": "217,194" }, "###Analog Inputs": { "Collapsed": "0", "Pos": "602,32", - "Size": "260,164" + "Size": "315,182" }, "###FMS": { "Collapsed": "0", - "Pos": "63,368", - "Size": "169,184" + "Pos": "2,431", + "Size": "202,214" }, "###Joysticks": { "Collapsed": "0", - "Pos": "250,148", - "Size": "796,222" + "Pos": "-1,431", + "Size": "976,179" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "925,564", + "Pos": "940,417", "Size": "750,401" }, "###NetworkTables Info": { "Collapsed": "0", - "Pos": "925,357", + "Pos": "940,2", "Size": "750,416" }, "###Other Devices": { - "Collapsed": "0", - "Pos": "925,20", + "Collapsed": "1", + "Pos": "914,23", "Size": "250,695" }, "###Plot <0>": { @@ -58,13 +58,13 @@ }, "###System Joysticks": { "Collapsed": "0", - "Pos": "409,560", - "Size": "204,218" + "Pos": "409,431", + "Size": "232,254" }, "###Timing": { "Collapsed": "0", - "Pos": "5,150", - "Size": "135,173" + "Pos": "3,164", + "Size": "162,194" }, "Debug##Default": { "Collapsed": "0", @@ -74,7 +74,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "99,116" + "Size": "118,134" } } } diff --git a/simgui.json b/simgui.json index f03f5fa..6d1703f 100644 --- a/simgui.json +++ b/simgui.json @@ -16,6 +16,7 @@ "/SmartDashboard/IMU": "Alerts", "/SmartDashboard/JSON": "Alerts", "/SmartDashboard/Motors": "Alerts", + "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/Pigeon 2 (v6) [9]": "Gyro", "/SmartDashboard/Subsystems/AlgaeIntake/Algae Intake Eject": "Command", "/SmartDashboard/Subsystems/AlgaeIntake/Algae Intake Grab": "Command", diff --git a/src/main/deploy/pathplanner/autos/leave start blue 1.auto b/src/main/deploy/pathplanner/autos/leave start blue 1.auto new file mode 100644 index 0000000..5f56bc1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/leave start blue 1.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "leave start blue 1" + } + } + ] + } + }, + "resetOdom": true, + "folder": "leave start blue", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/leave start blue 2.auto b/src/main/deploy/pathplanner/autos/leave start blue 2.auto new file mode 100644 index 0000000..5b962f3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/leave start blue 2.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": " leave start blue 2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "leave start blue", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/leave start blue 3.auto b/src/main/deploy/pathplanner/autos/leave start blue 3.auto new file mode 100644 index 0000000..b0acdfb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/leave start blue 3.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "leave start blue 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "leave start blue", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/leave start red 1.auto b/src/main/deploy/pathplanner/autos/leave start red 1.auto new file mode 100644 index 0000000..30c9b92 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/leave start red 1.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "leave start red 1" + } + } + ] + } + }, + "resetOdom": true, + "folder": "leave start red", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/leave start red 2.auto b/src/main/deploy/pathplanner/autos/leave start red 2.auto new file mode 100644 index 0000000..3f5fb3a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/leave start red 2.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "leave start red 2" + } + } + ] + } + }, + "resetOdom": true, + "folder": "leave start red", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/leave start red 3.auto b/src/main/deploy/pathplanner/autos/leave start red 3.auto new file mode 100644 index 0000000..9ffae24 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/leave start red 3.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "leave start red 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "leave start red", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ leave start blue 2.path b/src/main/deploy/pathplanner/paths/ leave start blue 2.path new file mode 100644 index 0000000..3763840 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ leave start blue 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.045797119140625, + "y": 6.154051513671875 + }, + "prevControl": null, + "nextControl": { + "x": 9.045797119140627, + "y": 6.154051513671875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.225384521484375, + "y": 6.154051513671875 + }, + "prevControl": { + "x": 5.225384521484375, + "y": 6.154051513671875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "leave start blue", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Back Left Reef - Blue Left Dock.path b/src/main/deploy/pathplanner/paths/Blue Back Left Reef - Blue Left Dock.path index b5b3ee2..e549193 100644 --- a/src/main/deploy/pathplanner/paths/Blue Back Left Reef - Blue Left Dock.path +++ b/src/main/deploy/pathplanner/paths/Blue Back Left Reef - Blue Left Dock.path @@ -59,7 +59,7 @@ "rotation": -125.00000000000001 }, "reversed": false, - "folder": "Blue Left Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 59.99999999999999 diff --git a/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Left Human.path b/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Left Human.path index f767eec..0fe6219 100644 --- a/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Left Human.path +++ b/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Left Human.path @@ -59,7 +59,7 @@ "rotation": -125.00000000000001 }, "reversed": false, - "folder": "Blue Left Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Right Human.path b/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Right Human.path index 7e7f3a1..72f2d68 100644 --- a/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Right Human.path +++ b/src/main/deploy/pathplanner/paths/Blue Back Reef - Blue Right Human.path @@ -66,7 +66,7 @@ "rotation": 125.00000000000001 }, "reversed": false, - "folder": "Blue Right Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Back Right Reef - Blue Right Human.path b/src/main/deploy/pathplanner/paths/Blue Back Right Reef - Blue Right Human.path index 9ef5fc4..c7d17de 100644 --- a/src/main/deploy/pathplanner/paths/Blue Back Right Reef - Blue Right Human.path +++ b/src/main/deploy/pathplanner/paths/Blue Back Right Reef - Blue Right Human.path @@ -66,7 +66,7 @@ "rotation": 125.00000000000001 }, "reversed": false, - "folder": "Blue Right Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": -59.99999999999999 diff --git a/src/main/deploy/pathplanner/paths/Blue Front Left Reef - Blue Left Human.path b/src/main/deploy/pathplanner/paths/Blue Front Left Reef - Blue Left Human.path index 7c25e46..abb9fb7 100644 --- a/src/main/deploy/pathplanner/paths/Blue Front Left Reef - Blue Left Human.path +++ b/src/main/deploy/pathplanner/paths/Blue Front Left Reef - Blue Left Human.path @@ -59,7 +59,7 @@ "rotation": -125.00000000000001 }, "reversed": false, - "folder": "Blue Left Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 119.99999999999999 diff --git a/src/main/deploy/pathplanner/paths/Blue Front Right Reef - Blue Right Human.path b/src/main/deploy/pathplanner/paths/Blue Front Right Reef - Blue Right Human.path index 7487540..b62546c 100644 --- a/src/main/deploy/pathplanner/paths/Blue Front Right Reef - Blue Right Human.path +++ b/src/main/deploy/pathplanner/paths/Blue Front Right Reef - Blue Right Human.path @@ -59,7 +59,7 @@ "rotation": 125.00000000000001 }, "reversed": false, - "folder": "Blue Right Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": -119.99999999999999 diff --git a/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Left Reef.path b/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Left Reef.path index 3758412..8ebd5f1 100644 --- a/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Left Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Left Reef.path @@ -59,7 +59,7 @@ "rotation": 59.99999999999999 }, "reversed": false, - "folder": "Blue Left Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": -125.00000000000001 diff --git a/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Reef.path b/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Reef.path index eb4ac60..ac28168 100644 --- a/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Left Human - Blue Back Reef.path @@ -59,7 +59,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Blue Left Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": -125.00000000000001 diff --git a/src/main/deploy/pathplanner/paths/Blue Left Start - Blue Front Left Reef.path b/src/main/deploy/pathplanner/paths/Blue Left Start - Blue Front Left Reef.path index 1cfafd1..a5e3e41 100644 --- a/src/main/deploy/pathplanner/paths/Blue Left Start - Blue Front Left Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Left Start - Blue Front Left Reef.path @@ -59,7 +59,7 @@ "rotation": 119.99999999999999 }, "reversed": false, - "folder": "Blue Start", + "folder": "other", "idealStartingState": { "velocity": 0.0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Reef.path b/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Reef.path index cdd0a3b..5483430 100644 --- a/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Reef.path @@ -66,7 +66,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Blue Right Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 125.00000000000001 diff --git a/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Right Reef.path b/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Right Reef.path index 5a51667..fc7f2a0 100644 --- a/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Right Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Right Human - Blue Back Right Reef.path @@ -66,7 +66,7 @@ "rotation": -59.99999999999999 }, "reversed": false, - "folder": "Blue Right Human", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 125.00000000000001 diff --git a/src/main/deploy/pathplanner/paths/Blue Right Start - Blue Front Right Reef.path b/src/main/deploy/pathplanner/paths/Blue Right Start - Blue Front Right Reef.path index 8231a7b..ac0ec7f 100644 --- a/src/main/deploy/pathplanner/paths/Blue Right Start - Blue Front Right Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Right Start - Blue Front Right Reef.path @@ -68,7 +68,7 @@ "rotation": -119.99999999999999 }, "reversed": false, - "folder": "Blue Start", + "folder": "other", "idealStartingState": { "velocity": 0.0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Left Reef.path b/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Left Reef.path index 0021427..bda4612 100644 --- a/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Left Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Left Reef.path @@ -59,7 +59,7 @@ "rotation": 119.99999999999999 }, "reversed": false, - "folder": "Blue Start", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Reef.path b/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Reef.path index d37ddb7..4b9256f 100644 --- a/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Reef.path @@ -59,7 +59,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Blue Start", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Right Reef.path b/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Right Reef.path index 8dcf8d3..f28f1a1 100644 --- a/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Right Reef.path +++ b/src/main/deploy/pathplanner/paths/Blue Start - Blue Front Right Reef.path @@ -59,7 +59,7 @@ "rotation": -119.99999999999999 }, "reversed": false, - "folder": "Blue Start", + "folder": "other", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/leave start blue 1.path b/src/main/deploy/pathplanner/paths/leave start blue 1.path new file mode 100644 index 0000000..974eff1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/leave start blue 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.10611572265625, + "y": 7.23231201171875 + }, + "prevControl": null, + "nextControl": { + "x": 9.106115722656252, + "y": 7.23231201171875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.254329833984375, + "y": 7.23231201171875 + }, + "prevControl": { + "x": 5.254329833984375, + "y": 7.23231201171875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "leave start blue", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/leave start blue 3.path b/src/main/deploy/pathplanner/paths/leave start blue 3.path new file mode 100644 index 0000000..0a28517 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/leave start blue 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.10535400390625, + "y": 5.031754150390625 + }, + "prevControl": null, + "nextControl": { + "x": 9.105354003906251, + "y": 5.031754150390625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.495509033203125, + "y": 5.031754150390625 + }, + "prevControl": { + "x": 5.495509033203125, + "y": 5.031754150390625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "leave start blue", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/leave start red 1.path b/src/main/deploy/pathplanner/paths/leave start red 1.path new file mode 100644 index 0000000..9998b8d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/leave start red 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.4584521484375, + "y": 3.0092480468749994 + }, + "prevControl": null, + "nextControl": { + "x": 10.458452148437502, + "y": 3.0092480468749994 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.337088623046874, + "y": 3.0092480468749994 + }, + "prevControl": { + "x": 10.337088623046874, + "y": 3.0092480468749994 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "leave start red", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/leave start red 2.path b/src/main/deploy/pathplanner/paths/leave start red 2.path new file mode 100644 index 0000000..e19d1ea --- /dev/null +++ b/src/main/deploy/pathplanner/paths/leave start red 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.45321533203125, + "y": 1.9018994140625 + }, + "prevControl": null, + "nextControl": { + "x": 10.453215332031252, + "y": 1.9018994140625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.2469677734375, + "y": 1.9018994140625 + }, + "prevControl": { + "x": 10.2469677734375, + "y": 1.9018994140625003 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "leave start red", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/leave start red 3.path b/src/main/deploy/pathplanner/paths/leave start red 3.path new file mode 100644 index 0000000..34b1638 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/leave start red 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.440885009765625, + "y": 0.784505615234375 + }, + "prevControl": null, + "nextControl": { + "x": 10.440885009765626, + "y": 0.784505615234375 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.223021240234376, + "y": 0.784505615234375 + }, + "prevControl": { + "x": 10.223021240234376, + "y": 0.7845056152343751 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "leave start red", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index ed93838..0b79c41 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,8 +2,15 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "other", + "leave start blue", + "leave start red" + ], + "autoFolders": [ + "leave start blue", + "leave start red" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index bfc052b..544d031 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.2.4", + "version": "2025.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.4" + "version": "2025.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.4", + "version": "2025.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 534b1fd1afebfe4dcafb0fdd8af3a54c25ba709d Mon Sep 17 00:00:00 2001 From: saturncorgi Date: Fri, 7 Mar 2025 16:08:56 -0800 Subject: [PATCH 3/6] trash far away estimates --- src/main/java/org/ironriders/vision/Vision.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/ironriders/vision/Vision.java b/src/main/java/org/ironriders/vision/Vision.java index 7f68392..012b797 100644 --- a/src/main/java/org/ironriders/vision/Vision.java +++ b/src/main/java/org/ironriders/vision/Vision.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; @@ -209,10 +210,10 @@ private Optional refineMacrodata(EstimatedRobotPose pose) { return Optional.empty(); // trash if estimate is too far from the believed current pose - // Transform2d differenceTransform = pose.estimatedPose.toPose2d().minus(swerveDrive.getPose()); - // if (Math.abs(differenceTransform.getX()) > 2 || Math.abs(differenceTransform.getY()) > 2) { - // return Optional.empty(); - // } + Transform2d differenceTransform = pose.estimatedPose.toPose2d().minus(swerveDrive.getPose()); + if (Math.abs(differenceTransform.getX()) > 1.5 || Math.abs(differenceTransform.getY()) > 1.5) { + return Optional.empty(); + } // return actual estimate if it gets through all that ^^^ hasPose=true; From c940ecfe029801f64f05bf0c5331101dc7469ade Mon Sep 17 00:00:00 2001 From: isabelle Date: Fri, 7 Mar 2025 16:15:01 -0800 Subject: [PATCH 4/6] small --- src/main/java/org/ironriders/core/RobotContainer.java | 1 - .../java/org/ironriders/wrist/algae/AlgaeWristConstants.java | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/ironriders/core/RobotContainer.java b/src/main/java/org/ironriders/core/RobotContainer.java index a996d0f..7415152 100644 --- a/src/main/java/org/ironriders/core/RobotContainer.java +++ b/src/main/java/org/ironriders/core/RobotContainer.java @@ -195,7 +195,6 @@ private void configureBindings() { secondaryController.button(20).onTrue(algaeIntakeCommands.set(State.EJECT)); // PRIMARY CONTROLS - primaryController.rightBumper().onFalse(robotCommands.scoreAlgae()); primaryController.leftBumper().onTrue(Commands.runOnce(() -> { diff --git a/src/main/java/org/ironriders/wrist/algae/AlgaeWristConstants.java b/src/main/java/org/ironriders/wrist/algae/AlgaeWristConstants.java index 1fd31e3..e9d0f1e 100644 --- a/src/main/java/org/ironriders/wrist/algae/AlgaeWristConstants.java +++ b/src/main/java/org/ironriders/wrist/algae/AlgaeWristConstants.java @@ -14,7 +14,7 @@ public class AlgaeWristConstants { // Need to tune public static final double P = 0.01; - public static final double I = 0.00; + public static final double I = 0.01; public static final double D = 0.0; public static final double t = 0.02; @@ -37,8 +37,7 @@ public class AlgaeWristConstants { public enum State { // max seems to be 40, min ~0 STARTING(-1), // Starting position is beyond lower limit - STOWED(HOME_ANGLE.in(Units.Degrees) - + 5), // Computed home position (will hit limit) + STOWED(HOME_ANGLE.in(Units.Degrees) + 5), // Computed home position (will hit limit) EXTENDED(0); final Angle angle; From f9f106d2e3a94ae01da076ed3e259fc89d8d17f2 Mon Sep 17 00:00:00 2001 From: Ryan Elbert Date: Fri, 7 Mar 2025 16:15:10 -0800 Subject: [PATCH 5/6] added new robot comman for autos --- src/main/java/org/ironriders/core/RobotCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/ironriders/core/RobotCommands.java b/src/main/java/org/ironriders/core/RobotCommands.java index 70bf5b5..274a637 100644 --- a/src/main/java/org/ironriders/core/RobotCommands.java +++ b/src/main/java/org/ironriders/core/RobotCommands.java @@ -75,13 +75,13 @@ public RobotCommands( this.scoreCoral(ElevatorConstants.Level.L4)); - NamedCommands.registerCommand("Prepare to Score Coral L1", + NamedCommands.registerCommand("Prepare to Score Coral L1", this.prepareToScoreCoral(ElevatorConstants.Level.L1)); NamedCommands.registerCommand("Prepare to Score Coral L2", this.prepareToScoreCoral(ElevatorConstants.Level.L2)); NamedCommands.registerCommand("Prepare to Score Coral L3", this.prepareToScoreCoral(ElevatorConstants.Level.L3)); - NamedCommands.registerCommand("Prepare to Score Coral L4", + NamedCommands.registerCommand("Prepare to Score Coral L4", this.prepareToScoreCoral(ElevatorConstants.Level.L4)); NamedCommands.registerCommand("Climber Down", climbCommands.set(ClimbConstants.State.DOWN)); From 41861b30f0cbf767291b282a10f03d10a904d29c Mon Sep 17 00:00:00 2001 From: saturncorgi Date: Fri, 7 Mar 2025 16:18:28 -0800 Subject: [PATCH 6/6] remove flase spam --- src/main/java/org/ironriders/wrist/WristSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/ironriders/wrist/WristSubsystem.java b/src/main/java/org/ironriders/wrist/WristSubsystem.java index 5dfe832..67db2b7 100644 --- a/src/main/java/org/ironriders/wrist/WristSubsystem.java +++ b/src/main/java/org/ironriders/wrist/WristSubsystem.java @@ -132,7 +132,6 @@ private TrapezoidProfile.State createSetpoint(Angle angle, double velocity) { } public boolean atPosition() { - System.out.println(Math.abs(getCurrentAngle().in(Units.Degrees) - goalSetpoint.position) < 0.5); return Math.abs(getCurrentAngle().in(Units.Degrees) - goalSetpoint.position) < 0.5; }