From 1693edfc677b333c18c7e207f7f8b677511b3379 Mon Sep 17 00:00:00 2001 From: James Ding Date: Tue, 9 Apr 2024 10:15:47 -0700 Subject: [PATCH] Changes after CABE --- .pathplanner/settings.json | 2 +- .../pathplanner/autos/New New Auto.auto | 18 +++++ .../deploy/pathplanner/autos/Park Amp.auto | 57 ++++++++++++++++ .../deploy/pathplanner/autos/Park Source.auto | 57 ++++++++++++++++ .../deploy/pathplanner/autos/Park Stage.auto | 57 ++++++++++++++++ .../pathplanner/autos/Troll from Amp.auto | 25 +++++++ ...Park Right.auto => Troll from Source.auto} | 12 +++- .../paths/#1 Note from Amp Side.path | 2 +- .../paths/#1 Note from Speaker.path | 2 +- .../deploy/pathplanner/paths/#2 Note.path | 2 +- .../deploy/pathplanner/paths/#3 Note.path | 2 +- .../deploy/pathplanner/paths/#4 Note.path | 2 +- .../deploy/pathplanner/paths/#5 Note.path | 2 +- .../deploy/pathplanner/paths/#7 Note.path | 2 +- .../deploy/pathplanner/paths/1m Test.path | 2 +- .../deploy/pathplanner/paths/2m Test.path | 2 +- .../deploy/pathplanner/paths/Amp troll.path | 68 +++++++++++++++++++ .../deploy/pathplanner/paths/Circle Test.path | 2 +- .../paths/Long Shot Speaker Amp Side.path | 2 +- .../paths/{Park Right.path => Park Amp.path} | 18 ++--- .../deploy/pathplanner/paths/Park Source.path | 52 ++++++++++++++ .../deploy/pathplanner/paths/Park Stage.path | 52 ++++++++++++++ .../deploy/pathplanner/paths/Playground.path | 10 +-- .../pathplanner/paths/Source Direct Left.path | 2 +- .../paths/Source Direct Middle.path | 2 +- .../paths/Source Direct Right.path | 2 +- .../pathplanner/paths/Source troll.path | 68 +++++++++++++++++++ src/main/java/org/rambots/Robot.kt | 4 +- src/main/java/org/rambots/RobotContainer.kt | 15 ++-- .../actions/ExtendClimberCommand.kt | 18 +++++ .../actions/ExtendClimberCommandGroup.kt | 3 +- .../actions/IntakeCommandGroup.kt | 5 +- .../actions/IntakeHomeCommandGroup.kt | 2 +- .../actions/RetractClimberCommand.kt | 3 +- .../primitive/ReverseIntakeCommand.kt | 2 + .../primitive/WristPositionCommand.kt | 3 + .../org/rambots/config/ElevatorConstants.kt | 4 +- .../org/rambots/config/ShooterConstants.kt | 2 +- .../subsystems/drive/DriveConstants.java | 21 +++++- .../rambots/subsystems/drive/GyroIO_ADIS.java | 15 ++-- .../subsystems/lighting/LightingSubsystem.kt | 14 ++++ .../superstructure/ShooterSubsystem.kt | 18 +++++ .../superstructure/WristSubsystem.kt | 15 +++- .../subsystems/vision/AprilTagVision.java | 8 +-- .../vision/AprilTagVisionIOLimelight.java | 10 ++- .../org/rambots/util/LimelightHelpers.java | 52 +++++++++++++- 46 files changed, 671 insertions(+), 67 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New New Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Park Amp.auto create mode 100644 src/main/deploy/pathplanner/autos/Park Source.auto create mode 100644 src/main/deploy/pathplanner/autos/Park Stage.auto create mode 100644 src/main/deploy/pathplanner/autos/Troll from Amp.auto rename src/main/deploy/pathplanner/autos/{Park Right.auto => Troll from Source.auto} (58%) create mode 100644 src/main/deploy/pathplanner/paths/Amp troll.path rename src/main/deploy/pathplanner/paths/{Park Right.path => Park Amp.path} (73%) create mode 100644 src/main/deploy/pathplanner/paths/Park Source.path create mode 100644 src/main/deploy/pathplanner/paths/Park Stage.path create mode 100644 src/main/deploy/pathplanner/paths/Source troll.path create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommand.kt diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index a003c26..04a133a 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -11,7 +11,7 @@ "Troll" ], "defaultMaxVel": 3.0, - "defaultMaxAccel": 1.0, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 360.0, "defaultMaxAngAccel": 360.0, "maxModuleSpeed": 4.331208 diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto new file mode 100644 index 0000000..07d4e33 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New New Auto.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Park Amp.auto b/src/main/deploy/pathplanner/autos/Park Amp.auto new file mode 100644 index 0000000..47d5b84 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Park Amp.auto @@ -0,0 +1,57 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ConfirmShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Park Amp" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Park Source.auto b/src/main/deploy/pathplanner/autos/Park Source.auto new file mode 100644 index 0000000..3f4d996 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Park Source.auto @@ -0,0 +1,57 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ConfirmShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Park Source" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Park Stage.auto b/src/main/deploy/pathplanner/autos/Park Stage.auto new file mode 100644 index 0000000..cdc1531 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Park Stage.auto @@ -0,0 +1,57 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ConfirmShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Park Stage" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Troll from Amp.auto b/src/main/deploy/pathplanner/autos/Troll from Amp.auto new file mode 100644 index 0000000..a8180cc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Troll from Amp.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.5001569895805271, + "y": 6.681985456994171 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Amp troll" + } + } + ] + } + }, + "folder": "Troll", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Park Right.auto b/src/main/deploy/pathplanner/autos/Troll from Source.auto similarity index 58% rename from src/main/deploy/pathplanner/autos/Park Right.auto rename to src/main/deploy/pathplanner/autos/Troll from Source.auto index 85c5bfb..7b89d1c 100644 --- a/src/main/deploy/pathplanner/autos/Park Right.auto +++ b/src/main/deploy/pathplanner/autos/Troll from Source.auto @@ -1,6 +1,12 @@ { "version": 1.0, - "startingPose": null, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, "command": { "type": "sequential", "data": { @@ -8,12 +14,12 @@ { "type": "path", "data": { - "pathName": "Park Right" + "pathName": "Source troll" } } ] } }, - "folder": null, + "folder": "Troll", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path b/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path index 10c74c1..fbd549c 100644 --- a/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path +++ b/src/main/deploy/pathplanner/paths/#1 Note from Amp Side.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path b/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path index 0c54c25..b70d665 100644 --- a/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path +++ b/src/main/deploy/pathplanner/paths/#1 Note from Speaker.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/#2 Note.path b/src/main/deploy/pathplanner/paths/#2 Note.path index 254f347..8b7109e 100644 --- a/src/main/deploy/pathplanner/paths/#2 Note.path +++ b/src/main/deploy/pathplanner/paths/#2 Note.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/#3 Note.path b/src/main/deploy/pathplanner/paths/#3 Note.path index a0c8389..ed4d6ce 100644 --- a/src/main/deploy/pathplanner/paths/#3 Note.path +++ b/src/main/deploy/pathplanner/paths/#3 Note.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/#4 Note.path b/src/main/deploy/pathplanner/paths/#4 Note.path index 89f4554..343881d 100644 --- a/src/main/deploy/pathplanner/paths/#4 Note.path +++ b/src/main/deploy/pathplanner/paths/#4 Note.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/#5 Note.path b/src/main/deploy/pathplanner/paths/#5 Note.path index 74277d8..b9b15b6 100644 --- a/src/main/deploy/pathplanner/paths/#5 Note.path +++ b/src/main/deploy/pathplanner/paths/#5 Note.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/#7 Note.path b/src/main/deploy/pathplanner/paths/#7 Note.path index 53a30fa..001d84e 100644 --- a/src/main/deploy/pathplanner/paths/#7 Note.path +++ b/src/main/deploy/pathplanner/paths/#7 Note.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/1m Test.path b/src/main/deploy/pathplanner/paths/1m Test.path index b3882fd..1caebd1 100644 --- a/src/main/deploy/pathplanner/paths/1m Test.path +++ b/src/main/deploy/pathplanner/paths/1m Test.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/2m Test.path b/src/main/deploy/pathplanner/paths/2m Test.path index 9c2b925..0b638bb 100644 --- a/src/main/deploy/pathplanner/paths/2m Test.path +++ b/src/main/deploy/pathplanner/paths/2m Test.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Amp troll.path b/src/main/deploy/pathplanner/paths/Amp troll.path new file mode 100644 index 0000000..05830df --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp troll.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.815000388498094, + "y": 7.0 + }, + "prevControl": { + "x": 7.815000388498094, + "y": 7.999999999999991 + }, + "nextControl": { + "x": 7.815000388498094, + "y": 6.17760178990841 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.815000388498094, + "y": 0.7636554807993345 + }, + "prevControl": { + "x": 7.785629023851966, + "y": 1.688853467152373 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Autonomous", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Circle Test.path b/src/main/deploy/pathplanner/paths/Circle Test.path index d6c79b2..72abb5a 100644 --- a/src/main/deploy/pathplanner/paths/Circle Test.path +++ b/src/main/deploy/pathplanner/paths/Circle Test.path @@ -87,7 +87,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path b/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path index 87455d3..1dfaaa4 100644 --- a/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path +++ b/src/main/deploy/pathplanner/paths/Long Shot Speaker Amp Side.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Park Right.path b/src/main/deploy/pathplanner/paths/Park Amp.path similarity index 73% rename from src/main/deploy/pathplanner/paths/Park Right.path rename to src/main/deploy/pathplanner/paths/Park Amp.path index 3aa8b31..6a42d5f 100644 --- a/src/main/deploy/pathplanner/paths/Park Right.path +++ b/src/main/deploy/pathplanner/paths/Park Amp.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0302151552424759, - "y": 2.32033780704413 + "x": 1.1183292491808605, + "y": 7.240041385270605 }, "prevControl": null, "nextControl": { - "x": 2.030215155242482, - "y": 2.32033780704413 + "x": 2.11832924918086, + "y": 7.240041385270605 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.995349382469785, - "y": 1.1161118565528736 + "x": 4.217008219347387, + "y": 7.240041385270605 }, "prevControl": { - "x": 3.9953493824697848, - "y": 1.1161118565528736 + "x": 3.2170082193473872, + "y": 7.240041385270605 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Park Source.path b/src/main/deploy/pathplanner/paths/Park Source.path new file mode 100644 index 0000000..1947cb3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Park Source.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.765872873427322, + "y": 4.126676732781015 + }, + "prevControl": null, + "nextControl": { + "x": 1.765872873427333, + "y": 4.126676732781015 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.585523879455631, + "y": 1.967881431290591 + }, + "prevControl": { + "x": 2.585523879455631, + "y": 1.967881431290591 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Autonomous", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Park Stage.path b/src/main/deploy/pathplanner/paths/Park Stage.path new file mode 100644 index 0000000..625ca98 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Park Stage.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5735854011958479, + "y": 5.698044741348875 + }, + "prevControl": null, + "nextControl": { + "x": 2.573585401195859, + "y": 5.698044741348875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.217008219347387, + "y": 5.698044741348875 + }, + "prevControl": { + "x": 3.2170082193473872, + "y": 5.698044741348875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Autonomous", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Playground.path b/src/main/deploy/pathplanner/paths/Playground.path index d5c2100..eb10909 100644 --- a/src/main/deploy/pathplanner/paths/Playground.path +++ b/src/main/deploy/pathplanner/paths/Playground.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.5442140365497197, - "y": 5.580559282764361 + "x": 0.2, + "y": 5.0 }, "prevControl": null, "nextControl": { - "x": 2.5442140365497243, - "y": 5.580559282764361 + "x": 1.2000000000000044, + "y": 5.0 }, "isLocked": false, "linkedName": null @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Source Direct Left.path b/src/main/deploy/pathplanner/paths/Source Direct Left.path index c16fb8e..78d87e0 100644 --- a/src/main/deploy/pathplanner/paths/Source Direct Left.path +++ b/src/main/deploy/pathplanner/paths/Source Direct Left.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Source Direct Middle.path b/src/main/deploy/pathplanner/paths/Source Direct Middle.path index ce194fe..d04c6d5 100644 --- a/src/main/deploy/pathplanner/paths/Source Direct Middle.path +++ b/src/main/deploy/pathplanner/paths/Source Direct Middle.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Source Direct Right.path b/src/main/deploy/pathplanner/paths/Source Direct Right.path index 080ccfb..c4734be 100644 --- a/src/main/deploy/pathplanner/paths/Source Direct Right.path +++ b/src/main/deploy/pathplanner/paths/Source Direct Right.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0 }, diff --git a/src/main/deploy/pathplanner/paths/Source troll.path b/src/main/deploy/pathplanner/paths/Source troll.path new file mode 100644 index 0000000..132112a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source troll.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5295283542266556, + "y": 1.4832539146294756 + }, + "prevControl": null, + "nextControl": { + "x": 2.5295283542266573, + "y": 1.4832539146294756 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.815000388498094, + "y": 0.7783411631223989 + }, + "prevControl": { + "x": 7.4058845585995705, + "y": 0.3887070394095215 + }, + "nextControl": { + "x": 8.12339971728244, + "y": 1.0720548095836802 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.815000388498094, + "y": 7.445640937793504 + }, + "prevControl": { + "x": 7.815000388498094, + "y": 6.03581543477935 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Autonomous", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/org/rambots/Robot.kt b/src/main/java/org/rambots/Robot.kt index 844c2c7..c90e202 100644 --- a/src/main/java/org/rambots/Robot.kt +++ b/src/main/java/org/rambots/Robot.kt @@ -15,6 +15,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher import org.littletonrobotics.junction.wpilog.WPILOGReader import org.littletonrobotics.junction.wpilog.WPILOGWriter import org.rambots.subsystems.LightingSubsystem +import org.rambots.subsystems.superstructure.WristSubsystem import org.rambots.util.LocalADStarAK @@ -74,9 +75,8 @@ object Robot : LoggedRobot() { // Access the RobotContainer object so that it is initialized. This will perform all our // button bindings, and put our autonomous chooser on the dashboard. RobotContainer - } - + } override fun robotPeriodic() { CommandScheduler.getInstance().run() diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index 85c5ff6..cf2406c 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -52,8 +52,8 @@ object RobotContainer { private var aprilTagVision: AprilTagVision // Controller - private val controller = CommandPS5Controller(0) - private val xbox = CommandXboxController(1) + val controller = CommandPS5Controller(0) + val xbox = CommandXboxController(1) // Dashboard inputs private val autoChooser: LoggedDashboardChooser @@ -61,8 +61,7 @@ object RobotContainer { val hasLocalized get() = aprilTagVision.hasSeenTarget() val hasActiveTag get() = aprilTagVision.isTargetVisible - // private final LoggedTunableNumber flywheelSpeedInput = - // new LoggedTunableNumber("Flywheel Speed", 1500.0); + // private final LoggedTunableNumber flywheelSpeedInput = new LoggedTunableNumber("Flywheel Speed", 1500.0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ init { @@ -79,7 +78,7 @@ object RobotContainer { ModuleIOTalonFX(moduleConfigs[3]) ) - aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight-three")) + aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight-three", drive)) } Constants.Mode.SIM -> { @@ -206,8 +205,7 @@ object RobotContainer { controller.L2().onTrue(IntakeCommandGroup()) controller.L2().onFalse(IntakeHomeCommandGroup()) - controller.povUp().onTrue(BackAmpCommandGroup()) - + controller.R1().onTrue(BackAmpCommandGroup()) controller.R2().whileTrue(AutoShootCommand(driveController) { drive.pose }) controller.square().whileTrue(IntakeCommand()) @@ -216,10 +214,13 @@ object RobotContainer { xbox.povUp().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition--})) xbox.povDown().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition++})) + xbox.povLeft().whileTrue(ReverseIntakeCommand()) + xbox.povRight().whileTrue(IntakeCommand()) xbox.a().onTrue(FullHomeCommandGroup()) xbox.x().onTrue(ExtendClimberCommandGroup()) xbox.y().whileTrue(RetractClimberCommand()) + xbox.b().whileTrue(ExtendClimberCommand()) xbox.rightTrigger().whileTrue(ShootCommand()) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommand.kt b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommand.kt new file mode 100644 index 0000000..464e6f4 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommand.kt @@ -0,0 +1,18 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.config.ElevatorConstants +import org.rambots.subsystems.superstructure.ElevatorSubsystem + +class ExtendClimberCommand : Command() { + + override fun execute() { + ElevatorSubsystem.desiredPosition += ElevatorConstants.EXTENSION_RATE + } + + override fun isFinished(): Boolean { + return false + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt index 8c15502..0aaa6a7 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt @@ -12,5 +12,6 @@ class ExtendClimberCommandGroup : SequentialCommandGroup( WristPositionCommand { 0.0 }, ElevatorPositionCommand({ 0.0 }, { it < 5.0 }), ArmPositionCommand({ 80.0 }, { it > 75.0 }), - ElevatorPositionCommand({ ElevatorConstants.ELEVATOR_MAX_HEIGHT }, { it > ElevatorConstants.ELEVATOR_MAX_HEIGHT - 5.0 }) + ElevatorPositionCommand({ ElevatorConstants.ELEVATOR_MAX_HEIGHT }, { it > ElevatorConstants.ELEVATOR_MAX_HEIGHT - 5.0 }), ) + diff --git a/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt index 25f08a5..171838a 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt @@ -9,12 +9,11 @@ import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand import org.rambots.commands.superstructure.primitive.WristPositionCommand import org.rambots.subsystems.LightingSubsystem import org.rambots.subsystems.superstructure.* -import org.rambots.util.LimelightHelpers class IntakeCommandGroup : SequentialCommandGroup( ArmPositionCommand ({ 0.0 }, { it < 5.0 }), - ElevatorPositionCommand ({ 55.0 }, { it > 20.0 }), - WristPositionCommand ({ 130.0 }, { true }), + WristPositionCommand ({ 125.0 }, { it > 35.0 }), + ElevatorPositionCommand ({ 55.0 }, { true }), Commands.runOnce ({ ShooterSubsystem.intake() }, ShooterSubsystem), diff --git a/src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt index c5cd051..787c6eb 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt @@ -12,6 +12,6 @@ import org.rambots.subsystems.superstructure.ShooterSubsystem class IntakeHomeCommandGroup : SequentialCommandGroup( Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem), - WristPositionCommand({ 0.0 }, { it < 65.0 }), + WristPositionCommand({ 0.0 }, { it < 115.0 }), ElevatorPositionCommand({ 0.0 }, { it < 5.0 }), ) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt b/src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt index 41cf982..a6a732c 100644 --- a/src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt +++ b/src/main/java/org/rambots/commands/superstructure/actions/RetractClimberCommand.kt @@ -2,12 +2,13 @@ package org.rambots.commands.superstructure.actions import edu.wpi.first.wpilibj2.command.Command +import org.rambots.config.ElevatorConstants import org.rambots.subsystems.superstructure.ElevatorSubsystem class RetractClimberCommand : Command() { override fun execute() { - ElevatorSubsystem.desiredPosition -= 0.5 + ElevatorSubsystem.desiredPosition -= ElevatorConstants.EXTENSION_RATE } override fun isFinished(): Boolean { diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt index f770fce..93e781a 100644 --- a/src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt +++ b/src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt @@ -6,6 +6,7 @@ import org.rambots.subsystems.superstructure.ShooterSubsystem class ReverseIntakeCommand : Command() { override fun initialize() { + ShooterSubsystem.reverseShooter() ShooterSubsystem.reverseIntake() } @@ -15,6 +16,7 @@ class ReverseIntakeCommand : Command() { override fun end(interrupted: Boolean) { ShooterSubsystem.stopIntake() + ShooterSubsystem.stopShooter() } } diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt index 773b0db..2b205aa 100644 --- a/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt +++ b/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt @@ -17,6 +17,9 @@ class WristPositionCommand( addRequirements(WristSubsystem) } + override fun initialize() { + } + override fun execute() { WristSubsystem.desiredPosition = angle.invoke() } diff --git a/src/main/java/org/rambots/config/ElevatorConstants.kt b/src/main/java/org/rambots/config/ElevatorConstants.kt index ef6f9cb..618e07b 100644 --- a/src/main/java/org/rambots/config/ElevatorConstants.kt +++ b/src/main/java/org/rambots/config/ElevatorConstants.kt @@ -10,6 +10,8 @@ object ElevatorConstants { val ELEVATOR_PID = PIDConstants(0.2, 0.0, 0.0) const val ELEVATOR_MIN_HEIGHT = 0.0 - const val ELEVATOR_MAX_HEIGHT = 69.0 + const val ELEVATOR_MAX_HEIGHT = 70.0 + + const val EXTENSION_RATE = 0.75 } \ No newline at end of file diff --git a/src/main/java/org/rambots/config/ShooterConstants.kt b/src/main/java/org/rambots/config/ShooterConstants.kt index 2ca0af1..a9cec5c 100644 --- a/src/main/java/org/rambots/config/ShooterConstants.kt +++ b/src/main/java/org/rambots/config/ShooterConstants.kt @@ -8,7 +8,7 @@ object ShooterConstants { const val INTAKE_BOTTOM_ID = 17 const val INTAKE_TOP_ID = 18 - const val INTAKE_OUTPUT = 0.45 + const val INTAKE_OUTPUT = 0.5 const val INTAKE_SPIKE_LIMIT = 30 diff --git a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java index 6f27e87..01a03d8 100644 --- a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java +++ b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java @@ -10,12 +10,13 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import org.rambots.Constants; +import org.rambots.Robot; /** * All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { - public static final double wheelRadius = Units.inchesToMeters(2.0); + public static final double wheelRadius = Units.inchesToMeters(3.89/2); public static final double odometryFrequency = switch (Constants.getRobot()) { case SIMBOT -> 50.0; @@ -25,10 +26,26 @@ public final class DriveConstants { switch (Constants.getRobot()) { default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); }; + + public static double getXyStdDevCoefficient() { + if (Robot.INSTANCE.isDisabled()) { + return 0.005; + } + else return xyStdDevCoefficient; + } + public static final double xyStdDevCoefficient = switch (Constants.getRobot()) { - default -> 0.065; + default -> 0.015; }; + + public static double getThetaStdDevCoefficient() { + if (Robot.INSTANCE.isDisabled()) { + return 0.0005; + } + else return thetaStdDevCoefficient; + } + public static final double thetaStdDevCoefficient = switch (Constants.getRobot()) { default -> 0.01; diff --git a/src/main/java/org/rambots/subsystems/drive/GyroIO_ADIS.java b/src/main/java/org/rambots/subsystems/drive/GyroIO_ADIS.java index 7f6977e..f8c2a87 100644 --- a/src/main/java/org/rambots/subsystems/drive/GyroIO_ADIS.java +++ b/src/main/java/org/rambots/subsystems/drive/GyroIO_ADIS.java @@ -8,19 +8,19 @@ import java.util.Queue; public class GyroIO_ADIS implements GyroIO { - private final ADIS16470_IMU navx = new ADIS16470_IMU(); + private final ADIS16470_IMU gyro = new ADIS16470_IMU(); private final Queue yawPositionQueue; public GyroIO_ADIS() { - navx.reset(); + gyro.reset(); yawPositionQueue = PhoenixOdometryThread.getInstance() .registerSignal( () -> { - boolean valid = navx.isConnected(); + boolean valid = gyro.isConnected(); if (valid) { - return OptionalDouble.of(navx.getAngle()); + return OptionalDouble.of(gyro.getAngle()); } else { return OptionalDouble.empty(); } @@ -29,12 +29,13 @@ public GyroIO_ADIS() { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = navx.isConnected(); - inputs.yawPosition = Rotation2d.fromDegrees(navx.getAngle()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRate()); + inputs.connected = gyro.isConnected(); + inputs.yawPosition = Rotation2d.fromDegrees(gyro.getAngle()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(gyro.getRate()); inputs.odometryYawPositions = yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); yawPositionQueue.clear(); } + } diff --git a/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt b/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt index ab3fff3..de700d1 100644 --- a/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt @@ -2,6 +2,8 @@ package org.rambots.subsystems import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger +import org.rambots.Robot +import org.rambots.RobotContainer import org.rambots.subsystems.lighting.LimelightLEDState import org.rambots.util.LimelightHelpers @@ -34,6 +36,18 @@ object LightingSubsystem : SubsystemBase() { Logger.recordOutput("Lighting/LimelightLEDState", limelightLEDState.name) + if (Robot.isDisabled) { + if (RobotContainer.hasLocalized && RobotContainer.hasActiveTag) { + blindLL() + } else if (RobotContainer.hasLocalized) { + blinkLL() + } else { + clearLL() + } + + } + + } } \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt index cbc492d..bccb425 100644 --- a/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt @@ -3,14 +3,18 @@ package org.rambots.subsystems.superstructure import com.revrobotics.CANSparkBase import com.revrobotics.CANSparkLowLevel import com.revrobotics.CANSparkMax +import edu.wpi.first.wpilibj.GenericHID import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger +import org.rambots.RobotContainer import org.rambots.config.ShooterConstants.INTAKE_BOTTOM_ID import org.rambots.config.ShooterConstants.INTAKE_OUTPUT import org.rambots.config.ShooterConstants.INTAKE_TOP_ID import org.rambots.config.ShooterConstants.SHOOTER_BOTTOM_ID import org.rambots.config.ShooterConstants.SHOOTER_TOP_ID import org.rambots.config.ShooterConstants.INTAKE_SPIKE_LIMIT +import java.awt.Robot +import kotlin.math.abs object ShooterSubsystem : SubsystemBase() { @@ -72,12 +76,26 @@ object ShooterSubsystem : SubsystemBase() { shooterBottom.set(-1.0) } + fun reverseShooter() { + shooterTop.set(INTAKE_OUTPUT) + shooterBottom.set(INTAKE_OUTPUT) + } + fun stopShooter() { shooterTop.set(0.0) shooterBottom.set(0.0) } override fun periodic() { + + if (abs(topVelocity) > 5000.0 && abs(bottomVelocity) > 5000.0) { + RobotContainer.controller.hid.setRumble(GenericHID.RumbleType.kBothRumble, 1.0) + RobotContainer.xbox.hid.setRumble(GenericHID.RumbleType.kBothRumble, 1.0) + } else { + RobotContainer.controller.hid.setRumble(GenericHID.RumbleType.kBothRumble, 0.0) + RobotContainer.xbox.hid.setRumble(GenericHID.RumbleType.kBothRumble, 0.0) + } + Logger.recordOutput("Shooter/Intake/Top/Output", intakeTopLead.appliedOutput) Logger.recordOutput("Shooter/Intake/Top/Velocity", intakeTopLead.encoder.velocity) Logger.recordOutput("Shooter/Intake/Top/Current", intakeTopLead.outputCurrent) diff --git a/src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt index 73eb085..8dd5d7f 100644 --- a/src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.rambots.config.WristConstants.WRIST_ID import org.rambots.config.WristConstants.WRIST_PID +import org.rambots.subsystems.superstructure.WristSubsystem.absoluteEncoderPosition +import org.rambots.subsystems.superstructure.WristSubsystem.dutyCycleEncoder object WristSubsystem : SubsystemBase() { @@ -17,12 +19,17 @@ object WristSubsystem : SubsystemBase() { val position get() = motor.encoder.position private val dutyCycleEncoder = DutyCycleEncoder(0).apply { - positionOffset = 0.5 + positionOffset = 0.081 } - private val absoluteEncoderPosition get() = dutyCycleEncoder.absolutePosition - dutyCycleEncoder.positionOffset + private val absoluteEncoderPosition get() = if ((dutyCycleEncoder.absolutePosition - dutyCycleEncoder.positionOffset) < 0) 0.0 else dutyCycleEncoder.absolutePosition - dutyCycleEncoder.positionOffset + private val relativeEncoderPosition get() = (position / 75.0) * (15.0 / 36.0) private val absoluteMotorPosition get() = absoluteEncoderPosition * 75.0 * (36.0 / 15.0) + fun sync() { + dutyCycleEncoder.positionOffset = position + } + private val motor = CANSparkMax(WRIST_ID, CANSparkLowLevel.MotorType.kBrushless).apply { restoreFactoryDefaults() @@ -38,6 +45,9 @@ object WristSubsystem : SubsystemBase() { } fun resetEncoder() { + if (!dutyCycleEncoder.isConnected) { + return + } motor.encoder.position = absoluteMotorPosition } @@ -46,6 +56,7 @@ object WristSubsystem : SubsystemBase() { Logger.recordOutput("Wrist/DesiredPosition", desiredPosition) Logger.recordOutput("Wrist/Offset", offset) + Logger.recordOutput("Wrist/RawPosition", dutyCycleEncoder.absolutePosition) Logger.recordOutput("Wrist/Position", motor.encoder.position) Logger.recordOutput("Wrist/PositionThroughEncoder", absoluteMotorPosition) Logger.recordOutput("Wrist/ShaftPosition", relativeEncoderPosition) diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java index b3c5dd1..6c91035 100644 --- a/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java @@ -12,14 +12,14 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; +import org.rambots.RobotContainer; import org.rambots.util.FieldConstants; import org.rambots.util.VisionHelpers; import java.util.*; import java.util.function.Consumer; -import static org.rambots.subsystems.drive.DriveConstants.thetaStdDevCoefficient; -import static org.rambots.subsystems.drive.DriveConstants.xyStdDevCoefficient; +import static org.rambots.subsystems.drive.DriveConstants.*; public class AprilTagVision extends SubsystemBase { @@ -175,9 +175,7 @@ private double calculateXYStdDev(VisionHelpers.PoseEstimate poseEstimates, int t * @return The standard deviation of the theta coordinate */ private double calculateThetaStdDev(VisionHelpers.PoseEstimate poseEstimates, int tagPosesSize) { - return thetaStdDevCoefficient - * Math.pow(poseEstimates.averageTagDistance(), 2.0) - / tagPosesSize; + return thetaStdDevCoefficient * Math.pow(poseEstimates.averageTagDistance(), 2.0) / tagPosesSize; } /** diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java index 493ec9b..a9efa56 100644 --- a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java @@ -7,10 +7,16 @@ package org.rambots.subsystems.vision; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.networktables.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import org.littletonrobotics.junction.Logger; +import org.rambots.Robot; +import org.rambots.subsystems.drive.Drive; import org.rambots.util.LimelightHelpers; import org.rambots.util.VisionHelpers; @@ -24,14 +30,16 @@ public class AprilTagVisionIOLimelight implements AprilTagVisionIO { private final StringSubscriber observationSubscriber; String limelightName; + Drive drive; /** * Constructs a new AprilTagVisionIOLimelight instance. * * @param identifier The identifier of the Limelight camera. */ - public AprilTagVisionIOLimelight(String identifier) { + public AprilTagVisionIOLimelight(String identifier, Drive drive) { limelightName = identifier; + this.drive = drive; NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable(identifier); LimelightHelpers.setPipelineIndex(limelightName, 0); diff --git a/src/main/java/org/rambots/util/LimelightHelpers.java b/src/main/java/org/rambots/util/LimelightHelpers.java index 59ed0ad..e9fed10 100644 --- a/src/main/java/org/rambots/util/LimelightHelpers.java +++ b/src/main/java/org/rambots/util/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.4.0 (March 21, 2024) +//LimelightHelpers v1.5.0 (March 27, 2024) package org.rambots.util; @@ -303,6 +303,9 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_orb_wpiblue") + public double[] botpose_orb_wpiblue; + @JsonProperty("botpose_tagcount") public double botpose_tagcount; @@ -330,6 +333,10 @@ public Pose3d getBotPose3d_wpiBlue() { return toPose3D(botpose_wpiblue); } + public Pose3d getBotPose3d_wpiBlue_MegaTag2() { + return toPose3D(botpose_orb_wpiblue); + } + public Pose2d getBotPose2d() { return toPose2D(botpose); } @@ -758,6 +765,17 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -782,6 +800,16 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -865,6 +893,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward;