From bb6994ec63421e0f37fcdb1a711f7ef171906aaa Mon Sep 17 00:00:00 2001 From: James Ding Date: Wed, 3 Apr 2024 15:32:50 -0700 Subject: [PATCH] Sync changes from 4/2/2024 --- .../deploy/pathplanner/autos/Troll Right.auto | 25 - src/main/deploy/pathplanner/navgrid.json | 2 +- ...{Troll Left Center 3.path => #2 Note.path} | 43 +- .../paths/{Troll Right.path => #3 Note.path} | 36 +- src/main/java/org/rambots/Robot.kt | 17 +- src/main/java/org/rambots/RobotContainer.kt | 174 +-- .../java/org/rambots/auto/AutoConstants.kt | 2 +- .../java/org/rambots/commands/AmpScoring.kt | 31 - .../org/rambots/commands/AutoAimCommand.kt | 11 +- .../java/org/rambots/commands/SourceIntake.kt | 26 - .../rambots/commands/WristPositionCommand.kt | 26 - .../actions/AmpScoreCommandGroup.kt | 16 + .../actions/AmpScoreHomeCommandGroup.kt | 16 + .../actions/AutoShootCommand.kt | 49 + .../actions/ExtendClimberCommandGroup.kt | 14 + .../actions/FullHomeCommandGroup.kt | 14 + .../actions/IntakeCommandGroup.kt | 36 + .../actions/IntakeHomeCommandGroup.kt | 17 + .../primitive}/ArmPositionCommand.kt | 12 +- .../primitive}/ElevatorPositionCommand.kt | 15 +- .../superstructure/primitive/IntakeCommand.kt | 20 + .../primitive/ReverseIntakeCommand.kt | 20 + .../primitive/WristPositionCommand.kt | 36 + .../java/org/rambots/config/ArmConstants.kt | 54 +- .../org/rambots/config/ElevatorConstants.kt | 6 +- .../org/rambots/config/ShooterConstants.kt | 9 +- .../java/org/rambots/config/WristConstants.kt | 2 +- .../org/rambots/subsystems/WristSubsystem.kt | 54 - .../org/rambots/subsystems/drive/Drive.java | 6 + .../subsystems/drive/DriveConstants.java | 26 +- .../rambots/subsystems/flywheel/Flywheel.java | 126 -- .../subsystems/flywheel/FlywheelIO.java | 56 - .../subsystems/flywheel/FlywheelIOSim.java | 68 - .../flywheel/FlywheelIOSparkMax.java | 89 -- .../flywheel/FlywheelIOTalonFX.java | 98 -- .../rambots/subsystems/lighting/Blinkin.kt | 12 - .../subsystems/lighting/BlinkinPattern.kt | 105 -- .../subsystems/lighting/LightingSubsystem.kt | 33 +- .../subsystems/lighting/LimelightLEDState.kt | 9 + .../{ => superstructure}/ArmSubsystem.kt | 37 +- .../{ => superstructure}/ElevatorSubsystem.kt | 28 +- .../{ => superstructure}/ShooterSubsystem.kt | 9 +- .../{ => superstructure}/SuperStructure.kt | 15 +- .../superstructure/WristSubsystem.kt | 63 + .../subsystems/vision/AprilTagVision.java | 17 +- .../vision/AprilTagVisionIOLimelight.java | 3 +- .../org/rambots/util/LimelightHelpers.java | 1335 ++++++++++------- .../java/org/rambots/util/SparkMaxUtil.kt | 11 + vendordeps/PathplannerLib.json | 8 +- vendordeps/Phoenix5.json | 24 +- vendordeps/Phoenix6.json | 50 +- vendordeps/REVLib.json | 12 +- 52 files changed, 1362 insertions(+), 1661 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Troll Right.auto rename src/main/deploy/pathplanner/paths/{Troll Left Center 3.path => #2 Note.path} (50%) rename src/main/deploy/pathplanner/paths/{Troll Right.path => #3 Note.path} (54%) delete mode 100644 src/main/java/org/rambots/commands/AmpScoring.kt delete mode 100644 src/main/java/org/rambots/commands/SourceIntake.kt delete mode 100644 src/main/java/org/rambots/commands/WristPositionCommand.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/FullHomeCommandGroup.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt rename src/main/java/org/rambots/commands/{ => superstructure/primitive}/ArmPositionCommand.kt (66%) rename src/main/java/org/rambots/commands/{ => superstructure/primitive}/ElevatorPositionCommand.kt (55%) create mode 100644 src/main/java/org/rambots/commands/superstructure/primitive/IntakeCommand.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt create mode 100644 src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt delete mode 100644 src/main/java/org/rambots/subsystems/WristSubsystem.kt delete mode 100644 src/main/java/org/rambots/subsystems/flywheel/Flywheel.java delete mode 100644 src/main/java/org/rambots/subsystems/flywheel/FlywheelIO.java delete mode 100644 src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSim.java delete mode 100644 src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSparkMax.java delete mode 100644 src/main/java/org/rambots/subsystems/flywheel/FlywheelIOTalonFX.java delete mode 100644 src/main/java/org/rambots/subsystems/lighting/Blinkin.kt delete mode 100644 src/main/java/org/rambots/subsystems/lighting/BlinkinPattern.kt create mode 100644 src/main/java/org/rambots/subsystems/lighting/LimelightLEDState.kt rename src/main/java/org/rambots/subsystems/{ => superstructure}/ArmSubsystem.kt (54%) rename src/main/java/org/rambots/subsystems/{ => superstructure}/ElevatorSubsystem.kt (80%) rename src/main/java/org/rambots/subsystems/{ => superstructure}/ShooterSubsystem.kt (91%) rename src/main/java/org/rambots/subsystems/{ => superstructure}/SuperStructure.kt (84%) create mode 100644 src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt create mode 100644 src/main/java/org/rambots/util/SparkMaxUtil.kt diff --git a/src/main/deploy/pathplanner/autos/Troll Right.auto b/src/main/deploy/pathplanner/autos/Troll Right.auto deleted file mode 100644 index 4067938..0000000 --- a/src/main/deploy/pathplanner/autos/Troll Right.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.9714724259502194, - "y": 2.511251677243963 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Troll Right" - } - } - ] - } - }, - "folder": "Troll", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index e9b200c..bab0da9 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Troll Left Center 3.path b/src/main/deploy/pathplanner/paths/#2 Note.path similarity index 50% rename from src/main/deploy/pathplanner/paths/Troll Left Center 3.path rename to src/main/deploy/pathplanner/paths/#2 Note.path index ea86af8..09a1914 100644 --- a/src/main/deploy/pathplanner/paths/Troll Left Center 3.path +++ b/src/main/deploy/pathplanner/paths/#2 Note.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.49, - "y": 6.87 + "x": 1.9113560946263224, + "y": 6.285472034271439 }, "prevControl": null, "nextControl": { - "x": 7.713587380878466, - "y": 7.767357877308347 + "x": 1.550088309478945, + "y": 5.9623870231640295 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.707945864965804, - "y": 5.008834749150981 + "x": 2.316680926742892, + "y": 5.565873600441298 }, "prevControl": { - "x": 8.011288207941806, - "y": 5.923686308384761 - }, - "nextControl": { - "x": 7.319631417040612, - "y": 3.8377154300604905 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.0, - "y": 1.12 - }, - "prevControl": { - "x": 8.97283311696253, - "y": 0.2231071082512538 + "x": 1.2945574370576298, + "y": 5.521816553472107 }, "nextControl": null, "isLocked": false, @@ -55,11 +39,14 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 180.0, + "rotateFast": true }, "reversed": false, "folder": "Autonomous", - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": -127.92965517984241, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Troll Right.path b/src/main/deploy/pathplanner/paths/#3 Note.path similarity index 54% rename from src/main/deploy/pathplanner/paths/Troll Right.path rename to src/main/deploy/pathplanner/paths/#3 Note.path index e889c78..4b3b134 100644 --- a/src/main/deploy/pathplanner/paths/Troll Right.path +++ b/src/main/deploy/pathplanner/paths/#3 Note.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.980663700146722, - "y": 0.9105123040299752 + "x": 1.86729904765713, + "y": 5.477759506502913 }, "prevControl": null, "nextControl": { - "x": 5.98066370014671, - "y": 0.9105123040299752 + "x": 1.8085563183648736, + "y": 5.0371890368109895 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.242258777891271, - "y": 1.2335973151375008 + "x": 2.3372408819951813, + "y": 4.376333332273105 }, "prevControl": { - "x": 6.586981820925471, - "y": 0.4782086008521783 - }, - "nextControl": { - "x": 8.299627905144566, - "y": 2.4525089479519377 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.021973543052633, - "y": 7.3722125261782985 - }, - "prevControl": { - "x": 7.947171529405674, - "y": 7.0638131973939515 + "x": 1.6470138128111684, + "y": 4.670046978734387 }, "nextControl": null, "isLocked": false, @@ -55,8 +39,8 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 156.80060338532698, + "rotateFast": true }, "reversed": false, "folder": "Autonomous", diff --git a/src/main/java/org/rambots/Robot.kt b/src/main/java/org/rambots/Robot.kt index c0b3d31..844c2c7 100644 --- a/src/main/java/org/rambots/Robot.kt +++ b/src/main/java/org/rambots/Robot.kt @@ -1,5 +1,6 @@ package org.rambots +import com.pathplanner.lib.commands.PathfindingCommand import com.pathplanner.lib.pathfinding.Pathfinding import edu.wpi.first.hal.FRCNetComm.tInstances import edu.wpi.first.hal.FRCNetComm.tResourceType @@ -13,6 +14,7 @@ import org.littletonrobotics.junction.Logger 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.util.LocalADStarAK @@ -30,13 +32,12 @@ object Robot : LoggedRobot() { private var autonomousCommand: Command? = null - override fun robotInit() { // Report the use of the Kotlin Language for "FRC Usage Report" statistics HAL.report(tResourceType.kResourceType_Language, tInstances.kLanguage_Kotlin, 0, WPILibVersion.Version) - - Pathfinding.setPathfinder(LocalADStarAK()); + Pathfinding.setPathfinder(LocalADStarAK()) + PathfindingCommand.warmupCommand().schedule() // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); @@ -76,18 +77,25 @@ object Robot : LoggedRobot() { } + override fun robotPeriodic() { CommandScheduler.getInstance().run() } override fun disabledInit() { - + RobotContainer.unlockAllMotors() + LightingSubsystem.clearLL() } override fun disabledPeriodic() { } + override fun disabledExit() { + RobotContainer.lockAllMotors() + LightingSubsystem.clearLL() + } + override fun autonomousInit() { autonomousCommand = RobotContainer.autonomousCommand autonomousCommand?.schedule() @@ -122,4 +130,5 @@ object Robot : LoggedRobot() { override fun simulationPeriodic() { } + } \ No newline at end of file diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index 4efc5ee..1a123ad 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -13,7 +13,6 @@ package org.rambots import com.pathplanner.lib.auto.AutoBuilder -import com.pathplanner.lib.auto.NamedCommands import edu.wpi.first.math.geometry.* import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands @@ -22,9 +21,13 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import org.littletonrobotics.junction.networktables.LoggedDashboardChooser import org.rambots.commands.* +import org.rambots.commands.superstructure.actions.* +import org.rambots.commands.superstructure.primitive.* +import org.rambots.commands.superstructure.actions.AutoShootCommand import org.rambots.subsystems.* import org.rambots.subsystems.drive.* import org.rambots.subsystems.drive.DriveConstants.moduleConfigs +import org.rambots.subsystems.superstructure.* import org.rambots.subsystems.vision.AprilTagVision import org.rambots.subsystems.vision.AprilTagVisionIO import org.rambots.subsystems.vision.AprilTagVisionIOLimelight @@ -41,10 +44,10 @@ import kotlin.math.pow */ object RobotContainer { // Subsystems - public var drive: Drive + var drive: Drive + val driveController = DriveController() + private var aprilTagVision: AprilTagVision - private lateinit var aprilTagVisionTwo: AprilTagVision - public val driveController = DriveController() // Controller private val controller = CommandPS5Controller(0) @@ -74,16 +77,7 @@ object RobotContainer { ModuleIOTalonFX(moduleConfigs[3]) ) - // flywheel = new Flywheel(new FlywheelIOSparkMax()); - // drive = new Drive( - // new GyroIOPigeon2(true), - // new ModuleIOTalonFX(0), - // new ModuleIOTalonFX(1), - // new ModuleIOTalonFX(2), - // new ModuleIOTalonFX(3)); -// flywheel = Flywheel(FlywheelIOTalonFX()) aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight-three")) - aprilTagVisionTwo = AprilTagVision(AprilTagVisionIOLimelight("limelight-two")) } Constants.Mode.SIM -> { @@ -96,20 +90,12 @@ object RobotContainer { ModuleIOSim(), ModuleIOSim() ) -// flywheel = Flywheel(FlywheelIOSim()) aprilTagVision = AprilTagVision( AprilTagVisionIOPhotonVisionSIM( "photonCamera1", Transform3d(Translation3d(0.5, 0.0, 0.5), Rotation3d(0.0, 0.0, 0.0)) ) { drive.drive }) - - aprilTagVisionTwo = - AprilTagVision( - AprilTagVisionIOPhotonVisionSIM( - "photonCamera2", - Transform3d(Translation3d(0.5, 0.0, 0.5), Rotation3d(0.0, 0.0, 0.0)) - ) { drive.drive }) } else -> { @@ -121,23 +107,21 @@ object RobotContainer { object : ModuleIO {}, object : ModuleIO {}, object : ModuleIO {}) -// flywheel = Flywheel(object : FlywheelIO {}) aprilTagVision = AprilTagVision(object : AprilTagVisionIO {}) - aprilTagVisionTwo = AprilTagVision(object: AprilTagVisionIO{}) } } - - // Registering named commands - NamedCommands.registerCommand("aim", AutoAimCommand(driveController) {drive.pose} ) - NamedCommands.registerCommand("startShooter", Commands.runOnce({ ShooterSubsystem.shoot() }, ShooterSubsystem)) - NamedCommands.registerCommand("intakeToShooter", Commands.runOnce({ShooterSubsystem.intake()}, ShooterSubsystem)) - NamedCommands.registerCommand("stopShooter", Commands.runOnce({ShooterSubsystem.stopShooter()}, ShooterSubsystem)) - NamedCommands.registerCommand("stopIntake", Commands.runOnce({ShooterSubsystem.stopIntake()}, ShooterSubsystem)) - NamedCommands.registerCommand("groundIntake", SuperStructure.intakeCommand) - NamedCommands.registerCommand("homeFromIntake", SuperStructure.homeCommandFromIntake) - NamedCommands.registerCommand("ampScore", SuperStructure.ampCommand) - NamedCommands.registerCommand("homeFromAmp", SuperStructure.ampHomingCommand) - NamedCommands.registerCommand("closeToSpeakerWristPosition", Commands.runOnce({WristPositionCommand({-75.0}, {it < -70.0})})) +// +// // Registering named commands +// NamedCommands.registerCommand("aim", AutoAimCommand(driveController) {drive.pose} ) +// NamedCommands.registerCommand("startShooter", Commands.runOnce({ ShooterSubsystem.shoot() }, ShooterSubsystem)) +// NamedCommands.registerCommand("intakeToShooter", Commands.runOnce({ ShooterSubsystem.intake()}, ShooterSubsystem)) +// NamedCommands.registerCommand("stopShooter", Commands.runOnce({ ShooterSubsystem.stopShooter()}, ShooterSubsystem)) +// NamedCommands.registerCommand("stopIntake", Commands.runOnce({ ShooterSubsystem.stopIntake()}, ShooterSubsystem)) +// NamedCommands.registerCommand("groundIntake", SuperStructure.intakeCommand) +// NamedCommands.registerCommand("homeFromIntake", SuperStructure.homeCommandFromIntake) +// NamedCommands.registerCommand("ampScore", SuperStructure.ampCommand) +// NamedCommands.registerCommand("homeFromAmp", SuperStructure.ampHomingCommand) +// NamedCommands.registerCommand("closeToSpeakerWristPosition", Commands.runOnce({ WristPositionCommand({-75.0}, {it < -70.0}) })) // Set up auto routines // NamedCommands.registerCommand( @@ -148,10 +132,7 @@ object RobotContainer { autoChooser = LoggedDashboardChooser("Auto Choices", AutoBuilder.buildAutoChooser()) // Set up SysId routines - autoChooser.addOption( - "Drive SysId (Quasistatic Forward)", - drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward) - ) + autoChooser.addOption("Drive SysId (Quasistatic Forward)", drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)) autoChooser.addOption( "Drive SysId (Quasistatic Reverse)", drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) @@ -180,14 +161,26 @@ object RobotContainer { aprilTagVision.setDataInterfaces { visionData: List? -> drive.addVisionData(visionData) } - aprilTagVisionTwo.setDataInterfaces { visionData: List? -> - drive.addVisionData(visionData) - } + driveController.setPoseSupplier { drive.pose } driveController.disableHeadingControl() configureButtonBindings() } + fun unlockAllMotors() { + drive.setBrakeMode(false) + + ArmSubsystem.setBrakeMode(false) + ElevatorSubsystem.setBrakeMode(false) + } + + fun lockAllMotors() { + drive.setBrakeMode(true) + + ArmSubsystem.setBrakeMode(true) + ElevatorSubsystem.setBrakeMode(true) + } + /** * Use this method to define your button->command mappings. Buttons can be created by * instantiating a [GenericHID] or one of its subclasses ([ ] or [XboxController]), and then passing it to a [ ]. @@ -206,55 +199,56 @@ object RobotContainer { ElevatorSubsystem.defaultCommand = ElevatorPositionCommand { ElevatorSubsystem.desiredPosition } WristSubsystem.defaultCommand = WristPositionCommand { WristSubsystem.desiredPosition } + controller.L1().onTrue(AmpScoreCommandGroup()) + controller.L1().onFalse(AmpScoreHomeCommandGroup()) - controller.L1().whileTrue(AmpScoring()) - controller.R2().whileTrue(SourceIntake()) -// -// controller.square().whileTrue(ArmPositionCommand { -70.0 }) -// controller.circle().whileTrue(ArmPositionCommand { 0.0 }) + controller.L2().onTrue(IntakeCommandGroup()) + controller.L2().onFalse(IntakeHomeCommandGroup()) -// controller.povUp().whileTrue(ElevatorPositionCommand { 0.0 }) -// controller.povDown().whileTrue(ElevatorPositionCommand { -48.0 }) -// -// controller.povLeft().whileTrue(WristPositionCommand { -125.0 }) -// controller.povRight().whileTrue(WristPositionCommand { 0.0 }) + controller.R2().whileTrue(AutoShootCommand(driveController) { drive.pose }) -// controller.cross().whileTrue(Commands.startEnd({ ShooterSubsystem.intake() }, { ShooterSubsystem.stopIntake()}, ShooterSubsystem )) -// controller.circle().whileTrue(Commands.startEnd({ShooterSubsystem.shoot(3000.0, 3000.0)}, {ShooterSubsystem.stopShooter()}, ShooterSubsystem)) + controller.square().whileTrue(IntakeCommand()) + controller.triangle().whileTrue(ReverseIntakeCommand()) - controller.cross().onTrue(SuperStructure.ampCommand) - controller.cross().onFalse(SuperStructure.ampHomingCommand) - controller.L2().onTrue(SuperStructure.intakeCommand) - controller.L2().onFalse(SuperStructure.homeCommandFromIntake) - controller.square().onTrue(Commands.runOnce({ ShooterSubsystem.intake() }, ShooterSubsystem)) - controller.square().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem)) + controller.povUp().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition--})) + controller.povDown().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition++})) - controller.triangle().onTrue(Commands.runOnce({ ShooterSubsystem.reverseIntake() }, ShooterSubsystem)) - controller.triangle().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem)) + xbox.a().onTrue(FullHomeCommandGroup()) + xbox.x().onTrue(ExtendClimberCommandGroup()) - controller.R1().whileTrue(AutoAimCommand(driveController) { drive.pose }) - - controller.povUp().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition++})) - controller.povDown().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition--})) - - - controller.povLeft().onTrue(Commands.runOnce({ ShooterSubsystem.shoot() }, ShooterSubsystem)) - controller.povLeft().onFalse(Commands.runOnce({ ShooterSubsystem.stopShooter()}, ShooterSubsystem)) - - - xbox.povUp().onTrue(Commands.runOnce({ ShooterSubsystem.intake() }, ShooterSubsystem)) - xbox.povUp().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake()}, ShooterSubsystem)) - - xbox.povDown().onTrue(Commands.runOnce({ ShooterSubsystem.reverseIntake() }, ShooterSubsystem)) - xbox.povDown().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake()}, ShooterSubsystem)) - - xbox.povLeft().onTrue(ElevatorPositionCommand { ElevatorSubsystem.desiredPosition + 5 }) - xbox.povRight().onTrue(ElevatorPositionCommand { ElevatorSubsystem.desiredPosition - 5}) - - xbox.leftBumper().onTrue(ArmPositionCommand { -70.0 }) - xbox.rightBumper().onTrue(ArmPositionCommand { -0.0 }) +// +// controller.cross().onTrue(SuperStructure.ampCommand) +// controller.cross().onFalse(SuperStructure.ampHomingCommand) +// +// controller.L2().onTrue(SuperStructure.intakeCommand) +// controller.L2().onFalse(SuperStructure.homeCommandFromIntake) +// +// controller.square().onTrue(Commands.runOnce({ ShooterSubsystem.intake() }, ShooterSubsystem)) +// controller.square().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem)) +// +// controller.triangle().onTrue(Commands.runOnce({ ShooterSubsystem.reverseIntake() }, ShooterSubsystem)) +// controller.triangle().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem)) +// +// controller.R1().whileTrue(AutoAimCommand(driveController) { drive.pose }) +// +// +// controller.povLeft().onTrue(Commands.runOnce({ ShooterSubsystem.shoot() }, ShooterSubsystem)) +// controller.povLeft().onFalse(Commands.runOnce({ ShooterSubsystem.stopShooter()}, ShooterSubsystem)) +// +// +// xbox.povUp().onTrue(Commands.runOnce({ ShooterSubsystem.intake() }, ShooterSubsystem)) +// xbox.povUp().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake()}, ShooterSubsystem)) +// +// xbox.povDown().onTrue(Commands.runOnce({ ShooterSubsystem.reverseIntake() }, ShooterSubsystem)) +// xbox.povDown().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake()}, ShooterSubsystem)) +// +// xbox.povLeft().onTrue(ElevatorPositionCommand { ElevatorSubsystem.desiredPosition + 5 }) +// xbox.povRight().onTrue(ElevatorPositionCommand { ElevatorSubsystem.desiredPosition - 5}) +// +// xbox.leftBumper().onTrue(ArmPositionCommand { -70.0 }) +// xbox.rightBumper().onTrue(ArmPositionCommand { -0.0 }) // controller.R2().whileTrue( // Commands.startEnd( @@ -305,20 +299,6 @@ object RobotContainer { // ) // ) - // controller - // .b() - // .onTrue( - // Commands.runOnce( - // () -> - // drive.setPose( - // new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - // drive) - // .ignoringDisable(true)); - // controller - // .a() - // .whileTrue( - // Commands.startEnd( - // () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)); } val autonomousCommand: Command diff --git a/src/main/java/org/rambots/auto/AutoConstants.kt b/src/main/java/org/rambots/auto/AutoConstants.kt index 11086a2..cbe8670 100644 --- a/src/main/java/org/rambots/auto/AutoConstants.kt +++ b/src/main/java/org/rambots/auto/AutoConstants.kt @@ -25,7 +25,7 @@ object AutoConstants { AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0))) ) - const val CLOSEST_AUTOAIM_DISTANCE = 1.5 + const val CLOSEST_AUTOAIM_DISTANCE = 1.6 const val FURTHEST_AUTOAIM_DISTANCE = 6.0 } \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/AmpScoring.kt b/src/main/java/org/rambots/commands/AmpScoring.kt deleted file mode 100644 index 776c7d1..0000000 --- a/src/main/java/org/rambots/commands/AmpScoring.kt +++ /dev/null @@ -1,31 +0,0 @@ -package org.rambots.commands - -import com.pathplanner.lib.auto.AutoBuilder -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands -import org.rambots.auto.AutoConstants -import org.rambots.subsystems.ShooterSubsystem -import org.rambots.subsystems.SuperStructure - -class AmpScoring: Command() { - - private val pathRun get() = AutoBuilder.pathfindToPose(AutoConstants.ampScorePose, AutoConstants.pathConstraints) - private var scoreCommand = Commands.sequence(pathRun) - - override fun initialize() { - super.initialize() - scoreCommand = Commands.sequence(pathRun) - scoreCommand.schedule() - } - - override fun end(interrupted: Boolean) { - if(!interrupted) SuperStructure.ampCommand.schedule() - super.end(interrupted) - scoreCommand.cancel() - } - - override fun isFinished(): Boolean { - return scoreCommand.isFinished - } - -} \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/AutoAimCommand.kt b/src/main/java/org/rambots/commands/AutoAimCommand.kt index 57fc84b..eb71391 100644 --- a/src/main/java/org/rambots/commands/AutoAimCommand.kt +++ b/src/main/java/org/rambots/commands/AutoAimCommand.kt @@ -5,11 +5,11 @@ import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap import edu.wpi.first.wpilibj2.command.Command import org.rambots.auto.AutoConstants.CLOSEST_AUTOAIM_DISTANCE import org.rambots.auto.AutoConstants.FURTHEST_AUTOAIM_DISTANCE +import org.rambots.commands.superstructure.primitive.WristPositionCommand import org.rambots.subsystems.AutoAimSubsystem import org.rambots.subsystems.LightingSubsystem -import org.rambots.subsystems.ShooterSubsystem +import org.rambots.subsystems.superstructure.ShooterSubsystem import org.rambots.subsystems.drive.DriveController -import org.rambots.subsystems.lighting.BlinkinPattern import org.rambots.util.AllianceFlipUtil import org.rambots.util.FieldConstants @@ -17,7 +17,7 @@ class AutoAimCommand(private val controller: DriveController, private val pose: private val wristAngle = InterpolatingDoubleTreeMap().apply { - put(CLOSEST_AUTOAIM_DISTANCE, -75.0) + put(CLOSEST_AUTOAIM_DISTANCE, 72.0) put(2.4, -60.0) put(2.5, -58.14) put(3.98, -42.4) @@ -36,11 +36,6 @@ class AutoAimCommand(private val controller: DriveController, private val pose: override fun execute() { val distance = pose.invoke().translation.getDistance(AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.translation)) - if (distance in CLOSEST_AUTOAIM_DISTANCE..FURTHEST_AUTOAIM_DISTANCE) { - LightingSubsystem.set(BlinkinPattern.DARK_GREEN) - } else { - LightingSubsystem.set(BlinkinPattern.RED) - } WristPositionCommand { wristAngle.get(distance) }.schedule() } diff --git a/src/main/java/org/rambots/commands/SourceIntake.kt b/src/main/java/org/rambots/commands/SourceIntake.kt deleted file mode 100644 index bbe0040..0000000 --- a/src/main/java/org/rambots/commands/SourceIntake.kt +++ /dev/null @@ -1,26 +0,0 @@ -package org.rambots.commands - -import com.pathplanner.lib.auto.AutoBuilder -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands -import org.rambots.auto.AutoConstants - -class SourceIntake: Command() { - private val pathRun get() = AutoBuilder.pathfindToPose(AutoConstants.sourceDirectIntake.left, AutoConstants.pathConstraints) - private var scoreCommand = Commands.sequence(pathRun) - - override fun initialize() { - super.initialize() - scoreCommand = Commands.sequence(pathRun) - scoreCommand.schedule() - } - - override fun end(interrupted: Boolean) { - super.end(interrupted) - scoreCommand.cancel() - } - - override fun isFinished(): Boolean { - return scoreCommand.isFinished - } -} \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/WristPositionCommand.kt b/src/main/java/org/rambots/commands/WristPositionCommand.kt deleted file mode 100644 index 499aec4..0000000 --- a/src/main/java/org/rambots/commands/WristPositionCommand.kt +++ /dev/null @@ -1,26 +0,0 @@ -package org.rambots.commands - -import edu.wpi.first.wpilibj2.command.Command -import org.rambots.subsystems.WristSubsystem - -class WristPositionCommand(private val angle: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() { - - constructor(angle: () -> Double) : this(angle, { true }) - - init { - addRequirements(WristSubsystem) - } - - override fun initialize() { - WristSubsystem.desiredPosition = angle.invoke() - } - - override fun execute() { - WristSubsystem.position = angle.invoke() - } - - override fun isFinished(): Boolean { - return finishCondition.invoke(WristSubsystem.position) - } - -} \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt new file mode 100644 index 0000000..585690d --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreCommandGroup.kt @@ -0,0 +1,16 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.rambots.commands.superstructure.primitive.ArmPositionCommand +import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand +import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class AmpScoreCommandGroup : SequentialCommandGroup( + ArmPositionCommand( { 50.0 }, { it > 35.0}), + ElevatorPositionCommand({ 68.0 }, { it > 5.0}), + WristPositionCommand({ 165.0 }, { it > 155.0 }), + Commands.runOnce({ ShooterSubsystem.reverseIntake() }, ShooterSubsystem) +) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt new file mode 100644 index 0000000..3f4dba3 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/AmpScoreHomeCommandGroup.kt @@ -0,0 +1,16 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.rambots.commands.superstructure.primitive.ArmPositionCommand +import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand +import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class AmpScoreHomeCommandGroup : SequentialCommandGroup( + Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem), + WristPositionCommand( { 0.0 }, { it < 10.0 }), + ElevatorPositionCommand ({ 0.0 }, { it < 10.0 }), + ArmPositionCommand ({ 0.0 }, { true }), +) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt b/src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt new file mode 100644 index 0000000..e28a202 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/AutoShootCommand.kt @@ -0,0 +1,49 @@ +package org.rambots.commands.superstructure.actions + +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.auto.AutoConstants +import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.subsystems.drive.DriveController +import org.rambots.subsystems.superstructure.ShooterSubsystem +import org.rambots.subsystems.superstructure.WristSubsystem +import org.rambots.util.AllianceFlipUtil +import org.rambots.util.FieldConstants + +class AutoShootCommand(private val controller: DriveController, private val pose: () -> Pose2d) : Command() { + + init { + addRequirements(WristSubsystem) + } + + private val wristAngle = InterpolatingDoubleTreeMap().apply { + put(AutoConstants.CLOSEST_AUTOAIM_DISTANCE, 68.0) + put(1.9, 66.5) + put(2.5, 63.14) + put(3.98, 42.4) + put(AutoConstants.FURTHEST_AUTOAIM_DISTANCE, 0.0) + } + + override fun initialize() { + controller.enableHeadingControl() + controller.enableSpeakerHeading() + ShooterSubsystem.shoot() + } + + override fun execute() { + val distance = pose.invoke().translation.getDistance(AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.translation)) + WristSubsystem.desiredPosition = wristAngle.get(distance) + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + controller.disableHeadingControl() + ShooterSubsystem.stopShooter() + WristSubsystem.home() + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt new file mode 100644 index 0000000..faef4fb --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/ExtendClimberCommandGroup.kt @@ -0,0 +1,14 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.rambots.commands.superstructure.primitive.ArmPositionCommand +import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand +import org.rambots.commands.superstructure.primitive.WristPositionCommand + +class ExtendClimberCommandGroup : SequentialCommandGroup( + WristPositionCommand { 0.0 }, + ElevatorPositionCommand({ 0.0 }, { it < 5.0 }), + ArmPositionCommand({ 80.0 }, { it > 75.0 }), + ElevatorPositionCommand({ 65.0 }, { it > 60.0 }), +) diff --git a/src/main/java/org/rambots/commands/superstructure/actions/FullHomeCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/FullHomeCommandGroup.kt new file mode 100644 index 0000000..f241b6a --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/FullHomeCommandGroup.kt @@ -0,0 +1,14 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.rambots.commands.superstructure.primitive.ArmPositionCommand +import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand +import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.subsystems.superstructure.WristSubsystem + +class FullHomeCommandGroup : SequentialCommandGroup( + WristPositionCommand({ 0.0 }, { it < 5.0 }), + ElevatorPositionCommand({ 0.0 }, { it < 5.0 }), + ArmPositionCommand({ 0.0 }, { it < 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 new file mode 100644 index 0000000..7a8b82b --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/IntakeCommandGroup.kt @@ -0,0 +1,36 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.rambots.commands.superstructure.primitive.ArmPositionCommand +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 ({ 125.0 }, { true }), + Commands.runOnce ({ + ShooterSubsystem.intake() + }, ShooterSubsystem), + WaitCommand(0.5), + Commands.waitUntil { ShooterSubsystem.intakeStalling }, + Commands.runOnce( { + LightingSubsystem.blinkLL() + SequentialCommandGroup(WaitCommand(5.0), Commands.runOnce( { + LightingSubsystem.clearLL() + })).schedule() + }), + IntakeHomeCommandGroup() +) { + + init { + addRequirements(ArmSubsystem, ElevatorSubsystem, WristSubsystem, LightingSubsystem) + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt b/src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt new file mode 100644 index 0000000..c5cd051 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/actions/IntakeHomeCommandGroup.kt @@ -0,0 +1,17 @@ +package org.rambots.commands.superstructure.actions + + +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.InstantCommand +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand +import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.subsystems.LightingSubsystem +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class IntakeHomeCommandGroup : SequentialCommandGroup( + Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem), + WristPositionCommand({ 0.0 }, { it < 65.0 }), + ElevatorPositionCommand({ 0.0 }, { it < 5.0 }), +) diff --git a/src/main/java/org/rambots/commands/ArmPositionCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/ArmPositionCommand.kt similarity index 66% rename from src/main/java/org/rambots/commands/ArmPositionCommand.kt rename to src/main/java/org/rambots/commands/superstructure/primitive/ArmPositionCommand.kt index 732946a..c7d3058 100644 --- a/src/main/java/org/rambots/commands/ArmPositionCommand.kt +++ b/src/main/java/org/rambots/commands/superstructure/primitive/ArmPositionCommand.kt @@ -1,7 +1,7 @@ -package org.rambots.commands +package org.rambots.commands.superstructure.primitive import edu.wpi.first.wpilibj2.command.Command -import org.rambots.subsystems.ArmSubsystem +import org.rambots.subsystems.superstructure.ArmSubsystem class ArmPositionCommand(private val angle: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() { @@ -11,16 +11,12 @@ class ArmPositionCommand(private val angle: () -> Double, private val finishCond addRequirements(ArmSubsystem) } - override fun initialize() { - ArmSubsystem.desiredPosition = angle.invoke() - } - override fun execute() { - ArmSubsystem.position = angle.invoke() + ArmSubsystem.desiredPosition = angle.invoke() } override fun isFinished(): Boolean { - return finishCondition.invoke(ArmSubsystem.position) + return finishCondition.invoke(ArmSubsystem.currentPosition) } } \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/ElevatorPositionCommand.kt similarity index 55% rename from src/main/java/org/rambots/commands/ElevatorPositionCommand.kt rename to src/main/java/org/rambots/commands/superstructure/primitive/ElevatorPositionCommand.kt index 2f405c1..ebe24aa 100644 --- a/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt +++ b/src/main/java/org/rambots/commands/superstructure/primitive/ElevatorPositionCommand.kt @@ -1,9 +1,12 @@ -package org.rambots.commands +package org.rambots.commands.superstructure.primitive import edu.wpi.first.wpilibj2.command.Command -import org.rambots.subsystems.ElevatorSubsystem +import org.rambots.subsystems.superstructure.ElevatorSubsystem -class ElevatorPositionCommand(private val position: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() { +class ElevatorPositionCommand( + private val position: () -> Double, + private val finishCondition: (position: Double) -> Boolean +) : Command() { constructor(position: () -> Double) : this(position, { true }) @@ -11,12 +14,8 @@ class ElevatorPositionCommand(private val position: () -> Double, private val fi addRequirements(ElevatorSubsystem) } - override fun initialize() { - ElevatorSubsystem.desiredPosition = position.invoke() - } - override fun execute() { - ElevatorSubsystem.position = position.invoke() + ElevatorSubsystem.desiredPosition = position.invoke() } override fun isFinished(): Boolean { diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/IntakeCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/IntakeCommand.kt new file mode 100644 index 0000000..ef61aa8 --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/primitive/IntakeCommand.kt @@ -0,0 +1,20 @@ +package org.rambots.commands.superstructure.primitive + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class IntakeCommand : Command() { + + override fun initialize() { + ShooterSubsystem.intake() + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + ShooterSubsystem.stopIntake() + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt new file mode 100644 index 0000000..f770fce --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/primitive/ReverseIntakeCommand.kt @@ -0,0 +1,20 @@ +package org.rambots.commands.superstructure.primitive + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.superstructure.ShooterSubsystem + +class ReverseIntakeCommand : Command() { + + override fun initialize() { + ShooterSubsystem.reverseIntake() + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + ShooterSubsystem.stopIntake() + } + +} diff --git a/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt b/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt new file mode 100644 index 0000000..b6d958a --- /dev/null +++ b/src/main/java/org/rambots/commands/superstructure/primitive/WristPositionCommand.kt @@ -0,0 +1,36 @@ +package org.rambots.commands.superstructure.primitive + +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.rambots.subsystems.superstructure.WristSubsystem + +class WristPositionCommand( + private val angle: () -> Double, + private val finishCondition: (position: Double) -> Boolean +) : Command() { + + constructor(angle: () -> Double) : this(angle, { true }) + + init { + addRequirements(WristSubsystem) + } + + override fun execute() { + WristSubsystem.desiredPosition = angle.invoke() + } + + override fun end(interrupted: Boolean) { + if (!interrupted && angle.invoke() == 0.0) { + SequentialCommandGroup(WaitCommand(2.0), Commands.runOnce({ + if (WristSubsystem.desiredPosition == 0.0) WristSubsystem.resetEncoder() + })) + } + } + + override fun isFinished(): Boolean { + return finishCondition.invoke(WristSubsystem.position) + } + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/config/ArmConstants.kt b/src/main/java/org/rambots/config/ArmConstants.kt index 9656a70..0b80a8b 100644 --- a/src/main/java/org/rambots/config/ArmConstants.kt +++ b/src/main/java/org/rambots/config/ArmConstants.kt @@ -3,62 +3,10 @@ package org.rambots.config import com.pathplanner.lib.util.PIDConstants object ArmConstants { - /** Arm Configuration */ + const val ARM_NEO_LEADER_ID = 13 const val ARM_NEO_FOLLOWER_ID = 14 - const val ARM_ACCEPTABLE_ERROR = 0.5 - val ARM_PID = PIDConstants(0.02, 0.0, 0.0) - - - - /** Elevator PID */ - const val ElevatorKP = 0.2 - const val ElevatorKI = 0.0 - const val ElevatorKD = 0.0 - - /** Intake Configuration */ - const val IntakeMotorOneCANID = 17 - const val IntakeMotorTwoCANID = 18 - const val IntakePower = -0.2 - const val IntakeCurrent = 20 - - /** Shooter Configuration */ - const val TopShooterCANID = 19 - const val BottomShooterCANID = 20 - - /** Shooter PID */ - const val ShooterKP = 0.00035 - const val ShooterKD = 0.15 - - /** Wrist Configuration */ - const val WristCANID = 21 - - /** Wrist PID */ - const val WristKP = 0.2 - const val WristKI = 0.0 - const val WristKD = 0.0 - - /** Intake Positions */ - const val ArmIntakePosition = 0.0 - const val WristIntakePosition = -63.0 - const val ElevatorIntakePosition = -50.0 - - /** Climb Positions */ - const val ArmClimbPosition = -70.0 - const val ElevatorClimbPosition = -70.0 - const val ElevatorRetractPosition = 0.0 - - /** Manual Arm Controls */ - // place holders - const val MaximumAngle = 90 - const val MinimumAngle = 30 - const val ArmIdlePosition = -45.0 - - /** Limelight */ - const val LimelightMountAngleDegrees = 25.0 - const val LimelightLensHeightInches = 20.0 - const val SpeakerGoalHeightInches = 50.0 } \ No newline at end of file diff --git a/src/main/java/org/rambots/config/ElevatorConstants.kt b/src/main/java/org/rambots/config/ElevatorConstants.kt index fcbac03..51e8384 100644 --- a/src/main/java/org/rambots/config/ElevatorConstants.kt +++ b/src/main/java/org/rambots/config/ElevatorConstants.kt @@ -4,9 +4,11 @@ import com.pathplanner.lib.util.PIDConstants object ElevatorConstants { - const val ELEVATOR_LEADER_ID = 15 - const val ELEVATOR_FOLLOWER_ID = 16 + const val ELEVATOR_LEADER_ID = 16 + const val ELEVATOR_FOLLOWER_ID = 15 val ELEVATOR_PID = PIDConstants(0.2, 0.0, 0.0) + const val ELEVATOR_MAX_HEIGHT = 0.0 + } \ 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 2caf319..2ca0af1 100644 --- a/src/main/java/org/rambots/config/ShooterConstants.kt +++ b/src/main/java/org/rambots/config/ShooterConstants.kt @@ -1,22 +1,15 @@ package org.rambots.config -import com.pathplanner.lib.util.PIDConstants - object ShooterConstants { const val SHOOTER_TOP_ID = 19 const val SHOOTER_BOTTOM_ID = 20 - val SHOOTER_PID = PIDConstants(0.00085, 0.0000011, 0.1) - - const val SHOOTER_DEFAULT_VELOCITY = 500.0 - const val INTAKE_BOTTOM_ID = 17 const val INTAKE_TOP_ID = 18 const val INTAKE_OUTPUT = 0.45 - const val INTAKE_STALL_CURRENT = 30 + const val INTAKE_SPIKE_LIMIT = 30 - const val VELOCITY_ERROR = 100 } \ No newline at end of file diff --git a/src/main/java/org/rambots/config/WristConstants.kt b/src/main/java/org/rambots/config/WristConstants.kt index 51e96d4..8bca40d 100644 --- a/src/main/java/org/rambots/config/WristConstants.kt +++ b/src/main/java/org/rambots/config/WristConstants.kt @@ -8,6 +8,6 @@ object WristConstants { val WRIST_PID = PIDConstants(0.07, 0.0, 0.0) - const val WRIST_INTAKE_POSITION = -116.0 + const val ABSOLUTE_WRIST_OFFSET = 78 } \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/WristSubsystem.kt b/src/main/java/org/rambots/subsystems/WristSubsystem.kt deleted file mode 100644 index dd9e0db..0000000 --- a/src/main/java/org/rambots/subsystems/WristSubsystem.kt +++ /dev/null @@ -1,54 +0,0 @@ -package org.rambots.subsystems - -import com.revrobotics.CANSparkBase -import com.revrobotics.CANSparkLowLevel -import com.revrobotics.CANSparkMax -import edu.wpi.first.wpilibj.DigitalInput -import edu.wpi.first.wpilibj2.command.Commands -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger -import org.rambots.Robot -import org.rambots.config.ArmConstants.ARM_PID -import org.rambots.config.WristConstants.WRIST_ID -import org.rambots.config.WristConstants.WRIST_PID -import org.rambots.util.AllianceFlipUtil -import org.rambots.util.FieldConstants - -object WristSubsystem : SubsystemBase() { - - var offset = 0.0 - var desiredPosition = 0.0 + offset - var hasBeenHomed = false - - var position - get() = motor.encoder.position - set(value) { - motor.pidController.setReference(value, CANSparkBase.ControlType.kPosition) - } - - private val motor = CANSparkMax(WRIST_ID, CANSparkLowLevel.MotorType.kBrushless).apply { - restoreFactoryDefaults() - - idleMode = CANSparkBase.IdleMode.kBrake - pidController.apply { - p = WRIST_PID.kP - i = WRIST_PID.kI - d = WRIST_PID.kD - } - - setSmartCurrentLimit(40) - } - - private fun stop() { - motor.stopMotor() - } - - override fun periodic() { - Logger.recordOutput("Wrist/DesiredPosition", desiredPosition) - Logger.recordOutput("Wrist/Offset", offset) - Logger.recordOutput("Wrist/Position", motor.encoder.position) - Logger.recordOutput("Wrist/Velocity", motor.encoder.velocity) - Logger.recordOutput("Wrist/Current", motor.outputCurrent) - } - -} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/drive/Drive.java b/src/main/java/org/rambots/subsystems/drive/Drive.java index 2d32656..6988821 100644 --- a/src/main/java/org/rambots/subsystems/drive/Drive.java +++ b/src/main/java/org/rambots/subsystems/drive/Drive.java @@ -359,4 +359,10 @@ public void addVisionData(List visionData addVisionMeasurement( visionUpdate.pose(), visionUpdate.timestamp(), visionUpdate.stdDevs())); } + + public void setBrakeMode(boolean brakeMode) { + for (var module : modules) { + module.setBrakeMode(brakeMode); + } + } } diff --git a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java index e270a7c..d706165 100644 --- a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java +++ b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java @@ -27,7 +27,7 @@ public final class DriveConstants { }; public static final double xyStdDevCoefficient = switch (Constants.getRobot()) { - default -> 0.01; + default -> 0.005; }; public static final double thetaStdDevCoefficient = switch (Constants.getRobot()) { @@ -88,22 +88,10 @@ public final class DriveConstants { public static ModuleConfig[] moduleConfigs = switch (Constants.getRobot()) { case COMPBOT -> new ModuleConfig[]{ - new ModuleConfig( - 1, 2, 9, Rotation2d.fromRotations(-.247).plus(Rotation2d.fromDegrees(180)), true), - new ModuleConfig( - 3, - 4, - 10, - Rotation2d.fromRotations(-.188).plus(Rotation2d.fromDegrees(180)), - true), - new ModuleConfig( - 5, - 6, - 11, - Rotation2d.fromRotations(-.339).plus(Rotation2d.fromDegrees(180)), - true), - new ModuleConfig( - 7, 8, 12, Rotation2d.fromRotations(.334).plus(Rotation2d.fromDegrees(180)), true) + new ModuleConfig(1, 2, 9, Rotation2d.fromRotations(.223).plus(Rotation2d.fromDegrees(180)), true), + new ModuleConfig(3, 4, 10, Rotation2d.fromRotations(-.039).plus(Rotation2d.fromDegrees(180)), true), + new ModuleConfig(5, 6, 11, Rotation2d.fromRotations(-.394).plus(Rotation2d.fromDegrees(180)), true), + new ModuleConfig(7, 8, 12, Rotation2d.fromRotations(-0.15).plus(Rotation2d.fromDegrees(180)), true) }; case SIMBOT -> { ModuleConfig[] configs = new ModuleConfig[4]; @@ -114,8 +102,8 @@ public final class DriveConstants { }; public static HeadingControllerConstants headingControllerConstants = switch (Constants.getRobot()) { - case COMPBOT -> new HeadingControllerConstants(3.0, 0.0); - case SIMBOT -> new HeadingControllerConstants(3.0, 0.0); + case COMPBOT -> new HeadingControllerConstants(1.7, 0.0); + case SIMBOT -> new HeadingControllerConstants(2.0, 0.0); }; private enum SwerveXReductions { diff --git a/src/main/java/org/rambots/subsystems/flywheel/Flywheel.java b/src/main/java/org/rambots/subsystems/flywheel/Flywheel.java deleted file mode 100644 index 51ed8b7..0000000 --- a/src/main/java/org/rambots/subsystems/flywheel/Flywheel.java +++ /dev/null @@ -1,126 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.flywheel; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.rambots.Constants; - -import static edu.wpi.first.units.Units.Volts; - -public class Flywheel extends SubsystemBase { - private final FlywheelIO io; - private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; - private final SysIdRoutine sysId; - - /** - * Creates a new Flywheel. - */ - public Flywheel(FlywheelIO io) { - this.io = io; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.getMode()) { - case REAL: - case REPLAY: - ffModel = new SimpleMotorFeedforward(0.1, 0.05); - io.configurePID(1.0, 0.0, 0.0); - break; - case SIM: - ffModel = new SimpleMotorFeedforward(0.0, 0.03); - io.configurePID(0.5, 0.0, 0.0); - break; - default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); - break; - } - - // Configure SysId - sysId = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - null, - null, - (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), - new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Flywheel", inputs); - } - - /** - * Run open loop at the specified voltage. - */ - public void runVolts(double volts) { - io.setVoltage(volts); - } - - /** - * Run closed loop at the specified velocity. - */ - public void runVelocity(double velocityRPM) { - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); - - // Log flywheel setpoint - Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); - } - - /** - * Stops the flywheel. - */ - public void stop() { - io.stop(); - } - - /** - * Returns a command to run a quasistatic test in the specified direction. - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); - } - - /** - * Returns a command to run a dynamic test in the specified direction. - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } - - /** - * Returns the current velocity in RPM. - */ - @AutoLogOutput - public double getVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); - } - - /** - * Returns the current velocity in radians per second. - */ - public double getCharacterizationVelocity() { - return inputs.velocityRadPerSec; - } -} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIO.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIO.java deleted file mode 100644 index 56c5d66..0000000 --- a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIO.java +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.flywheel; - -import org.littletonrobotics.junction.AutoLog; - -public interface FlywheelIO { - /** - * Updates the set of loggable inputs. - */ - public default void updateInputs(FlywheelIOInputs inputs) { - } - - /** - * Run open loop at the specified voltage. - */ - public default void setVoltage(double volts) { - } - - /** - * Run closed loop at the specified velocity. - */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) { - } - - /** - * Stop in open loop. - */ - public default void stop() { - } - - /** - * Set velocity PID constants. - */ - public default void configurePID(double kP, double kI, double kD) { - } - - @AutoLog - public static class FlywheelIOInputs { - public double positionRad = 0.0; - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; - public double[] currentAmps = new double[]{}; - } -} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSim.java deleted file mode 100644 index f242885..0000000 --- a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSim.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.flywheel; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; - -public class FlywheelIOSim implements FlywheelIO { - private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); - private PIDController pid = new PIDController(0.0, 0.0, 0.0); - - private boolean closedLoop = false; - private double ffVolts = 0.0; - private double appliedVolts = 0.0; - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - if (closedLoop) { - appliedVolts = - MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); - sim.setInputVoltage(appliedVolts); - } - - sim.update(0.02); - - inputs.positionRad = 0.0; - inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; - inputs.currentAmps = new double[]{sim.getCurrentDrawAmps()}; - } - - @Override - public void setVoltage(double volts) { - closedLoop = false; - appliedVolts = volts; - sim.setInputVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - closedLoop = true; - pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; - } - - @Override - public void stop() { - setVoltage(0.0); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } -} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSparkMax.java deleted file mode 100644 index 477b603..0000000 --- a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOSparkMax.java +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.flywheel; - -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.ArbFFUnits; -import edu.wpi.first.math.util.Units; - -/** - * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with - * "CANSparkFlex". - */ -public class FlywheelIOSparkMax implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - - private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); - private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkPIDController pid = leader.getPIDController(); - - public FlywheelIOSparkMax() { - leader.restoreFactoryDefaults(); - follower.restoreFactoryDefaults(); - - leader.setCANTimeout(250); - follower.setCANTimeout(250); - - leader.setInverted(false); - follower.follow(leader, false); - - leader.enableVoltageCompensation(12.0); - leader.setSmartCurrentLimit(30); - - leader.burnFlash(); - follower.burnFlash(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); - inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); - inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.currentAmps = new double[]{leader.getOutputCurrent(), follower.getOutputCurrent()}; - } - - @Override - public void setVoltage(double volts) { - leader.setVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - pid.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, - ControlType.kVelocity, - 0, - ffVolts, - ArbFFUnits.kVoltage); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setP(kP, 0); - pid.setI(kI, 0); - pid.setD(kD, 0); - pid.setFF(0, 0); - } -} diff --git a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOTalonFX.java deleted file mode 100644 index 8c5084b..0000000 --- a/src/main/java/org/rambots/subsystems/flywheel/FlywheelIOTalonFX.java +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.flywheel; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; - -public class FlywheelIOTalonFX implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - - private final TalonFX leader = new TalonFX(0); - private final TalonFX follower = new TalonFX(1); - - private final StatusSignal leaderPosition = leader.getPosition(); - private final StatusSignal leaderVelocity = leader.getVelocity(); - private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); - private final StatusSignal leaderCurrent = leader.getStatorCurrent(); - private final StatusSignal followerCurrent = follower.getStatorCurrent(); - - public FlywheelIOTalonFX() { - var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 30.0; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; - leader.getConfigurator().apply(config); - follower.getConfigurator().apply(config); - follower.setControl(new Follower(leader.getDeviceID(), false)); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - leader.optimizeBusUtilization(); - follower.optimizeBusUtilization(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll( - leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / GEAR_RATIO; - inputs.velocityRadPerSec = - Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; - inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); - inputs.currentAmps = - new double[]{leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; - } - - @Override - public void setVoltage(double volts) { - leader.setControl(new VoltageOut(volts)); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - leader.setControl( - new VelocityVoltage( - Units.radiansToRotations(velocityRadPerSec), - 0.0, - true, - ffVolts, - 0, - false, - false, - false)); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - var config = new Slot0Configs(); - config.kP = kP; - config.kI = kI; - config.kD = kD; - leader.getConfigurator().apply(config); - } -} diff --git a/src/main/java/org/rambots/subsystems/lighting/Blinkin.kt b/src/main/java/org/rambots/subsystems/lighting/Blinkin.kt deleted file mode 100644 index 9527806..0000000 --- a/src/main/java/org/rambots/subsystems/lighting/Blinkin.kt +++ /dev/null @@ -1,12 +0,0 @@ -package org.rambots.subsystems.lighting - -import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj.motorcontrol.Spark - -class Blinkin(channel: Int) : Spark(channel) { - - fun set(pattern: BlinkinPattern) { - super.set(pattern.pwmPower) - } - -} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/lighting/BlinkinPattern.kt b/src/main/java/org/rambots/subsystems/lighting/BlinkinPattern.kt deleted file mode 100644 index 235b4cb..0000000 --- a/src/main/java/org/rambots/subsystems/lighting/BlinkinPattern.kt +++ /dev/null @@ -1,105 +0,0 @@ -package org.rambots.subsystems.lighting -enum class BlinkinPattern(val pwmPower: Double) { - - RAINBOW(-0.99), - PARTY_RAINBOW(-0.97), - OCEAN_RAINBOW(-0.95), - LAVA_RAINBOW(-0.93), - FOREST_RAINBOW(-0.91), - RAINBOW_GLITTER(-0.89), - CONFETTI(-0.87), - SHOT_RED(-0.85), - SHOT_BLUE(-0.83), - SHOT_WHITE(-0.81), - SINE_RAINBOW(-0.79), - SINE_PARTY(-0.77), - SINE_OCEAN(-0.75), - SINE_LAVA(-0.73), - SINE_FOREST(-0.71), - BPM_RAINBOW(-0.69), - BPM_PARTY(-0.67), - BPM_OCEAN(-0.65), - BPM_LAVA(-0.63), - BPM_FOREST(-0.61), - FIRE_MEDIUM(-0.59), - FIRE_LARGE(-0.57), - TWINKLES_RAINBOW(-0.55), - TWINKLES_PARTY(-0.53), - TWINKLES_OCEAN(-0.51), - TWINKLES_LAVA(-0.49), - TWINKLES_FOREST(-0.47), - COLOR_WAVES_RAINBOW(-0.45), - COLOR_WAVES_PARTY(-0.43), - COLOR_WAVES_OCEAN(-0.41), - COLOR_WAVES_LAVA(-0.39), - COLOR_WAVES_FOREST(-0.37), - LARSON_RED(-0.35), - LARSON_GRAY(-0.33), - LIGHT_CHASE_RED(-0.31), - LIGHT_CHASE_BLUE(-0.29), - LIGHT_CHASE_GRAY(-0.27), - HEARTBEAT_RED(-0.25), - HEARTBEAT_BLUE(-0.23), - HEARTBEAT_WHITE(-0.21), - HEARTBEAT_GRAY(-0.19), - BREATH_RED(-0.17), - BREATH_BLUE(-0.15), - BREATH_GRAY(-0.13), - STROBE_RED(-0.11), - STROBE_BLUE(-0.09), - STROBE_GOLD(-0.07), - STROBE_WHITE(-0.05), - END_TO_END_BLEND(-0.03), - LARSON_SCANNER(-0.01), - OFF(0.0), - LIGHT_CHASE(0.01), - HEARTBEAT_SLOW(0.03), - HEARTBEAT_MEDIUM(0.05), - HEARTBEAT_FAST(0.07), - BREATH_SLOW(0.09), - BREATH_FAST(0.11), - SHOT(0.13), - STROBE(0.15), - END_TO_END_BLEND_BlACK(0.17), - LARSON_SCANNER2(0.19), - LIGHT_CHASE2(0.21), - HEARTBEAT_SLOW2(0.23), - HEARTBEAT_MEDIUM2(0.25), - HEARTBEAT_FAST2(0.27), - BREATH_SLOW2(0.29), - BREATH_FAST2(0.31), - SHOT2(0.33), - STROBE2(0.35), - SPARKLE_COLOR1_ON_COLOR2(0.37), - SPARKLE_COLOR2_ON_COLOR1(0.39), - COLOR_GRADIENT(0.41), - BPM_COLOR1_AND_COLOR2(0.43), - END_TO_END_BLEND_COLOR1_TO_2(0.45), - END_TO_END_BLEND2(0.47), - COLOR1_AND_COLOR2_NO_BLEND(0.49), - TWINKLES_COLOR1_AND_COLOR2(0.51), - COLOR_WAVES_COLOR1_AND_COLOR2(0.53), - SINE_COLOR1_AND_COLOR2(0.55), - HOT_PINK(0.57), - DARK_RED(0.59), - RED(0.61), - RED_ORANGE(0.63), - ORANGE(0.65), - GOLD(0.67), - YELLOW(0.69), - LAWN_GREEN(0.71), - LIME(0.73), - DARK_GREEN(0.75), - GREEN(0.77), - BLUE_GREEN(0.79), - AQUA(0.81), - SKY_BLUE(0.83), - DARK_BLUE(0.85), - BLUE(0.87), - BLUE_VIOLET(0.89), - VIOLET(0.91), - WHITE(0.93), - GRAY(0.95), - DARK_GRAY(0.97), - BLACK(0.99), -} diff --git a/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt b/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt index df9faf0..ab3fff3 100644 --- a/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/lighting/LightingSubsystem.kt @@ -2,21 +2,38 @@ package org.rambots.subsystems import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger -import org.rambots.subsystems.lighting.Blinkin -import org.rambots.subsystems.lighting.BlinkinPattern +import org.rambots.subsystems.lighting.LimelightLEDState +import org.rambots.util.LimelightHelpers object LightingSubsystem : SubsystemBase() { - private val blinkin = Blinkin(4) - private var pattern = BlinkinPattern.OFF + private var limelightLEDState = LimelightLEDState.LIMELIGHT_OFF + + private const val limelightName = "limelight-three" + + fun blindLL() { // "Blindlight" + limelightLEDState = LimelightLEDState.LIMELIGHT_ON + LimelightHelpers.setLEDMode_ForceOn(limelightName) + } + + fun blinkLL() { + limelightLEDState = LimelightLEDState.LIMELIGHT_BLINK + LimelightHelpers.setLEDMode_ForceBlink(limelightName) + } + + fun clearLL() { + limelightLEDState = LimelightLEDState.LIMELIGHT_OFF + LimelightHelpers.setLEDMode_ForceOff(limelightName) + } + + fun power(double: Double) { - fun set(bp: BlinkinPattern) { - blinkin.set(bp) - pattern = bp } override fun periodic() { - Logger.recordOutput("Lighting/Pattern", pattern.name) + + Logger.recordOutput("Lighting/LimelightLEDState", limelightLEDState.name) + } } \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/lighting/LimelightLEDState.kt b/src/main/java/org/rambots/subsystems/lighting/LimelightLEDState.kt new file mode 100644 index 0000000..27639c9 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/lighting/LimelightLEDState.kt @@ -0,0 +1,9 @@ +package org.rambots.subsystems.lighting + +enum class LimelightLEDState { + + LIMELIGHT_ON, + LIMELIGHT_OFF, + LIMELIGHT_BLINK + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/ArmSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/ArmSubsystem.kt similarity index 54% rename from src/main/java/org/rambots/subsystems/ArmSubsystem.kt rename to src/main/java/org/rambots/subsystems/superstructure/ArmSubsystem.kt index d56a7ff..6b42631 100644 --- a/src/main/java/org/rambots/subsystems/ArmSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/ArmSubsystem.kt @@ -1,34 +1,33 @@ -package org.rambots.subsystems +package org.rambots.subsystems.superstructure import com.revrobotics.CANSparkBase import com.revrobotics.CANSparkLowLevel import com.revrobotics.CANSparkMax -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.motorcontrol.Spark +import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.rambots.config.ArmConstants.ARM_NEO_FOLLOWER_ID import org.rambots.config.ArmConstants.ARM_NEO_LEADER_ID import org.rambots.config.ArmConstants.ARM_PID +import org.rambots.util.SparkMaxUtil object ArmSubsystem : SubsystemBase() { var offset = 0.0 - var desiredPosition = 0.0 + offset - var position - get() = leader.encoder.position - set(value) { - leader.pidController.setReference(value, CANSparkBase.ControlType.kPosition) - } + var desiredPosition = 0.0 + + val currentPosition get() = leader.encoder.position private val leader = CANSparkMax(ARM_NEO_LEADER_ID, CANSparkLowLevel.MotorType.kBrushless).apply { restoreFactoryDefaults() - - idleMode = CANSparkBase.IdleMode.kBrake + pidController.apply { p = ARM_PID.kP i = ARM_PID.kI d = ARM_PID.kD } + idleMode = CANSparkBase.IdleMode.kBrake setSmartCurrentLimit(40) } @@ -44,11 +43,23 @@ object ArmSubsystem : SubsystemBase() { } override fun periodic() { + leader.pidController.setReference(desiredPosition + offset, CANSparkBase.ControlType.kPosition) + Logger.recordOutput("Arm/DesiredPosition", desiredPosition) Logger.recordOutput("Arm/Offset", offset) - Logger.recordOutput("Arm/Position", leader.encoder.position) - Logger.recordOutput("Arm/Velocity", leader.encoder.velocity) - Logger.recordOutput("Arm/Current", leader.outputCurrent) + + Logger.recordOutput("Arm/Leader/Position", leader.encoder.position) + Logger.recordOutput("Arm/Leader/Velocity", leader.encoder.velocity) + Logger.recordOutput("Arm/Leader/Current", leader.outputCurrent) + + Logger.recordOutput("Arm/Follower/Position", follower.encoder.position) + Logger.recordOutput("Arm/Follower/Velocity", follower.encoder.velocity) + Logger.recordOutput("Arm/Follower/Current", follower.outputCurrent) + } + + fun setBrakeMode(brake: Boolean) { + leader.idleMode = SparkMaxUtil.getBrake(brake) + follower.idleMode = SparkMaxUtil.getBrake(brake) } } \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/ElevatorSubsystem.kt similarity index 80% rename from src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt rename to src/main/java/org/rambots/subsystems/superstructure/ElevatorSubsystem.kt index 6215115..a2d04a9 100644 --- a/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/ElevatorSubsystem.kt @@ -1,37 +1,32 @@ -package org.rambots.subsystems +package org.rambots.subsystems.superstructure import com.revrobotics.CANSparkBase import com.revrobotics.CANSparkLowLevel import com.revrobotics.CANSparkMax import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger -import org.rambots.config.ArmConstants.ARM_NEO_FOLLOWER_ID -import org.rambots.config.ArmConstants.ARM_NEO_LEADER_ID -import org.rambots.config.ArmConstants.ARM_PID import org.rambots.config.ElevatorConstants.ELEVATOR_FOLLOWER_ID import org.rambots.config.ElevatorConstants.ELEVATOR_LEADER_ID import org.rambots.config.ElevatorConstants.ELEVATOR_PID +import org.rambots.util.SparkMaxUtil object ElevatorSubsystem : SubsystemBase() { var offset = 0.0 - var desiredPosition = 0.0 + offset - var position - get() = leader.encoder.position - set(value) { - leader.pidController.setReference(value, CANSparkBase.ControlType.kPosition) - } + var desiredPosition = 0.0 + + val position get() = leader.encoder.position private val leader = CANSparkMax(ELEVATOR_LEADER_ID, CANSparkLowLevel.MotorType.kBrushless).apply { restoreFactoryDefaults() - - idleMode = CANSparkBase.IdleMode.kBrake + pidController.apply { p = ELEVATOR_PID.kP i = ELEVATOR_PID.kI d = ELEVATOR_PID.kD } + idleMode = CANSparkBase.IdleMode.kBrake setSmartCurrentLimit(40) } @@ -46,10 +41,12 @@ object ElevatorSubsystem : SubsystemBase() { follower.follow(leader, true) } - override fun periodic() { + leader.pidController.setReference(desiredPosition + offset, CANSparkBase.ControlType.kPosition) + Logger.recordOutput("Elevator/DesiredPosition", desiredPosition) Logger.recordOutput("Elevator/Offset", offset) + Logger.recordOutput("Elevator/Leader/Position", leader.encoder.position) Logger.recordOutput("Elevator/Leader/Velocity", leader.encoder.velocity) Logger.recordOutput("Elevator/Leader/Current", leader.outputCurrent) @@ -59,4 +56,9 @@ object ElevatorSubsystem : SubsystemBase() { Logger.recordOutput("Elevator/Follower/Current", follower.outputCurrent) } + fun setBrakeMode(brake: Boolean) { + leader.idleMode = SparkMaxUtil.getBrake(brake) + follower.idleMode = SparkMaxUtil.getBrake(brake) + } + } \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/ShooterSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt similarity index 91% rename from src/main/java/org/rambots/subsystems/ShooterSubsystem.kt rename to src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt index e612fd2..6a45cb1 100644 --- a/src/main/java/org/rambots/subsystems/ShooterSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/ShooterSubsystem.kt @@ -1,19 +1,16 @@ -package org.rambots.subsystems +package org.rambots.subsystems.superstructure import com.revrobotics.CANSparkBase import com.revrobotics.CANSparkLowLevel import com.revrobotics.CANSparkMax -import com.revrobotics.SparkMaxPIDController import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger 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_DEFAULT_VELOCITY -import org.rambots.config.ShooterConstants.SHOOTER_PID import org.rambots.config.ShooterConstants.SHOOTER_TOP_ID -import org.rambots.config.ShooterConstants.INTAKE_STALL_CURRENT +import org.rambots.config.ShooterConstants.INTAKE_SPIKE_LIMIT object ShooterSubsystem : SubsystemBase() { @@ -48,7 +45,7 @@ object ShooterSubsystem : SubsystemBase() { private val intakeCurrent get() = intakeTopLead.outputCurrent - val intakeStalling get() = intakeCurrent > INTAKE_STALL_CURRENT + val intakeStalling get() = intakeCurrent > INTAKE_SPIKE_LIMIT init { intakeBottomFollower.follow(intakeTopLead, false) diff --git a/src/main/java/org/rambots/subsystems/SuperStructure.kt b/src/main/java/org/rambots/subsystems/superstructure/SuperStructure.kt similarity index 84% rename from src/main/java/org/rambots/subsystems/SuperStructure.kt rename to src/main/java/org/rambots/subsystems/superstructure/SuperStructure.kt index c9232ff..d6ef4b5 100644 --- a/src/main/java/org/rambots/subsystems/SuperStructure.kt +++ b/src/main/java/org/rambots/subsystems/superstructure/SuperStructure.kt @@ -1,21 +1,19 @@ -package org.rambots.subsystems +package org.rambots.subsystems.superstructure import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.Commands.waitUntil -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.WaitCommand -import org.rambots.RobotContainer -import org.rambots.commands.* -import org.rambots.subsystems.drive.Drive -import org.rambots.subsystems.lighting.BlinkinPattern +import org.rambots.commands.superstructure.primitive.ArmPositionCommand +import org.rambots.commands.superstructure.primitive.ElevatorPositionCommand +import org.rambots.commands.superstructure.primitive.WristPositionCommand +import org.rambots.subsystems.LightingSubsystem object SuperStructure { val homeCommandFromIntake get() = SequentialCommandGroup( Commands.runOnce({ ShooterSubsystem.stopIntake() - LightingSubsystem.set(BlinkinPattern.GREEN) }, ShooterSubsystem), WristPositionCommand({ 0.0 }, { it > -55.0 }), ElevatorPositionCommand({ 0.0 }, { it > -5.0 }) @@ -27,7 +25,6 @@ object SuperStructure { WristPositionCommand ({ -125.0 }, { true }), Commands.runOnce ({ ShooterSubsystem.intake() - LightingSubsystem.set(BlinkinPattern.OFF) }, ShooterSubsystem), WaitCommand(0.5), waitUntil { ShooterSubsystem.intakeStalling }, @@ -42,7 +39,7 @@ object SuperStructure { ) val ampHomingCommand get() = SequentialCommandGroup( - Commands.runOnce({ ShooterSubsystem.stopIntake()}, ShooterSubsystem), + Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem), WristPositionCommand( { 0.0 }, { it > -10.0 }), ElevatorPositionCommand ({ 0.0 }, { it > -10.0 }), ArmPositionCommand ({ 0.0 }, { true }), diff --git a/src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt b/src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt new file mode 100644 index 0000000..73eb085 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/superstructure/WristSubsystem.kt @@ -0,0 +1,63 @@ +package org.rambots.subsystems.superstructure + +import com.revrobotics.CANSparkBase +import com.revrobotics.CANSparkLowLevel +import com.revrobotics.CANSparkMax +import edu.wpi.first.wpilibj.DutyCycleEncoder +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 + +object WristSubsystem : SubsystemBase() { + + var offset = 0.0 + var desiredPosition = 0.0 + + val position get() = motor.encoder.position + + private val dutyCycleEncoder = DutyCycleEncoder(0).apply { + positionOffset = 0.5 + } + private val absoluteEncoderPosition get() = 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) + + private val motor = CANSparkMax(WRIST_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + pidController.apply { + p = WRIST_PID.kP + i = WRIST_PID.kI + d = WRIST_PID.kD + } + + inverted = true + idleMode = CANSparkBase.IdleMode.kBrake + setSmartCurrentLimit(40) + } + + fun resetEncoder() { + motor.encoder.position = absoluteMotorPosition + } + + override fun periodic() { + motor.pidController.setReference(desiredPosition + offset, CANSparkBase.ControlType.kPosition) + + Logger.recordOutput("Wrist/DesiredPosition", desiredPosition) + Logger.recordOutput("Wrist/Offset", offset) + Logger.recordOutput("Wrist/Position", motor.encoder.position) + Logger.recordOutput("Wrist/PositionThroughEncoder", absoluteMotorPosition) + Logger.recordOutput("Wrist/ShaftPosition", relativeEncoderPosition) + Logger.recordOutput("Wrist/Velocity", motor.encoder.velocity) + Logger.recordOutput("Wrist/Current", motor.outputCurrent) + + Logger.recordOutput("Wrist/Encoder/Connected", dutyCycleEncoder.isConnected) + Logger.recordOutput("Wrist/Encoder/Position", absoluteEncoderPosition) + } + + fun home() { + desiredPosition = 0.0 + } + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java index 318e468..b3c5dd1 100644 --- a/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVision.java @@ -37,8 +37,7 @@ public class AprilTagVision extends SubsystemBase { private final AprilTagVisionIO[] io; private final AprilTagVisionIO.AprilTagVisionIOInputs[] inputs; private boolean enableVisionUpdates = true; - private Consumer> visionConsumer = x -> { - }; + private Consumer> visionConsumer = x -> {}; private Map lastFrameTimes = new HashMap<>(); private Map lastTagDetectionTimes = new HashMap<>(); private boolean hasSeenTarget = false; @@ -61,8 +60,7 @@ public AprilTagVision(AprilTagVisionIO... io) { FieldConstants.aprilTags.getTags().forEach(tag -> lastTagDetectionTimes.put(tag.ID, 0.0)); } - public void setDataInterfaces( - Consumer> visionConsumer) { + public void setDataInterfaces(Consumer> visionConsumer) { this.visionConsumer = visionConsumer; } @@ -193,13 +191,13 @@ private double calculateThetaStdDev(VisionHelpers.PoseEstimate poseEstimates, in private void logData( int instanceIndex, double timestamp, Pose3d robotPose, List tagPoses) { Logger.recordOutput( - VISION_PATH + Integer.toString(instanceIndex) + "/LatencySecs", + VISION_PATH + instanceIndex + "/LatencySecs", Timer.getFPGATimestamp() - timestamp); Logger.recordOutput( - VISION_PATH + Integer.toString(instanceIndex) + "/RobotPose", robotPose.toPose2d()); - Logger.recordOutput(VISION_PATH + Integer.toString(instanceIndex) + "/RobotPose3D", robotPose); + VISION_PATH + instanceIndex + "/RobotPose", robotPose.toPose2d()); + Logger.recordOutput(VISION_PATH + instanceIndex + "/RobotPose3D", robotPose); Logger.recordOutput( - VISION_PATH + Integer.toString(instanceIndex) + "/TagPoses", + VISION_PATH + instanceIndex + "/TagPoses", tagPoses.toArray(new Pose3d[tagPoses.size()])); logTagPoses(); } @@ -217,8 +215,7 @@ private void logTagPoses() { } } } - Logger.recordOutput( - "AprilTagVision/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); + Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); } /** diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java index 1242876..493ec9b 100644 --- a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java @@ -57,7 +57,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { double timestamp = timestampedString.timestamp / 1e6; // Parses the JSON dump and retrieves the targeting results - LimelightHelpers.Results results = LimelightHelpers.parseJsonDump(timestampedString.value).targetingResults; + LimelightHelpers.Results results = LimelightHelpers.parseJSONDump(timestampedString.value).targetingResults; // Retrieves the alliance information from the DriverStation Optional allianceOptional = DriverStation.getAlliance(); @@ -69,6 +69,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) { double latencyMS = results.latency_capture + results.latency_pipeline; // Calculates the total latency in milliseconds Pose3d poseEstimation = results.getBotPose3d_wpiBlue(); // Retrieves the pose estimation for the robot + double averageTagDistance = 0.0; // Initializes the average tag distance to 0.0 timestamp -= (latencyMS / 1e3); // Adjusts the timestamp by subtracting the latency in seconds diff --git a/src/main/java/org/rambots/util/LimelightHelpers.java b/src/main/java/org/rambots/util/LimelightHelpers.java index 74e58e8..59ed0ad 100644 --- a/src/main/java/org/rambots/util/LimelightHelpers.java +++ b/src/main/java/org/rambots/util/LimelightHelpers.java @@ -1,18 +1,17 @@ -// LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.4.0 (March 21, 2024) package org.rambots.util; -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +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.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; import java.io.IOException; import java.net.HttpURLConnection; @@ -20,748 +19,936 @@ import java.net.URL; import java.util.concurrent.CompletableFuture; +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + public class LimelightHelpers { - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - private static ObjectMapper mapper; + public static class LimelightTarget_Retro { - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; - } + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - public static Pose3d toPose3D(double[] inData) { - if (inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d( - Units.degreesToRadians(inData[3]), - Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - private static Pose2d toPose2D(double[] inData) { - if (inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } + @JsonProperty("ta") + public double ta; - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } + @JsonProperty("tx") + public double tx; - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } + @JsonProperty("txp") + public double tx_pixels; - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } + @JsonProperty("ty") + public double ty; - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } + @JsonProperty("typ") + public double ty_pixels; - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } + @JsonProperty("ts") + public double ts; - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; } - return null; - } - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); } - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } + public static class LimelightTarget_Fiducial { - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } + @JsonProperty("fID") + public double fiducialID; - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } + @JsonProperty("fam") + public String fiducialFamily; - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + @JsonProperty("ta") + public double ta; - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + @JsonProperty("tx") + public double tx; - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } + @JsonProperty("txp") + public double tx_pixels; - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } + @JsonProperty("ty") + public double ty; - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } + @JsonProperty("typ") + public double ty_pixels; - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } + @JsonProperty("ts") + public double ts; - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } } - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } + public static class LimelightTarget_Barcode { - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); } - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } + public static class LimelightTarget_Classifier { - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } + @JsonProperty("class") + public String className; - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } + @JsonProperty("classID") + public double classID; - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } + @JsonProperty("conf") + public double confidence; - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } + @JsonProperty("zone") + public double zone; - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } + @JsonProperty("tx") + public double tx; - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } + @JsonProperty("txp") + public double tx_pixels; - ///// - ///// + @JsonProperty("ty") + public double ty; - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } } - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + public static class LimelightTarget_Detector { - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } + @JsonProperty("class") + public String className; - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { + @JsonProperty("classID") + public double classID; - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - } + @JsonProperty("conf") + public double confidence; - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { + @JsonProperty("ta") + public double ta; - double[] result = getBotPose(limelightName); - return toPose2D(result); - } + @JsonProperty("tx") + public double tx; - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } + @JsonProperty("txp") + public double tx_pixels; - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } + @JsonProperty("ty") + public double ty; - /** - * The LEDs will be controlled by Limelight pipeline settings, and not by robot code. - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } + @JsonProperty("typ") + public double ty_pixels; - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); + public LimelightTarget_Detector() { + } } - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } + public static class Results { - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } + @JsonProperty("pID") + public double pipelineID; - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } + @JsonProperty("tl") + public double latency_pipeline; - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } + @JsonProperty("cl") + public double latency_capture; - ///// - ///// + public double latency_jsonParse; - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; - /** - * Sets the crop window. The crop window in the UI must be completely open for dynamic cropping to - * work. - */ - public static void setCropWindow( - String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } + @JsonProperty("botpose") + public double[] botpose; - 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; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; - /** - * Asynchronously take snapshot. - */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync( - () -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } + @JsonProperty("botpose_span") + public double botpose_span; - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; - /** - * Parses Limelight's JSON results dump into a LimelightResults Object - */ - public static LimelightResults getLatestResults(String limelightName) { + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = - new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + public Pose3d getBotPose3d() { + return toPose3D(botpose); } - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); } - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); } - return results; - } - - public static LimelightResults parseJsonDump(String jsonDump) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = - new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + public Pose2d getBotPose2d() { + return toPose2D(botpose); } - try { - results = mapper.readValue(jsonDump, LimelightResults.class); - } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); } - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); } - return results; - } + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; - public static class LimelightTarget_Retro { + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; - @JsonProperty("ta") - public double ta; - @JsonProperty("tx") - public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") - public double ty; - @JsonProperty("typ") - public double ty_pixels; - @JsonProperty("ts") - public double ts; - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; } + } - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public String error; + + public LimelightResults() { + targetingResults = new Results(); + error = ""; } - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; } + } - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } + } - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; } + return name; + } - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } } - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; } - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; } - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); } } + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } ///// ///// - public static class LimelightTarget_Fiducial { + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } - @JsonProperty("fID") - public double fiducialID; + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } - @JsonProperty("fam") - public String fiducialFamily; - @JsonProperty("ta") - public double ta; - @JsonProperty("tx") - public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") - public double ty; - @JsonProperty("typ") - public double ty_pixels; - @JsonProperty("ts") - public double ts; - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); } - public static class LimelightTarget_Barcode { + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); } - ///// - ///// + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } - public static class LimelightTarget_Classifier { + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - @JsonProperty("class") - public String className; + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } - @JsonProperty("classID") - public double classID; + /** + * 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(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } - @JsonProperty("conf") - public double confidence; + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { - @JsonProperty("zone") - public double zone; + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); - @JsonProperty("tx") - public double tx; + } - @JsonProperty("txp") - public double tx_pixels; + /** + * 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(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } - @JsonProperty("ty") - public double ty; + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { - @JsonProperty("typ") - public double ty_pixels; + double[] result = getBotPose(limelightName); + return toPose2D(result); - public LimelightTarget_Classifier() { - } } - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } - @JsonProperty("conf") - public double confidence; + ///// + ///// - @JsonProperty("ta") - public double ta; + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } - @JsonProperty("tx") - public double tx; - @JsonProperty("txp") - public double tx_pixels; + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } - @JsonProperty("ty") - public double ty; + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } - @JsonProperty("typ") - public double ty_pixels; + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } - public LimelightTarget_Detector() { - } + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); } - public static class Results { + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } - @JsonProperty("pID") - public double pipelineID; + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } - @JsonProperty("tl") - public double latency_pipeline; + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } - @JsonProperty("cl") - public double latency_capture; + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } - public double latency_jsonParse; + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; + 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; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } - @JsonProperty("botpose") - public double[] botpose; + ///// + ///// - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; + ///// + ///// - public Results() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - } + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); } + return false; + } - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); + public static LimelightResults parseJSONDump(String dump) { + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); } - public Pose2d getBotPose2d() { - return toPose2D(botpose); + try { + results = mapper.readValue(dump, LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); } - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); } - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } + return results; } - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public LimelightResults() { - targetingResults = new Results(); - } + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + String dump = getJSONDump(limelightName); + return parseJSONDump(dump); } -} + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/util/SparkMaxUtil.kt b/src/main/java/org/rambots/util/SparkMaxUtil.kt new file mode 100644 index 0000000..1d6ffa6 --- /dev/null +++ b/src/main/java/org/rambots/util/SparkMaxUtil.kt @@ -0,0 +1,11 @@ +package org.rambots.util + +import com.revrobotics.CANSparkBase.IdleMode + +object SparkMaxUtil { + + fun getBrake(brake: Boolean): IdleMode { + return if (brake) IdleMode.kBrake else IdleMode.kCoast + } + +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index efb7498..028b131 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -35,4 +35,4 @@ ] } ] -} +} \ No newline at end of file diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json index f572dcc..143bd31 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix5.json", "name": "CTRE-Phoenix (v5)", - "version": "5.33.0", + "version": "5.33.1", "frcYear": 2024, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -20,19 +20,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.33.0" + "version": "5.33.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.33.0" + "version": "5.33.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.0", + "version": "5.33.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +45,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.0", + "version": "5.33.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -60,7 +60,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -75,7 +75,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -105,7 +105,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -120,7 +120,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -135,7 +135,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -148,4 +148,4 @@ "simMode": "swsim" } ] -} +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 42d1c3e..46a2550 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,4 +336,4 @@ "simMode": "swsim" } ] -} +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 6bb009c..3f7beed 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.1", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.1" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.1", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.1", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.1", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -71,4 +71,4 @@ ] } ] -} +} \ No newline at end of file