From f58841d3c3cb8ba7d391511d4fd7c842d1a9094d Mon Sep 17 00:00:00 2001 From: michael-m-2983 <116770284+michael-m-2983@users.noreply.github.com> Date: Thu, 27 Feb 2025 07:57:09 -0600 Subject: [PATCH] Workingsidedetection (#11) * Autos basics * added boilerplate manipulator code * filled in elevatorsubsystem renamed newelevatorsubsystem to elevatorsubsystem * partially filled out algae subsystem * partially filled in coral subsystem * basic climber things * Fixed FWERB code * Fixed FWERB code (#3) Co-authored-by: Yunju607 * setting.json and updates * changed elevator to zero by motor feedback * corrected some motors to sparkflex * changed coral intake to talonfx * Updates 2 :) * updated coralarm logic * typo * add feedforward to algaearm * coral position mirroring * add algaeintake beambreak and update intake logic * updated coralintake logic * custom coral arm stuff i guess * coral virtual representation * Auto Works * Improve code readability * readded vision * renamed subsystem files i think this should work * Update/Add theoretical manipulator constants. Fix building on windows. * Add theoretical presets * add coral wrist roll restrictions * update coral subsystem setter getters * put everything in robotcontainers and added bindings some reorganization too * reorganized commands * coral simulation init untested as usual * refactored coral scoring/intaking command * algae simulation init * removed unused references * AYAAYAYA * fixed the minimum to get simulation working * Revert "Merge branch 'Autos' into addManipulators" This reverts commit ef4addfeb3f2132eb3f30a2e9baf2bc4b184ce0e, reversing changes made to 759031f383d118d2d9f3282426b6bd16642df86b. * Add debugging, command based sequencing, update control loops, etc. * Hopefully fix for analog wrist encoders. * Update CAN IDs for comp bot. * Add debugging facilities and more encoder configuration. * Simple feedforwards for pitch/roll. Rough tuning of pitch (sim) * Pivot, etc. simulation & rudimentary tuning. * Rough elevator PID tuning. * Sim pidtuning (#5) * Pivot, etc. simulation & rudimentary tuning. * Rough elevator PID tuning. * cleanup code and reitegrate simulation objects * Tuned roll, pitch, pivot, drives! * Tuned elevator, pivot PIDs. intake working. * Minor simulation fixes for pivot joint, analog encoders, etc. * Fix motion commands, tweak operator controls, rumble, coral intaking simulation. * Works, but flings coral. * Scoring is OK, intake is OK. not perfect, but it works alright. * Pitch tweak, move current limit too constants. * Assisted scoring init, based on LL * small tweaks type * Accounting for intake * mt1 implementation (still need to fix prelim pos!!!) * pseudocode for left vs right * Finn's constants * minor error fixes * Added apriltags (FIX PRELIM POSITION) * odometry this prolly doesn't work * Intaking and constants tweak * Fixed odometry * debugging * tweaks for functionality * Auto stuff, base climber (manual). * Climber support * Rudimentary support for ultrasonic auto-mirroring. * Removed unstable climber code * Removed (commented) limelight/odometry * Updated limelight config * After filler matches. * Tweaks at the end of week 0 * Climber ready-ish for soft limits. * Smoother constants, PID tuning utilities. * just a tiny wee little thing * Disabled auto set mirror (hopefully) * Velocity display for debugging. * V2 sequncnign???? * Climber stops, zeroing in TEST mode. * Better intaking currenting. * fix LL mt1 * Disable PID debugging. * Should flip arm based on odometry * Tweak command and auto mirror usage in RobotContainer. * Switched LL To mode 0 * Working side detection * Fixed.... things? Pushing now for debugging * Remove comments from CoralIntake class * Remove comments from Constants --------- Co-authored-by: Yunju607 Co-authored-by: Spoopr Co-authored-by: voidReq Co-authored-by: NifleySnifley Co-authored-by: Yunju607 <149122412+Yunju607@users.noreply.github.com> Co-authored-by: Nifley <67334280+NifleySnifley@users.noreply.github.com> Co-authored-by: voidReq <100312344+voidReq@users.noreply.github.com> Co-authored-by: Colten Kiehne Co-authored-by: NetLockJ <96214113+NetLockJ@users.noreply.github.com> --- build.gradle | 2 +- gradlew | 0 simgui-ds.json | 102 +++ src/main/deploy/pathplanner/navgrid.json | 1 + src/main/deploy/pathplanner/settings.json | 32 + src/main/java/frc/robot/Constants.java | 371 ++++++-- src/main/java/frc/robot/LimelightHelpers.java | 811 ++++++++++++++++-- src/main/java/frc/robot/RobotContainer.java | 350 ++++++-- .../java/frc/robot/commands/DriveCommand.java | 18 +- .../frc/robot/commands/ElevatorCommand.java | 135 +-- .../robot/commands/ElevatorFollowCommand.java | 55 -- .../commands/algae/ShootAlgaeCommand.java | 29 + .../coral/CoralWristFollowCommand.java | 24 + .../commands/coral/IntakeCoralCommand.java | 45 + .../coral/PurgeCoralIntakeCommand.java | 23 + .../commands/coral/ScoreCoralCommand.java | 48 ++ .../commands/coral/motion/MoveElevator.java | 37 + .../commands/coral/motion/MovePitch.java | 39 + .../commands/coral/motion/MovePivot.java | 38 + .../robot/commands/coral/motion/MoveRoll.java | 38 + .../robot/commands/coral/motion/StowArm.java | 30 + .../coral/motion/WaitArmClearance.java | 28 + .../coral/motion/WaitElevatorApproach.java | 28 + .../coral/motion/WaitRollFinished.java | 27 + .../robot/subsystems/ClimberSubsystem.java | 84 ++ .../robot/subsystems/ElevatorSubsystem.java | 238 ----- .../java/frc/robot/subsystems/Limelight.java | 226 +++++ .../frc/robot/subsystems/SwerveModule.java | 12 +- .../frc/robot/subsystems/SwerveSubsystem.java | 128 +-- .../frc/robot/subsystems/algae/AlgaeArm.java | 87 ++ .../robot/subsystems/algae/AlgaeIntake.java | 79 ++ .../subsystems/algae/AlgaeSubsystem.java | 54 ++ .../frc/robot/subsystems/coral/CoralArm.java | 479 +++++++++++ .../robot/subsystems/coral/CoralElevator.java | 189 ++++ .../robot/subsystems/coral/CoralIntake.java | 105 +++ .../subsystems/coral/CoralSubsystem.java | 349 ++++++++ .../java/frc/robot/util/AllianceFlipUtil.java | 56 ++ .../frc/robot/util/LimelightAssistance.java | 35 + .../frc/robot/util/LimelightContainer.java | 176 ++++ src/main/java/frc/robot/util/Reef.java | 102 +++ 40 files changed, 4067 insertions(+), 643 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 simgui-ds.json create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/settings.json delete mode 100644 src/main/java/frc/robot/commands/ElevatorFollowCommand.java create mode 100644 src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java create mode 100644 src/main/java/frc/robot/commands/coral/CoralWristFollowCommand.java create mode 100644 src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java create mode 100644 src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java create mode 100644 src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/MoveElevator.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/MovePitch.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/MovePivot.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/MoveRoll.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/StowArm.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java create mode 100644 src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java create mode 100644 src/main/java/frc/robot/subsystems/ClimberSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/Limelight.java create mode 100644 src/main/java/frc/robot/subsystems/algae/AlgaeArm.java create mode 100644 src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java create mode 100644 src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/coral/CoralArm.java create mode 100644 src/main/java/frc/robot/subsystems/coral/CoralElevator.java create mode 100644 src/main/java/frc/robot/subsystems/coral/CoralIntake.java create mode 100644 src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java create mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java create mode 100644 src/main/java/frc/robot/util/LimelightAssistance.java create mode 100644 src/main/java/frc/robot/util/LimelightContainer.java create mode 100644 src/main/java/frc/robot/util/Reef.java diff --git a/build.gradle b/build.gradle index 9d8ae14..c6fea4f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..72a979e --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,102 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + {}, + { + "useGamepad": true + }, + { + "guid": "030000005e040000fd02000003090000", + "useGamepad": true + } + ] +} diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"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],[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,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,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,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,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,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,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,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,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,true,true,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,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,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,true,true,true,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,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,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,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,false,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,false,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,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]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..e54abce --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 0.5, + "defaultMaxAccel": 0.5, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 639b994..7546819 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,19 +4,34 @@ package frc.robot; -import com.revrobotics.spark.config.LimitSwitchConfig.Type; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.subsystems.SwerveSubsystem.RotationStyle; +import frc.robot.util.AllianceFlipUtil; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -39,16 +54,19 @@ public static class ControllerConstants { public static class RobotConstants { public static final double robotWidthMeters = Units.inchesToMeters(25.0); public static final double robotLengthMeters = Units.inchesToMeters(25.0); - - // TODO: ############## REPLACE PLACEHOLDERS ############## - public static final double TOTAL_MASS_KG = 10; - public static final double MOMENT_OF_INERTIA = 1; + + // TODO: ############## REPLACE PLACEHOLDERS ############## + public static final double TOTAL_MASS_KG = 10; + public static final double MOMENT_OF_INERTIA = 1; } public static final class FieldConstants { public static final double GRAVITY = 9.81; public static final double SPEAKER_HEIGHT = 2.05; // Meters + public static final double FIELD_LENGTH = Units.inchesToMeters(690.876); + public static final double FIELD_WIDTH = Units.inchesToMeters(317); + public static Alliance getAlliance() { if (DriverStation.getAlliance().isPresent()) { return DriverStation.getAlliance().get(); @@ -56,6 +74,37 @@ public static Alliance getAlliance() { return Alliance.Blue; } + + public static ArrayList getSourcePoses() { + Pose2d leftCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(291.176), + Rotation2d.fromDegrees(90 - 144.011)); + Pose2d rightCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824), + Rotation2d.fromDegrees(144.011 - 90)); + + if (getAlliance() == Alliance.Red) { + leftCenterFace = AllianceFlipUtil.flip(leftCenterFace); + rightCenterFace = AllianceFlipUtil.flip(rightCenterFace); + } + + ArrayList poses = new ArrayList(); + + poses.add(leftCenterFace); + poses.add(rightCenterFace); + + return poses; + } + + public static Pose2d getReefPose() { + Pose2d reef = new Pose2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501), new Rotation2d()); + if(getAlliance() == Alliance.Red) { + AllianceFlipUtil.flip(reef); + } + return reef; + } } public static class SwerveModuleConstants { @@ -81,34 +130,34 @@ public static class SwerveModuleConstants { public static final double MODULE_KD = 0.0066806;// 0.0057682; //0.0076954; // --------- Front Left Module --------- \\ - public static final int FL_DRIVE_ID = 4; + public static final int FL_DRIVE_ID = 34; public static final int FL_STEER_ID = 4; - public static final int FL_ABSOLUTE_ENCODER_PORT = 4; - public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(0.389893) + Math.PI * 0.5 + Math.PI; + public static final int FL_ABSOLUTE_ENCODER_PORT = 54; + public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(-0.310303) + Math.PI * 0.5 + Math.PI; public static final boolean FL_ABSOLUTE_ENCODER_REVERSED = true; public static final boolean FL_MOTOR_REVERSED = true; // --------- Front Right Module --------- \\ - public static final int FR_DRIVE_ID = 1; + public static final int FR_DRIVE_ID = 31; public static final int FR_STEER_ID = 1; - public static final int FR_ABSOLUTE_ENCODER_PORT = 1; - public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(0.323730) + Math.PI * 0.5 + Math.PI; + public static final int FR_ABSOLUTE_ENCODER_PORT = 51; + public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(-0.253906) + Math.PI * 0.5 + Math.PI; public static final boolean FR_ABSOLUTE_ENCODER_REVERSED = true; public static final boolean FR_MOTOR_REVERSED = true; // --------- Back Right Module --------- \\ - public static final int BR_DRIVE_ID = 2; + public static final int BR_DRIVE_ID = 32; public static final int BR_STEER_ID = 2; - public static final int BR_ABSOLUTE_ENCODER_PORT = 2; - public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(-0.360107) + Math.PI * 0.5 + Math.PI; + public static final int BR_ABSOLUTE_ENCODER_PORT = 52; + public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(0.353027) + Math.PI * 0.5 + Math.PI; public static final boolean BR_ABSOLUTE_ENCODER_REVERSED = true; public static final boolean BR_MOTOR_REVERSED = true; // --------- Back Left Module --------- \\ - public static final int BL_DRIVE_ID = 3; + public static final int BL_DRIVE_ID = 33; public static final int BL_STEER_ID = 3; - public static final int BL_ABSOLUTE_ENCODER_PORT = 3; - public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(0.399902) + Math.PI * 0.5 + Math.PI; + public static final int BL_ABSOLUTE_ENCODER_PORT = 53; + public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(-0.134033) + Math.PI * 0.5 + Math.PI; public static final boolean BL_ABSOLUTE_ENCODER_REVERSED = true; public static final boolean BL_MOTOR_REVERSED = true; @@ -122,10 +171,12 @@ public static class DriveConstants { public static final double MAX_ROBOT_RAD_VELOCITY = 12.0; // Approx. Measured rads/sec // TODO: ############## REPLACE PLACEHOLDERS ############## - public static final double MAX_MODULE_CURRENT = 10; + public static final double MAX_MODULE_CURRENT = 30; public static final double TRACK_WIDTH = Units.inchesToMeters(19.75); public static final double WHEEL_BASE = Units.inchesToMeters(19.75); + + public static final double FULL_ROBOT_WIDTH = Units.inchesToMeters(37.520); // TODO: Set this for FWERB V2 public static final Rotation2d NAVX_ANGLE_OFFSET = Rotation2d.fromDegrees(-90); // TODO: I'm not going to touch this... but it seems important! @@ -152,45 +203,239 @@ public static class CommonConstants { public static final boolean LOG_INTO_FILE_ENABLED = true; } - public static class Elevator { - public static final int elevatorOnePort = 10; - public static final int elevatorTwoPort = 11; + // TODO: ##################### PLACEHOLDERS ##################### + public static class Climber { + public static final int MOTOR_PORT = 20; + public static final ProfiledPIDController PID = new ProfiledPIDController( + 1, + 0.0, + 0.0, + new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)); + + public static boolean DBG_DISABLED = false; + public static double GEAR_RATIO = 45.0; + + public static double DEPLOY_SOFT_LIMIT = -6.0; + public static double CLIMB_SOFT_LIMIT = -3.10; + } + + public static class Coral { + public static int LEFT_ULTRASONIC_PORT = 0; + public static int RIGHT_ULTRASONIC_PORT = 1; + + public static boolean DEBUG_PIDS = false; + + public static class Pivot { + public static final int MOTOR_PORT = 14; + public static final int ENCODER_PORT = 28; + public static boolean DBG_DISABLED = false; + + public static final boolean ENCODER_INVERTED = false; + + public static final double MAXIMUM_ANGLE = Units.degreesToRadians(80); + public static final double ELEVATOR_BORDER_ANGLE = Units.degreesToRadians(10); + + // TODO: Tune in simulation + public static final ProfiledPIDController PID = new ProfiledPIDController( + 12.0, + 0.0, + 0.005, + new TrapezoidProfile.Constraints(7.0, 15.0)); + + // Updated with THEORETICAL values + public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward( + 0.0, + 0.0, // V + 0.0, // 1.0, // V*s/rad + 0.00// V*s^2/rad + ); + + public static class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getNeoVortex(1); + public static final double NET_REDUCTION = 96.0; + public static final double MASS_KG = 4.7727; + public static final double ARM_LENGTH_METERS = 0.510; + public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(23.0); + public static final double MOI = 0.2875548495; // Kg*m^2 + } + } + + public static class Roll { + public static final int MOTOR_PORT = 15; + public static final boolean MOTOR_INVERTED = false; + public static final boolean ENCODER_INVERTED = false; + public static final double ENCODER_OFFSET_VOLTS = -1.85; + public static boolean DBG_DISABLED = false; + + public static final ProfiledPIDController PID = new ProfiledPIDController( + 6, + 0.0, + 0.02, + new TrapezoidProfile.Constraints(15, 25.0)); + + public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90); + + public static class PhysicalConstants { + public static DCMotor MOTOR = DCMotor.getNeo550(1); + public static final double NET_REDUCTION = 45.0; + public static final double MASS_KG = 2.85; // Includes a coral + public static final double ARM_LENGTH_METERS = 0.083; + public static final double JOINT_LENGTH_METERS = 0.10; + public static final double MOI = 0.0403605447; // Kg*m^2 + } + } + + public static class Pitch { + public static final int MOTOR_PORT = 16; + public static final boolean MOTOR_INVERTED = true; + public static final boolean ENCODER_INVERTED = true; + public static final double ENCODER_OFFSET_VOLTS = -2.7; + public static boolean DBG_DISABLED = false; + + public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0); + + public static final ProfiledPIDController PID = new ProfiledPIDController( + 7.0, + 0.0, + 0.0, + new TrapezoidProfile.Constraints(10.0, 30.0)); // Radians + + public static class PhysicalConstants { + public static DCMotor MOTOR = DCMotor.getNeo550(1); + public static final double NET_REDUCTION = 92.85714286; // Yeah this is cursed + public static final double MASS_KG = 2.16; // Includes a coral + public static final double ARM_LENGTH_METERS = 0.101; + public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(13.875); + public static final double MOI = 0.00200055915; // Kg*m^2 + } + } + + public static class Intake { + public static final int MOTOR_PORT = 17; + public static boolean DBG_DISABLED = false; + public static boolean MOTOR_INVERTED = true; + + public static final double POSITIVE_RATE_LIMIT = 200.0; // Fast shoot + public static final double NEGATIVE_RATE_LIMIT = 100.0; // Slow intake + + public static final double SCORE_EXTRA_SECONDS = 1.0; + + public static final double CURRENT_LIMIT = 12.5; // Stator + + public static final class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getFalcon500(1); + public static final double MOI = 0.1; // J*KG / M^2 + public static final double GEARING = 1; + } + } + } + + // TODO: ##################### PLACEHOLDERS ##################### + public static final class Algae { + public static final class Pivot { + public static final int MOTOR_PORT = 18; + public static final int ENCODER_PORT = 27; + + public static final ProfiledPIDController PID = new ProfiledPIDController( + 1, + 0.0, + 0.0, + new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)); + public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward( + 1, + 1, + 1); + + public static final double RETRACTED_LIMIT_DEGREES = 10.0; + public static final double EXTENDED_LIMIT_DEGREES = 90.0; + + public static class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getNeoVortex(1); + public static final double GEARING = 1; // TODO: this is the only placeholder + public static final double NET_REDUCTION = 42.66666667; // Yeah this is cursed + public static final double MASS_KG = 3.18; + public static final double ARM_LENGTH_METERS = 0.2349863728; + public static final double MOI = 0.1114866914; // Kg*m^2 + } + } - public static boolean elevatorOneInverted = true; - public static boolean elevatorTwoInverted = false; + public static final class Intake { + public static final int MOTOR_PORT = 19; + public static final int BEAMBREAK_PORT = 0; // NOTE: Beambreak will *probably* be a rockwell proximity sensor + // wired into the SPARK max - public static Type bottomLimitMode = Type.kNormallyOpen; + public static final double POSITIVE_RATE_LIMIT = 5.0; + public static final double NEGATIVE_RATE_LIMIT = -5.0; + public static final class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getNeo550(1); + public static final double MOI = 0.1; // J*KG / M^2 + public static final double GEARING = 1; + } + } + } + + // See + // https://cad.onshape.com/documents/fa9a0365dfdf7e376f93f1b4/w/36bfb0cc9de95ef5933791e3/e/700ba3cf920578fe61d3ec24 + public static final class Elevator { + public static final class Leader { + public static final int MOTOR_PORT = 10; + public static final boolean INVERTED = false; + } + + public static final class Follower { + public static final int MOTOR_PORT = 11; + public static final boolean INVERTED = true; + } - public static double motorTurnsPerMeter = 39.44; + public static boolean DBG_DISABLED = false; + public static double MOTOR_REVOLUTIONS_PER_METER = 32.81; + + // TODO: Tune! Use FWERB for now public static class PID { - public static double kP = 20.0; // 9.0; + public static double kP = 28.0; public static double kI = 0.0; - public static double kD = 0.5; // 4.0; - public static double MAX_VELOCITY = 2.8; - public static double MAX_ACCELERATION = 18.0; + public static double kD = 0.01; + public static double MAX_VELOCITY = 2.80; + // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if + // the elevator can make or exceed this + public static double MAX_ACCELERATION = 15.0; } - // TODO: For the first testing, set these all to zero for safety reasons - // Remind me to pad the top and bottom of the elevator with poodles to make sure - // we don't damage it. - public static class Feedforward { + // TODO: PAD THE ELEVATOR!!!!!!! + public static class FeedforwardConstants { public static double Ks = 0.0; - public static double Kv = 4.0; - public static double Ka = 0.03; - public static double Kg = 0.1; + public static double Kv = 2.45 + + ; + public static double Ka = 0.1; + public static double Kg = 0.05; // TODO: Check this!!! } + public static ElevatorFeedforward FEEDFORWARD = new ElevatorFeedforward( + FeedforwardConstants.Ks, + FeedforwardConstants.Kg, + FeedforwardConstants.Kv, + FeedforwardConstants.Ka); + public static class PhysicalParameters { - public static double gearReduction = 9.0 / 2.0; - public static double driveRadiusMeters = 0.0182; - public static double carriageMassKg = 1.5; - public static double elevatorHeightMeters = Units.inchesToMeters(50.0); - public static double elevatorBottomFromFloorMeters = Units.inchesToMeters(12.0); - public static double elevatorCarriageHeightMeters = Units.inchesToMeters(6.0); - public static double elevatorForwardsFromRobotCenterMeters = Units.inchesToMeters(25.0 / 2); - public static DCMotor simMotor = DCMotor.getNeoVortex(2); + public static final double GEARING = 5.0 / 2.0; + public static final double DRIVE_RADIUS_METERS = 0.0121; + public static final double CARRIAGE_MASS_KG = 6.0; // Load on the SECOND stage NOTE: This includes the weight + // "reduction" due to CF spring counterbalance! + + public static final double MAX_TRAVEL = Units.inchesToMeters(59.5); + public static final double BOTTOM_TO_FLOOR = Units.inchesToMeters(3.0); // Relative to bottom of stage 2 + public static final double CARRIAGE_HEIGHT = Units.inchesToMeters(8.0); // Bottom to top of stage 2 + + public static final double CORAL_PIVOT_VERTICAL_OFFSET = Units.inchesToMeters(6.0); // From bottom of stage 2 to + // coral arm pivot axis + public static final double CORAL_PIVOT_HORIZONTAL_OFFSET = Units.inchesToMeters(7.5); // From bottom of stage 2 to + // coral arm pivot axis + public static final double ELEVATOR_FORWARDS_OFFSET = Units.inchesToMeters(1); // To mid-plane of elevator + + public static final DCMotor MOTOR = DCMotor.getNeoVortex(2); } } @@ -199,22 +444,21 @@ public static final class PathPlannerConstants { public static final PIDConstants ROTATION_PID = new PIDConstants(5, 0, 0.2); public static final PPHolonomicDriveController HOLONOMIC_FOLLOWER_CONTROLLER = new PPHolonomicDriveController( - TRANSLATION_PID, - ROTATION_PID - ); + TRANSLATION_PID, + ROTATION_PID); public static final RobotConfig ROBOT_CONFIG = new RobotConfig( - RobotConstants.TOTAL_MASS_KG, - RobotConstants.MOMENT_OF_INERTIA, - new ModuleConfig( - SwerveModuleConstants.WHEEL_DIAMETER/2, - DriveConstants.MAX_MODULE_VELOCITY, - SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS ############## - DCMotor.getKrakenX60(1), - DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ############## - 4 - ) - ); + RobotConstants.TOTAL_MASS_KG, + RobotConstants.MOMENT_OF_INERTIA, + new ModuleConfig( + SwerveModuleConstants.WHEEL_DIAMETER / 2, + DriveConstants.MAX_MODULE_VELOCITY, + SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS + // ############## + DCMotor.getKrakenX60(1), + DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ############## + 1), + DriveConstants.KINEMATICS.getModules()); } public static final class PoseConstants { @@ -225,5 +469,16 @@ public static final class PoseConstants { public static final double kVisionStdDevY = 5; public static final double kVisionStdDevX = 5; public static final double kVisionStdDevTheta = 500; + + private static AprilTagFieldLayout tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + + public final static HashMap tagPoses = new HashMap() { + { + for (int i = 0; i < 22; ++i) { + if (tagLayout.getTagPose(i + 1).isPresent()) + put(i, tagLayout.getTagPose(i + 1).get().toPose2d()); + } + } + }; } } diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index 3f0b9af..854968f 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,10 +1,14 @@ -//LimelightHelpers v1.5.0 (March 27, 2024) +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) package frc.robot; +import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.LimelightHelpers.LimelightResults; +import frc.robot.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,6 +21,7 @@ import java.net.HttpURLConnection; import java.net.MalformedURLException; import java.net.URL; +import java.util.Map; import java.util.concurrent.CompletableFuture; import com.fasterxml.jackson.annotation.JsonFormat; @@ -25,9 +30,19 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ public class LimelightHelpers { + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ public static class LimelightTarget_Retro { @JsonProperty("t6c_ts") @@ -92,16 +107,22 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("tx") public double tx; + + @JsonProperty("ty") + public double ty; @JsonProperty("txp") public double tx_pixels; - @JsonProperty("ty") - public double ty; - @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + @JsonProperty("ts") public double ts; @@ -115,6 +136,9 @@ public LimelightTarget_Retro() { } + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ public static class LimelightTarget_Fiducial { @JsonProperty("fID") @@ -186,15 +210,21 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("tx") public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") public double ty; + @JsonProperty("txp") + public double tx_pixels; + @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + @JsonProperty("ts") public double ts; @@ -207,10 +237,58 @@ public LimelightTarget_Fiducial() { } } + /** + * Represents a Barcode Target Result extracted from JSON Output + */ public static class LimelightTarget_Barcode { + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } } + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ public static class LimelightTarget_Classifier { @JsonProperty("class") @@ -241,6 +319,9 @@ public LimelightTarget_Classifier() { } } + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ public static class LimelightTarget_Detector { @JsonProperty("class") @@ -258,21 +339,32 @@ public static class LimelightTarget_Detector { @JsonProperty("tx") public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") public double ty; + @JsonProperty("txp") + public double tx_pixels; + @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + public LimelightTarget_Detector() { } } - public static class Results { - + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + @JsonProperty("pID") public double pipelineID; @@ -357,7 +449,7 @@ public Pose2d getBotPose2d_wpiBlue() { @JsonProperty("Barcode") public LimelightTarget_Barcode[] targets_Barcode; - public Results() { + public LimelightResults() { botpose = new double[6]; botpose_wpired = new double[6]; botpose_wpiblue = new double[6]; @@ -369,30 +461,21 @@ public Results() { targets_Barcode = new LimelightTarget_Barcode[0]; } - } - - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public String error; - - public LimelightResults() { - targetingResults = new Results(); - error = ""; - } } + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ 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 int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { @@ -406,6 +489,47 @@ public RawFiducial(int id, double txnc, double tync, double ta, double distToCam } } + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -414,11 +538,28 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials) { + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { this.pose = pose; this.timestampSeconds = timestampSeconds; @@ -428,9 +569,45 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; } + } + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + private static ObjectMapper mapper; /** @@ -445,7 +622,13 @@ static final String sanitizeName(String name) { return name; } - private static Pose3d toPose3D(double[] inData){ + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); @@ -457,7 +640,14 @@ private static Pose3d toPose3D(double[] inData){ Units.degreesToRadians(inData[5]))); } - private static Pose2d toPose2D(double[] inData){ + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 2D Pose Data!"); @@ -468,7 +658,44 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } - private static double extractBotPoseEntry(double[] inData, int position){ + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ if(inData.length < position+1) { return 0; @@ -476,27 +703,35 @@ private static double extractBotPoseEntry(double[] inData, int position){ return inData[position]; } - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); - var poseArray = poseEntry.getDoubleArray(new double[0]); + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + 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); - - + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial*tagCount; - + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + if (poseArray.length != expectedTotalVals) { // Don't populate fiducials - } - else{ + } else { for(int i = 0; i < tagCount; i++) { int baseIndex = 11 + (i * valsPerFiducial); int id = (int)poseArray[baseIndex]; @@ -509,11 +744,89 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); } } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } - return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; } - private static void printPoseEstimate(PoseEstimate pose) { + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { if (pose == null) { System.out.println("No PoseEstimate available."); return; @@ -526,6 +839,7 @@ private static void printPoseEstimate(PoseEstimate pose) { 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.printf("Is MegaTag2: %b%n", pose.isMegaTag2); System.out.println(); if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { @@ -548,14 +862,30 @@ private static void printPoseEstimate(PoseEstimate pose) { } } + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { return getLimelightNTTable(tableName).getEntry(entryName); } + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + public static double getLimelightNTDouble(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); } @@ -572,10 +902,16 @@ public static double[] getLimelightNTDoubleArray(String tableName, String entryN return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); } + public static String getLimelightNTString(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getString(""); } + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; @@ -590,30 +926,171 @@ public static URL getLimelightURLString(String tableName, String request) { ///// ///// + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ public static double getTX(String limelightName) { return getLimelightNTDouble(limelightName, "tx"); } + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ public static double getTY(String limelightName) { return getLimelightNTDouble(limelightName, "ty"); } + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ public static double getTA(String limelightName) { return getLimelightNTDouble(limelightName, "ta"); } + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ public static double getLatency_Pipeline(String limelightName) { return getLimelightNTDouble(limelightName, "tl"); } + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ public static double getLatency_Capture(String limelightName) { return getLimelightNTDouble(limelightName, "cl"); } + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ public static double getCurrentPipelineIndex(String limelightName) { return getLimelightNTDouble(limelightName, "getpipe"); } + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ public static String getJSONDump(String limelightName) { return getLimelightNTString(limelightName, "json"); } @@ -691,6 +1168,10 @@ public static String getNeuralClassID(String limelightName) { return getLimelightNTString(limelightName, "tclass"); } + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + ///// ///// @@ -699,36 +1180,71 @@ public static Pose3d getBotPose3d(String limelightName) { return toPose3D(poseArray); } + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ public static Pose3d getBotPose3d_wpiRed(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); return toPose3D(poseArray); } + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ public static Pose3d getBotPose3d_wpiBlue(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); return toPose3D(poseArray); } + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ public static Pose3d getBotPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); return toPose3D(poseArray); } + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); return toPose3D(poseArray); } + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); return toPose3D(poseArray); } + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); return toPose3D(poseArray); } + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); return toPose3D(poseArray); @@ -748,25 +1264,24 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE - * alliance + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE - * alliance + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); } /** @@ -790,7 +1305,7 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); + return getBotPoseEstimate(limelightName, "botpose_wpired", false); } /** @@ -800,7 +1315,7 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); } /** @@ -816,9 +1331,21 @@ public static Pose2d getBotPose2d(String limelightName) { return toPose2D(result); } - - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); } ///// @@ -834,8 +1361,8 @@ public static void setPriorityTagID(String limelightName, int ID) { } /** - * The LEDs will be controlled by Limelight pipeline settings, and not by robot - * code. + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera */ public static void setLEDMode_PipelineControl(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 0); @@ -853,29 +1380,38 @@ public static void setLEDMode_ForceOn(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 3); } + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_Standard(String limelightName) { setLimelightNTDouble(limelightName, "stream", 0); } + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_PiPMain(String limelightName) { setLimelightNTDouble(limelightName, "stream", 1); } + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_PiPSecondary(String limelightName) { setLimelightNTDouble(limelightName, "stream", 2); } - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - /** - * Sets the crop window. The crop window in the UI must be completely open for - * dynamic cropping to work. + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) */ public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { double[] entries = new double[4]; @@ -885,10 +1421,44 @@ public static void setCropWindow(String limelightName, double cropXMin, double c entries[3] = cropYMax; setLimelightNTDoubleArray(limelightName, "crop", entries); } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { double[] entries = new double[6]; entries[0] = yaw; @@ -898,8 +1468,48 @@ public static void SetRobotOrientation(String limelightName, double yaw, double entries[4] = roll; entries[5] = rollRate; setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); } + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { double[] validIDsDouble = new double[validIDs.length]; for (int i = 0; i < validIDs.length; i++) { @@ -907,7 +1517,50 @@ public static void SetFiducialIDFiltersOverride(String limelightName, int[] vali } setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ 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; @@ -964,7 +1617,9 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) } /** - * Parses Limelight's JSON results dump into a LimelightResults Object + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data */ public static LimelightResults getLatestResults(String limelightName) { @@ -982,7 +1637,7 @@ public static LimelightResults getLatestResults(String limelightName) { long end = System.nanoTime(); double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; + results.latency_jsonParse = millis; if (profileJSON) { System.out.printf("lljson: %.2f\r\n", millis); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0bbe4cc..6a5ff23 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,20 +6,43 @@ import frc.robot.Constants.*; import frc.robot.commands.*; -import frc.robot.commands.ElevatorCommand.ElevatorPresets; +import frc.robot.commands.algae.ShootAlgaeCommand; +import frc.robot.commands.coral.IntakeCoralCommand; +import frc.robot.commands.coral.PurgeCoralIntakeCommand; +import frc.robot.commands.coral.ScoreCoralCommand; +import frc.robot.commands.coral.motion.MoveElevator; +import frc.robot.commands.coral.motion.MovePitch; +import frc.robot.commands.coral.motion.MovePivot; +import frc.robot.commands.coral.motion.MoveRoll; +import frc.robot.commands.coral.motion.StowArm; +import frc.robot.commands.coral.motion.WaitArmClearance; +import frc.robot.commands.coral.motion.WaitElevatorApproach; +import frc.robot.commands.coral.motion.WaitRollFinished; +import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.SwerveDriveBrake; import com.pathplanner.lib.auto.AutoBuilder; import frc.robot.subsystems.*; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; -import java.util.function.DoubleSupplier; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.*; +import frc.robot.util.LimelightAssistance; +import frc.robot.util.LimelightContainer; +import frc.robot.subsystems.Limelight.LimelightType;import frc.robot.util.LimelightContainer; +import frc.robot.subsystems.Limelight.LimelightType; /** * This class is where the bulk of the robot should be declared. Since @@ -32,23 +55,36 @@ */ public class RobotContainer { + private static final Limelight LL_BF = new Limelight(LimelightType.LL4, "limelight-bf", true, true); + private static final Limelight LL_BR = new Limelight(LimelightType.LL4, "limelight-br", true, true); + private static final Limelight LL_BL = new Limelight(LimelightType.LL4, "limelight-bl", true, true); + + public static final LimelightContainer LLContainer = new LimelightContainer(LL_BF, LL_BR, LL_BL); + private final CommandXboxController driverXbox = new CommandXboxController( ControllerConstants.DRIVER_CONTROLLER_PORT); private final CommandXboxController operatorXbox = new CommandXboxController( ControllerConstants.OPERATOR_CONTROLLER_PORT); + private final CommandXboxController debugXboxController = new CommandXboxController(3); // private final CommandXboxController debugXbox = new CommandXboxController(0); private final SendableChooser autoChooser; - private final SwerveSubsystem swerveDriveSubsystem = new SwerveSubsystem(); + public final SwerveSubsystem swerveDriveSubsystem = new SwerveSubsystem(); + public final LimelightAssistance limelightAssistance = new LimelightAssistance(swerveDriveSubsystem); // private final LimeLightSubsystem limeLightSubsystem = new // LimeLightSubsystem(); + private final UsbCamera intakeCam = CameraServer.startAutomaticCapture(); private final DriveCommand normalDrive = new DriveCommand(swerveDriveSubsystem, driverXbox.getHID()); - private final ElevatorSubsystem elevator = new ElevatorSubsystem(); + private final CoralSubsystem coralSubsystem = new CoralSubsystem(limelightAssistance,swerveDriveSubsystem); + + private final AlgaeSubsystem algaeSubsystem = new AlgaeSubsystem(); + + private final ClimberSubsystem climberSubsystem = new ClimberSubsystem(operatorXbox.getHID()); /* * The container for the robot. Contains subsystems, OI devices, and commands. @@ -60,54 +96,102 @@ public RobotContainer() { DataLogManager.logNetworkTables(true); DataLogManager.start(); - // NamedCommands.registerCommand("NoNote", new ( - // new WaitForCommand(1.0), - // new WaitUntilCommand(new BooleanSupplier() { - // @Override - // public boolean getAsBoolean() { - // return !intake.containsNote().getAsBoolean(); - // } - // }) - // )); - - /* - * NamedCommands.registerCommand("Shoot Close", new SequentialCommandGroup( - * new InstantCommand(() -> {arm.setArmPreset(Presets.SHOOT_HIGH);}), - * new WaitCommand(2), - * new AlignNoteCommand(intake, shooter), - * new PrepNoteCommand(shooter, intake), - * new PrepShooterCommand(intake, shooter, 0.8), - * new ShootCommand(shooter, intake) - * )); - * NamedCommands.registerCommand("Pickup", new SequentialCommandGroup( - * new InstantCommand(() -> {arm.setArmPreset(Presets.INTAKE);}), - * new IntakeCommand(intake))); - */ - // Test Auto Week 0 - // return new SequentialCommandGroup( - // new InstantCommand(() -> {arm.setArmPreset(Presets.SHOOT_HIGH);}), - // new WaitCommand(2), - // new AlignNoteCommand(intake, shooter), - // new PrepNoteCommand(shooter, intake), - // new PrepShooterCommand(intake, shooter, 0.8), - // new InstantCommand(() -> System.out.println("HELLLLLOOO")), - // new ShootCommand(shooter, intake) - // ); - autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); swerveDriveSubsystem.setDefaultCommand(normalDrive); } - // Command shootAction = - // Command alignAction = ; // Self-deadlines - // Command spoolAction = - // Command intakeAction = ; + private CoralPresets selectedScoringPreset = CoralPresets.STOW; + private CoralPresets lockedPreset = CoralPresets.STOW; + private boolean isScoring = false; + + Trigger coralAquisition = new Trigger(coralSubsystem.isHoldingSupplier()); + Trigger coralInPosition = new Trigger(new BooleanSupplier() { + public boolean getAsBoolean() { + return coralSubsystem.isSupposedToBeInPosition(); + }; + }); + + private void lockCoralArmPreset(CoralPresets preset) { + lockedPreset = preset; + SmartDashboard.putString("Locked Coral Preset", preset.toString()); + } + + private Supplier currentLockedPresetSupplier = new Supplier() { + public CoralPresets get() { + return lockedPreset; + }; + }; + + // go to preset position: + // 1. Stow arm & wrist + // 2. Move elevator + // 3. Deploy arm + // 4. Rotate wrist + private Command getGoToLockedPresetCommand() { + return new InstantCommand(() -> { + //coralSubsystem.autoSetMirror(); + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString()); + }).andThen(new StowArm(coralSubsystem)) + .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier)) + .andThen(new ParallelCommandGroup( + new MovePivot(coralSubsystem, currentLockedPresetSupplier), + new MoveRoll(coralSubsystem, currentLockedPresetSupplier))) + .andThen(new MovePitch(coralSubsystem, currentLockedPresetSupplier)) + .andThen(new InstantCommand(() -> { + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done"); + })); + } + + private Command getGoToLockedPresetCommandV2() { + return new InstantCommand(() -> { + if (currentLockedPresetSupplier.get() == CoralPresets.INTAKE) { + coralSubsystem.autoSetMirrorIntake(); + } else { + coralSubsystem.autoSetMirrorScoring(); + } + + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString()); + }).andThen(new StowArm(coralSubsystem)) + .andThen(new ParallelCommandGroup( + new MoveElevator(coralSubsystem, currentLockedPresetSupplier), + new MovePivot(coralSubsystem, currentLockedPresetSupplier), + new WaitArmClearance(coralSubsystem) + .andThen(new MoveRoll(coralSubsystem, currentLockedPresetSupplier)), + new WaitRollFinished(coralSubsystem).andThen(new WaitElevatorApproach(coralSubsystem, 0.5)) + .andThen(new MovePitch(coralSubsystem, currentLockedPresetSupplier)))) + .andThen(new InstantCommand(() -> { + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done"); + })); + } + + // Goes to a preset more quickly by moving pitch+pivot+roll at the same time, + // but can throw coral. Good for intaking + private Command getGoToLockedPresetFASTCommand() { + return new InstantCommand(() -> { + if (currentLockedPresetSupplier.get() == CoralPresets.INTAKE) { + coralSubsystem.autoSetMirrorIntake(); + } else { + coralSubsystem.autoSetMirrorScoring(); + } + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString()); + }).andThen(new StowArm(coralSubsystem)) + .andThen(new MoveElevator(coralSubsystem, currentLockedPresetSupplier)) + .andThen(new ParallelCommandGroup( + new MovePivot(coralSubsystem, currentLockedPresetSupplier), + new MoveRoll(coralSubsystem, currentLockedPresetSupplier), + new MovePitch(coralSubsystem, currentLockedPresetSupplier))) + .andThen(new InstantCommand(() -> { + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done"); + })); + } - private ElevatorCommand elevatorToTop = new ElevatorCommand(elevator, ElevatorPresets.TOP, 0.0); - private ElevatorCommand elevatorToMiddle = new ElevatorCommand(elevator, ElevatorPresets.MIDDLE, 0.0); - private ElevatorCommand elevatorToStow = new ElevatorCommand(elevator, ElevatorPresets.STOW, 0.0); + private Command getStowCommand() { + return new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.STOW); + }).andThen(getGoToLockedPresetFASTCommand()); + } /** * Use this method to define your trigger->command mappings. Triggers can be @@ -124,28 +208,174 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - operatorXbox.a() - .onTrue(elevatorToStow); - operatorXbox.x() - .onTrue(elevatorToMiddle); - operatorXbox.y() - .onTrue(elevatorToTop); - - operatorXbox.b().whileTrue(new ElevatorFollowCommand(elevator, new DoubleSupplier() { + /* + * operator + */ + // low algae TODO: Make a preset for low reef algae + // operatorXbox.leftBumper().onTrue(new InstantCommand(() -> { + // algaeSubsystem.setAlgaePreset(AlgaePresets.FLOOR); + // })); + // // high algae TODO: Make a preset for high reef algae + // operatorXbox.rightBumper().onTrue(new InstantCommand(() -> { + // algaeSubsystem.setAlgaePreset(AlgaePresets.STOW); + // })); + + // L1 + operatorXbox.a().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_1; + })); + // L2 + operatorXbox.x().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_2; + })); + // L3 + operatorXbox.y().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_3; + })); + // L4 + operatorXbox.b().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_4; + })); + + // Score!!! + operatorXbox.rightTrigger().whileTrue((new InstantCommand(() -> { + // coralSubsystem.setCoralPreset(currentCoralPreset); + lockCoralArmPreset(selectedScoringPreset); + if (!coralSubsystem.isHolding()) + isScoring = true; + }).andThen(getGoToLockedPresetCommandV2().andThen( + new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 1.0); + }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 0.0); + }))))).onlyIf(coralSubsystem.isHoldingSupplier())) + .whileFalse(getStowCommand().alongWith(new InstantCommand(() -> { + isScoring = false; + }))); + + // wrist adjustment + // Hold for now, until everything else is working + // operatorXbox.rightStick().and(new BooleanSupplier() { + // // deadzone + // @Override + // public boolean getAsBoolean() { + // return Math.sqrt(Math.pow(operatorXbox.getRightX(), 2) + + // Math.pow(operatorXbox.getRightY(), 2)) > 0.25; + // } + // }).whileTrue(new CoralWristFollowCommand(coralSubsystem, operatorXbox)); + + // Score coral + /* + * .and(new BooleanSupplier() { + * + * @Override + * public boolean getAsBoolean() { + * return coralSubsystem.isHolding() && isScoring; + * } + * }) + */ + driverXbox.rightBumper().whileTrue(new ScoreCoralCommand(coralSubsystem)); + operatorXbox.rightBumper().whileFalse(getStowCommand()); + driverXbox.rightBumper().whileFalse(getStowCommand()); + + // Intake coral + operatorXbox.rightTrigger().and(new BooleanSupplier() { @Override - public double getAsDouble() { - return (operatorXbox.getLeftY() * -0.5 + 0.5) - * Constants.Elevator.PhysicalParameters.elevatorHeightMeters; + public boolean getAsBoolean() { + return !coralSubsystem.isHolding() && !isScoring; } + }).whileTrue(new InstantCommand(() -> { + System.out.println("Intaking"); + lockCoralArmPreset(CoralPresets.INTAKE); + }).andThen(getGoToLockedPresetFASTCommand()).andThen(new IntakeCoralCommand(coralSubsystem)) + .andThen(getStowCommand())).whileFalse(getStowCommand()); + + // purge coral + operatorXbox.button(7).whileTrue(new PurgeCoralIntakeCommand(coralSubsystem)); + operatorXbox.button(8).onTrue(new InstantCommand(() -> { + climberSubsystem.resetClimberDeploy(); })); - operatorXbox.povUp().debounce(0.02).onTrue(new InstantCommand(() -> { - elevator.setPosition(elevator.getGoalPosition() + 0.1); + coralAquisition.onChange(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 1.0); + driverXbox.setRumble(RumbleType.kBothRumble, 1.0); + }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 0.0); + driverXbox.setRumble(RumbleType.kBothRumble, 0.0); + }))); + + operatorXbox.leftBumper().onTrue(new InstantCommand(() -> { + coralSubsystem.mirrorArm(); })); - operatorXbox.povDown().debounce(0.02).onTrue(new InstantCommand(() -> { - elevator.setPosition(elevator.getGoalPosition() - 0.1); + /* + * driver + */ + // stop the climber + driverXbox.x().onTrue(new InstantCommand(() -> { + climberSubsystem.setOutput(0); })); + // move the climber + driverXbox.y().and(new BooleanSupplier() { + private boolean deployed = true; + + @Override + public boolean getAsBoolean() { + deployed = !(deployed); + return deployed; + } + }).onFalse(new InstantCommand(() -> { + climberSubsystem.setOutput(1); + })).onTrue(new InstantCommand(() -> { + climberSubsystem.setOutput(-1); + })); + // TODO: something something maintainence + driverXbox.button(6).onTrue(new SequentialCommandGroup( + new InstantCommand(() -> { + System.out.print("swaaws"); + }))); + // set field orientation + driverXbox.button(7).onTrue(new InstantCommand(() -> { + swerveDriveSubsystem.setHeading(0); + })); + + /* + * coop + */ + // algae floor / shoot + driverXbox.leftBumper().and(new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return algaeSubsystem.isHolding(); + } + }).whileTrue(new ShootAlgaeCommand(algaeSubsystem)); + + /////////////////// DEBUGGING ////////////////// + debugXboxController.a().onTrue(new InstantCommand(() -> { + coralSubsystem.setCoralPresetPitch(CoralPresets.LEVEL_4); + })).onFalse(new InstantCommand(() -> { + coralSubsystem.setCoralPresetPitch(CoralPresets.STOW); + })); + + debugXboxController.b().onTrue(new InstantCommand(() -> { + coralSubsystem.setCoralPresetRoll(CoralPresets.LEVEL_4); + })).onFalse(new InstantCommand(() -> { + coralSubsystem.setCoralPresetRoll(CoralPresets.STOW); + })); + + debugXboxController.x().onTrue(new InstantCommand(() -> { + coralSubsystem.setCoralPresetPivot(CoralPresets.LEVEL_4); + })).onFalse(new InstantCommand(() -> { + coralSubsystem.setCoralPresetPivot(CoralPresets.STOW); + })); + debugXboxController.y().onTrue(new InstantCommand(() -> { + coralSubsystem.setCoralPresetElevator(CoralPresets.LEVEL_4); + })).onFalse(new InstantCommand(() -> { + coralSubsystem.setCoralPresetElevator(CoralPresets.STOW); + })); + + debugXboxController.rightBumper().whileTrue(new IntakeCoralCommand(coralSubsystem)); + debugXboxController.leftBumper().whileTrue(new ScoreCoralCommand(coralSubsystem)); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index a7707f2..556c6c9 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -140,22 +140,22 @@ public void execute() { // } // Drive Non Field Oriented - if (xbox.getRightBumper()) { - speeds = new ChassisSpeeds(-xSpeed, ySpeed, -zSpeed); + // if (xbox.getRightBumper()) { + // speeds = new ChassisSpeeds(-xSpeed, ySpeed, -zSpeed); - } else if (!xbox.getLeftBumper()) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(-ySpeed, xSpeed, zSpeed, - new Rotation2d( - -swerveSubsystem.getRotation2d().rotateBy(DriveConstants.NAVX_ANGLE_OFFSET).getRadians())); + // } else if (!xbox.getLeftBumper()) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(-ySpeed, xSpeed, zSpeed, + new Rotation2d( + -swerveSubsystem.getRotation2d().rotateBy(DriveConstants.NAVX_ANGLE_OFFSET).getRadians())); // speeds = ChassisSpeeds.fromFieldRelativeSpeeds(-ySpeed, xSpeed, // zSpeed,swerveSubsystem.odometry.getEstimatedPosition().getRotation().rotateBy( // new Rotation2d(FieldConstants.getAlliance() == Alliance.Red ? Math.PI : 0) // )); - } else { + // } else { // Normal non-field oriented - speeds = new ChassisSpeeds(-xSpeed, -ySpeed, zSpeed); - } + // speeds = new ChassisSpeeds(-xSpeed, -ySpeed, zSpeed); + // } // State transition logic switch (state) { diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java index 4018b4f..c933f2d 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorCommand.java @@ -1,51 +1,84 @@ -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.subsystems.ElevatorSubsystem; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorCommand extends Command { - private final ElevatorSubsystem elevatorSub; - - public enum ElevatorPresets { - STOW(0.0), - MIDDLE(Constants.Elevator.PhysicalParameters.elevatorHeightMeters / 2), - TOP(Constants.Elevator.PhysicalParameters.elevatorHeightMeters); - - private ElevatorPresets(double pos_meters) { - this.position_m = pos_meters; - } - - private double position_m; - } - - private ElevatorPresets target = ElevatorPresets.STOW; - private double offset; - - public ElevatorCommand(ElevatorSubsystem elevatorSub, ElevatorPresets targetPosition, double targetOffset) { - this.elevatorSub = elevatorSub; - this.target = targetPosition; - this.offset = targetOffset; - addRequirements(elevatorSub); - } - - @Override - public void initialize() { - double tgt = target.position_m + offset; - elevatorSub.setGoal(MathUtil.clamp(tgt, 0, Constants.Elevator.PhysicalParameters.elevatorHeightMeters)); - SmartDashboard.putString("Elevator Command", target.toString()); - } - - @Override - public boolean isFinished() { - return elevatorSub.isInPosition(); - } - - @Override - public void end(boolean interrupted) { - // NOTE: Don't disable (so it position-holds with feedforward) - // elevatorSub.disable(); - } -} +// package frc.robot.commands; + +// import frc.robot.Constants; +// import frc.robot.subsystems.ElevatorSubsystem; +// import edu.wpi.first.math.MathUtil; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; + +// import static frc.robot.Constants.Elevator.PhysicalParameters.elevatorTravelMeters; + +// /** +// * This command tells the elevator to go to a preset. +// */ +// public class ElevatorCommand extends Command { +// private final ElevatorSubsystem elevatorSub; // A reference to the elevator subsystem + +// private final ElevatorPreset target; // The current selected target +// private final double offset; // An offset for the current target + +// /** +// * Create a new instance of the command. +// * +// * @param elevatorSystem The elevator subsystem +// * @param targetPosition The target position - one of 'STOW', 'MIDDLE' or 'TOP'. See the {@link ElevatorCommand.ElevatorPresets} enum for more info. +// * @param offset An offset for the target position +// */ +// public ElevatorCommand(ElevatorSubsystem elevatorSystem, ElevatorPreset targetPosition, double targetOffset) { +// this.elevatorSub = elevatorSystem; +// this.target = targetPosition; +// this.offset = targetOffset; + +// addRequirements(elevatorSub); +// } + +// @Override +// public void initialize() { +// double tgt = target.getPosition() + offset; // The actual target position, in meters + +// // Set the goal of the elevator subsystem after clamping it to a reasonable value. +// elevatorSub.setGoal(MathUtil.clamp(tgt, 0, elevatorTravelMeters)); + +// // Put it on smartdashboard +// SmartDashboard.putString("Elevator Command", target.toString()); +// } + +// /** +// * Check if the elevator is in its goal position. +// * +// * @return If the elevator is in the current 'goal' position +// */ +// @Override +// public boolean isFinished() { +// return elevatorSub.isInPosition(); +// } + +// @Override +// public void end(boolean interrupted) { +// // NOTE: Don't disable (so it position-holds with feedforward) +// // elevatorSub.disable(); +// } + +// /** +// * Various presets for the elevator +// */ +// public enum ElevatorPreset { +// STOW(0.0), +// MIDDLE(elevatorTravelMeters / 2), +// TOP(elevatorTravelMeters); + +// // The position of the preset in meters +// private final double position; + +// private ElevatorPreset(double pos_meters) { +// this.position = pos_meters; +// } + +// /** +// * Returns the position of the elevator preset in meters +// */ +// public double getPosition() { +// return this.position; +// } +// } +// } diff --git a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java b/src/main/java/frc/robot/commands/ElevatorFollowCommand.java deleted file mode 100644 index 03a2aa1..0000000 --- a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java +++ /dev/null @@ -1,55 +0,0 @@ -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.subsystems.ElevatorSubsystem; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorFollowCommand extends Command { - private final ElevatorSubsystem elevatorSub; - private double lastTarget = 0; - private double lastTime = 0; - private DoubleSupplier target; - private SlewRateLimiter limiter = new SlewRateLimiter(Constants.Elevator.PID.MAX_ACCELERATION / 10.0); - - public ElevatorFollowCommand(ElevatorSubsystem elevatorSub, DoubleSupplier target) { - this.elevatorSub = elevatorSub; - this.target = target; - addRequirements(elevatorSub); - } - - @Override - public void initialize() { - elevatorSub.enable(); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void execute() { - double tgt = target.getAsDouble(); - double dX = tgt - lastTarget; - double dt = Timer.getFPGATimestamp() - lastTime; - lastTime = Timer.getFPGATimestamp(); - lastTarget = tgt; - elevatorSub.setGoal(new TrapezoidProfile.State( - MathUtil.clamp(limiter.calculate(tgt), 0, - Constants.Elevator.PhysicalParameters.elevatorHeightMeters), - 0.0)); - } - - @Override - public void end(boolean interrupted) { - // NOTE: Don't disable (so it position-holds with feedforward) - // elevatorSub.disable(); - } -} diff --git a/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java new file mode 100644 index 0000000..f46b5df --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java @@ -0,0 +1,29 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; + +public class ShootAlgaeCommand extends Command { + private final AlgaeSubsystem subsystem; + + public ShootAlgaeCommand(AlgaeSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.SHOOT); + } + + @Override + public void end(boolean interrupted) { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP); + } + + @Override + public boolean isFinished() { + return !subsystem.isHolding(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/CoralWristFollowCommand.java b/src/main/java/frc/robot/commands/coral/CoralWristFollowCommand.java new file mode 100644 index 0000000..216069c --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/CoralWristFollowCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.coral.CoralSubsystem; + +public class CoralWristFollowCommand extends Command { + + private final CoralSubsystem subsystem; + private final CommandXboxController operator; + + public CoralWristFollowCommand(CoralSubsystem subsystem, CommandXboxController operator) { + this.subsystem = subsystem; + this.operator = operator; + + addRequirements(subsystem); + } + + @Override + public void execute() { + subsystem.setCustomPitchDegrees((operator.getRightY() * 10) + subsystem.getPitchGoalDegrees()); + subsystem.setCustomRollDegrees((operator.getRightX() * 10) + subsystem.getRollGoalDegrees()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java new file mode 100644 index 0000000..6d77952 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java @@ -0,0 +1,45 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets; + +public class IntakeCoralCommand extends Command { + private final CoralSubsystem subsystem; + + public IntakeCoralCommand(CoralSubsystem subsystem) { + this.subsystem = subsystem; + // addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setCoralIntakePreset(CoralIntakePresets.INTAKE); + SmartDashboard.putString("Intake Command", "Started"); + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Coral", true); + } + } + + @Override + public void end(boolean interrupted) { + // if intaking failed + if (!subsystem.isHolding()) { + subsystem.setCoralIntakePreset(CoralIntakePresets.STOP); + SmartDashboard.putString("Intake Command", "Stopped"); + } else { + SmartDashboard.putString("Intake Command", "Holding"); + + subsystem.setCoralIntakePreset(CoralIntakePresets.HOLD); + } + // else continue to hold the piece + } + + @Override + public boolean isFinished() { + return subsystem.isHolding(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java b/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java new file mode 100644 index 0000000..030008b --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java @@ -0,0 +1,23 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets; + +public class PurgeCoralIntakeCommand extends Command { + private final CoralSubsystem subsystem; + + public PurgeCoralIntakeCommand(CoralSubsystem subsystem) { + this.subsystem = subsystem; + } + + @Override + public void initialize() { + subsystem.setCoralIntakePreset(CoralIntakePresets.PURGE); + } + + @Override + public void end(boolean interrupted) { + subsystem.setCoralIntakePreset(CoralIntakePresets.STOP); + } +} diff --git a/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java new file mode 100644 index 0000000..6d68113 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java @@ -0,0 +1,48 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets; + +public class ScoreCoralCommand extends Command { + + private final CoralSubsystem subsystem; + private double coralExitTime = 0; + + public ScoreCoralCommand(CoralSubsystem subsystem) { + this.subsystem = subsystem; + // addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setCoralIntakePreset(CoralIntakePresets.SCORE); + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Coral", false); + } + } + + @Override + public void end(boolean interrupted) { + subsystem.setCoralIntakePreset(CoralIntakePresets.STOP); + } + + @Override + public boolean isFinished() { + // if (!subsystem.isHolding() && coralExitTime == 0) { + // coralExitTime = Timer.getFPGATimestamp(); + // } + // return !subsystem.isHolding() + // && ((Timer.getFPGATimestamp() - coralExitTime) > + // Constants.Coral.Intake.SCORE_EXTRA_SECONDS); + + // Just keep going at it! + // For autos, might want a different command? + return Robot.isSimulation(); + } + +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java new file mode 100644 index 0000000..cb01ef6 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java @@ -0,0 +1,37 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MoveElevator extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MoveElevator(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + this.presetSupplier = presetSupplier; + addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Elevator"); + SmartDashboard.putString("Move Elevator", "Started"); + coralSub.setCoralPresetElevator(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Elevator", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isElevatorSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java new file mode 100644 index 0000000..5694dcc --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java @@ -0,0 +1,39 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MovePitch extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MovePitch(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + this.presetSupplier = presetSupplier; + // HACK: This is much sketch + // addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Pitch"); + SmartDashboard.putString("Move Pitch", "Start"); + + coralSub.setCoralPresetPitch(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Pitch", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isPitchSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java new file mode 100644 index 0000000..f4dddfc --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java @@ -0,0 +1,38 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MovePivot extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MovePivot(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + this.presetSupplier = presetSupplier; + // addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Pivot"); + SmartDashboard.putString("Move Pivot", "Start"); + + coralSub.setCoralPresetPivot(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Pivot", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isPivotSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java new file mode 100644 index 0000000..e380020 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java @@ -0,0 +1,38 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MoveRoll extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MoveRoll(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + // addRequirements(coralSub); + this.presetSupplier = presetSupplier; + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Roll"); + SmartDashboard.putString("Move Roll", "Start"); + + coralSub.setCoralPresetRoll(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Roll", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isRollSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/StowArm.java b/src/main/java/frc/robot/commands/coral/motion/StowArm.java new file mode 100644 index 0000000..9a73749 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/StowArm.java @@ -0,0 +1,30 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class StowArm extends Command { + private CoralSubsystem coralSub; + + public StowArm(CoralSubsystem coralSub) { + this.coralSub = coralSub; + addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Stowing Arm"); + coralSub.setCoralPresetPitch(CoralPresets.STOW); + coralSub.setCoralPresetRoll(CoralPresets.STOW); + coralSub.setCoralPresetPivot(CoralPresets.STOW); + } + + @Override + public boolean isFinished() { + // TODO: For the future... might want to wait? or not. + return true; + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java b/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java new file mode 100644 index 0000000..54402d6 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java @@ -0,0 +1,28 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WaitArmClearance extends Command { + private CoralSubsystem coralSub; + + public WaitArmClearance(CoralSubsystem coralSub) { + this.coralSub = coralSub; + // addRequirements(coralSub); + } + + @Override + public void initialize() { + + } + + @Override + public boolean isFinished() { + return Math.abs(coralSub.getPivotPositionDegrees()) > Units + .radiansToDegrees(Constants.Coral.Pivot.ELEVATOR_BORDER_ANGLE); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java b/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java new file mode 100644 index 0000000..0df8c59 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java @@ -0,0 +1,28 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WaitElevatorApproach extends Command { + private CoralSubsystem coralSub; + private double metersBefore; + + public WaitElevatorApproach(CoralSubsystem coralSub, double metersBefore) { + this.coralSub = coralSub; + this.metersBefore = metersBefore; + } + + @Override + public void initialize() { + System.out.printf("Finishing %f meters before\n", metersBefore); + } + + @Override + public boolean isFinished() { + return coralSub.getElevator().getPosition() > (coralSub.getElevator().getGoalPosition() - metersBefore); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java b/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java new file mode 100644 index 0000000..3489ad5 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java @@ -0,0 +1,27 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WaitRollFinished extends Command { + private CoralSubsystem coralSub; + + public WaitRollFinished(CoralSubsystem coralSub) { + this.coralSub = coralSub; + // addRequirements(coralSub); + } + + @Override + public void initialize() { + + } + + @Override + public boolean isFinished() { + return (Math.abs(coralSub.getRollGoalDegrees()) > 10) && coralSub.isRollSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..114f06c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Climber; + +public class ClimberSubsystem extends SubsystemBase { + private final SparkMax climberMotor = new SparkMax(Climber.MOTOR_PORT, MotorType.kBrushless); + private SparkMaxConfig climberConfig = new SparkMaxConfig(); + private RelativeEncoder climberEncoder; + XboxController operator; + + boolean isTested = false; + + public ClimberSubsystem(XboxController operator) { + this.operator = operator; + climberMotor.configure(climberConfig.idleMode(IdleMode.kBrake) + .apply(new SoftLimitConfig().reverseSoftLimit(Constants.Climber.DEPLOY_SOFT_LIMIT) + .reverseSoftLimitEnabled(true).forwardSoftLimit(0.0) + .forwardSoftLimitEnabled(true)) + .apply( + new EncoderConfig().positionConversionFactor(1.0 / Constants.Climber.GEAR_RATIO) + .velocityConversionFactor( + 1.0 / Constants.Climber.GEAR_RATIO)), + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + + climberEncoder = climberMotor.getEncoder(); + } + + private double output = 0.0; + private boolean deployed = false; + + @Override + public void periodic() { + SmartDashboard.putNumber("Climber Encoder", climberEncoder.getPosition()); + if (climberEncoder.getPosition() < Constants.Climber.CLIMB_SOFT_LIMIT && + !deployed) { + deployed = true; + climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit( + Constants.Climber.CLIMB_SOFT_LIMIT)), + ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + if (DriverStation.isTestEnabled() & !isTested) { + climberMotor.configure( + climberConfig.apply( + climberConfig.softLimit.forwardSoftLimitEnabled(false).reverseSoftLimitEnabled(false)), + ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + isTested = false; + } + + if (!Constants.Climber.DBG_DISABLED) + climberMotor.set(operator.getLeftBumper() ? (MathUtil.applyDeadband(operator.getLeftY(), 0.05)) : 0.0); + } + + public void resetClimberDeploy() { + climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(0.0)), + ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + public void setOutput(double output) { + this.output = output; + } + + public double getOutput() { + return output; + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java deleted file mode 100644 index 26f9baa..0000000 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ /dev/null @@ -1,238 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkLimitSwitch; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.datalog.DoubleLogEntry; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; -import frc.robot.Constants; -import frc.robot.Robot; - -public class ElevatorSubsystem extends ProfiledPIDSubsystem { - // NOTE: Elevator motor one has both the encoder used for positioning, and the - // limit switch used for zeroing - private final SparkFlex elevatorMotorOne = new SparkFlex(Constants.Elevator.elevatorOnePort, - MotorType.kBrushless); - private final SparkFlex elevatorMotorTwo = new SparkFlex(Constants.Elevator.elevatorTwoPort, - MotorType.kBrushless); - private final ElevatorFeedforward feedForward = new ElevatorFeedforward( - Constants.Elevator.Feedforward.Ks, - Constants.Elevator.Feedforward.Kg, - Constants.Elevator.Feedforward.Kv, - Constants.Elevator.Feedforward.Ka - - ); - - private final SparkMaxConfig elevatorConfigOne = new SparkMaxConfig(); - private final SparkMaxConfig elevatorConfigTwo = new SparkMaxConfig(); - - /* - * DCMotor gearbox, - * double gearing, - * double carriageMassKg, - * double drumRadiusMeters, - * double minHeightMeters, - * double maxHeightMeters, - * boolean simulateGravity, - * double startingHeightMeters, - * Matrix measurementStdDevs - */ - private boolean isZeroed = false; - private final RelativeEncoder elevatorEncoderOne; - private final RelativeEncoder elevatorEncoderTwo; - private final SparkLimitSwitch bottomLimit; - - private final ElevatorSim simulation = new ElevatorSim( - Constants.Elevator.PhysicalParameters.simMotor, - Constants.Elevator.PhysicalParameters.gearReduction, - Constants.Elevator.PhysicalParameters.carriageMassKg, - Constants.Elevator.PhysicalParameters.driveRadiusMeters, - 0.0, - Constants.Elevator.PhysicalParameters.elevatorHeightMeters, - true, - 0.0, - 0.001, - 0.001 - ); - - private DoubleLogEntry elevatorTargetP = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/target/position"); - private DoubleLogEntry elevatorTargetV = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/target/velocity"); - private DoubleLogEntry elevatorP = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/state/position"); - private DoubleLogEntry elevatorV = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/state/velocity"); - private DoubleLogEntry elevatorOneOutput = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/output1"); - private DoubleLogEntry elevatorTwoOutput = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/output2"); - private DoubleLogEntry elevatorOneCurrent = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/current1"); - private DoubleLogEntry elevatorTwoCurrent = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/current2"); - - private Mechanism2d mechanism2D = new Mechanism2d(Constants.RobotConstants.robotLengthMeters, - Constants.Elevator.PhysicalParameters.elevatorBottomFromFloorMeters - + Constants.Elevator.PhysicalParameters.elevatorHeightMeters - + Constants.Elevator.PhysicalParameters.elevatorCarriageHeightMeters / 2.0); - private MechanismRoot2d rootMechanism = mechanism2D.getRoot("climber", - Constants.RobotConstants.robotLengthMeters / 2.0 - + Constants.Elevator.PhysicalParameters.elevatorForwardsFromRobotCenterMeters, - Constants.Elevator.PhysicalParameters.elevatorBottomFromFloorMeters); - private MechanismLigament2d elevatorMechanism; - - public ElevatorSubsystem() { - super( - new ProfiledPIDController( - Constants.Elevator.PID.kP, - Constants.Elevator.PID.kI, - Constants.Elevator.PID.kD, - new TrapezoidProfile.Constraints( - Constants.Elevator.PID.MAX_VELOCITY, - Constants.Elevator.PID.MAX_ACCELERATION)), - 0.0); - - this.getController().setTolerance(Units.inchesToMeters(0.5)); - - elevatorConfigOne - .idleMode(IdleMode.kBrake) - .inverted(Constants.Elevator.elevatorOneInverted); - elevatorConfigOne.encoder - .positionConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter) - .velocityConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter); - elevatorConfigOne.limitSwitch.reverseLimitSwitchType(Constants.Elevator.bottomLimitMode); - - elevatorConfigTwo - .idleMode(IdleMode.kBrake) - .inverted(Constants.Elevator.elevatorTwoInverted); - elevatorConfigTwo.encoder - .positionConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter) - .velocityConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter); - - elevatorMotorOne.configure(elevatorConfigOne, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - elevatorMotorTwo.configure(elevatorConfigTwo, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - elevatorEncoderOne = elevatorMotorOne.getEncoder(); - elevatorEncoderTwo = elevatorMotorTwo.getEncoder(); - - elevatorMotorOne.getEncoder().setPosition(0); - elevatorMotorTwo.getEncoder().setPosition(0); - - bottomLimit = elevatorMotorOne.getReverseLimitSwitch(); - - this.enable(); - - this.elevatorMechanism = rootMechanism.append(new MechanismLigament2d("elevator", 0.0, 90)); - } - - public void setPosition(double positionMeters) { - setGoal(MathUtil.clamp(positionMeters, 0.0, Constants.Elevator.PhysicalParameters.elevatorHeightMeters)); - } - - public double getPosition() { - return getMeasurement(); - } - - public double getGoalPosition() { - return this.getController().getGoal().position; - } - - double lastVelocity = 0.0; - double lastTime = 0.0; - - @Override - public void useOutput(double output, TrapezoidProfile.State setpoint) { - double dv = setpoint.velocity - lastVelocity; - double dt = Timer.getFPGATimestamp() - lastTime; - lastTime = Timer.getFPGATimestamp(); - - lastVelocity = setpoint.velocity; - - double ff = feedForward.calculate(setpoint.velocity, dv / dt); - - if (isZeroed || Robot.isSimulation()) { - // Controller output voltage - double op = ff + output; - - elevatorMotorOne.setVoltage(op); - elevatorMotorTwo.setVoltage(op); - - elevatorTargetP.append(setpoint.position); - elevatorTargetV.append(setpoint.velocity); - - SmartDashboard.putNumber("Elevator/Current Position", getMeasurement()); - SmartDashboard.putNumber("Elevator/Current Velocity", elevatorEncoderOne.getVelocity()); - - SmartDashboard.putNumber("Elevator/Target Position", setpoint.position); - SmartDashboard.putNumber("Elevator/Target Velocity", setpoint.velocity); - - if (Robot.isSimulation()) { - simulation.setInputVoltage(MathUtil.clamp((output + ff), -12.0, 12.0)); - } - } else { - // Move down a little bit to zero - elevatorMotorOne.set(-0.05); - elevatorMotorTwo.set(-0.05); - } - } - - @Override - public double getMeasurement() { - return Robot.isSimulation() ? simulation.getPositionMeters() : elevatorEncoderOne.getPosition(); - } - - @Override - public void periodic() { - // TODO Auto-generated method stub - - if ((bottomLimit.isPressed()) && (!isZeroed)) { - elevatorEncoderOne.setPosition(0.0); - setGoal(0.0); - isZeroed = true; - } - - super.periodic(); - - elevatorP.append(getMeasurement()); - elevatorV.append(elevatorEncoderOne.getVelocity()); - elevatorOneOutput.append(elevatorMotorOne.getAppliedOutput()); - elevatorTwoOutput.append(elevatorMotorTwo.getAppliedOutput()); - elevatorOneCurrent.append(elevatorMotorOne.getOutputCurrent()); - elevatorTwoCurrent.append(elevatorMotorTwo.getOutputCurrent()); - this.elevatorMechanism - .setLength(getMeasurement() + Constants.Elevator.PhysicalParameters.elevatorCarriageHeightMeters / 2.0); - - // SmartDashboard.putNumber("Current Elevator Position", getMeasurement()); - // SmartDashboard.putNumber("Goal Elevator Position", - // this.getController().getGoal().position); - SmartDashboard.putBoolean("Elevator Bottom Limit", bottomLimit.isPressed()); - SmartDashboard.putData("Elevator Mechanism", mechanism2D); - } - - @Override - public void simulationPeriodic() { - // TODO Auto-generated method stub - super.simulationPeriodic(); - - simulation.update(0.020); - - RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(simulation.getCurrentDrawAmps())); - } - - public boolean isInPosition() { - return this.getController().atGoal(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java new file mode 100644 index 0000000..fdf448a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -0,0 +1,226 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; +import frc.robot.LimelightHelpers.RawFiducial; +import frc.robot.util.LimelightContainer; + +public class Limelight extends SubsystemBase { + public enum LimelightType { + // LL1 doesn't have specs listed, so these could be incorrect + LL1(54, 41), + LL2(62.5, 48.9), + LL2Plus(62.5, 48.9), + LL3(62.5, 48.9), + LL3G(82, 56.2), + LL4(82, 56.2); + + private double HFOV; + private double VFOV; + + private LimelightType(double HFOV, double VFOV) { + this.HFOV = HFOV; + this.VFOV = VFOV; + } + } + + private final LimelightType limelightType; + private final String name; + private boolean isEnabled; + private boolean cropEnabled; + private double lastPoseEstimate = 0; + private int counter = 0; + private double lastFrame = 0; + + private final StructPublisher publisher; + + + public Limelight(LimelightType limelightType, String name, boolean isEnabled, boolean cropEnabled) { + this.limelightType = limelightType; + this.name = name; + this.isEnabled = isEnabled; + this.cropEnabled = cropEnabled; + + publisher = NetworkTableInstance.getDefault().getStructTopic(name, Pose2d.struct).publish(); + + LimelightHelpers.SetIMUMode(name, 1); + } + + @Override + public void periodic() { + if (isEnabled) { + if (cropEnabled) { + if (numTargets() > 0) { + smartCrop(); + } else { + restoreCrop(); + } + } + } + } + + public int numTargets() { + return LimelightHelpers.getRawFiducials(name).length; + } + + public void smartCrop() { + LimelightHelpers.PoseEstimate poseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + if(poseEstimate == null){return;} + if (lastPoseEstimate != poseEstimate.timestampSeconds){ + counter = ++counter % 50; + if(counter > 40){ // For every 5 frames, out of 50, check the entire screen for apriltags + restoreCrop(); // 45 will be cropped onto whichever limelights they find + return; + } + lastPoseEstimate = poseEstimate.timestampSeconds; //making sure we're only running this process on new frames + RawFiducial[] targets = LimelightHelpers.getRawFiducials(name); //get our targets + boolean useCrop = true; //we are currently using it, more changes will be implemented + Translation2d[] targetTranslations = new Translation2d[targets.length]; + double area = 0; + + for (int i = 0; i < targetTranslations.length; ++i) { + targetTranslations[i] = new Translation2d(targets[i].txnc, targets[i].tync); + area = Math.max(area, targets[i].ta); //Finding the largest area of the targets + } + + double xc = 0, yc = 0; + + for (int i = 0; i < targetTranslations.length; ++i) { // Add to the width, depending on the locations of the other targets + xc += targetTranslations[i].getX(); //If one is at (2, 1), the width of our box increases on each size by 2, and the height by 1 + yc += targetTranslations[i].getY(); //xc & xy increase per target + } + + xc /= targetTranslations.length; //Then, average depending on the number of targets + yc /= targetTranslations.length; // (2,1), (.05, .25), (-1, 2) -> xc =.35, yc= .75 + + xc = xc / (limelightType.HFOV / 2); // scales it to FOV + yc = yc / (limelightType.VFOV / 2); + + double borderx = 0, bordery = 0; + + if (targetTranslations.length > 1) { // for more than one tag + borderx = (area + 0.75) * 0.22 * targetTranslations.length + .2; // relatively randomly generated border + bordery = (area + 0.5) * 0.22 * targetTranslations.length; + } + + //todo optimize + + else { + if (area > 0.75){ + borderx = 1; + bordery = .5; + } + else if (area > 0.5) { //Remember that "area" only refers to the area of the largest apriltag + borderx = 0.8; + bordery = 0.5; + } + else if (area > .015){ + borderx = 1; + bordery = 0.25; + } + else if (area > .005){ + borderx = 1; + bordery = .5; + } + else { + borderx = 1; + bordery = .5; + } + } + + /*else { + if (area < 0.015) { + borderx = 0.5; + bordery = 0.25; + } + else if (area > .005){ + borderx = 1; + bordery = 0.5; + } + else { + borderx = 0.5; + bordery = 0.5; + } + } + */ + //borderx = 1; + //bordery = .25; + // Doesn't let borders extend beyond -1, 1 for x & y + double xlim = (xc - borderx < -1) ? -1 : xc - borderx; + double xlim2 = (xc - borderx > 1) ? 1 : xc + borderx; + double ylim = (yc - bordery < -1) ? -1 : yc - borderx; + double ylim2 = (yc - bordery > 1) ? 1 : xc + borderx; + + if (useCrop) { + LimelightHelpers.setCropWindow(name, xlim, xlim2, ylim, ylim2); + // Finds the center of the targets, tries to build a big enough box from there + } + } + } + + public void setIMUMode(int mode) { + LimelightHelpers.SetIMUMode(name, mode); + } + + public void restoreCrop() { + LimelightHelpers.setCropWindow(name, -1, 1, -1, 1); + } + + + public void setEnabled(boolean enabled) { + this.isEnabled = enabled; + } + + public void setCropEnabled(boolean enabled) { + this.cropEnabled = enabled; + } + + public boolean isEnabled() { + return isEnabled; + } + + public boolean isCropEnabled() { + return cropEnabled; + } + + public double getLastFrameTime(){ + return lastFrame; + } + + public void setLastFrame(double lastFrameTime){ + lastFrame = lastFrameTime; + } + + public double getVFOV() { + return limelightType.VFOV; + } + + public double getHFOV() { + return limelightType.HFOV; + } + + public LimelightType getLimelightType() { + return limelightType; + } + + public String getName() { + return name; + } + + /** + * Used to put a pose on Shuffleboard for debugging - Don't repeatadly call + * this! + * + * @param name Name of pose + * @param pose Pose2d pose + */ + public void pushPoseToShuffleboard(String name, Pose2d pose) { + publisher.set(pose); + } + +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 7da0d6c..acad780 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -55,7 +55,8 @@ public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, dou driveMotor = new TalonFX(driveCanID); driveConfigurator = driveMotor.getConfigurator(); driveConfig = new MotorOutputConfigs(); - driveConfig.Inverted = motorReversed ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + driveConfig.Inverted = motorReversed ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; driveConfig.NeutralMode = NeutralModeValue.Brake; driveConfigurator.apply(driveConfig); @@ -100,7 +101,7 @@ public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, dou } public void simulate_step() { - driveEncSim += 0.02 * driveMotor.get() * (DriveConstants.MAX_MODULE_VELOCITY); + driveEncSim += 0.02 * driveMotor.get() * (DriveConstants.MAX_MODULE_VELOCITY) / SwerveModuleConstants.DRIVE_GEAR_RATIO * 0.8; steerEncSim += 0.02 * steerMotor.get() * (10.0); } @@ -109,12 +110,12 @@ public double getDrivePosition() { return driveEncSim; // return driveMotorEncoder.getPosition(); // TODO: Do the conversion in the motor - return driveMotor.getPosition().getValueAsDouble()* SwerveModuleConstants.DRIVE_ROTATION_TO_METER; + return driveMotor.getPosition().getValueAsDouble() * SwerveModuleConstants.DRIVE_ROTATION_TO_METER; } public double getDriveVelocity() { // return driveMotorEncoder.getVelocity(); - return driveMotor.getVelocity().getValueAsDouble() * SwerveModuleConstants.DRIVE_ROTATION_TO_METER; + return driveMotor.getVelocity().getValueAsDouble() * SwerveModuleConstants.DRIVE_METERS_PER_MINUTE; } public double getSteerPosition() { @@ -152,7 +153,8 @@ public SwerveModulePosition getModulePosition() { public void setModuleStateRaw(SwerveModuleState state) { state.optimize(new Rotation2d(getSteerPosition())); double drive_command = state.speedMetersPerSecond / DriveConstants.MAX_MODULE_VELOCITY; - // SmartDashboard.putNumber("Module " + Integer.toString(this.thisModuleNumber) + " Drive", drive_command); + // SmartDashboard.putNumber("Module " + Integer.toString(this.thisModuleNumber) + // + " Drive", drive_command); driveMotor.set(drive_command * (motor_inv ? -1.0 : 1.0)); // This is stupid diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 45338f4..7d92bbd 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -19,6 +19,8 @@ import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -35,6 +37,7 @@ import frc.robot.LimelightHelpers; import frc.robot.Robot; import frc.robot.Constants.*; +import frc.robot.RobotContainer; public class SwerveSubsystem extends SubsystemBase { SwerveModule frontLeft = new SwerveModule(SwerveModuleConstants.FL_STEER_ID, SwerveModuleConstants.FL_DRIVE_ID, @@ -87,6 +90,8 @@ public enum RotationStyle { private Field2d field = new Field2d(); + private final StructPublisher publisher; + boolean isalliancereset = false; // TODO: Properly set starting pose @@ -111,7 +116,8 @@ public SwerveSubsystem() { this::getPose, // Robot pose supplier this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT + // RELATIVE ChassisSpeeds PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, PathPlannerConstants.ROBOT_CONFIG, () -> { @@ -130,6 +136,8 @@ public SwerveSubsystem() { this // Reference to this subsystem to set requirements ); + publisher = NetworkTableInstance.getDefault().getStructTopic("Odometry Pose", Pose2d.struct).publish(); + NamedCommands.registerCommand("namedCommand", new PrintCommand("Ran namedCommand")); chassisAccelX = new DoubleLogEntry(DataLogManager.getLog(), "Chassis/acceleration/x"); @@ -143,24 +151,21 @@ public SwerveSubsystem() { @Override public void periodic() { - if (!isalliancereset && DriverStation.getAlliance().isPresent()) { - Translation2d pospose = getPose().getTranslation(); - odometry.resetPosition(getRotation2d(), getModulePositions(), - new Pose2d(pospose, new Rotation2d(FieldConstants.getAlliance() == Alliance.Blue ? 0.0 : Math.PI))); + if ((!isalliancereset && DriverStation.getAlliance().isPresent())) { + navX.setAngleAdjustment(navxSim); + RobotContainer.LLContainer.estimateMT1OdometryPrelim(odometry, lastChassisSpeeds, navX, getModulePositions()); + SmartDashboard.putString("Prelim odometry position", odometry.getEstimatedPosition().toString()); isalliancereset = true; } - // TODO: Test - // WARNING: REMOVE IF USING TAG FOLLOW!!! - // updateVisionOdometry(); - - if (DriverStation.isTeleopEnabled()) { - updateMegaTagOdometry(); - } else { - updateVisionOdometry(); - } + RobotContainer.LLContainer.estimateMT1Odometry(odometry, lastChassisSpeeds, navX); odometry.update(getRotation2d(), getModulePositions()); + + SmartDashboard.putNumber("NavX angle", getHeading()); + + publisher.set(getPose()); + // System.out.println(getPose().getX()); // if (DriverStation.getAlliance().isPresent()) { // switch (DriverStation.getAlliance().get()) { // case Red: @@ -327,27 +332,28 @@ public Command loadPath(String name) { public Command followPathCommand(String pathName) { try { PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - - return new FollowPathCommand( - path, - this::getPose, // Robot pose supplier - this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, - PathPlannerConstants.ROBOT_CONFIG, - () -> { - // Boolean supplier that controls when the path will be mirrored for the red - // alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements + return new FollowPathCommand( + path, + this::getPose, // Robot pose supplier + this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given + // ROBOT RELATIVE ChassisSpeeds + PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, + PathPlannerConstants.ROBOT_CONFIG, + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements ); } catch (Exception exception) { return Commands.none(); @@ -372,60 +378,6 @@ public PathPlannerPath generateOTFPath(PathPoint... pathPoints) { return path; } - public void updateVisionOdometry() { - boolean doRejectUpdate = false; - LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight"); - - if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { - if (mt1.rawFiducials[0].ambiguity > .7) { - doRejectUpdate = true; - } - // if (mt1.rawFiducials[0].distToCamera > 3) { - if (mt1.rawFiducials[0].distToCamera > 5) { // TODO: TUNE!!! - - doRejectUpdate = true; - } - } - if (mt1.tagCount == 0) { - doRejectUpdate = true; - } - - if (!doRejectUpdate) { - // odometry.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); - odometry.setVisionMeasurementStdDevs(createVisionMeasurementStdDevs( - PoseConstants.kVisionStdDevX, - PoseConstants.kVisionStdDevY, - PoseConstants.kVisionStdDevTheta)); - odometry.addVisionMeasurement( - mt1.pose, - mt1.timestampSeconds); - } - } - - public void updateMegaTagOdometry() { - boolean doRejectUpdate = false; - LimelightHelpers.SetRobotOrientation("limelight", odometry.getEstimatedPosition().getRotation().getDegrees(), 0, - 0, 0, 0, 0); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); - if (Math.abs(navX.getRate()) > 720) // if our angular velocity is greater than 720 degrees per second, ignore - // vision updates - { - doRejectUpdate = true; - } - - if (mt2.tagCount <= 0) { - doRejectUpdate = true; - } - if (!doRejectUpdate) { - // odometry.setVisionMeasurementStdDevs(VecBuilder.fill(2,2,2.0*PoseConstants.kVisionStdDevTheta)); - odometry.setVisionMeasurementStdDevs(VecBuilder.fill(2, 2, 9999999)); - - odometry.addVisionMeasurement( - mt2.pose, - mt2.timestampSeconds); - } - } - public Vector createStateStdDevs(double x, double y, double theta) { return VecBuilder.fill(x, y, Units.degreesToRadians(theta)); } diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java new file mode 100644 index 0000000..cfe5410 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems.algae; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.sim.SparkFlexSim; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.Constants.Algae; + +public class AlgaeArm extends SubsystemBase { + private final SparkFlex pivotMotor = new SparkFlex(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless); + // sim motor + private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Algae.Pivot.PhysicalConstants.MOTOR); + + public final CANcoder pivotEncoder = new CANcoder(Algae.Pivot.ENCODER_PORT); + + // physics simulation + private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim( + Algae.Pivot.PhysicalConstants.MOTOR, + Algae.Pivot.PhysicalConstants.GEARING, + Algae.Pivot.PhysicalConstants.MOI, + Algae.Pivot.PhysicalConstants.ARM_LENGTH_METERS, + Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES), + Units.degreesToRadians(Algae.Pivot.EXTENDED_LIMIT_DEGREES), + true, + Units.degreesToRadians(Algae.Pivot.RETRACTED_LIMIT_DEGREES)); + + private final SparkMaxConfig pivotConfig = new SparkMaxConfig(); + + private final ProfiledPIDController pivotPID = Algae.Pivot.PID; + private final ArmFeedforward pivotFeedforward = Algae.Pivot.FEEDFORWARD; + + public AlgaeArm() { + pivotConfig.idleMode(IdleMode.kBrake); + pivotMotor.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void periodic() { + double output = pivotPID.calculate( + pivotEncoder.getAbsolutePosition().getValueAsDouble() + + pivotFeedforward.calculate(Units.degreesToRadians(this.getGoalDegrees()), 0.0)); + + pivotMotor.set(output); + if (Robot.isSimulation()) + simPivotMotor.setAppliedOutput(output); + } + + @Override + public void simulationPeriodic() { + // update physics + simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage()); + simPivotPhysics.update(0.02); + + // update sim objects + simPivotMotor.iterate( + Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()), + RoboRioSim.getVInVoltage(), + 0.02); + } + + public void setGoalDegrees(double goal) { + pivotPID.setGoal(MathUtil.clamp(goal, Algae.Pivot.RETRACTED_LIMIT_DEGREES, Algae.Pivot.EXTENDED_LIMIT_DEGREES)); + } + + public double getGoalDegrees() { + return pivotPID.getGoal().position; + } + + public double getPositionDegrees() { + return Robot.isReal() + ? pivotEncoder.getAbsolutePosition().getValueAsDouble() + : Units.radiansToDegrees(simPivotPhysics.getAngleRads()); + } +} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java new file mode 100644 index 0000000..66e6419 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems.algae; + +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.Algae; +import frc.robot.Robot; + +public class AlgaeIntake extends SubsystemBase { + private final SparkMax intakeMotor = new SparkMax(Algae.Intake.MOTOR_PORT, MotorType.kBrushless); + // sim motor + private final SparkMaxSim simIntakeMotor = new SparkMaxSim(intakeMotor, Algae.Intake.PhysicalConstants.MOTOR); + + // physics simulation + private final FlywheelSim simIntakePhysics = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + Algae.Intake.PhysicalConstants.MOTOR, + Algae.Intake.PhysicalConstants.MOI, + Algae.Intake.PhysicalConstants.GEARING), + Algae.Intake.PhysicalConstants.MOTOR); + + private final DigitalInput intakeBeambreak = new DigitalInput(Algae.Intake.BEAMBREAK_PORT); + + private final SlewRateLimiter intakeProfile = new SlewRateLimiter( + Algae.Intake.POSITIVE_RATE_LIMIT, + Algae.Intake.NEGATIVE_RATE_LIMIT, + 0); + + private double outputPercentage = 0.0; + + @Override + public void periodic() { + double output; + // if no algae + if (intakeBeambreak.get()) { + output = intakeProfile.calculate(outputPercentage); + } else { + // hold + output = Math.min(0.05, outputPercentage); + } + + intakeMotor.set(output); + if (Robot.isSimulation()) + simIntakeMotor.setAppliedOutput(output); + } + + @Override + public void simulationPeriodic() { + // update physics + simIntakePhysics.setInput(simIntakeMotor.getAppliedOutput() * RoboRioSim.getVInVoltage()); + simIntakePhysics.update(0.02); + + // update sim objects + simIntakeMotor.iterate( + simIntakePhysics.getAngularVelocityRPM(), + RoboRioSim.getVInVoltage(), + 0.02); + } + + public void setOutputPercentage(double outputPercentage) { + this.outputPercentage = MathUtil.clamp(outputPercentage, -1, 1); + } + + public double getOutputPercentage() { + return outputPercentage; + } + + public boolean isHolding() { + return intakeBeambreak.get(); + } +} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java new file mode 100644 index 0000000..00dd63b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.algae; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class AlgaeSubsystem extends SubsystemBase { + + public enum AlgaePresets { + STOW(10), + FLOOR(90); + + public double armAngle; + + private AlgaePresets(double armAngle) { + this.armAngle = armAngle; + } + } + + public enum AlgaeIntakePresets { + INTAKING(1), + PURGE(-1), + SHOOT(-1), + STOP(0); + + public double outputPercentage; + + AlgaeIntakePresets(double outputPercentage) { + this.outputPercentage = outputPercentage; + } + } + private final AlgaeArm arm = new AlgaeArm(); + private final AlgaeIntake intake = new AlgaeIntake(); + + private AlgaePresets currentPreset = AlgaePresets.STOW; + private AlgaeIntakePresets currentIntakePreset = AlgaeIntakePresets.STOP; + + + public void setAlgaePreset(AlgaePresets preset) { + if (preset != currentPreset) { + arm.setGoalDegrees(preset.armAngle); + currentPreset = preset; + } + } + + public void setAlgaeIntakePreset(AlgaeIntakePresets preset) { + if (preset != currentIntakePreset) { + intake.setOutputPercentage(preset.outputPercentage); + currentIntakePreset = preset; + } + } + + public boolean isHolding() { + return intake.isHolding(); + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java new file mode 100644 index 0000000..365d971 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java @@ -0,0 +1,479 @@ +package frc.robot.subsystems.coral; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.sim.SparkAnalogSensorSim; +import com.revrobotics.sim.SparkFlexSim; +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.sim.SparkRelativeEncoderSim; +import com.revrobotics.spark.SparkAnalogSensor; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.AnalogSensorConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.Constants.Coral; + +public class CoralArm extends SubsystemBase { + private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless); + private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless); + private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless); + // sim motors + private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR); + private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR); + private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR); + + private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT); + private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog(); + private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog(); + // sim encoders + private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor); + private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor); + private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor); + + // physics simulations + private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim( + Coral.Pivot.PhysicalConstants.MOTOR, + Coral.Pivot.PhysicalConstants.NET_REDUCTION, + Coral.Pivot.PhysicalConstants.MOI, + Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS, + 0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE, + 0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI); + private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim( + Coral.Roll.PhysicalConstants.MOTOR, + Coral.Roll.PhysicalConstants.NET_REDUCTION, + Coral.Roll.PhysicalConstants.MOI, + Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS, + Coral.Roll.MAXIMUM_ANGLE * -1, + Coral.Roll.MAXIMUM_ANGLE, + false, + 0); + private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim( + Coral.Pitch.PhysicalConstants.MOTOR, + Coral.Pitch.PhysicalConstants.NET_REDUCTION, + Coral.Pitch.PhysicalConstants.MOI, + Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS, + Coral.Pitch.MAXIMUM_ANGLE * -1, + Coral.Pitch.MAXIMUM_ANGLE, + false, + 0); + + private final SparkFlexConfig pivotConfig = new SparkFlexConfig(); + private final SparkMaxConfig rollConfig = new SparkMaxConfig(); + private final SparkMaxConfig pitchConfig = new SparkMaxConfig(); + + private final ProfiledPIDController pivotPID = Coral.Pivot.PID; + + public ProfiledPIDController getPivotPID() { + return pivotPID; + } + + public ProfiledPIDController getRollPID() { + return rollPID; + } + + public ProfiledPIDController getPitchPID() { + return pitchPID; + } + + private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD; + private final ProfiledPIDController rollPID = Coral.Roll.PID; + private final ProfiledPIDController pitchPID = Coral.Pitch.PID; + + private double pivotGoal = 0.0; // Rads + private double rollGoal = 0.0; // Rads + private double pitchGoal = 0.0; // Rads + + LinearFilter pitchEncFilter = LinearFilter.movingAverage(3); + LinearFilter rollEncFilter = LinearFilter.movingAverage(3); + + public CoralArm() { + AnalogSensorConfig wristEncConfig = new AnalogSensorConfig(); + + pivotMotor.configure( + pivotConfig.idleMode(IdleMode.kBrake).apply( + new EncoderConfig() + .positionConversionFactor( + (1.0 * 2.0 * Math.PI) + / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION) + .velocityConversionFactor( + (60.0 * 2.0 * Math.PI) + / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION)), // Encoder + // -> + // Rotations & + // Seconds + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rollMotor.configure( + rollConfig.idleMode(IdleMode.kBrake) + .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)) + .apply(new SparkMaxConfig().inverted( + Constants.Coral.Pitch.MOTOR_INVERTED)), + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + pitchMotor.configure(pitchConfig + .idleMode(IdleMode.kBrake).apply(wristEncConfig + .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)) + .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)), + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + pivotMotor.getEncoder().setPosition(readPivotEncoderPosition()); + + /* + * .apply(new EncoderConfig() + * .positionConversionFactor( + * (1.0 * 2.0 * Math.PI) + * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION) + * .velocityConversionFactor((60.0 * 2.0 * Math.PI) + * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)), + */ + // Not sure how to get + + // Disabling this for now because we don't want the wrist wire bundle to explode + // rollPID.enableContinuousInput(-180, 180); + + // TODO: Are these reasonable? who knows. + pivotPID.setTolerance(Units.degreesToRadians(3.0)); + rollPID.setTolerance(Units.degreesToRadians(3.0)); + pitchPID.setTolerance(Units.degreesToRadians(3.0)); + + if (Constants.Coral.DEBUG_PIDS) { + SmartDashboard.putNumber("Coral/Pivot/PID/P", pivotPID.getP()); + SmartDashboard.putNumber("Coral/Pivot/PID/I", pivotPID.getI()); + SmartDashboard.putNumber("Coral/Pivot/PID/D", pivotPID.getD()); + + SmartDashboard.putNumber("Coral/Roll/PID/P", rollPID.getP()); + SmartDashboard.putNumber("Coral/Roll/PID/I", rollPID.getI()); + SmartDashboard.putNumber("Coral/Roll/PID/D", rollPID.getD()); + + SmartDashboard.putNumber("Coral/Pitch/PID/P", pitchPID.getP()); + SmartDashboard.putNumber("Coral/Pitch/PID/I", pitchPID.getI()); + SmartDashboard.putNumber("Coral/Pitch/PID/D", pitchPID.getD()); + } + + // Warm up the filters + for (int i = 0; i < 3; ++i) { + readPitchEncoderPosition(); + readRollEncoderPosition(); + } + } + + double lastPitchReading = 0.0; + double lastRollReading = 0.0; + + @Override + public void periodic() { + // if entering the frame border + // && wrist is at neutral + if ((pivotPID.getGoal().position != pivotGoal) + && (rollPID.atGoal() && pitchPID.atGoal())) { + // allow arm to enter frame border + pivotPID.setGoal(pivotGoal); + } else if ( // if exiting the frame border && has exited the frame border + ((rollGoal != rollPID.getGoal().position) || (pitchGoal != pitchPID.getGoal().position)) + && (Math.abs(this.getPitchPositionDegrees()) > Coral.Pivot.ELEVATOR_BORDER_ANGLE)) { + // set roll and pitch back to their goals + rollPID.setGoal(rollGoal); + pitchPID.setGoal(pitchGoal); + } + + double rollPosition = readRollEncoderPosition(); + double pitchPosition = readPitchEncoderPosition(); + double dRollDt = (rollPosition - lastRollReading) / 0.02; + double dPitchDt = (pitchPosition - lastPitchReading) / 0.02; + lastPitchReading = pitchPosition; + lastRollReading = rollPosition; + + if (Constants.Coral.DEBUG_PIDS) { + pivotPID.setP(SmartDashboard.getNumber("Coral/Pivot/PID/P", pivotPID.getP())); + pivotPID.setI(SmartDashboard.getNumber("Coral/Pivot/PID/I", pivotPID.getI())); + pivotPID.setD(SmartDashboard.getNumber("Coral/Pivot/PID/D", pivotPID.getD())); + + rollPID.setP(SmartDashboard.getNumber("Coral/Roll/PID/P", rollPID.getP())); + rollPID.setI(SmartDashboard.getNumber("Coral/Roll/PID/I", rollPID.getI())); + rollPID.setD(SmartDashboard.getNumber("Coral/Roll/PID/D", rollPID.getD())); + + pitchPID.setP(SmartDashboard.getNumber("Coral/Pitch/PID/P", pitchPID.getP())); + pitchPID.setI(SmartDashboard.getNumber("Coral/Pitch/PID/I", pitchPID.getI())); + pitchPID.setD(SmartDashboard.getNumber("Coral/Pitch/PID/D", pitchPID.getD())); + } + /* + * run the motors + */ + + // double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition() + // : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition(); + double pivotPosition = readPivotEncoderPosition(); + + double pivotFFout = pivotFeedforward.calculate( + Math.PI * 0.5 + pivotPosition, + pivotPID.getSetpoint().velocity); + double pivotPIDout = pivotPID.calculate(pivotPosition); + SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout); + SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout); + SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout); + SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition); + SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position); + SmartDashboard.putNumber("Coral/Pivot/velocity_target", pivotPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Pivot/velocity", readPivotEncoderVelocity()); + SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position); + if (!Constants.Coral.Pivot.DBG_DISABLED) + pivotMotor.setVoltage( + (Math.abs(pivotGoal) < Units.degreesToRadians(3)) ? MathUtil.applyDeadband( + pivotPIDout + pivotFFout, 0.6) : pivotPIDout + pivotFFout); + if (Robot.isSimulation()) + simPivotMotor.setAppliedOutput(pivotPIDout + pivotFFout); + + double rollPIDout = rollPID.calculate(readRollEncoderPosition()); + double rollFFout = 0.0;// Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Roll/position", readRollEncoderPosition()); + SmartDashboard.putNumber("Coral/Roll/velocity", dRollDt); + SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position); + SmartDashboard.putNumber("Coral/Roll/velocity_target", rollPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position); + SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout); + SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout); + SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout); + if (!Constants.Coral.Roll.DBG_DISABLED) + rollMotor.setVoltage(rollPIDout + rollFFout); + if (Robot.isSimulation()) + simRollMotor.setAppliedOutput(rollPIDout); + + double pitchPIDout = pitchPID.calculate(readPitchEncoderPosition()); + double pitchFFout = 0.0;// Constants.Coral.Pitch.FEEDFORWARD.calculate(pitchPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Pitch/position", readPitchEncoderPosition()); + SmartDashboard.putNumber("Coral/Pitch/velocity", dPitchDt); + SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position); + SmartDashboard.putNumber("Coral/Pitch/velocity_target", pitchPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position); + SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout); + SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout); + SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout); + if (!Constants.Coral.Pitch.DBG_DISABLED) + pitchMotor.setVoltage(pitchPIDout + pitchFFout); + if (Robot.isSimulation()) + simPitchMotor.setAppliedOutput(pitchPIDout); + } + + @Override + public void simulationPeriodic() { + // update physics + simPivotPhysics + .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), + -12.0, 12.0)); + simRollPhysics + .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), + -12.0, 12.0)); + simPitchPhysics + .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), + -12.0, 12.0)); + SmartDashboard.putNumber("simPitchMotor.getPosition", simPitchPhysics.getAngleRads() + * RoboRioSim.getVInVoltage()); + + simPivotPhysics.update(0.02); + simRollPhysics.update(0.02); + simPitchPhysics.update(0.02); + + // update sim objects + simPivotMotor.iterate( + Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()), + RoboRioSim.getVInVoltage(), + 0.02); + simRollMotor.iterate( + Units.radiansPerSecondToRotationsPerMinute(simRollPhysics.getVelocityRadPerSec()), + RoboRioSim.getVInVoltage(), + 0.02); + simPitchMotor.iterate( + Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()), + RoboRioSim.getVInVoltage(), + 0.02); + + simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3 + - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS); + simRollEncoder.iterate( + Units.radiansToRotations(simRollPhysics.getVelocityRadPerSec()) * 3.3, + 0.02); + simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3 + - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS); + simPitchEncoder.iterate( + Units.radiansToRotations(simPitchPhysics.getVelocityRadPerSec()) * 3.3, + 0.02); + + simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5); + simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02); + } + + // this should be relative to straight upwards. + // i.e. 0 should be straight vertical, + // 20 would be to the right, -20 to the left + public void setPivotGoalDegrees(double goal) { + double goalRads = Units.degreesToRadians(goal); + pivotGoal = MathUtil.clamp(goalRads, + -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE); + // if passing through the frame border + /* + * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0)) + * && (Math.abs(this.getPivotPositionDegrees()) < + * Coral.Pivot.FRAME_BORDER_ANGLE)) { + * // set the pivot to stop at the frame border to allow the wrist to move + * // neutral + * pivotPID.setGoal( + * Coral.Pivot.FRAME_BORDER_ANGLE + * (pivotGoal < 0 ? -1 : 1)); + * rollPID.setGoal(0); + * pitchPID.setGoal(0); + * } else { + * pivotPID.setGoal(pivotGoal); + * } + */ + + pivotPID.setGoal(pivotGoal); + } + + // similarly, this ranges from -180 to 180 + public void setRollGoalDegrees(double goal) { + double goalRads = Units.degreesToRadians(goal); + rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE); + /* + * // if outside of frame border + * if (Math.abs(this.getPivotPositionDegrees()) > + * Coral.Pivot.FRAME_BORDER_ANGLE) { + * // start moving to goal + * rollPID.setGoal(rollGoal); + * } + */ + rollPID.setGoal(rollGoal); + } + + public void setPitchGoalDegrees(double goal) { + double goalRads = Units.degreesToRadians(goal); + pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MAXIMUM_ANGLE * -1, Coral.Pitch.MAXIMUM_ANGLE); + /* + * // if outside the frame border + * if (Math.abs(this.getPivotPositionDegrees()) > + * Coral.Pivot.FRAME_BORDER_ANGLE) { + * // start moving to goal + * pitchPID.setGoal(pitchGoal); + * } + */ + pitchPID.setGoal(pitchGoal); + } + + public double getPivotGoalDegrees() { + return Units.radiansToDegrees(pivotGoal); + } + + public double getRollGoalDegrees() { + return Units.radiansToDegrees(rollGoal); + } + + public double getPitchGoalDegrees() { + return Units.radiansToDegrees(pitchGoal); + } + + public double getPivotPositionDegrees() { + return Units.radiansToDegrees(readPivotEncoderPosition()); + } + + public double getRollPositionDegrees() { + return Units.radiansToDegrees(readRollEncoderPosition()); + } + + public double getPitchPositionDegrees() { + return Units.radiansToDegrees(readPitchEncoderPosition()); + } + + public boolean isPitchInPosition() { + return pitchPID.atGoal(); + } + + public boolean isRollInPosition() { + return rollPID.atGoal(); + } + + public boolean isPivotInPosition() { + return pivotPID.atGoal(); + } + + public double readPitchEncoderPosition() { + return MathUtil + .angleModulus(((pitchEncFilter + .calculate(Robot.isSimulation() ? simPitchEncoder.getPosition() + : pitchEncoder.getPosition()) + + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS) + / 3.3) + * 2 * Math.PI); + } + + public double readPitchEncoderVelocity() { + return MathUtil + .angleModulus(((pitchEncFilter + .calculate(Robot.isSimulation() ? simPitchEncoder.getVelocity() + : pitchEncoder.getVelocity())) + / 3.3) + * 2 * Math.PI); + } + + public double readRollEncoderPosition() { + return MathUtil + .angleModulus(((rollEncFilter + .calculate(Robot.isSimulation() ? simRollEncoder.getPosition() + : rollEncoder.getPosition()) + + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) / 3.3) + * 2 * Math.PI); + } + + public double readRollEncoderVelocity() { + return MathUtil + .angleModulus(((rollEncFilter + .calculate(Robot.isSimulation() ? simRollEncoder.getVelocity() + : rollEncoder.getVelocity())) + / 3.3) + * 2 * Math.PI); + } + + public double readPivotEncoderPosition() { + return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5) + : Units.rotationsToRadians((pivotEncoder.getPosition().getValueAsDouble() + * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0))); + } + + public double readPivotEncoderVelocity() { + return Robot.isSimulation() ? (simPivotPhysics.getVelocityRadPerSec()) + : Units.rotationsToRadians((pivotEncoder.getVelocity().getValueAsDouble() + * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0))); + } + + public boolean isPitchSupposedToBeInPosition() { + // return MathUtil.isNear(this.pitchPID.getSetpoint().position, + // this.pitchPID.getGoal().position, 0.01); + return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01); + } + + public boolean isPivotSupposedToBeInPosition() { + // return MathUtil.isNear(this.pivotPID.getSetpoint().position, + // this.pivotPID.getGoal().position, 0.01); + return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01); + } + + public boolean isRollSupposedToBeInPosition() { + // return MathUtil.isNear(this.rollPID.getSetpoint().position, + // this.rollPID.getGoal().position, 0.01); + return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01); + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java new file mode 100644 index 0000000..cfab666 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java @@ -0,0 +1,189 @@ +package frc.robot.subsystems.coral; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.sim.SparkFlexSim; +import com.revrobotics.sim.SparkRelativeEncoderSim; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.Constants.Elevator; + +@Logged +public class CoralElevator extends SubsystemBase { + + private final SparkFlex leaderMotor = new SparkFlex(Elevator.Leader.MOTOR_PORT, MotorType.kBrushless); + private final SparkFlex followerMotor = new SparkFlex(Elevator.Follower.MOTOR_PORT, MotorType.kBrushless); + // sim motors + private final SparkFlexSim simLeaderMotor = new SparkFlexSim(leaderMotor, DCMotor.getNeoVortex(1)); + private final SparkFlexSim simFollowerMotor = new SparkFlexSim(followerMotor, DCMotor.getNeoVortex(1)); + + private final RelativeEncoder leaderEncoder = leaderMotor.getEncoder(); + private final RelativeEncoder followerEncoder = followerMotor.getEncoder(); + // sim encoders + private final SparkRelativeEncoderSim simLeaderEncoder = new SparkRelativeEncoderSim(leaderMotor); + + // physics simulations + private final ElevatorSim simElevator = new ElevatorSim( + Elevator.PhysicalParameters.MOTOR, + Elevator.PhysicalParameters.GEARING, + Elevator.PhysicalParameters.CARRIAGE_MASS_KG, + Elevator.PhysicalParameters.DRIVE_RADIUS_METERS, + 0, + Elevator.PhysicalParameters.MAX_TRAVEL, + true, + 0); + + private final SparkFlexConfig leaderConfig = new SparkFlexConfig(); + private final SparkFlexConfig followerConfig = new SparkFlexConfig(); + + private final ElevatorFeedforward feedForward = Elevator.FEEDFORWARD; + private final ProfiledPIDController elevatorPID = new ProfiledPIDController( + Elevator.PID.kP, + Elevator.PID.kI, + Elevator.PID.kD, + new TrapezoidProfile.Constraints( + Elevator.PID.MAX_VELOCITY, + Elevator.PID.MAX_ACCELERATION)); + + public CoralElevator() { + leaderConfig + .idleMode(IdleMode.kBrake) + .inverted(Elevator.Leader.INVERTED); + leaderConfig.encoder + .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER) + .velocityConversionFactor(60.0 / Elevator.MOTOR_REVOLUTIONS_PER_METER); // Multiply by 60/RpM to get m/s + + followerConfig + .idleMode(IdleMode.kBrake) + .inverted(Elevator.Follower.INVERTED); + followerConfig.encoder + .positionConversionFactor(1 / Elevator.MOTOR_REVOLUTIONS_PER_METER) + .velocityConversionFactor(60.0 / Elevator.MOTOR_REVOLUTIONS_PER_METER); + + leaderMotor.configure(leaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + leaderEncoder.setPosition(0); + followerEncoder.setPosition(0); + + // Add some tolerance to the elevator controller + elevatorPID.setTolerance(Units.inchesToMeters(0.5)); + } + + private boolean isZeroed = false; + + private double lastVelocity = 0.0; + private double lastTime = 0.0; + + @Override + public void periodic() { + // check if needs to be zeroed and is at zero + // TODO: ######################### PLACEHOLDERS AGAIN ######################### + if (!isZeroed + && (((leaderMotor.getOutputCurrent() + followerMotor.getOutputCurrent()) / 2) > 20 + || Robot.isSimulation())) { + leaderEncoder.setPosition(0); + isZeroed = true; + } + + // check if initial zero had been run + if (isZeroed) { + TrapezoidProfile.State setpoint = elevatorPID.getSetpoint(); + double deltaVelocity = setpoint.velocity - lastVelocity; + double deltaTime = Timer.getFPGATimestamp() - lastTime; + + lastTime = Timer.getFPGATimestamp(); + lastVelocity = setpoint.velocity; + + double pid_out = elevatorPID.calculate(getPosition()); + double ff_out = feedForward.calculate(setpoint.velocity, deltaVelocity / deltaTime); + double output = ff_out + pid_out; + + SmartDashboard.putNumber("Elevator/output", output); + SmartDashboard.putNumber("Elevator/position", getPosition()); + SmartDashboard.putNumber("Elevator/target", setpoint.position); + SmartDashboard.putNumber("Elevator/goal", elevatorPID.getGoal().position); + SmartDashboard.putNumber("Elevator/pid_out", pid_out); + SmartDashboard.putNumber("Elevator/ff_out", ff_out); + + if (!Constants.Elevator.DBG_DISABLED) { + leaderMotor.setVoltage(output); + followerMotor.setVoltage(output); + } + + if (Robot.isSimulation()) { + simLeaderMotor.setAppliedOutput(output); + simFollowerMotor.setAppliedOutput(output); + } + } else { + if (!Constants.Elevator.DBG_DISABLED) { + // slowly move down to zero + leaderMotor.set(-0.05); + followerMotor.set(-0.05); + } + } + } + + @Override + public void simulationPeriodic() { + // update physics + simElevator + .setInput(MathUtil.clamp(simLeaderMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), -12.0, 12.0)); + simElevator.update(0.02); + + // update sim objects + simLeaderMotor.iterate( + simElevator.getVelocityMetersPerSecond(), + RoboRioSim.getVInVoltage(), + 0.02); + simFollowerMotor.iterate( + simElevator.getVelocityMetersPerSecond(), + RoboRioSim.getVInVoltage(), + 0.02); + } + + public void setGoalPosition(double position) { + elevatorPID.setGoal(MathUtil.clamp(position, 0.0, Elevator.PhysicalParameters.MAX_TRAVEL)); + } + + public double getGoalPosition() { + return elevatorPID.getGoal().position; + } + + public double getPosition() { + return Robot.isReal() + ? leaderEncoder.getPosition() + : simElevator.getPositionMeters(); + } + + public boolean isInPosition() { + return elevatorPID.atGoal(); + } + + public boolean isSupposedToBeInPosition() { + return MathUtil.isNear(elevatorPID.getSetpoint().position, elevatorPID.getGoal().position, + Units.inchesToMeters(1 / 16.0)); + } + + public void zeroElevator() { + isZeroed = false; + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java new file mode 100644 index 0000000..126cf40 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java @@ -0,0 +1,105 @@ +package frc.robot.subsystems.coral; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.Constants.Coral; + +public class CoralIntake extends SubsystemBase { + private final TalonFX intakeMotor = new TalonFX(Coral.Intake.MOTOR_PORT); + // sim motor + private TalonFXSimState simIntakeMotor; + + // physics simulation + private final FlywheelSim simIntakePhysics = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + Coral.Intake.PhysicalConstants.MOTOR, + Coral.Intake.PhysicalConstants.MOI, + Coral.Intake.PhysicalConstants.GEARING), + Coral.Intake.PhysicalConstants.MOTOR); + + private final SlewRateLimiter intakeProfile = new SlewRateLimiter( + Coral.Intake.POSITIVE_RATE_LIMIT, + -Coral.Intake.NEGATIVE_RATE_LIMIT, + 0); + + public CoralIntake() { + intakeMotor.setNeutralMode(NeutralModeValue.Brake); + intakeMotor.getConfigurator() + .apply(new CurrentLimitsConfigs().withStatorCurrentLimit(Constants.Coral.Intake.CURRENT_LIMIT) + .withStatorCurrentLimitEnable(true)); + intakeMotor.getConfigurator() + .apply(new MotorOutputConfigs() + .withInverted(Constants.Coral.Intake.MOTOR_INVERTED ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive)); + intakeMotor.getConfigurator() + .apply(new HardwareLimitSwitchConfigs().withForwardLimitEnable(false).withReverseLimitEnable(false)); + } + + private double outputPercentage = 0.0; + // private boolean lastHolding = false; + + @Override + public void periodic() { + boolean curHolding = isHolding(); + // if no coral + if (!curHolding) { + double output = outputPercentage;// intakeProfile.calculate(outputPercentage); + if (!Constants.Coral.Intake.DBG_DISABLED) + intakeMotor.set(output); + } else { + // hold, negative is out so intake a bit. + // Set to 0.1 to be good + if (!Constants.Coral.Intake.DBG_DISABLED) + intakeMotor.set(Math.min(0.1, outputPercentage)); + } + + SmartDashboard.putBoolean("Holding Coral", curHolding); + } + + @Override + public void simulationPeriodic() { + simIntakeMotor = intakeMotor.getSimState(); + simIntakeMotor.setSupplyVoltage(RoboRioSim.getVInVoltage()); + + // update physics + simIntakePhysics.setInput(simIntakeMotor.getMotorVoltage() * RoboRioSim.getVInVoltage()); + simIntakePhysics.update(0.02); + + // update sim objects + // rpm to rps + simIntakeMotor.setRotorVelocity(simIntakePhysics.getAngularVelocityRPM() / 60); + } + + public void setOutputPercentage(double outputPercentage) { + this.outputPercentage = MathUtil.clamp(outputPercentage, -1, 1); + } + + public void setStatorLimit(double amps) { + intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(amps)); + } + + public double getOutputPercentage() { + return outputPercentage; + } + + public boolean isHolding() { + return Robot.isReal() + ? intakeMotor.getForwardLimit().getValue().value == 0 + : false; + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java new file mode 100644 index 0000000..d2817f9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java @@ -0,0 +1,349 @@ +package frc.robot.subsystems.coral; + +import java.lang.reflect.Field; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.Ultrasonic; +// import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.util.LimelightAssistance; +import frc.robot.RobotContainer; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.Limelight; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.util.LimelightAssistance; + +import frc.robot.util.LimelightContainer; + +public class CoralSubsystem extends SubsystemBase { + + private final CoralArm arm = new CoralArm(); + private final CoralIntake intake = new CoralIntake(); + + private final CoralElevator elevator = new CoralElevator(); + + private final SwerveSubsystem swerveSubsystem; + + private final Mechanism2d coralMechanism = new Mechanism2d(2, 3); + private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 1.0, 0.0); + private final MechanismLigament2d elevatorMechanism = rootMechanism.append( + new MechanismLigament2d("Elevator", Constants.Elevator.PhysicalParameters.BOTTOM_TO_FLOOR, 90)); + private final MechanismLigament2d pivotMechanism = elevatorMechanism.append( + new MechanismLigament2d("Coral", Constants.Coral.Pivot.PhysicalConstants.JOINT_LENGTH_METERS, 0)); + private final MechanismLigament2d pitchMechanism = pivotMechanism.append( + new MechanismLigament2d("Pitch", Constants.Coral.Pitch.PhysicalConstants.JOINT_LENGTH_METERS, 0)); + private final MechanismLigament2d rollMechanism = pivotMechanism.append( + new MechanismLigament2d("Roll", Constants.Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS, 90)); + + private LimelightAssistance llAssist = null; + + private final AnalogPotentiometer leftUltrasonic = new AnalogPotentiometer(Constants.Coral.LEFT_ULTRASONIC_PORT, + 254.0, 0.0); + private final AnalogPotentiometer rightUltrasonic = new AnalogPotentiometer(Constants.Coral.RIGHT_ULTRASONIC_PORT, + 254.0, 0.0); + + public enum CoralPresets { + LEVEL_1(0.05, Units.radiansToDegrees(0.635), Units.radiansToDegrees(1.0), Units.radiansToDegrees(1.636)), + LEVEL_2(0.247-0.02, 16.532, 90, 98.068), + LEVEL_3(0.650-0.02, 16.532, 90, 98.068), + LEVEL_4(1.342, 20.0, 90, 110.062), + INTAKE(0.05, 18.0, 90, 30.0), + STOW(0.05, 0.0, 0.0, 0.0), + + CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN); + + double elevatorHeightM; // Elevator height (relative to bottom of elevator/fully retracted) + double pivotAngleDeg; // Looking at the robot from the FRONT (algae intake side), positive to the + // right, and negative to the left (positive=CW) + double rollAngleDeg; // Wrist 1 angle, degrees from pointing at the bumpers on the CORAL ARM side of + // the robot. positive=CCW + double pitchAngleDeg; // Wrist 2 angle, degrees from pointing straight up (max: 115deg) + + private CoralPresets(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) { + this.elevatorHeightM = elevatorHeight; + this.pivotAngleDeg = pivotAngle; + this.rollAngleDeg = rollAngle; + this.pitchAngleDeg = pitchAngle; + } + } + + public CoralSubsystem(LimelightAssistance llAssist, SwerveSubsystem swerveSubsystem) { + this.llAssist = llAssist; + this.swerveSubsystem = swerveSubsystem; + // Epilogue.bind(this); + } + + public enum MirrorPresets { + RIGHT(false), + LEFT(true), + STARBOARD(false), + PORT(true); + + boolean isMirrored; + + private MirrorPresets(boolean isMirrored) { + this.isMirrored = isMirrored; + } + } + + public enum CoralIntakePresets { + INTAKE(1, 40.0), + HOLD(0.4, 12.5), + PURGE(-1, 40.0), + SCORE(-1, 30.0), + STOP(0, 12.5), + + CUSTOM(Double.NaN, 40.0); + + double intakePercentage; + double intakeCurrent; + + private CoralIntakePresets(double intakePercentage, double intakeCurrent) { + this.intakePercentage = intakePercentage; + this.intakeCurrent = intakeCurrent; + } + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Ultra Left", this.leftUltrasonic.get()); + SmartDashboard.putNumber("Ultra Right", this.rightUltrasonic.get()); + + // i have no idea what any of the getPositions output + elevatorMechanism + .setLength(elevator.getPosition() + Constants.Elevator.PhysicalParameters.CORAL_PIVOT_VERTICAL_OFFSET); + pivotMechanism.setAngle(arm.getPivotPositionDegrees()); + pitchMechanism.setAngle(arm.getPitchPositionDegrees()); + rollMechanism.setLength(Math.cos(Units.degreesToRadians(arm.getRollPositionDegrees())) + * Constants.Coral.Roll.PhysicalConstants.JOINT_LENGTH_METERS); + + SmartDashboard.putData("Coral Mechanism", coralMechanism); + SmartDashboard.putBoolean("Elevator in position", isElevatorInPosition()); + SmartDashboard.putBoolean("Roll in position", isRollInPosition()); + SmartDashboard.putBoolean("Pitch in position", isPitchInPosition()); + SmartDashboard.putBoolean("Pivot in position", isPivotInPosition()); + + SmartDashboard.putBoolean("Elevator SUPPOSED to be in position", isElevatorSupposedToBeInPosition()); + SmartDashboard.putBoolean("Roll SUPPOSED to be in position", isRollSupposedToBeInPosition()); + SmartDashboard.putBoolean("Pitch SUPPOSED to be in position", isPitchSupposedToBeInPosition()); + SmartDashboard.putBoolean("Pivot SUPPOSED to be in position", isPivotSupposedToBeInPosition()); + } + + private CoralPresets currentPreset = CoralPresets.STOW; + private MirrorPresets mirrorSetting = MirrorPresets.RIGHT; + private CoralIntakePresets currentIntakePreset = CoralIntakePresets.STOP; + + public void setCoralPresetDIRECT(CoralPresets preset) { + if (preset == CoralPresets.CUSTOM) { + // uhhh i don't now how to throw an exception and i don't feel like figuring it + // out + } else if (preset != currentPreset) { + elevator.setGoalPosition(preset.elevatorHeightM); + arm.setPivotGoalDegrees( + preset.pivotAngleDeg + * (mirrorSetting.isMirrored ? -1 : 1)); + arm.setRollGoalDegrees(preset.rollAngleDeg); + arm.setPitchGoalDegrees(preset.pitchAngleDeg); + + currentPreset = preset; + } + } + + public void setCoralPresetElevator(CoralPresets preset) { + elevator.setGoalPosition(preset.elevatorHeightM); + currentPreset = preset; + } + + public boolean isElevatorInPosition() { + return elevator.isInPosition(); + } + + public void setCoralPresetPivot(CoralPresets preset) { + SmartDashboard.putNumber("Pivot Pre", preset.pivotAngleDeg); + arm.setPivotGoalDegrees( + preset.pivotAngleDeg + * (mirrorSetting.isMirrored ? -1 : 1)); + currentPreset = preset; + } + + public boolean isPivotInPosition() { + return arm.isPivotInPosition(); + } + + public void setCoralPresetPitch(CoralPresets preset) { + arm.setPitchGoalDegrees( + preset.pitchAngleDeg); + currentPreset = preset; + } + + public boolean isPitchInPosition() { + return arm.isPitchInPosition(); + } + + public void setCoralPresetRoll(CoralPresets preset) { + arm.setRollGoalDegrees( + preset.rollAngleDeg + * (mirrorSetting.isMirrored ? -1 : 1)); + currentPreset = preset; + } + + public boolean isRollInPosition() { + return arm.isRollInPosition(); + } + + public double getPivotGoalDegrees() { + return arm.getPivotGoalDegrees(); + } + + public double getRollGoalDegrees() { + return arm.getRollGoalDegrees(); + } + + public double getPitchGoalDegrees() { + return arm.getPitchGoalDegrees(); + } + + public boolean isHolding() { + return Robot.isSimulation() ? SmartDashboard.getBoolean("[SIM] Holding Coral", false) : intake.isHolding(); + } + + public BooleanSupplier isHoldingSupplier() { + return new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return isHolding(); + } + }; + } + + public void setCustomPosition(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) { + currentPreset = CoralPresets.CUSTOM; + + elevator.setGoalPosition(elevatorHeight); + arm.setPivotGoalDegrees(pivotAngle); + arm.setRollGoalDegrees(rollAngle); + arm.setPitchGoalDegrees(pitchAngle); + } + + public void setCustomElevatorMeters(double elevatorHeight) { + currentPreset = CoralPresets.CUSTOM; + elevator.setGoalPosition(elevatorHeight); + } + + public void setCustomPivotDegrees(double pivotAngle) { + currentPreset = CoralPresets.CUSTOM; + arm.setPivotGoalDegrees(pivotAngle); + } + + public void setCustomRollDegrees(double rollAngle) { + currentPreset = CoralPresets.CUSTOM; + arm.setRollGoalDegrees(rollAngle); + } + + public void setCustomPitchDegrees(double pitchAngle) { + currentPreset = CoralPresets.CUSTOM; + arm.setPitchGoalDegrees(pitchAngle); + } + + public void mirrorArm() { + SmartDashboard.putBoolean("Called", true); + if (llAssist.isTagOnRight()) { + mirrorSetting = MirrorPresets.RIGHT; + } else if (!llAssist.isTagOnRight()) + mirrorSetting = MirrorPresets.LEFT; + } + + public void mirrorArm(MirrorPresets preset) { + mirrorSetting = preset; + } + + public void autoSetMirrorIntake() { + Pose2d robotPose = swerveSubsystem.getPose(); + Pose2d closestSource = robotPose.nearest(FieldConstants.getSourcePoses()); + Pose2d left = robotPose.transformBy(new Transform2d(0, -0.2, new Rotation2d())); + Pose2d right = robotPose.transformBy(new Transform2d(0, 0.2, new Rotation2d())); + + this.mirrorSetting = left.getTranslation().getDistance(closestSource.getTranslation()) < right.getTranslation() + .getDistance(closestSource.getTranslation()) ? MirrorPresets.LEFT : MirrorPresets.RIGHT; + + System.out.println("Mirror Side" + mirrorSetting.name()); + + // this.mirrorSetting = (this.leftUltrasonic.get() < this.rightUltrasonic.get()) + // ? MirrorPresets.LEFT + // : MirrorPresets.RIGHT; + } + + public void autoSetMirrorScoring() { + Pose2d robotPose = swerveSubsystem.getPose(); + Pose2d left = robotPose.transformBy(new Transform2d(0, -0.2, new Rotation2d())); + Pose2d right = robotPose.transformBy(new Transform2d(0, 0.2, new Rotation2d())); + + this.mirrorSetting = left.getTranslation().getDistance(FieldConstants.getReefPose().getTranslation()) < right.getTranslation() + .getDistance(FieldConstants.getReefPose().getTranslation()) ? MirrorPresets.LEFT : MirrorPresets.RIGHT; + + System.out.println("Mirror Side" + mirrorSetting.name()); + } + + public void setCoralIntakePreset(CoralIntakePresets preset) { + SmartDashboard.putString("Coral Intake Preset", preset.toString()); + if (preset != currentIntakePreset) { + intake.setOutputPercentage(preset.intakePercentage); + intake.setStatorLimit(preset.intakeCurrent); + currentIntakePreset = preset; + } + } + + public void setCustomIntakePercent(double percentage) { + currentIntakePreset = CoralIntakePresets.CUSTOM; + intake.setOutputPercentage(percentage); + intake.setStatorLimit(currentIntakePreset.intakeCurrent); + } + + public double getPivotPositionDegrees() { + return arm.getPivotPositionDegrees(); + } + + public boolean isElevatorSupposedToBeInPosition() { + return elevator.isSupposedToBeInPosition(); + } + + public boolean isPitchSupposedToBeInPosition() { + return arm.isPitchSupposedToBeInPosition(); + } + + public boolean isRollSupposedToBeInPosition() { + return arm.isRollSupposedToBeInPosition(); + } + + public boolean isPivotSupposedToBeInPosition() { + return arm.isPivotSupposedToBeInPosition(); + } + + public boolean isSupposedToBeInPosition() { + return isPivotSupposedToBeInPosition() && isRollSupposedToBeInPosition() && isElevatorSupposedToBeInPosition() + && isPitchSupposedToBeInPosition(); + } + + public CoralArm getCoralArm() { + return arm; + } + + public CoralIntake getIntake() { + return intake; + } + + public CoralElevator getElevator() { + return elevator; + } +} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..2d58872 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,56 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants.FieldConstants; + +public class AllianceFlipUtil +{ + + public static double applyX(double x) + { + return shouldFlip() ? FieldConstants.FIELD_LENGTH - x : x; + } + + public static double applyY(double y) + { + return shouldFlip() ? FieldConstants.FIELD_WIDTH - y : y; + } + + public static Translation2d apply(Translation2d translation) + { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) + { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) + { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Pose2d flip(Pose2d pose) + { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } + + public static boolean shouldFlip() + { + return (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/LimelightAssistance.java b/src/main/java/frc/robot/util/LimelightAssistance.java new file mode 100644 index 0000000..dd20d94 --- /dev/null +++ b/src/main/java/frc/robot/util/LimelightAssistance.java @@ -0,0 +1,35 @@ +package frc.robot.util; + +import frc.robot.RobotContainer; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.Constants.PoseConstants; +import frc.robot.subsystems.SwerveSubsystem; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + + +public class LimelightAssistance{ + + SwerveSubsystem swerveSub = null; + + public LimelightAssistance(SwerveSubsystem swerveSub){ + this.swerveSub = swerveSub; + } + + public boolean isTagOnRight(){ + + SmartDashboard.putString("Recieved position to isTagOnLeft: ", swerveSub.odometry.getEstimatedPosition().toString()); + int nearestTag = RobotContainer.LLContainer.findNearestTagPos(swerveSub.odometry); + SmartDashboard.putNumber("Recieved nearest tag to isTagOnLeft: ", nearestTag); + SmartDashboard.putString("Tag angle, robot angle: ", ""+PoseConstants.tagPoses.get(nearestTag).getRotation().getDegrees() + swerveSub.odometry.getEstimatedPosition().getRotation().getDegrees()); + if(Math.abs(PoseConstants.tagPoses.get(nearestTag).getRotation().getDegrees() - swerveSub.odometry.getEstimatedPosition().getRotation().getDegrees()) > 180){ + SmartDashboard.putBoolean("Tag is on left: ", true); + return true; + } + else{ + SmartDashboard.putBoolean("Tag is on left: ", false); + return false; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/LimelightContainer.java b/src/main/java/frc/robot/util/LimelightContainer.java new file mode 100644 index 0000000..c689b00 --- /dev/null +++ b/src/main/java/frc/robot/util/LimelightContainer.java @@ -0,0 +1,176 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import java.util.ArrayList; + +import com.studica.frc.AHRS; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.LimelightHelpers; +import frc.robot.subsystems.Limelight; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.Constants.PoseConstants; + +public class LimelightContainer { + static int SIMCOUNTER = 0; + static int RLCOUNTER = 0; + static int RLCountermt1 = 0; + private static ArrayList limelights = new ArrayList(); + + public LimelightContainer(Limelight... limelights) { + for (Limelight limelight : limelights) { + LimelightContainer.limelights.add(limelight); + LimelightHelpers.SetIMUMode(limelight.getName(), 0); + } + enableLimelights(true); + + } + + public void enableLimelights(boolean enable) { + for (Limelight limelight : limelights) { + limelight.setEnabled(enable); + } + } + + public static void estimateSimOdometry() { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); + if (mt2 == null) { // in case not all limelights are connected + continue; + } + if (mt2.tagCount == 0) { + doRejectUpdate = true; + } + if (!doRejectUpdate) { + SmartDashboard.putString("Simulated Pos", mt2.pose.toString() + SIMCOUNTER); + SIMCOUNTER++; + } + } + } + + public void estimateMT1OdometryPrelim(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx, + SwerveModulePosition[] swerveModulePositions) { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + + if (mt1 == null) { + continue; + } + + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } + + if (Math.abs(navx.getRate()) > 720) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + + // odometry.setVisionMeasurementStdDevs(VecBuilder.fill(.2, .2, .2)); + // odometry.addVisionMeasurement( + // mt1.pose, + // mt1.timestampSeconds); + + // odometry.setVisionMeasurementStdDevs(VecBuilder.fill(.2, .2, 20)); + // odometry.addVisionMeasurement( + // mt1.pose, + // mt1.timestampSeconds); + + odometry.resetPosition(mt1.pose.getRotation(), swerveModulePositions, mt1.pose); + + SmartDashboard.putString("Pos MT1 prelim: ", mt1.pose.toString() + " " + RLCountermt1); + } + + RLCountermt1++; + } + } + + public void estimateMT1Odometry(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx) { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + + if (mt1 == null) { + continue; + } + + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } + + if(mt1.avgTagDist < Units.feetToMeters(10)) + + if (Math.abs(navx.getRate()) > 720) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + + odometry.setVisionMeasurementStdDevs(VecBuilder.fill(3, 3, 9999)); + odometry.addVisionMeasurement( + mt1.pose, + mt1.timestampSeconds); + + SmartDashboard.putString("Pos MT1: ", mt1.pose.toString() + " " + RLCountermt1); + + limelight.pushPoseToShuffleboard(limelight.getName() + "mt1", mt1.pose); + } + + RLCountermt1++; + } + } + + public void estimateMT2Odometry(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx) { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + LimelightHelpers.SetRobotOrientation(limelight.getName(), navx.getAngle(), 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); + if(Math.abs(navx.getRate()) > 720) // if our angular velocity is greater than 720 degrees per second, ignore vision updates + { + doRejectUpdate = true; + } + if(mt2.tagCount == 0) + { + doRejectUpdate = true; + } + if(!doRejectUpdate) + { + limelight.pushPoseToShuffleboard(limelight.getName() + "mt2", mt2.pose); + odometry.setVisionMeasurementStdDevs(VecBuilder.fill(3,3,9999999)); + odometry.addVisionMeasurement( + mt2.pose, + mt2.timestampSeconds); + } + } + } + + public int findNearestTagPos(SwerveDrivePoseEstimator odometry) { + double min = 43490824; + int toReturn = 0; + for (int i = 0; i < 22; i++) { + double thing = Math.sqrt(Math.abs(PoseConstants.tagPoses.get(i).getX() - odometry.getEstimatedPosition().getX())); + if (thing < min) { + min = thing; + toReturn = i + 1; + } + } + return toReturn; + } + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Reef.java b/src/main/java/frc/robot/util/Reef.java new file mode 100644 index 0000000..aa5e265 --- /dev/null +++ b/src/main/java/frc/robot/util/Reef.java @@ -0,0 +1,102 @@ +package frc.robot.util; + +import java.util.HashMap; +import java.util.Map; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; + +public class Reef { + public enum ReefBranch { + A, B, C, D, E, F, G, H, I, J, K, L; + } + + private static final Pose2d[] centerFaces = new Pose2d[] { + new Pose2d( + Units.inchesToMeters(144.003), + Units.inchesToMeters(158.500), + Rotation2d.fromDegrees(180)), + new Pose2d( + Units.inchesToMeters(160.373), + Units.inchesToMeters(186.857), + Rotation2d.fromDegrees(120)), + new Pose2d( + Units.inchesToMeters(193.116), + Units.inchesToMeters(186.858), + Rotation2d.fromDegrees(60)), + new Pose2d( + Units.inchesToMeters(209.489), + Units.inchesToMeters(158.502), + Rotation2d.fromDegrees(0)), + new Pose2d( + Units.inchesToMeters(193.118), + Units.inchesToMeters(130.145), + Rotation2d.fromDegrees(300)), + new Pose2d( + Units.inchesToMeters(160.375), + Units.inchesToMeters(130.144), + Rotation2d.fromDegrees(240)) + }; + + public static final Translation2d center = new Translation2d(Units.inchesToMeters(176.746), + Units.inchesToMeters(158.501)); + // Starting off facing DS wall + public static final double centerOffset = Units.inchesToMeters(32); + public static final double faceOffset = Units.inchesToMeters(6.469); + + public static final Map branches = new HashMap() { + { + ReefBranch[] branchName = ReefBranch.values(); + for (int i = 0; i < 12; i += 2) { + Pose2d face = centerFaces[i / 2]; + put(branchName[i], face.transformBy( + new Transform2d(DriveConstants.FULL_ROBOT_WIDTH / 2.0, faceOffset, new Rotation2d()))); + put(branchName[i + 1], face.transformBy( + new Transform2d(DriveConstants.FULL_ROBOT_WIDTH / 2.0, -faceOffset, new Rotation2d()))); + } + } + }; + + public static Pose2d getBranchPose2d(ReefBranch branch) { + return branches.get(branch); + } + + /** + * Used to put a pose on Shuffleboard for debugging - Don't repeatadly call this! + * @param name Name of pose + * @param pose Pose2d pose + */ + public static void pushPoseToShuffleboard(String name, Pose2d pose) { + StructPublisher publisher = NetworkTableInstance.getDefault().getStructTopic(name, Pose2d.struct) + .publish(); + publisher.set(pose); + } + + /** + * Just for visualization for poses + */ + public static void putToShuffleboard() { + for (ReefBranch branch : branches.keySet()) { + System.out.println(branch.name()); + Pose2d branchPosition = branches.get(branch); + + StructPublisher publisher = NetworkTableInstance.getDefault() + .getStructTopic(branch.name(), Pose2d.struct).publish(); + + publisher.set(branchPosition); + } + + } +} \ No newline at end of file