diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..f2805d9 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,18 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "type": "java", + "name": "Current File", + "request": "launch", + "mainClass": "${file}" + }, + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "2025RobotCode" + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json index 385f27a..ce0a249 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,3 +1,4 @@ { - "java.configuration.updateBuildConfiguration": "interactive" + "java.configuration.updateBuildConfiguration": "interactive", + "java.import.gradle.annotationProcessing.enabled": false } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 305e0e3..c0b3708 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2025beta", - "teamNumber": 5414 + "teamNumber": 9994 } diff --git a/AdvantageScope Simulation.json b/AdvantageScope Simulation.json deleted file mode 100644 index d1fe779..0000000 --- a/AdvantageScope Simulation.json +++ /dev/null @@ -1,208 +0,0 @@ -{ - "hubs": [ - { - "x": 303, - "y": 184, - "width": 1101, - "height": 650, - "state": { - "sidebar": { - "width": 294, - "expanded": [ - "/swerve/advantagescope", - "/SmartDashboard/Field", - "/swerve", - "/SmartDashboard/field", - "/AdvantageKit", - "/AdvantageKit/RealOutputs", - "/AdvantageKit/RealOutputs/FieldSimulation", - "/AdvantageKit/RealOutputs/Odometry", - "/AdvantageKit/RealOutputs/Vision", - "/AdvantageKit/RealOutputs/Vision/AprilTags", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/0", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/0/angle", - "/AdvantageKit/Drive/Module0", - "/AdvantageKit/Drive/Module0/TurnAbsolutePosition", - "/Robot", - "/Robot/SwerveDrivetrain/Sim", - "/Robot/SwerveDrivetrain/Sim/GroundTruthPose", - "/Robot/SwerveTeleopControlCommand", - "/Robot/SwerveTeleopControlCommand/CommandedSpeeds", - "/Robot/SwerveDrivetrain", - "/Robot/SwerveDrivetrain/State", - "/Robot/SwerveDrivetrain/State/ModuleStates", - "/SmartDashboard/simulation field", - "/AdvantageKit/RealOutputs/Field", - "/AdvantageKit/RealOutputs/Shooter", - "/Pose", - "/AdvantageKit/RealOutputs/Vision/MeasurementErrors", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/ValidPoseEstimationsMultiTags/0", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/ValidPoseEstimationsMultiTags/0/rotation", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/ValidPoseEstimationsMultiTags/0/rotation/q", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/ValidPoseEstimationsSingleTags/0", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/ValidPoseEstimationsSingleTags/0/rotation", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/ValidPoseEstimationsSingleTags/0/rotation/q", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/ValidPoseEstimationsSingleTags/1", - "/AdvantageKit/RealOutputs/Vision/AprilTags/MeasurementErrors", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Results", - "/AdvantageKit/RealOutputs/Vision/AprilTags/Filtering/rotation estimations", - "/AdvantageKit/RealOutputs/LoggedRobot", - "/SmartDashboard", - "/PathPlanner", - "/SmartDashboard/SwerveDrive", - "/AdvantageKit/RealOutputs/CTREMotor", - "/AdvantageKit/RealOutputs/CTREMotor/1", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/1", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/1/angle", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/2", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/2/angle", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/3", - "/AdvantageKit/RealOutputs/SwerveStates/Measured/3/angle", - "/AdvantageKit/RealOutputs/CTREMotor/3", - "/AdvantageKit/RealOutputs/SwerveStates", - "/AdvantageKit/RealOutputs/Odometry/Robot", - "/AdvantageKit/RealOutputs/SwerveStates/Measured" - ] - }, - "tabs": { - "selected": 1, - "tabs": [ - { - "type": 0, - "title": "", - "controller": null, - "controllerUUID": "yq3yn60elpo6599mgcl2zblor1vbvqkd", - "renderer": "#/", - "controlsHeight": 0 - }, - { - "type": 3, - "title": "3D Field", - "controller": { - "sources": [ - { - "type": "ghost", - "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot", - "logType": "Pose2d", - "visible": true, - "options": { - "model": "Crab Bot", - "color": "#00ff00" - } - }, - { - "type": "robot", - "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/RobotPosition", - "logType": "Pose2d", - "visible": true, - "options": { - "model": "Crab Bot" - } - }, - { - "type": "swerveStates", - "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Setpoints", - "logType": "SwerveModuleState[]", - "visible": true, - "options": { - "color": "#00ff00", - "arrangement": "0,1,2,3" - } - }, - { - "type": "swerveStates", - "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", - "logType": "SwerveModuleState[]", - "visible": true, - "options": { - "color": "#ff8c00", - "arrangement": "0,1,2,3" - } - } - ], - "game": "2024 Field", - "origin": "blue" - }, - "controllerUUID": "9p30xnq3grwk3msk2y4y54whg8n1qz7x", - "renderer": { - "cameraIndex": -1, - "orbitFov": 50, - "cameraPosition": [ - 1.7313564491338231, - 5.968403479160117, - -10.365530546036977 - ], - "cameraTarget": [ - 3.4044435760286507, - -1.5105765402205444, - 0.3822893203493872 - ] - }, - "controlsHeight": 200 - }, - { - "type": 9, - "title": "Swerve", - "controller": { - "sources": [ - { - "type": "states", - "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured", - "logType": "SwerveModuleState[]", - "visible": true, - "options": { - "color": "#ff0000", - "arrangement": "0,1,2,3" - } - }, - { - "type": "states", - "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Setpoints", - "logType": "SwerveModuleState[]", - "visible": true, - "options": { - "color": "#0000ff", - "arrangement": "0,1,2,3" - } - }, - { - "type": "rotation", - "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot/rotation", - "logType": "Rotation2d", - "visible": true, - "options": {} - } - ], - "maxSpeed": 5, - "sizeX": 0.65, - "sizeY": 0.65, - "orientation": 1 - }, - "controllerUUID": "cmc14v22quwz2mrzq00id113vvoyy7id", - "renderer": null, - "controlsHeight": 200 - }, - { - "type": 8, - "title": "Joysticks", - "controller": [ - "Xbox Controller (White)", - "None", - "None", - "None", - "None", - "None" - ], - "controllerUUID": "1mu7lxp5m9rejv61d0z96e8ltd6p7xmi", - "renderer": null, - "controlsHeight": 85 - } - ] - } - } - } - ], - "satellites": [], - "version": "4.0.0-beta-3" -} diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json deleted file mode 100644 index e3c2d54..0000000 --- a/AdvantageScope Swerve Calibration.json +++ /dev/null @@ -1,463 +0,0 @@ -{ - "hubs": [ - { - "x": 139, - "y": 95, - "width": 1100, - "height": 650, - "state": { - "sidebar": { - "width": 300, - "expanded": [ - "/Drive", - "/RealOutputs", - "/RealOutputs/SwerveStates", - "/RealOutputs/SwerveChassisSpeeds", - "/RealOutputs/Odometry" - ] - }, - "tabs": { - "selected": 1, - "tabs": [ - { - "type": 0, - "title": "", - "controller": null, - "controllerUUID": "grimvlgu9aluj8btvwotco2wpj0pto2h", - "renderer": "#/", - "controlsHeight": 0 - }, - { - "type": 9, - "title": "Drive Overview", - "controller": { - "sources": [ - { - "type": "states", - "logKey": "/RealOutputs/SwerveStates/Measured", - "logType": "SwerveModuleState[]", - "visible": true, - "options": { - "color": "#ff0000", - "arrangement": "0,1,2,3" - } - }, - { - "type": "states", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", - "logType": "SwerveModuleState[]", - "visible": true, - "options": { - "color": "#00ffff", - "arrangement": "0,1,2,3" - } - }, - { - "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", - "logType": "ChassisSpeeds", - "visible": true, - "options": { - "color": "#ff0000" - } - }, - { - "type": "chassisSpeeds", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", - "logType": "ChassisSpeeds", - "visible": true, - "options": { - "color": "#00ffff" - } - }, - { - "type": "rotation", - "logKey": "/RealOutputs/Odometry/Robot/rotation", - "logType": "Rotation2d", - "visible": true, - "options": {} - } - ], - "maxSpeed": 5, - "sizeX": 0.65, - "sizeY": 0.65, - "orientation": 1 - }, - "controllerUUID": "prdx7t2eedan6n46dxrjfu7eisf16f9o", - "renderer": null, - "controlsHeight": 200 - }, - { - "type": 3, - "title": "Field", - "controller": { - "sources": [ - { - "type": "robot", - "logKey": "/RealOutputs/Odometry/Robot", - "logType": "Pose2d", - "visible": true, - "options": { - "model": "Crab Bot" - } - }, - { - "type": "trajectory", - "logKey": "/RealOutputs/Odometry/Trajectory", - "logType": "Pose2d[]", - "visible": true, - "options": { - "color": "#ff8c00", - "size": "normal" - } - }, - { - "type": "ghost", - "logKey": "/RealOutputs/Odometry/TrajectorySetpoint", - "logType": "Pose2d", - "visible": true, - "options": { - "model": "Crab Bot", - "color": "#ff8c00" - } - } - ], - "game": "2024 Field", - "origin": "blue" - }, - "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", - "renderer": { - "cameraIndex": -1, - "orbitFov": 50, - "cameraPosition": [ - 1.4695761589768238e-15, - 5.999999999999999, - -12 - ], - "cameraTarget": [ - 0, - 0.5, - 0 - ] - }, - "controlsHeight": 200 - }, - { - "type": 1, - "title": "Turn Calibration", - "controller": { - "leftSources": [ - { - "type": "stepped", - "logKey": "/Drive/Module0/TurnPosition/value", - "logType": "Number", - "visible": true, - "options": { - "color": "#e5b31b", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/Drive/Module1/TurnPosition/value", - "logType": "Number", - "visible": true, - "options": { - "color": "#af2437", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/Drive/Module2/TurnPosition/value", - "logType": "Number", - "visible": true, - "options": { - "color": "#80588e", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/Drive/Module3/TurnPosition/value", - "logType": "Number", - "visible": true, - "options": { - "color": "#e48b32", - "size": "normal" - } - } - ], - "rightSources": [], - "discreteSources": [], - "leftLockedRange": null, - "rightLockedRange": null, - "leftUnitConversion": { - "type": null, - "factor": 1 - }, - "rightUnitConversion": { - "type": null, - "factor": 1 - }, - "leftFilter": 0, - "rightFilter": 0 - }, - "controllerUUID": "ol8lk80m83ma3aegae849d6k1d29sp7d", - "renderer": null, - "controlsHeight": 200 - }, - { - "type": 1, - "title": "Drive Calibration", - "controller": { - "leftSources": [ - { - "type": "stepped", - "logKey": "/Drive/Module0/DrivePositionRad", - "logType": "Number", - "visible": true, - "options": { - "color": "#e5b31b", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/Drive/Module1/DrivePositionRad", - "logType": "Number", - "visible": true, - "options": { - "color": "#af2437", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/Drive/Module2/DrivePositionRad", - "logType": "Number", - "visible": true, - "options": { - "color": "#80588e", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/Drive/Module3/DrivePositionRad", - "logType": "Number", - "visible": true, - "options": { - "color": "#e48b32", - "size": "normal" - } - } - ], - "rightSources": [], - "discreteSources": [], - "leftLockedRange": null, - "rightLockedRange": null, - "leftUnitConversion": { - "type": null, - "factor": 1 - }, - "rightUnitConversion": { - "type": null, - "factor": 1 - }, - "leftFilter": 0, - "rightFilter": 0 - }, - "controllerUUID": "7jj1f83nsv59dd2ou11ls3ccz5tq1293", - "renderer": null, - "controlsHeight": 200 - }, - { - "type": 1, - "title": "Turn PID Tuning", - "controller": { - "leftSources": [ - { - "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value", - "logType": "Number", - "visible": true, - "options": { - "color": "#2b66a2", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", - "logType": "Number", - "visible": true, - "options": { - "color": "#e5b31b", - "size": "normal" - } - } - ], - "rightSources": [], - "discreteSources": [], - "leftLockedRange": null, - "rightLockedRange": null, - "leftUnitConversion": { - "type": null, - "factor": 1 - }, - "rightUnitConversion": { - "type": null, - "factor": 1 - }, - "leftFilter": 0, - "rightFilter": 0 - }, - "controllerUUID": "kj6tf6sxm5g04wnjrhed6mx2wpccmqtn", - "renderer": null, - "controlsHeight": 200 - }, - { - "type": 1, - "title": "Drive PID Tuning", - "controller": { - "leftSources": [ - { - "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", - "logType": "Number", - "visible": true, - "options": { - "color": "#2b66a2", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", - "logType": "Number", - "visible": true, - "options": { - "color": "#e5b31b", - "size": "normal" - } - } - ], - "rightSources": [], - "discreteSources": [], - "leftLockedRange": null, - "rightLockedRange": null, - "leftUnitConversion": { - "type": null, - "factor": 1 - }, - "rightUnitConversion": { - "type": null, - "factor": 1 - }, - "leftFilter": 0, - "rightFilter": 0 - }, - "controllerUUID": "bctxg9pmwpo31m9bv9ofr92oqqtbpqzp", - "renderer": null, - "controlsHeight": 200 - }, - { - "type": 1, - "title": "Max Speed Measurement", - "controller": { - "leftSources": [ - { - "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", - "logType": "Number", - "visible": true, - "options": { - "color": "#2b66a2", - "size": "normal" - } - }, - { - "type": "stepped", - "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", - "logType": "Number", - "visible": true, - "options": { - "color": "#e5b31b", - "size": "normal" - } - } - ], - "rightSources": [], - "discreteSources": [], - "leftLockedRange": null, - "rightLockedRange": null, - "leftUnitConversion": { - "type": null, - "factor": 1 - }, - "rightUnitConversion": { - "type": null, - "factor": 1 - }, - "leftFilter": 0, - "rightFilter": 0 - }, - "controllerUUID": "oy3vmjuhr7g36evikwnmjhfacl30mzn5", - "renderer": null, - "controlsHeight": 200 - }, - { - "type": 1, - "title": "Slip Current Measurement", - "controller": { - "leftSources": [ - { - "type": "stepped", - "logKey": "/Drive/Module0/DriveCurrentAmps", - "logType": "Number", - "visible": true, - "options": { - "color": "#2b66a2", - "size": "normal" - } - } - ], - "rightSources": [ - { - "type": "stepped", - "logKey": "/Drive/Module0/DriveVelocityRadPerSec", - "logType": "Number", - "visible": true, - "options": { - "color": "#e5b31b", - "size": "normal" - } - } - ], - "discreteSources": [], - "leftLockedRange": null, - "rightLockedRange": null, - "leftUnitConversion": { - "type": null, - "factor": 1 - }, - "rightUnitConversion": { - "type": null, - "factor": 1 - }, - "leftFilter": 0, - "rightFilter": 0 - }, - "controllerUUID": "efudylbvebbv1ga2kosknov0uegmtj7h", - "renderer": null, - "controlsHeight": 200 - } - ] - } - } - } - ], - "satellites": [], - "version": "4.0.0-beta-3" -} diff --git a/README copy.md b/README copy.md deleted file mode 100644 index a1ba8dc..0000000 --- a/README copy.md +++ /dev/null @@ -1,21 +0,0 @@ -# AdvantageKit Talon Swerve Template with maple-sim - -[Original Project](https://github.com/Mechanical-Advantage/AdvantageKit/releases/download/v4.0.0-beta-1/AdvantageKit_TalonFXSwerveTemplate.zip) - -This is the AdvantageKit Swerve Template with CTRE hardware, enhanced with maple-sim integration for improved chassis physics simulation. - -Not many changes were made to the original project—only the necessary ones to implement maple-sim. See the [changelog from the original project](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/commit/9c010d3ee9037b16c2a7a462ea68a09af3814441). - -The control loops of the chassis run on CTRE motor controllers. During simulation, they are replaced with WpiLib `PIDController`, so there might be slight differences between the real and simulated robot. - -For a more precise simulation of CTRE motor controllers, see [AdvantageKit Talon Swerve Template with Advanced Phoenix Simulation](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/tree/main/templates/AdvantageKit_TalonSwerveTemplate_EnhancedPhoenixSimulation). - -## Getting Started -The usage of this project is identical to the original. See the [AdvantageKit Online Documentation](https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template) for instructions on how to use this template. - -## Running the Simulation -Change `Constants.currentRobotMode` to `SIM` and run `simulateJava`. -Connect the robot from AdvantageScope, open [AdvantageScope Simulation Layout](./AdvantageScope%20Simulation.json), and drive your robot around! - -## Using Vision -The [AdvantageKit Vision Example](https://docs.advantagekit.org/getting-started/template-projects/vision-template) is also included in this project. Note that there is a slight difference when setting up vision if you're using maple-sim. See the [code](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/blob/87588b4fb0554383b3881a4b0f56147519c32c68/templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/RobotContainer.java#L100-L105). diff --git a/README.md b/README.md index ba67ea5..8e00536 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,27 @@ -# 2025RobotCode -Initial Repo for 2025 Robot code +## 2025RobotCode + FRC Team 5414's code for the 2025 competition robot for Reefscape 🪸, Pearracuda 🐠. The code is written in Java and uses WPILib's Java command-based structure. + + [Simulation Instructions](ascope_assets/README.md) + + ![Pearracuda](/pearracuda.png) + +## Code Highlights +- Field-Centric Swerve Drive + + The robot's drivetrain is a [standard swerve drivetrain with field-centric control](https://github.com/Pearadox/2025RobotCode/blob/main/src/main/java/frc/robot/RobotContainer.java) using SDS MK4i modules with double Kraken X60 motors. The drivetrain uses encoders, a Pigeon 2 gyro, and odometry to control movement during the autonomous and teleoperated phases. The rotation of the drivetrain can be controlled either through [speed](https://github.com/Pearadox/2025RobotCode/blob/22e8898f33335ec9af541d74a5766eed62fa08fb/src/main/java/frc/robot/RobotContainer.java#L417) or [heading](https://github.com/Pearadox/2024BetaRobot/blob/main/src/main/java/frc/robot/subsystems/Drivetrain.java#L215). + +- Elevator + Arm, End Effector/Intake Rollers, Climber Winch + + The robot uses WPILib subsystems and enums to effectively create a state machine that controls each mechanism. The arm and elevator use [Motion Magic](https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/motion-magic.html) to ensure precise, repeatable control, with both fixed setpoints and [dynamic positions](https://github.com/Pearadox/2025RobotCode/blob/22e8898f33335ec9af541d74a5766eed62fa08fb/src/main/java/frc/robot/commands/AutoAlign.java#L325C1-L371C1) determined through [inverse kinematics](https://www.desmos.com/calculator/jvobhlmrbn). The end effector uses voltage control to intake coral and algae, and [position control](https://github.com/Pearadox/2025RobotCode/blob/4b8c491f6c4f645ceead8bfcd7b8b43b8ccf90e0/src/main/java/frc/robot/subsystems/EndEffector.java#L231) to hold onto a game piece. The climber implements a [state machine](https://github.com/Pearadox/2025RobotCode/blob/22e8898f33335ec9af541d74a5766eed62fa08fb/src/main/java/frc/robot/subsystems/Climber.java#L104) with positional PID control to grasp the deep cage and lift the robot. + +- Autonomous Path Following + + The robot uses [Team 3015's PathPlanner](https://github.com/mjansen4857/pathplanner), a motion profile generator for FRC robots, to generate and follow autonomous trajectories. Autonomous routines are created using PathPlanner's built-in AutoBuilder and declaring [NamedCommands](https://github.com/Pearadox/2025RobotCode/blob/22e8898f33335ec9af541d74a5766eed62fa08fb/src/main/java/frc/robot/RobotContainer.java#L273) with the PathPlanner application, and selected through [sendable choosers](https://github.com/Pearadox/2025RobotCode/blob/22e8898f33335ec9af541d74a5766eed62fa08fb/src/main/java/frc/robot/RobotContainer.java#L112) in [Elastic](https://frc-elastic.gitbook.io/docs). + +- Reef and Coral Station Alignment/Limelight Vision + + The robot uses [Limelight's Vision Software](https://limelightvision.io/) to manage two mounted Limelight 4 cameras that provide real-time positional data of the robot based on field elements with [Team 3636 General's](https://github.com/frc3636) [vision backend code](https://github.com/Pearadox/2024BetaRobot/blob/main/src/main/java/frc/lib/drivers/vision/LimelightBackend.java) modified to support multiple cameras and [MegaTag2](https://github.com/Pearadox/2024BetaRobot/blob/main/src/main/java/frc/lib/drivers/vision/LimelightBackend.java#L32). A [single button press](https://github.com/Pearadox/2025RobotCode/blob/22e8898f33335ec9af541d74a5766eed62fa08fb/src/main/java/frc/robot/RobotContainer.java#L169) autonomously drives the robot to a pose a given offset from an April Tag using PID controllers for translational and rotational velocity, enabling swift [alignment](https://github.com/Pearadox/2025RobotCode/blob/main/src/main/java/frc/robot/commands/AutoAlign.java) to the reef and coral stations. + +- Logging, Simulation, and Replay + + The robot uses [AdvantageKit](https://docs.advantagekit.org/) made by [6328 Mechanical Advantage](https://github.com/Mechanical-Advantage) to log useful information about the robot for debugging purposes. This data can be analyzed in [AdvantageScope](https://docs.advantagescope.org/) or [Pearascope](https://github.com/Pearadox/PearascopeV2), a custom spreadsheet generator that outputs key entries of interest to tell a story of what happened throughout a match. [WPILib's simulation classes](https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/introduction.html) output data that can be used to verify code logic without a physical robot, and Team 5516's [Maple Sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim) library models interactions between robots, field elements, and game pieces using a physics engine. diff --git a/ascope_assets/README.md b/ascope_assets/README.md new file mode 100644 index 0000000..7a34d4c --- /dev/null +++ b/ascope_assets/README.md @@ -0,0 +1,8 @@ +# AdvantageScope Quick Setup + +- Open AdvantageScope, and navigate to `Help > Show Assets Folder` +- Copy the `Robot_Pearracuda/` folder into the `userAssets/` directory +- Next, navigate to File > Import Layout +- Select the `advantagescope_sim_layout.json` + +Learn more [here](https://docs.google.com/presentation/d/1dn9uMHjd4_f11lE1L4BZJm6liR8zQ6XoB1TOTw9aH5k/edit?usp=sharing) diff --git a/ascope_assets/Robot_Pearracuda/config.json b/ascope_assets/Robot_Pearracuda/config.json new file mode 100644 index 0000000..1237603 --- /dev/null +++ b/ascope_assets/Robot_Pearracuda/config.json @@ -0,0 +1,166 @@ +{ + "name": "Pearracuda", + "sourceUrl": "https://github.com/Pearadox/2025RobotCode", + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 180 + } + ], + "position": [ + 0, + 0, + 0 + ], + "cameras": [ + { + "name": "Northstar 0", + "rotations": [ + { + "axis": "y", + "degrees": -28.125 + }, + { + "axis": "z", + "degrees": 30.0 + } + ], + "position": [ + 0.225425, + 0.2667, + 0.20955 + ], + "resolution": [ + 1600, + 1200 + ], + "fov": 75 + }, + { + "name": "Northstar 1", + "rotations": [ + { + "axis": "y", + "degrees": -16.875 + }, + { + "axis": "z", + "degrees": -4.709 + } + ], + "position": [ + 0.08255, + 0.127, + 0.16256 + ], + "resolution": [ + 1600, + 1200 + ], + "fov": 45 + }, + { + "name": "Northstar 2", + "rotations": [ + { + "axis": "y", + "degrees": -28.125 + }, + { + "axis": "z", + "degrees": -30.0 + } + ], + "position": [ + 0.225425, + -0.2667, + 0.20955 + ], + "resolution": [ + 1600, + 1200 + ], + "fov": 75 + }, + { + "name": "Northstar 3", + "rotations": [ + { + "axis": "y", + "degrees": -33.75 + }, + { + "axis": "z", + "degrees": 176.386 + } + ], + "position": [ + -0.4064, + -0.3048, + 0.2159 + ], + "resolution": [ + 1600, + 1200 + ], + "fov": 90 + } + ], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 180 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 180 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + -1.0023799104 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 180 + } + ], + "zeroedPosition": [ + 0.0, + -0.3429, + -0.2286 + ] + } + ] +} diff --git a/ascope_assets/Robot_Pearracuda/info.txt b/ascope_assets/Robot_Pearracuda/info.txt new file mode 100644 index 0000000..2ae739c --- /dev/null +++ b/ascope_assets/Robot_Pearracuda/info.txt @@ -0,0 +1,6 @@ +model.glb = base + stationary portions of subsystems +model_0.glb = elevator stage 1 +model_1.glb = arm +model_2.glb = climber + +Learn more here: https://docs.advantagescope.org/more-features/custom-assets/ diff --git a/ascope_assets/Robot_Pearracuda/model.glb b/ascope_assets/Robot_Pearracuda/model.glb new file mode 100644 index 0000000..36595cb Binary files /dev/null and b/ascope_assets/Robot_Pearracuda/model.glb differ diff --git a/ascope_assets/Robot_Pearracuda/model_0.glb b/ascope_assets/Robot_Pearracuda/model_0.glb new file mode 100644 index 0000000..39934d7 Binary files /dev/null and b/ascope_assets/Robot_Pearracuda/model_0.glb differ diff --git a/ascope_assets/Robot_Pearracuda/model_1.glb b/ascope_assets/Robot_Pearracuda/model_1.glb new file mode 100644 index 0000000..e7aeb94 Binary files /dev/null and b/ascope_assets/Robot_Pearracuda/model_1.glb differ diff --git a/ascope_assets/Robot_Pearracuda/model_2.glb b/ascope_assets/Robot_Pearracuda/model_2.glb new file mode 100644 index 0000000..a6ba750 Binary files /dev/null and b/ascope_assets/Robot_Pearracuda/model_2.glb differ diff --git a/ascope_assets/advantagescope_sim_layout.json b/ascope_assets/advantagescope_sim_layout.json new file mode 100644 index 0000000..e27a2db --- /dev/null +++ b/ascope_assets/advantagescope_sim_layout.json @@ -0,0 +1,397 @@ +{ + "hubs": [ + { + "x": -8, + "y": -8, + "width": 1296, + "height": 736, + "state": { + "sidebar": { + "width": 353, + "expanded": [ + "/AdvantageKit", + "/AdvantageKit/RealOutputs", + "/AdvantageKit/RealOutputs/FieldSimulation", + "/AdvantageKit/RealOutputs/EndEffector", + "/AdvantageKit/Arm/PivotData", + "/AdvantageKit/Arm/ExtensionData", + "/AdvantageKit/Arm/WristData", + "/AdvantageKit/EndEffector/LeftData", + "/AdvantageKit/EndEffector/RightData", + "/AdvantageKit/EndEffector/TopData", + "/AdvantageKit/RealOutputs/Odometry", + "/AdvantageKit/RealOutputs/SwerveChassisSpeeds", + "/AdvantageKit/RealOutputs/SwerveStates", + "/AdvantageKit/Arm" + ] + }, + "tabs": { + "selected": 1, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "829h1tyw58kkzg7koull64mh81j8lky5", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 3, + "title": "Sim Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Pose", + "logType": "Pose3d", + "visible": true, + "options": { + "model": "Pearracuda" + } + }, + { + "type": "component", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Components", + "logType": "Transform3d[]", + "visible": true, + "options": {} + }, + { + "type": "mechanism", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Mechanism Visualizer", + "logType": "Mechanism2d", + "visible": false, + "options": {} + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Coral", + "logType": "Pose3d[]", + "visible": true, + "options": { + "variant": "Coral" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Held Coral", + "logType": "Pose3d", + "visible": true, + "options": { + "variant": "Coral" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Algae", + "logType": "Pose3d[]", + "visible": true, + "options": { + "variant": "Algae" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Held Algae", + "logType": "Pose3d", + "visible": true, + "options": { + "variant": "Algae" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Staged Algae", + "logType": "Pose3d[]", + "visible": true, + "options": { + "variant": "Algae" + } + }, + { + "type": "ghost", + "logKey": "NT:/AdvantageKit/RealOutputs/Telemetry/Pose", + "logType": "Pose2d", + "visible": false, + "options": { + "model": "Pearracuda", + "color": "#00ff00" + } + }, + { + "type": "trajectory", + "logKey": "NT:/PathPlanner/activePath", + "logType": "Pose2d[]", + "visible": false, + "options": { + "color": "#00ff00", + "size": "normal" + } + } + ], + "game": "2025 Field (Welded)", + "origin": "blue" + }, + "controllerUUID": "ln1lwpda3q415hmlkbdfevdrl6bpgm7o", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + -11.182498625012554, + 4.454027345376526, + -0.04860750494090679 + ], + "cameraTarget": [ + -0.8657623461981836, + -1.9041575504751056, + 0.00952385989003098 + ] + }, + "controlsHeight": 149 + }, + { + "type": 10, + "title": "2D Visualizer", + "controller": [ + { + "type": "mechanism", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Mechanism Visualizer", + "logType": "Mechanism2d", + "visible": true, + "options": {} + } + ], + "controllerUUID": "d82al9n4tu44ht14xmx8jq0s56quy8w7", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Logged Data", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/Arm/Cur Setpoint", + "logType": "Number", + "visible": true, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/Arm/PositionRots", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/Arm/Adjust", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/Elevator/Setpoint", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/Elevator/Position Rots", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/Elevator/Offset", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/LoggedRobot/FullCycleMS", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/AdvantageKit/DriverStation/Enabled", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "NT:/AdvantageKit/RealOutputs/EE/Has Coral", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/AdvantageKit/RealOutputs/Elevator/IsCoral", + "logType": "Boolean", + "visible": false, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "stripes", + "logKey": "NT:/AdvantageKit/RealOutputs/Elevator Mode", + "logType": "String", + "visible": false, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "stripes", + "logKey": "NT:/AdvantageKit/RealOutputs/Climber/ClimbStateString", + "logType": "String", + "visible": false, + "options": { + "color": "#e5b31b" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "gncvtbak2944cxw1i2ma8q6wjp462shc", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 8, + "title": "Joysticks", + "controller": [ + "Xbox Controller (White)", + "Xbox Controller (Blue)", + "None", + "None", + "None", + "None" + ], + "controllerUUID": "brh960mapv74faj42gcgdguofb4r06ni", + "renderer": null, + "controlsHeight": 85 + }, + { + "type": 9, + "title": "Swerve", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/Telemetry/Module States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/Telemetry/Module Targets", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ff00", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/Telemetry/Module States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/AdvantageKit/RealOutputs/Telemetry/Module Targets", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ff00", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/AdvantageKit/RealOutputs/Telemetry/Speeds", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ffff00" + } + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "0befpfom0xfuae67mcc7h75niq1s41m4", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.1.6" +} diff --git a/build.gradle b/build.gradle index 0a5591a..867476a 100644 --- a/build.gradle +++ b/build.gradle @@ -3,6 +3,7 @@ plugins { id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.peterabeles.gversion" version "1.10" id("com.diffplug.spotless") version "7.0.0.BETA4" + id "io.freefair.lombok" version "8.4" } java { diff --git a/pearracuda.png b/pearracuda.png new file mode 100644 index 0000000..d2af944 Binary files /dev/null and b/pearracuda.png differ diff --git a/src/main/deploy/pathplanner/autos/2R_EF-L4.auto b/src/main/deploy/pathplanner/autos/2R_EF-L4.auto deleted file mode 100644 index e6f5ace..0000000 --- a/src/main/deploy/pathplanner/autos/2R_EF-L4.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "HoldCoral" - } - }, - { - "type": "named", - "data": { - "name": "LevelFour" - } - }, - { - "type": "path", - "data": { - "pathName": "2R_EF" - } - }, - { - "type": "named", - "data": { - "name": "Auto Align Left" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "Outtake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3B_IJ-L4.auto b/src/main/deploy/pathplanner/autos/3B_IJ-L4.auto deleted file mode 100644 index 96fe409..0000000 --- a/src/main/deploy/pathplanner/autos/3B_IJ-L4.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "LevelFour" - } - }, - { - "type": "path", - "data": { - "pathName": "3B_IJ" - } - }, - { - "type": "named", - "data": { - "name": "Outtake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3B_KL-L4.auto b/src/main/deploy/pathplanner/autos/3B_KL-L4.auto deleted file mode 100644 index 6c89c8d..0000000 --- a/src/main/deploy/pathplanner/autos/3B_KL-L4.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "LevelFour" - } - }, - { - "type": "path", - "data": { - "pathName": "3B_KL" - } - }, - { - "type": "named", - "data": { - "name": "Outtake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3R_CD-L4.auto b/src/main/deploy/pathplanner/autos/3R_CD-L4.auto deleted file mode 100644 index 686970e..0000000 --- a/src/main/deploy/pathplanner/autos/3R_CD-L4.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "LevelFour" - } - }, - { - "type": "path", - "data": { - "pathName": "3R_CD" - } - }, - { - "type": "named", - "data": { - "name": "Outtake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3R_EF-L4.auto b/src/main/deploy/pathplanner/autos/3R_EF-L4.auto deleted file mode 100644 index 4321d33..0000000 --- a/src/main/deploy/pathplanner/autos/3R_EF-L4.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "LevelFour" - } - }, - { - "type": "path", - "data": { - "pathName": "3R_EF" - } - }, - { - "type": "named", - "data": { - "name": "Outtake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1-GH.auto b/src/main/deploy/pathplanner/autos/Center 1 Coral.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-GH.auto rename to src/main/deploy/pathplanner/autos/Center 1 Coral.auto diff --git a/src/main/deploy/pathplanner/autos/Center 2 Algae.auto b/src/main/deploy/pathplanner/autos/Center 2 Algae.auto new file mode 100644 index 0000000..893d64c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 2 Algae.auto @@ -0,0 +1,263 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "HoldCoral" + } + }, + { + "type": "path", + "data": { + "pathName": "1_GH" + } + }, + { + "type": "named", + "data": { + "name": "Auto Align Right" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "named", + "data": { + "name": "Outtake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "GH_BACK" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.25 + } + }, + { + "type": "named", + "data": { + "name": "Set Algae" + } + }, + { + "type": "named", + "data": { + "name": "LevelTwo" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Align Mid" + } + }, + { + "type": "named", + "data": { + "name": "Algae Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GH_BACK" + } + }, + { + "type": "named", + "data": { + "name": "LevelFour" + } + }, + { + "type": "path", + "data": { + "pathName": "GH_BARGE" + } + }, + { + "type": "named", + "data": { + "name": "Algae Outtake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BARGE_IJ" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "LevelThree" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Align Mid" + } + }, + { + "type": "named", + "data": { + "name": "Algae Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "IJ_BARGE" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "LevelFour" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Algae Outtake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BARGE_BACK" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Stop EE" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Set Coral" + } + }, + { + "type": "named", + "data": { + "name": "LevelStation" + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2B_IJ-CS-KL-CS-KL.auto b/src/main/deploy/pathplanner/autos/Left.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2B_IJ-CS-KL-CS-KL.auto rename to src/main/deploy/pathplanner/autos/Left.auto diff --git a/src/main/deploy/pathplanner/autos/Push2B_IJ-CS-KL-CS-KL.auto b/src/main/deploy/pathplanner/autos/Push Left.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Push2B_IJ-CS-KL-CS-KL.auto rename to src/main/deploy/pathplanner/autos/Push Left.auto diff --git a/src/main/deploy/pathplanner/autos/Push2R-EF-CS-CD-CS-CD.auto b/src/main/deploy/pathplanner/autos/Push Right.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Push2R-EF-CS-CD-CS-CD.auto rename to src/main/deploy/pathplanner/autos/Push Right.auto diff --git a/src/main/deploy/pathplanner/autos/2R-EF-CS-CD-CS-CD.auto b/src/main/deploy/pathplanner/autos/Right.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2R-EF-CS-CD-CS-CD.auto rename to src/main/deploy/pathplanner/autos/Right.auto diff --git a/src/main/deploy/pathplanner/autos/Working 2B_IJ-L4.auto b/src/main/deploy/pathplanner/autos/Working 2B_IJ-L4.auto deleted file mode 100644 index 19e34c6..0000000 --- a/src/main/deploy/pathplanner/autos/Working 2B_IJ-L4.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "HoldCoral" - } - }, - { - "type": "named", - "data": { - "name": "LevelFour" - } - }, - { - "type": "path", - "data": { - "pathName": "2B_IJ" - } - }, - { - "type": "named", - "data": { - "name": "Auto Align Left" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "Outtake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/untested Algae Steal.auto b/src/main/deploy/pathplanner/autos/untested Algae Steal.auto new file mode 100644 index 0000000..12682a7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/untested Algae Steal.auto @@ -0,0 +1,281 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "HoldCoral" + } + }, + { + "type": "path", + "data": { + "pathName": "1_GH" + } + }, + { + "type": "named", + "data": { + "name": "Auto Align Right" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "named", + "data": { + "name": "Outtake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "GH_BACK" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.25 + } + }, + { + "type": "named", + "data": { + "name": "Set Algae" + } + }, + { + "type": "named", + "data": { + "name": "LevelTwo" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Align Mid" + } + }, + { + "type": "named", + "data": { + "name": "Algae Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "GH_BACK" + } + }, + { + "type": "named", + "data": { + "name": "LevelFour" + } + }, + { + "type": "path", + "data": { + "pathName": "GH_BARGE" + } + }, + { + "type": "named", + "data": { + "name": "Algae Outtake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BARGE_OPP-EF" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "LevelStation" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "ElevatorStowedMode" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Set Algae" + } + }, + { + "type": "named", + "data": { + "name": "LevelThree" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Align Mid" + } + }, + { + "type": "named", + "data": { + "name": "Algae Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OPP-EF_OPP-BARGE" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "LevelFour" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Algae Outtake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OPP-BARGE_BACK" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Set Coral" + } + }, + { + "type": "named", + "data": { + "name": "LevelStation" + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Align_2B_IJ-CS-KL-CS-KL.auto b/src/main/deploy/pathplanner/autos/untested Tilted Start Left.auto similarity index 84% rename from src/main/deploy/pathplanner/autos/Align_2B_IJ-CS-KL-CS-KL.auto rename to src/main/deploy/pathplanner/autos/untested Tilted Start Left.auto index 32ed8ee..88be4bc 100644 --- a/src/main/deploy/pathplanner/autos/Align_2B_IJ-CS-KL-CS-KL.auto +++ b/src/main/deploy/pathplanner/autos/untested Tilted Start Left.auto @@ -7,7 +7,7 @@ { "type": "named", "data": { - "name": "Stop EE" + "name": "HoldCoral" } }, { @@ -19,13 +19,13 @@ { "type": "path", "data": { - "pathName": "2B_IJ" + "pathName": "Tilted 2B_IJ" } }, { "type": "named", "data": { - "name": "Auto Align Left" + "name": "Auto Align Right" } }, { @@ -43,13 +43,7 @@ { "type": "path", "data": { - "pathName": "Align_IJ-CS" - } - }, - { - "type": "named", - "data": { - "name": "Auto Align Station" + "pathName": "IJ-CS" } }, { @@ -91,13 +85,7 @@ { "type": "path", "data": { - "pathName": "Align_KL-CS" - } - }, - { - "type": "named", - "data": { - "name": "Auto Align Station" + "pathName": "KL-CS" } }, { diff --git a/src/main/deploy/pathplanner/autos/Algae1-GH.auto b/src/main/deploy/pathplanner/autos/untested Tilted Start Right.auto similarity index 51% rename from src/main/deploy/pathplanner/autos/Algae1-GH.auto rename to src/main/deploy/pathplanner/autos/untested Tilted Start Right.auto index 795ce46..7ae69a1 100644 --- a/src/main/deploy/pathplanner/autos/Algae1-GH.auto +++ b/src/main/deploy/pathplanner/autos/untested Tilted Start Right.auto @@ -10,16 +10,22 @@ "name": "HoldCoral" } }, + { + "type": "named", + "data": { + "name": "LevelFour" + } + }, { "type": "path", "data": { - "pathName": "1_GH" + "pathName": "Tilted 2R_EF" } }, { "type": "named", "data": { - "name": "Auto Align Right" + "name": "Auto Align Left" } }, { @@ -37,62 +43,91 @@ { "type": "path", "data": { - "pathName": "GH_BACK" + "pathName": "Align_EF-CS" } }, { "type": "named", "data": { - "name": "Set Algae" + "name": "Stop" } }, { "type": "named", "data": { - "name": "LevelTwo" + "name": "Intake" } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.2 + "pathName": "CS-CD" } }, { - "type": "parallel", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Auto Align Mid" - } - }, - { - "type": "named", - "data": { - "name": "Algae Intake" - } - } - ] + "name": "Auto Align Left" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "named", + "data": { + "name": "Outtake" } }, { "type": "path", "data": { - "pathName": "GH_BACK" + "pathName": "CD-CS" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "named", + "data": { + "name": "Intake" } }, { "type": "path", "data": { - "pathName": "GH_BARGE" + "pathName": "CS-CD" + } + }, + { + "type": "named", + "data": { + "name": "Auto Align Right" + } + }, + { + "type": "named", + "data": { + "name": "Stop" + } + }, + { + "type": "named", + "data": { + "name": "Outtake" } }, { "type": "path", "data": { - "pathName": "BARGE_BACK" + "pathName": "CD_BackUp" } } ] diff --git a/src/main/deploy/pathplanner/paths/2R_EF.path b/src/main/deploy/pathplanner/paths/2R_EF.path index 352825f..4e40257 100644 --- a/src/main/deploy/pathplanner/paths/2R_EF.path +++ b/src/main/deploy/pathplanner/paths/2R_EF.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.667790529704936, - "y": 1.9318828060633533 + "x": 6.799080130595383, + "y": 2.241326961617036 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.262 }, "prevControl": { - "x": 5.9670000000000005, - "y": 1.4314999999999998 + "x": 5.765407041386808, + "y": 2.3403447298799485 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Align_EF-CS.path b/src/main/deploy/pathplanner/paths/Align_EF-CS.path index 311db4a..e417ea6 100644 --- a/src/main/deploy/pathplanner/paths/Align_EF-CS.path +++ b/src/main/deploy/pathplanner/paths/Align_EF-CS.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 1.0725, - "y": 0.7977499999999997 + "x": 1.286335227272727, + "y": 0.6296732954545451 }, "prevControl": { - "x": 2.301, - "y": 1.1584999999999996 + "x": 2.514835227272725, + "y": 0.9904232954545452 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "CSP" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/BARGE_BACK.path b/src/main/deploy/pathplanner/paths/BARGE_BACK.path index f1dd3cf..87386b2 100644 --- a/src/main/deploy/pathplanner/paths/BARGE_BACK.path +++ b/src/main/deploy/pathplanner/paths/BARGE_BACK.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.45, + "x": 7.6, "y": 6.19 }, "prevControl": null, "nextControl": { - "x": 7.2, + "x": 7.35, "y": 6.19 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 6.620250000000001, + "x": 6.8, "y": 6.19 }, "prevControl": { - "x": 6.870250000000001, + "x": 7.05, "y": 6.19 }, "nextControl": null, @@ -32,25 +32,12 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.5, - "maxWaypointRelativePos": 1.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.197975253093364, "constraints": { - "maxVelocity": 0.6, + "maxVelocity": 0.5, "maxAcceleration": 0.8, - "maxAngularVelocity": 90.0, - "maxAngularAcceleration": 180.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.35, - "maxWaypointRelativePos": 0.5, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 1.5, - "maxAngularVelocity": 270.0, + "maxAngularVelocity": 30.0, "maxAngularAcceleration": 180.0, "nominalVoltage": 12.0, "unlimited": false @@ -60,7 +47,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.5, + "maxVelocity": 2.0, "maxAcceleration": 1.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0, @@ -68,7 +55,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": 180.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/BARGE_IJ.path b/src/main/deploy/pathplanner/paths/BARGE_IJ.path new file mode 100644 index 0000000..0617921 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BARGE_IJ.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.6, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 7.352108029324875, + "y": 4.967602918730179 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 5.0 + }, + "prevControl": { + "x": 8.051368411005575, + "y": 5.0339069691173615 + }, + "nextControl": { + "x": 6.750129909075028, + "y": 4.991941609270682 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.743636363636364, + "y": 5.40606534090909 + }, + "prevControl": { + "x": 6.296304541527494, + "y": 5.074305453543509 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.7469066366703955, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 0.8, + "maxAngularVelocity": 15.0, + "maxAngularAcceleration": 90.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BARGE_OPP-EF.path b/src/main/deploy/pathplanner/paths/BARGE_OPP-EF.path new file mode 100644 index 0000000..e6a2730 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BARGE_OPP-EF.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.6, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 7.1624351502378945, + "y": 4.959284636541231 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 5.0 + }, + "prevControl": { + "x": 7.822102611250565, + "y": 5.0 + }, + "nextControl": { + "x": 6.746042106599463, + "y": 5.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 5.605 + }, + "prevControl": { + "x": 7.747984994482547, + "y": 5.605 + }, + "nextControl": { + "x": 8.25, + "y": 5.605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.8, + "y": 5.605 + }, + "prevControl": { + "x": 8.258956574311384, + "y": 5.605 + }, + "nextControl": { + "x": 9.355682087342174, + "y": 5.605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.8, + "y": 5.605 + }, + "prevControl": { + "x": 9.382989869664307, + "y": 5.617966854858608 + }, + "nextControl": { + "x": 10.049879226596378, + "y": 5.597230050476597 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.8, + "y": 5.605 + }, + "prevControl": { + "x": 11.113588501676585, + "y": 5.603322421757715 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.5, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.5, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 15.0, + "maxAngularAcceleration": 90.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CD-CS.path b/src/main/deploy/pathplanner/paths/CD-CS.path index 6260ef2..b24e9aa 100644 --- a/src/main/deploy/pathplanner/paths/CD-CS.path +++ b/src/main/deploy/pathplanner/paths/CD-CS.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 0.94575, - "y": 0.7782499999999993 + "x": 1.286335227272727, + "y": 0.6296732954545451 }, "prevControl": { - "x": 1.893006599474605, - "y": 1.4546029680195027 + "x": 2.360602257090394, + "y": 1.3967132870108712 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "CSP" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/CD_BackUp.path b/src/main/deploy/pathplanner/paths/CD_BackUp.path index 1f9e032..a73c351 100644 --- a/src/main/deploy/pathplanner/paths/CD_BackUp.path +++ b/src/main/deploy/pathplanner/paths/CD_BackUp.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.027, - "y": 3.206 + "x": 3.6894886363636368, + "y": 2.663877840909091 }, "prevControl": null, "nextControl": { - "x": 3.5944125146900747, - "y": 2.4909331646298996 + "x": 3.312854373834507, + "y": 2.0289417725280563 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.13094262295082 }, "prevControl": { - "x": 3.5546669418485175, - "y": 2.436349431810464 + "x": 3.5943606673974298, + "y": 2.5061428569337916 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/CS-CD.path b/src/main/deploy/pathplanner/paths/CS-CD.path index adca8b9..f7bb858 100644 --- a/src/main/deploy/pathplanner/paths/CS-CD.path +++ b/src/main/deploy/pathplanner/paths/CS-CD.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.6910000000000003, - "y": 1.5875 + "x": 2.6721290336477224, + "y": 2.0398215804960262 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.237, - "y": 2.126 + "x": 2.4335040983606557, + "y": 1.8132684426229502 }, "prevControl": { - "x": 2.9445000000000006, - "y": 1.7532499999999993 + "x": 2.0185866925222187, + "y": 1.4247316009675033 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ }, "goalEndState": { "velocity": 1.25, - "rotation": -120.96375653207355 + "rotation": -129.28940686250039 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/GH_BACK.path b/src/main/deploy/pathplanner/paths/GH_BACK.path index 65c1268..68121bd 100644 --- a/src/main/deploy/pathplanner/paths/GH_BACK.path +++ b/src/main/deploy/pathplanner/paths/GH_BACK.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.908499999999999, + "x": 5.908, "y": 4.055 }, "prevControl": null, "nextControl": { - "x": 6.1584983228173975, - "y": 4.0540842552277745 + "x": 6.168723661025379, + "y": 4.071232237568991 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.522750000000001, + "x": 6.2, "y": 4.055 }, "prevControl": { - "x": 6.2727511834235505, - "y": 4.054230772872073 + "x": 5.95288724809312, + "y": 4.01711480704802 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/GH_BARGE.path b/src/main/deploy/pathplanner/paths/GH_BARGE.path index d59ccb5..e21aaa8 100644 --- a/src/main/deploy/pathplanner/paths/GH_BARGE.path +++ b/src/main/deploy/pathplanner/paths/GH_BARGE.path @@ -3,71 +3,47 @@ "waypoints": [ { "anchor": { - "x": 6.523, + "x": 6.2, "y": 4.055 }, "prevControl": null, "nextControl": { - "x": 6.522750000000001, - "y": 4.414999999999999 + "x": 6.352991681381408, + "y": 4.795643042852445 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.93225, - "y": 6.189500000000001 + "x": 7.6, + "y": 5.0 }, "prevControl": { - "x": 6.3370886024147675, - "y": 6.084556987729567 - }, - "nextControl": { - "x": 7.178451938253052, - "y": 6.232912044416733 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.45, - "y": 6.189500000000001 - }, - "prevControl": { - "x": 7.1474386025254635, - "y": 6.185353898636076 + "x": 6.843747629959738, + "y": 4.823367782882626 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 180.0 + } + ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.0, - "maxWaypointRelativePos": 2.0, + "minWaypointRelativePos": 0.8008998875140646, + "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 0.6, "maxAcceleration": 0.8, - "maxAngularVelocity": 90.0, - "maxAngularAcceleration": 180.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7014388489208656, - "maxWaypointRelativePos": 1.0035971223021625, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 1.5, - "maxAngularVelocity": 270.0, - "maxAngularAcceleration": 180.0, + "maxAngularVelocity": 45.0, + "maxAngularAcceleration": 90.0, "nominalVoltage": 12.0, "unlimited": false } @@ -79,7 +55,7 @@ "maxVelocity": 2.0, "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 650.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -91,7 +67,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -0.07646229381622528 + "rotation": -0.076 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/IJ-CS.path b/src/main/deploy/pathplanner/paths/IJ-CS.path index 09f903e..142be7b 100644 --- a/src/main/deploy/pathplanner/paths/IJ-CS.path +++ b/src/main/deploy/pathplanner/paths/IJ-CS.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.943250000000001, + "x": 4.943, "y": 5.155999999999999 }, "prevControl": null, "nextControl": { - "x": 5.60625, + "x": 5.605999999999999, "y": 5.975 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/IJ_BARGE.path b/src/main/deploy/pathplanner/paths/IJ_BARGE.path new file mode 100644 index 0000000..10e8aaa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/IJ_BARGE.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.2472128378378375, + "y": 5.329391891891891 + }, + "prevControl": null, + "nextControl": { + "x": 5.787297062046397, + "y": 5.65550890366035 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.93225, + "y": 6.189500000000001 + }, + "prevControl": { + "x": 6.682263175792025, + "y": 6.1920666558809065 + }, + "nextControl": { + "x": 7.671234713105698, + "y": 6.181912722287259 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.6, + "y": 6.189500000000001 + }, + "prevControl": { + "x": 7.350001267371807, + "y": 6.190296042899166 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.250843644544404, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 0.6, + "maxAngularVelocity": 45.0, + "maxAngularAcceleration": 90.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/KL-BackUp.path b/src/main/deploy/pathplanner/paths/KL-BackUp.path index 64cc3ca..2c62ef7 100644 --- a/src/main/deploy/pathplanner/paths/KL-BackUp.path +++ b/src/main/deploy/pathplanner/paths/KL-BackUp.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.027, - "y": 4.815 + "x": 3.799176136363636, + "y": 5.316321022727273 }, "prevControl": null, "nextControl": { - "x": 3.8485260656584583, - "y": 5.14169880413003 + "x": 3.6207022020220943, + "y": 5.643019826857302 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/OPP-BARGE_BACK.path b/src/main/deploy/pathplanner/paths/OPP-BARGE_BACK.path new file mode 100644 index 0000000..95e56ad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OPP-BARGE_BACK.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.951647727272727, + "y": 6.373 + }, + "prevControl": null, + "nextControl": { + "x": 9.701647727272727, + "y": 6.373 + }, + "isLocked": false, + "linkedName": "OPP_BARGE" + }, + { + "anchor": { + "x": 10.8, + "y": 6.373 + }, + "prevControl": { + "x": 11.05, + "y": 6.373 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.197975253093364, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 0.8, + "maxAngularVelocity": 30.0, + "maxAngularAcceleration": 180.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OPP-EF_OPP-BARGE.path b/src/main/deploy/pathplanner/paths/OPP-EF_OPP-BARGE.path new file mode 100644 index 0000000..950d48c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OPP-EF_OPP-BARGE.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 12.364772727272728, + "y": 5.336264204545455 + }, + "prevControl": null, + "nextControl": { + "x": 12.024784993662488, + "y": 5.758861989500107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 12.115482954545454, + "y": 5.765042613636362 + }, + "prevControl": { + "x": 12.34561991357673, + "y": 5.616049625316372 + }, + "nextControl": { + "x": 11.842358642890522, + "y": 5.941866047593262 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.951647727272727, + "y": 6.373309659090909 + }, + "prevControl": { + "x": 9.68550524399205, + "y": 6.403611375985368 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "OPP_BARGE" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.6, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.65, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 1.0, + "maxAngularVelocity": 90.0, + "maxAngularAcceleration": 90.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Tilted 2B_IJ.path b/src/main/deploy/pathplanner/paths/Tilted 2B_IJ.path new file mode 100644 index 0000000..31c74ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Tilted 2B_IJ.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.024795081967212, + "y": 7.111834016393442 + }, + "prevControl": null, + "nextControl": { + "x": 6.531292136223208, + "y": 6.6679553639121 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.528250000000001, + "y": 5.789750000000001 + }, + "prevControl": { + "x": 5.9188448985072455, + "y": 6.1242194993567995 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 60.06826078897191 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 57.49601104443748 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Tilted 2R_EF.path b/src/main/deploy/pathplanner/paths/Tilted 2R_EF.path new file mode 100644 index 0000000..fed2154 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Tilted 2R_EF.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.024795081967212, + "y": 0.734375 + }, + "prevControl": null, + "nextControl": { + "x": 6.849886305569407, + "y": 0.9130000820546282 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.528, + "y": 2.262 + }, + "prevControl": { + "x": 5.702875893027341, + "y": 2.0833427246432716 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.25, + "rotation": -59.95008637901256 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -60.01836063115067 + }, + "useDefaultConstraints": false +} \ 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 5827652..8bccb52 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,14 +7,40 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.units.measure.Frequency; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.generated.TunerConstants; +import frc.robot.util.RobotIdentity; +import java.util.ArrayList; +import java.util.List; public class Constants { + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : Mode.SIM; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + + public static final RobotIdentity IDENTITY = RobotIdentity.getRobotIdentity(); + public static final String IDENTITY_STRING = RobotIdentity.getRobotIdentityString(); + public static final TunerConstants TUNER_CONSTANTS = IDENTITY.tunerConstants; public static class VisionConstants { public static final String LL_NAME = "limelight-back"; @@ -34,10 +60,11 @@ public static final class FieldConstants { public static final double FIELD_LENGTH = Units.inchesToMeters(690.876); public static final double FIELD_WIDTH = Units.inchesToMeters(317); - public static final int[] BLUE_REEF_TAG_IDS = {18, 19, 20, 21, 17, 22}; + public static final int[] BLUE_REEF_TAG_IDS = {18, 19, 20, 21, 22, 17}; public static final int[] BLUE_CORAL_STATION_TAG_IDS = {12, 13}; public static final int[] RED_REEF_TAG_IDS = {7, 6, 11, 10, 9, 8}; public static final int[] RED_CORAL_STATION_TAG_IDS = {1, 2}; + public static final int[] ALL_REEF_TAG_IDS = {18, 19, 20, 21, 22, 17, 7, 6, 11, 10, 9, 8}; public static final Translation2d BLUE_FAR_CAGE = new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779)); @@ -55,6 +82,66 @@ public static final class FieldConstants { public static final Translation2d[] BLUE_CAGES = {BLUE_FAR_CAGE, BLUE_MID_CAGE, BLUE_CLOSE_CAGE}; public static final Translation2d[] RED_CAGES = {RED_FAR_CAGE, RED_MID_CAGE, RED_CLOSE_CAGE}; + + public static final Translation2d BLUE_REEF_CENTER = new Translation2d(4.5, 4); + + public static final List CORAL_STATIONS = new ArrayList<>(); + + static { + for (int tag : BLUE_CORAL_STATION_TAG_IDS) { + CORAL_STATIONS.add( + VisionConstants.aprilTagLayout.getTagPose(tag).get().toPose2d()); + } + for (int tag : RED_CORAL_STATION_TAG_IDS) { + CORAL_STATIONS.add( + VisionConstants.aprilTagLayout.getTagPose(tag).get().toPose2d()); + } + } + + // the top of the branch (L4) is ~2" behind the april tag + public static final double BRANCH_OFFSET_BEHIND_APRILTAG = Units.inchesToMeters(2.049849); + public static final double L4_HEIGHT = Units.inchesToMeters(72); + + public static final Pose3d[] REEF_TAG_POSES = new Pose3d[RED_REEF_TAG_IDS.length + BLUE_REEF_TAG_IDS.length]; + + static { + int i = 0; + for (int tag : FieldConstants.RED_REEF_TAG_IDS) { + REEF_TAG_POSES[i++] = + VisionConstants.aprilTagLayout.getTagPose(tag).get(); + } + for (int tag : FieldConstants.BLUE_REEF_TAG_IDS) { + REEF_TAG_POSES[i++] = + VisionConstants.aprilTagLayout.getTagPose(tag).get(); + } + } + + public static final List REEF_TAGS = new ArrayList<>(); + + static { + for (Pose3d tag : REEF_TAG_POSES) { + REEF_TAGS.add(tag.toPose2d()); + } + } + + public static final Transform3d HIGH_ALGAE_TRANSFORM = + new Transform3d(Units.inchesToMeters(-6), 0, Units.inchesToMeters(39.575), Rotation3d.kZero); + public static final Transform3d LOW_ALGAE_TRANSFORM = + new Transform3d(Units.inchesToMeters(-6), 0, Units.inchesToMeters(23.675), Rotation3d.kZero); + + public static final Pose3d[] REEF_ALGAE_POSES = new Pose3d[REEF_TAG_POSES.length]; + + static { + for (int i = 0; i < REEF_ALGAE_POSES.length; i++) { + REEF_ALGAE_POSES[i] = REEF_TAG_POSES[i].plus(i % 2 == 0 ? HIGH_ALGAE_TRANSFORM : LOW_ALGAE_TRANSFORM); + } + } + + public static final double BARGE_X = FIELD_LENGTH / 2.0; + public static final double BARGE_WIDTH = Units.inchesToMeters(40) / 2.0; + public static final double BARGE_HEIGHT = Units.inchesToMeters(74 + 8); + public static final double BARGE_HEIGHT_TOLERANCE = Units.inchesToMeters(12); + public static final double EE_TOLERANCE = Units.inchesToMeters(16); } public static final class DriveConstants { @@ -67,46 +154,39 @@ public static final class DriveConstants { } public static final class AlignConstants { - // TODO: possible align pid adjustment - // public static final double ALIGN_STRAFE_KP = 0.06; - // public static final double ALIGN_STRAFE_KI = 0.001; - // public static final double ALIGN_FORWARD_KP = 0.04; // -0.06 - - public static final double ALIGN_KS = 0.02; // 0.009 - - // tx and ty tolerances with setpoint - public static final double ALIGN_TOLERANCE_PIXELS = 0.5; - // don't try translationally aligning unless rotation is already aligned within this tolerance - public static final double ALIGN_ROT_TOLERANCE_DEGREES = 10; - - // reduce speed by 1/4 every tick when an april tag is not seen - public static final double ALIGN_DAMPING_FACTOR = 0.75; - public static final double ALIGN_SPEED_DEADBAND = 0.025; - public static final double BRANCH_SPACING = Units.inchesToMeters(12.97 / 2.0); // 12.94 //12.97 // target relative - public static final double REEF_ALIGN_MID_TX = 0; // 0.28575 + public static final double REEF_ALIGN_MID_TX = -0.02; // 0.08; // 0.28575 // 0 public static final double REEF_ALIGN_LEFT_TX = -BRANCH_SPACING - 0.05 + 0.01; - public static final double REEF_ALIGN_RIGHT_TX = BRANCH_SPACING - 0.03 + 0.01; - public static final double REEF_ALIGN_TZ = -0; // target relative - - public static final double STATION_ALIGN_TX = 0.07; - public static final double STATION_ALIGN_TZ = 0; - - public static final double REEF_kP = 0.5; // Tune all PID values - public static final double REEF_kI = 0; - public static final double REEF_kD = 0; - - public static final double REEF_Forward_kP = 0.2; // Tune all PID values - - public static final double ROT_REEF_kP = 0.02; // Tune all PID values - public static final double ROT_REEF_kI = 0; - public static final double ROT_REEF_kD = 0; + public static final double REEF_ALIGN_RIGHT_TX = BRANCH_SPACING - 0.03 + 0.02; + public static final double REEF_ALIGN_TZ = Units.inchesToMeters(18); // try lowering + public static final double REEF_STATION_ALIGN_TZ = Units.inchesToMeters(12); + + public static final Translation2d LEFT_BRANCH_OFFSET = new Translation2d(REEF_ALIGN_TZ, -BRANCH_SPACING); + public static final Translation2d RIGHT_BRANCH_OFFSET = new Translation2d(REEF_ALIGN_TZ, BRANCH_SPACING); + public static final Translation2d MID_OFFSET = new Translation2d(REEF_ALIGN_TZ, REEF_ALIGN_MID_TX); + public static final Translation2d STATION_OFFSET = new Translation2d(REEF_STATION_ALIGN_TZ, 0.0); + + public static final double DRIVE_kP = 2.5; // m/s per m error + public static final double DRIVE_kI = 0.0; + public static final double DRIVE_kD = 0.0; + public static final double MAX_DRIVE_VELOCITY = 3.0; // m/s + public static final double MAX_DRIVE_ACCELERATION = 10; // m/s^2 + + public static final double ROT_kP = 3.0; // rad/s per rad error + public static final double ROT_kI = 0.0; + public static final double ROT_kD = 0.0; + public static final double MAX_ROT_VELOCITY = 8; // rad/s + public static final double MAX_ROT_ACCELERATION = 20; // rad/s^2 // the top of the branch (L4) is ~2" behind the april tag public static final double BRANCH_OFFSET_BEHIND_APRILTAG = Units.inchesToMeters(2.049849); + + // real heights of branches public static final double L4_HEIGHT = Units.inchesToMeters(72); + public static final double L3_HEIGHT = Units.inchesToMeters(48); + public static final double L2_HEIGHT = Units.inchesToMeters(36); // these are from the arm's pivot point to the bottom of a held coral public static final double PIVOT_TO_CORAL_RADIUS = Units.inchesToMeters(23.4106654653); @@ -114,11 +194,13 @@ public static final class AlignConstants { public static final double ARM_STARTING_ANGLE = Units.degreesToRadians(-96); // from the arm's pivot point to floor - public static final double ELEVATOR_STARTING_HEIGHT = Units.inchesToMeters(39); + public static final double ELEVATOR_STARTING_HEIGHT = 1.0023799104; // Units.inchesToMeters(39); + + public static final double ALIGN_ROT_TOLERANCE = Units.degreesToRadians(4); + public static final double ALIGN_TRANSLATION_TOLERANCE = Units.inchesToMeters(4); } public static final class ElevatorConstants { - public static final int ELEVATOR_ID = 21; public static final int ELEVATOR_FOLLOWER_ID = 20; public static final NeutralModeValue MODE = NeutralModeValue.Brake; @@ -141,10 +223,10 @@ public static final class ElevatorConstants { // Inches public static final double STOWED_HEIGHT = 0; public static final double STATION_HEIGHT = - 15; // Home Field is 1in higher than official field - official is 15.4 + 15.7; // Home Field is 1in higher than official field - official is 15.4 // 15 public static final double LEVEL_TWO_HEIGHT = 0; // 10.9; // was 12, 7 This is slightly away from the reef for clearance // - public static final double LEVEL_THREE_HEIGHT = 1.5; // 15 //TODO l3 height + public static final double LEVEL_THREE_HEIGHT = 1.1; // 15 // 1.5 // public static final double LEVEL_FOUR_HEIGHT = 24.6; // 29.625; // public static final double ALGAE_LOW_HEIGHT = 6.4; // 6.7 @@ -194,15 +276,16 @@ public static final class ArmConstants { public static final double ARM_LEVEL_2_ROT = Units.degreesToRotations(147) * ARM_GEAR_RATIO - 6.382; // was -85, then -74.455078125[\] //79.08 - public static final double ARM_INTAKE_ROT = - Units.degreesToRotations(-15) * ARM_GEAR_RATIO; // was 61 //-299 // -311 + public static final double ARM_INTAKE_ROT = Units.degreesToRotations(IDENTITY == RobotIdentity.EVE ? -20 : -15) + * ARM_GEAR_RATIO; // was 61 //-299 // -311 public static final double ARM_STOWED_ROT = Units.degreesToRotations(0) * ARM_GEAR_RATIO; // should be 0 public static final double ARM_ALGAE_LOW = Units.degreesToRotations(73) * ARM_GEAR_RATIO; public static final double ARM_ALGAE_HIGH = Units.degreesToRotations(82) * ARM_GEAR_RATIO; public static final double ARM_BARGE = Units.degreesToRotations(160) * ARM_GEAR_RATIO - 2.1; + // public static final double ARM_BARGE_BACKWARDS = -ARM_BARGE; // public static final double ARM_CLIMB = Units.degreesToRotations(-50) * ARM_GEAR_RATIO; - public static final double ARM_LOLLIPOP = Units.degreesToRotations(-50) * ARM_GEAR_RATIO; + public static final double ARM_LOLLIPOP = Units.degreesToRotations(-50) * ARM_GEAR_RATIO; // TODO: try negate public static final double ARM_L4_BEHIND_CORAL = 22.057; // rots public static final double ARM_STATION_BEHIND_CORAL = Units.degreesToRotations(-15) * ARM_GEAR_RATIO - 1.63; @@ -211,10 +294,10 @@ public static final class ArmConstants { public static final double UPDATE_FREQ = 50; public static final double kG = 0.0; // 0.35; - public static final double kS = 0.0; // 0.15; - public static final double kV = 0.0; // 0.2 + public static final double kS = 0.15; // 0.15; + public static final double kV = 7.2; // 0.2 public static final double kA = 0.0; // 0.07 - public static final double kP = 0.0; // 0.3 + public static final double kP = 0.4; // 0.3 public static final double kI = 0.0; public static final double kD = 0.0; // 0.05 @@ -230,6 +313,15 @@ public static final class ClimbConstants { public static final double CLIMB_ROTS = -126.0; public static final double CLIMB_SETPOINT = -210.0; + + public static final double CLIMBER_LENGTH = Units.inchesToMeters(8); + public static final double MASS_KG = Units.lbsToKilograms(3); + public static final double MOI = MASS_KG * CLIMBER_LENGTH * CLIMBER_LENGTH; // most of the mass is the barb + public static final double GEAR_RATIO = 290.0 * 4.0; + public static final double MIN_ANGLE = Units.degreesToRadians(0); + public static final double MAX_ANGLE = Units.degreesToRadians(90); + public static final double STARTING_ANGLE = MIN_ANGLE; + public static final double ZERO_ANGLE = MAX_ANGLE; } public static final class IntakeConstants { @@ -268,13 +360,20 @@ public static final class EndEffectorConstants { public static final double PULL_SPEED = -0.3; - public static final double PUSH_SPEED = 0.6; // 0.3 + public static final double PUSH_SPEED = 0.5; // 0.3 + public static final double L3_PUSH_SPEED = 0.3; // 0.3 + public static final double L2_PUSH_SPEED = 0.2; // 0.3 public static final double ALGAE_PULL_SPEED = 0.8; public static final double ALGAE_PUSH_SPEED = -1.0; public static final double HOLD_SPEED = -0.075; public static final int END_SENSOR_CHANNEL = 0; + + public static final double PEARRACUDA_CORAL_CURRENT = 30; + public static final double EVE_CORAL_CURRENT = 45; + + public static final double CORAL_CURRENT = Constants.IDENTITY == RobotIdentity.EVE ? 45 : 30; } public static final class LEDConstants { @@ -288,4 +387,40 @@ public static final class LEDConstants { public static final Dimensionless BLINK_BRIGHTNESS = Percent.of(50); public static final Time BREATHE_PERIOD = Seconds.of(1); } + + public static class SimulationConstants { + public static final boolean SIMULATE_GRAVITY = true; + + public static final double ARM_MASS = Units.lbsToKilograms(8); + public static final double ARM_LENGTH = Units.inchesToMeters(12); + public static final double ARM_MOI = SingleJointedArmSim.estimateMOI(ARM_LENGTH, ARM_MASS); + public static final double MIN_ANGLE = Double.NEGATIVE_INFINITY; + public static final double MAX_ANGLE = Double.POSITIVE_INFINITY; + public static final double STARTING_ANGLE = Units.degreesToRadians(-96); + + // joint of ee to bottom of coral + public static final double CAM_LENGTH = Units.inchesToMeters(14.5); + + // joint of ee to top plane of coral + public static final double EE_TO_CORAL_HEIGHT = Units.inchesToMeters(2.5); + public static final double EE_TO_CORAL_WIDTH = Units.inchesToMeters(4.25); + public static final double CORAL_LENGTH = Units.inchesToMeters(11.875); + + public static final double CARRIAGE_MASS_KG = 4.2; + public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(ElevatorConstants.PULLEY_DIAMETER / 2.0); + public static final double MIN_HEIGHT = Units.inchesToMeters(0); + public static final double MAX_HEIGHT = MIN_HEIGHT + Units.inchesToMeters(ElevatorConstants.BARGE_HEIGHT + 1); + public static final double STARTING_HEIGHT = MIN_HEIGHT; + + // new EE viz + public static final double PIVOT_TO_MIDDLE_OF_CORAL_ANG_OFFSET = Units.degreesToRadians(20.2531597269); + public static final double PIVOT_TO_MIDDLE_OF_CORAL_RADIUS = Units.inchesToMeters(23.249031544); + public static final double PIVOT_ANGLE_TO_CORAL_ANGLE = Units.degreesToRadians(-243.986 + 15); + public static final double CORAL_Y_OFFSET = 0.02; + + public static final double ARM_CAD_ZERO_Z = AlignConstants.ELEVATOR_STARTING_HEIGHT; + public static final double CLIMBER_CAD_ZERO_Y = Units.inchesToMeters(13.5); + public static final double CLIMBER_CAD_ZERO_Z = Units.inchesToMeters(9); + public static final double CLIMBER_CAD_ANG_OFFSET = Units.degreesToRadians(65); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1546f88..dadbe56 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,10 @@ package frc.robot; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.elevator.MechVisualizer; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -19,32 +21,47 @@ public class Robot extends LoggedRobot { private final RobotContainer m_robotContainer; public Robot() { - m_robotContainer = new RobotContainer(); Logger.recordMetadata("2025RobotCode", "Comp Bot Code"); // Set a metadata value - - if (isReal()) { - Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick ("/U/logs") - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - } else { - setUseTiming(false); // Run as fast as possible - String logPath = // "/media/sda1/"; - LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) - Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver( - new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + Logger.recordMetadata("Identity/Rio Serial", RobotController.getSerialNumber()); + Logger.recordMetadata("Identity/Name", Constants.IDENTITY.name()); + + // Set up data receivers & replay source + switch (Constants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; } Logger.start(); + + m_robotContainer = new RobotContainer(); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - RobotContainer.poseEstimation.periodic(0); + MechVisualizer.getInstance().periodic(); } @Override - public void disabledInit() {} + public void disabledInit() { + m_robotContainer.resetSimulation(); + } @Override public void disabledPeriodic() {} @@ -92,5 +109,7 @@ public void testPeriodic() {} public void testExit() {} @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + m_robotContainer.displaySimFieldToAdvantageScope(); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c55ac16..f3c478b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,13 +1,13 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.vision.VisionConstants.*; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathfindingCommand; import com.pathplanner.lib.events.EventTrigger; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -21,50 +21,60 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.AlignConstants; -import frc.robot.Constants.VisionConstants; import frc.robot.commands.ArmHold; import frc.robot.commands.AutoAlign; +import frc.robot.commands.DriveCommands; import frc.robot.commands.ElevatorHold; -import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Arm.ArmMode; -import frc.robot.subsystems.Climber; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.EndEffector; -import frc.robot.util.vision.PoseEstimation; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.Arm.ArmMode; +import frc.robot.subsystems.arm.ArmIO; +import frc.robot.subsystems.arm.ArmIOReal; +import frc.robot.subsystems.arm.ArmIOSim; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.ClimberIOReal; +import frc.robot.subsystems.climber.ClimberIOSim; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.GyroIOSim; +import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorIO; +import frc.robot.subsystems.elevator.ElevatorIOReal; +import frc.robot.subsystems.elevator.ElevatorIOSim; +import frc.robot.subsystems.endeffector.EndEffector; +import frc.robot.subsystems.endeffector.EndEffectorIO; +import frc.robot.subsystems.endeffector.EndEffectorIOReal; +import frc.robot.subsystems.endeffector.EndEffectorIOSim; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIOLimelight; +import frc.robot.util.RobotIdentity; +import frc.robot.util.SmarterDashboard; +import frc.robot.util.simulation.AlgaeHandler; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.littletonrobotics.junction.Logger; public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + private Drive drive; + private Elevator elevator; + public static Arm arm; + private EndEffector endEffector; + private Vision vision; + private Climber climber; + public static AutoAlign align; - public static final Elevator elevator = Elevator.getInstance(); - public static final Arm arm = Arm.getInstance(); - public static final EndEffector endEffector = EndEffector.getInstance(); - public static final Climber climber = Climber.getInstance(); - public static final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - public static final AutoAlign align = new AutoAlign(() -> drivetrain.getState().Pose); + private SwerveDriveSimulation driveSimulation = null; - public static final PoseEstimation poseEstimation = new PoseEstimation(); // public static final LEDStrip ledstrip = LEDStrip.getInstance(); - private final SwerveRequest.FieldCentric fieldOrientedDrive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final SwerveRequest.RobotCentric robotOrientedDrive = new SwerveRequest.RobotCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - private final Telemetry logger = new Telemetry(MaxSpeed); - // Controller public static final XboxController driverController = new XboxController(0); public static final XboxController opController = new XboxController(1); - // Driver Controller + // ---------------------------- Driver Controller --------------------------------- // + private final JoystickButton resetHeading_Start = new JoystickButton(driverController, XboxController.Button.kStart.value); @@ -80,7 +90,8 @@ public class RobotContainer { private final Trigger strafe_Triggers = new Trigger( () -> Math.abs(driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()) > 0.1); - // Op Controller + // ----------------------------- Op Controller -------------------------------- // + private final JoystickButton homeElevator_Start = new JoystickButton(opController, XboxController.Button.kStart.value); private final JoystickButton station_Back = new JoystickButton(opController, XboxController.Button.kBack.value); @@ -106,102 +117,117 @@ public class RobotContainer { public final SendableChooser autoChooser; public RobotContainer() { + switch (Constants.currentMode) { + + // Real robot, instantiate hardware IO implementations + + case REAL: + drive = new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontLeft), + new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.FrontRight), + new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.BackLeft), + new ModuleIOTalonFX(Constants.TUNER_CONSTANTS.BackRight), + (robotPose) -> {}); + vision = new Vision(drive::accept, new VisionIOLimelight(camera0Name, drive::getRotation)); + // new VisionIOLimelight(camera1Name, drive::getRotation)); + elevator = new Elevator(new ElevatorIOReal()); + arm = new Arm(new ArmIOReal()); + endEffector = new EndEffector(new EndEffectorIOReal()); + climber = new Climber(new ClimberIOReal()); + break; + + // Sim robot, instantiate physics sim IO implementations + + case SIM: + driveSimulation = new SwerveDriveSimulation(Drive.mapleSimConfig, new Pose2d(12, 2, new Rotation2d())); + SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation); + + drive = new Drive( + new GyroIOSim(driveSimulation.getGyroSimulation()), + new ModuleIOSim(driveSimulation.getModules()[0]), + new ModuleIOSim(driveSimulation.getModules()[1]), + new ModuleIOSim(driveSimulation.getModules()[2]), + new ModuleIOSim(driveSimulation.getModules()[3]), + driveSimulation::setSimulationWorldPose); + + elevator = new Elevator(new ElevatorIOSim()); + arm = new Arm(new ArmIOSim()); + endEffector = new EndEffector(new EndEffectorIOSim( + () -> driveSimulation.getSimulatedDriveTrainPose(), // drivetrain.getState().Pose, + () -> driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(), + () -> elevator.getElevatorPositionMeters(), + () -> arm.getArmAngleRadsToHorizontal())); + vision = new Vision(drive::accept); // , new + // VisionIOQuestNavSim(driveSimulation::getSimulatedDriveTrainPose)); + climber = new Climber(new ClimberIOSim()); + break; + + // Replayed robot, disable IO implementations + + default: + elevator = new Elevator(new ElevatorIO() {}); + arm = new Arm(new ArmIO() {}); + endEffector = new EndEffector(new EndEffectorIO() {}); + climber = new Climber(new ClimberIO() {}); + break; + } + + align = new AutoAlign(drive::getPose); + if (Constants.currentMode == Constants.Mode.SIM) { + align.setRobotSupplier(driveSimulation::getSimulatedDriveTrainPose); + } + setDefaultCommands(); registerNamedCommands(); - autoChooser = AutoBuilder.buildAutoChooser("2R_EF-L4"); + autoChooser = AutoBuilder.buildAutoChooser("Right"); + // autoChooser.addOption("Drive FF Ch", DriveCommands.feedforwardCharacterization(drive)); + // autoChooser.addOption("red barge 2 algae", new PathPlannerAuto("untested Center 2 Algae", true)); SmartDashboard.putData("Auto Mode", autoChooser); configureBindings(); - PathfindingCommand.warmupCommand().schedule(); + SmarterDashboard.putString("RoboRio Serial", RobotIdentity.getRoboRioSerial()); + SmarterDashboard.putString("Robot Identity", RobotIdentity.getRobotIdentityString()); + + // ------------------------------ Event Markers ---------------------------------- // - // Event Markers new EventTrigger("LevelStation") .onTrue(new InstantCommand(() -> elevator.setElevatorStationMode()) .andThen(new InstantCommand(() -> arm.setArmIntake()))); new EventTrigger("LevelFour") .onTrue(new InstantCommand(() -> elevator.setElevatorLevelFourMode()) .andThen(new InstantCommand(() -> arm.setArmL4()))); + + DriverStation.silenceJoystickConnectionWarning(true); } private void configureBindings() { - // Driver Bindings - resetHeading_Start.onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - - // climbAlign_Y.whileTrue(drivetrain.applyRequest( - // () -> drive.withVelocityX(drivetrain.frontLimiter.calculate(-driverController.getLeftY()) - // * MaxSpeed - // * drivetrain.getSpeedMultipler()) // Drive forward with negative Y (forward) - // .withVelocityY(drivetrain.sideLimiter.calculate(-driverController.getLeftX()) - // * MaxSpeed - // * drivetrain.getSpeedMultipler()) // Drive left with negative - // // X (left) - // .withRotationalRate(align.getAlignRotationSpeedPercent( - // (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) - // ? Rotation2d.kCW_90deg - // : Rotation2d.kCCW_90deg) - // * MaxAngularRate) // Drive - // // counterclockwise - // // with negative X - // // (left) - // )); - - stationAlign_PovUp.whileTrue(drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(-align.getAlignForwardSpeedPercent( - AlignConstants.STATION_ALIGN_TZ, align.getStationAlignTag(), VisionConstants.LL_B_NAME) - * MaxSpeed) - .withVelocityY(-align.getAlignStrafeSpeedPercent( - AlignConstants.STATION_ALIGN_TX, align.getStationAlignTag(), VisionConstants.LL_B_NAME) - * MaxSpeed) - .withRotationalRate( - align.getAlignRotationSpeedPercent(align.getAlignAngleStation()) * MaxAngularRate))); - // .alongWith(ledstrip.aligning(() -> align.isAligned()))); - - reefAlignCenter_PovDown.whileTrue(drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(align.getAlignForwardSpeedPercent( - AlignConstants.REEF_ALIGN_TZ, align.getReefAlignTag(), VisionConstants.LL_NAME) - * MaxSpeed) - .withVelocityY(align.getAlignStrafeSpeedPercent( - AlignConstants.REEF_ALIGN_MID_TX, align.getReefAlignTag(), VisionConstants.LL_NAME) - * MaxSpeed) - .withRotationalRate(align.getAlignRotationSpeedPercent(align.getAlignAngleReef()) * MaxAngularRate))); - // .alongWith(ledstrip.aligning(() -> align.isAligned()))); - - reefAlignLeft_PovLeft.whileTrue(drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(align.getAlignForwardSpeedPercent( - AlignConstants.REEF_ALIGN_TZ, align.getReefAlignTag(), VisionConstants.LL_NAME) - * MaxSpeed) - .withVelocityY(align.getAlignStrafeSpeedPercent( - AlignConstants.REEF_ALIGN_LEFT_TX, align.getReefAlignTag(), VisionConstants.LL_NAME) - * MaxSpeed) - .withRotationalRate(align.getAlignRotationSpeedPercent(align.getAlignAngleReef()) * MaxAngularRate))); - // .alongWith(ledstrip.aligning(() -> align.isAligned()))); - - reefAlignRight_PovRight.whileTrue( - drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(align.getAlignForwardSpeedPercent( - AlignConstants.REEF_ALIGN_TZ, align.getReefAlignTag(), VisionConstants.LL_NAME) - * MaxSpeed) - .withVelocityY(align.getAlignStrafeSpeedPercent( - AlignConstants.REEF_ALIGN_RIGHT_TX, - align.getReefAlignTag(), - VisionConstants.LL_NAME) - * MaxSpeed) - .withRotationalRate( - align.getAlignRotationSpeedPercent(align.getAlignAngleReef()) * MaxAngularRate)) - // .alongWith(ledstrip.aligning(() -> align.isAligned())) - ); - - slowMode_A.onTrue(new InstantCommand(() -> drivetrain.changeSpeedMultiplier())); + + // ------------------------------- Driver Bindings ------------------------------- // + + // Reset gyro / odometry + final Runnable resetOdometry = Constants.currentMode == Constants.Mode.SIM + ? () -> drive.resetOdometry(driveSimulation.getSimulatedDriveTrainPose()) + : () -> drive.resetOdometry(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); + resetHeading_Start.onTrue(Commands.runOnce(resetOdometry).ignoringDisable(true)); + + reefAlignLeft_PovLeft.whileTrue(align.reefAlignLeft(drive)); + reefAlignRight_PovRight.whileTrue(align.reefAlignRight(drive)); + reefAlignCenter_PovDown.whileTrue(align.reefAlignMid(drive)); + stationAlign_PovUp.whileTrue(align.stationAlign(drive)); + + strafe_Triggers.whileTrue(DriveCommands.joystickDrive( + drive, + () -> 0, + () -> (driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()) * 0.25, + () -> 0, + false)); + + slowMode_A.onTrue(new InstantCommand(() -> drive.changeSpeedMultiplier())); zeroClimber_back.onTrue(new InstantCommand(() -> climber.zeroClimber())); - strafe_Triggers.whileTrue(drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(-driverController.getLeftY() * MaxSpeed) - .withVelocityY((driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()) - * 0.1 - * MaxSpeed) - .withRotationalRate(-driverController.getRightX() * MaxAngularRate))); + // ------------------------------- Operator Bindings ------------------------------- // - // Operator Bindings homeElevator_Start .whileTrue(new InstantCommand(() -> elevator.setZeroing(true))) .onFalse(new InstantCommand(() -> elevator.stopElevator()) @@ -231,11 +257,11 @@ private void configureBindings() { coralMode_LB.onTrue(new InstantCommand(() -> elevator.setCoral()) .andThen(new InstantCommand(() -> arm.setCoral())) - .andThen(new InstantCommand(() -> endEffector.setCoral())) - .andThen(new InstantCommand(() -> endEffector.stop()))); + .andThen(new InstantCommand(() -> endEffector.setCoralMode())) + .andThen(new InstantCommand(() -> endEffector.stopEE()))); algaeMode_RB.onTrue(new InstantCommand(() -> elevator.setAlgae()) .andThen(new InstantCommand(() -> arm.setAlgae())) - .andThen(new InstantCommand(() -> endEffector.setAlgae()))); + .andThen(new InstantCommand(() -> endEffector.setAlgaeMode()))); climberAdjustUp_PovUp .whileTrue(new RunCommand(() -> climber.climberUp())) @@ -244,7 +270,13 @@ private void configureBindings() { .whileTrue(new RunCommand(() -> climber.climberDown())) .onFalse(new InstantCommand(() -> climber.stop())); climberStateInc_PovLeft.onTrue(new InstantCommand(() -> climber.decrementClimbState())); - climberStateDec_PovRight.onTrue(new InstantCommand(() -> climber.incrementClimbState())); + climberStateDec_PovRight.onTrue(new InstantCommand(() -> { + elevator.setElevatorStowedMode(); + arm.setAlgae(); + arm.setStowed(); + }) + .andThen(new WaitCommand(0.2)) + .andThen(new InstantCommand(() -> climber.incrementClimbState()))); elevatorAdjust.whileTrue( new RunCommand(() -> elevator.changeElevatorOffset(.01 * Math.signum(-opController.getLeftY())))); @@ -255,7 +287,10 @@ private void configureBindings() { .onFalse(new InstantCommand(() -> arm.setAligning(false)) .andThen(new InstantCommand(() -> elevator.setAligning(false)))); - drivetrain.registerTelemetry(logger::telemeterize); + // if (Constants.currentMode == Constants.Mode.SIM) { + // csDropLB.onTrue(new InstantCommand(() -> dropCoralFromStation(false)).ignoringDisable(true)); + // csDropRB.onTrue(new InstantCommand(() -> dropCoralFromStation(true)).ignoringDisable(true)); + // } } public Command getAutonomousCommand() { @@ -270,6 +305,8 @@ public static boolean isRedAlliance() { return false; } + // -------------------------------- PathPlanner Commands -------------------------------- /w + public void registerNamedCommands() { NamedCommands.registerCommand( "LevelStation", @@ -291,9 +328,9 @@ public void registerNamedCommands() { new WaitCommand(0.2), Commands.none(), () -> arm.getArmMode() == ArmMode.Stowed)) .andThen(new InstantCommand(() -> arm.setArmL4()))); - NamedCommands.registerCommand("Outtake", new InstantCommand(() -> endEffector.coralOut())); + NamedCommands.registerCommand("Outtake", new InstantCommand(() -> endEffector.outtakeCoral())); NamedCommands.registerCommand( - "Intake", new RunCommand(() -> endEffector.coralIn()).until(() -> endEffector.hasCoral())); + "Intake", new RunCommand(() -> endEffector.intakeCoral()).until(() -> endEffector.hasCoral())); NamedCommands.registerCommand("Stop EE", new InstantCommand(() -> endEffector.stopCoral())); NamedCommands.registerCommand("Hold Coral", new InstantCommand(() -> endEffector.holdCoral())); NamedCommands.registerCommand( @@ -305,127 +342,73 @@ public void registerNamedCommands() { .andThen(new InstantCommand(() -> elevator.zeroElevator())) .andThen(new InstantCommand(() -> elevator.setZeroing(false)))); - NamedCommands.registerCommand( - "Stop", - drivetrain - .applyRequest(() -> robotOrientedDrive - .withVelocityX(0) - .withVelocityY(0) - .withRotationalRate(0)) - .withTimeout(0.05)); + NamedCommands.registerCommand("Stop", new InstantCommand(() -> drive.stopWithX())); NamedCommands.registerCommand( "Auto Align Left", - new InstantCommand(() -> align.setReefAlignTagIDtoClosest()) - .andThen((drivetrain.applyRequest( - () -> robotOrientedDrive - .withVelocityX(align.getAlignForwardSpeedPercent( - AlignConstants.REEF_ALIGN_TZ, - align.getReefAlignTag(), - VisionConstants.LL_NAME) - * MaxSpeed) - .withVelocityY(align.getAlignStrafeSpeedPercent( - AlignConstants.REEF_ALIGN_LEFT_TX, - align.getReefAlignTag(), - VisionConstants.LL_NAME) - * MaxSpeed) - .withRotationalRate( - align.getAlignRotationSpeedPercent(align.getAlignAngleReef()) - * MaxAngularRate) // Drive counterclockwise with - // negative X (left) - ) - // .alongWith(ledstrip.aligning(() -> align.isAligned())) - ) - .until(() -> align.isAlignedTest()) - .withTimeout(3)) - .andThen(new WaitCommand(0.2))); + align.reefAlignLeft(drive).until(align::isAlignedDebounced).withTimeout(2)); NamedCommands.registerCommand( - "Auto Align Mid", - new InstantCommand(() -> align.setReefAlignTagIDtoClosest()) - .andThen((drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(align.getAlignForwardSpeedPercent( - AlignConstants.REEF_ALIGN_TZ, - align.getReefAlignTag(), - VisionConstants.LL_NAME) - * MaxSpeed) - .withVelocityY(align.getAlignStrafeSpeedPercent( - AlignConstants.REEF_ALIGN_MID_TX, - align.getReefAlignTag(), - VisionConstants.LL_NAME) - * MaxSpeed) - .withRotationalRate( - align.getAlignRotationSpeedPercent(align.getAlignAngleReef()) - * MaxAngularRate)) - // .alongWith(ledstrip.aligning(() -> align.isAligned())) - ) - .until(() -> align.isAlignedTest())) - .andThen(new WaitCommand(0.2))); + "Auto Align Mid", align.reefAlignMid(drive).withTimeout(2)); NamedCommands.registerCommand( "Auto Align Right", - new InstantCommand(() -> align.setReefAlignTagIDtoClosest()) - .andThen((drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(align.getAlignForwardSpeedPercent( - AlignConstants.REEF_ALIGN_TZ, - align.getReefAlignTag(), - VisionConstants.LL_NAME) - * MaxSpeed) - .withVelocityY(align.getAlignStrafeSpeedPercent( - AlignConstants.REEF_ALIGN_RIGHT_TX, - align.getReefAlignTag(), - VisionConstants.LL_NAME) - * MaxSpeed) - .withRotationalRate( - align.getAlignRotationSpeedPercent(align.getAlignAngleReef()) - * MaxAngularRate)) - // .alongWith(ledstrip.aligning(() -> align.isAligned())) - ) - .until(() -> align.isAlignedTest()) - .withTimeout(3) - .andThen(new WaitCommand(0.2)))); + align.reefAlignRight(drive).until(align::isAlignedDebounced).withTimeout(2)); NamedCommands.registerCommand( "Auto Align Station", - (drivetrain.applyRequest(() -> robotOrientedDrive - .withVelocityX(-align.getAlignForwardSpeedPercent( - AlignConstants.STATION_ALIGN_TZ, - align.getStationAlignTag(), - VisionConstants.LL_B_NAME) - * MaxSpeed) - .withVelocityY(-align.getAlignStrafeSpeedPercent( - AlignConstants.STATION_ALIGN_TX, - align.getStationAlignTag(), - VisionConstants.LL_B_NAME) - * MaxSpeed) - .withRotationalRate(align.getAlignRotationSpeedPercent(align.getAlignAngleStation()) - * MaxAngularRate))) - // .alongWith(ledstrip.aligning(() -> align.isAligned())) - .withTimeout(0.5)); + align.stationAlign(drive).until(align::isAlignedDebounced).withTimeout(2)); NamedCommands.registerCommand( "Set Algae", new InstantCommand(() -> elevator.setAlgae()) .andThen(new InstantCommand(() -> arm.setAlgae())) - .andThen(new InstantCommand(() -> endEffector.setAlgae()))); + .andThen(new InstantCommand(() -> endEffector.setAlgaeMode()))); + NamedCommands.registerCommand( + "Set Coral", + new InstantCommand(() -> elevator.setAlgae()) + .andThen(new InstantCommand(() -> arm.setCoral())) + .andThen(new InstantCommand(() -> endEffector.setCoralMode()))); NamedCommands.registerCommand( - "Algae Intake", new RunCommand(() -> endEffector.algaeIn()).until(() -> endEffector.hasCoral())); + "Algae Intake", new RunCommand(() -> endEffector.intakeAlgae()).until(() -> endEffector.hasCoral())); + NamedCommands.registerCommand("Algae Outtake", new InstantCommand(() -> endEffector.outtakeAlgae())); } + // ----------------------------------- Default Commands -------------------------------- // + public void setDefaultCommands() { - drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> fieldOrientedDrive - .withVelocityX(drivetrain.frontLimiter.calculate(-driverController.getLeftY()) - * MaxSpeed - * drivetrain.getSpeedMultipler()) - .withVelocityY(drivetrain.sideLimiter.calculate(-driverController.getLeftX()) - * MaxSpeed - * drivetrain.getSpeedMultipler()) - .withRotationalRate(drivetrain.turnLimiter.calculate(-driverController.getRightX()) * MaxAngularRate))); - - elevator.setDefaultCommand(new ElevatorHold()); - arm.setDefaultCommand(new ArmHold()); + drive.setDefaultCommand(DriveCommands.joystickDrive( + drive, + () -> -driverController.getLeftY() * drive.getDriveMultiplier(), + () -> -driverController.getLeftX() * drive.getDriveMultiplier(), + () -> -driverController.getRightX() * drive.getTurnMultiplier(), + true)); + + elevator.setDefaultCommand(new ElevatorHold(elevator)); + arm.setDefaultCommand(new ArmHold(arm)); // climber.setDefaultCommand(new ClimbCommand()); // ledstrip.setDefaultCommand(ledstrip.defaultCommand(() -> endEffector.isCoral())); } + + public void resetSimulation() { + if (Constants.currentMode != Constants.Mode.SIM) return; + + drive.resetOdometry(new Pose2d(12, 2, new Rotation2d())); + SimulatedArena.getInstance().resetFieldForAuto(); + AlgaeHandler.getInstance().reset(); + } + + public void displaySimFieldToAdvantageScope() { + if (Constants.currentMode != Constants.Mode.SIM) return; + + SimulatedArena.getInstance().simulationPeriodic(); + Logger.recordOutput("FieldSimulation/Pose", new Pose3d(driveSimulation.getSimulatedDriveTrainPose())); + Logger.recordOutput( + "FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral")); + Logger.recordOutput( + "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae")); + Logger.recordOutput( + "FieldSimulation/Staged Algae", AlgaeHandler.getInstance().periodic()); + } } diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java deleted file mode 100644 index 0f19dfd..0000000 --- a/src/main/java/frc/robot/Telemetry.java +++ /dev/null @@ -1,144 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import org.littletonrobotics.junction.Logger; - -public class Telemetry { - private final double MaxSpeed; - - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.setPath("/media/sda1/ctre/ctre-logs/"); - // SignalLogger.start(); //might be needed for SysID - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = - driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = - driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable - .getStructArrayTopic("ModuleStates", SwerveModuleState.struct) - .publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable - .getStructArrayTopic("ModuleTargets", SwerveModuleState.struct) - .publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable - .getStructArrayTopic("ModulePositions", SwerveModulePosition.struct) - .publish(); - private final DoublePublisher driveTimestamp = - driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = - driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = - table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; - - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; - - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - Logger.recordOutput("Telemetry/Pose", state.Pose); - Logger.recordOutput("Telemetry/Speeds", state.Speeds); - Logger.recordOutput("Telemetry/Module States", state.ModuleStates); - Logger.recordOutput("Telemetry/Module Targets", state.ModuleTargets); - Logger.recordOutput("Telemetry/Module Positions", state.ModulePositions); - Logger.recordOutput("Telemetry/Timestamp", state.Timestamp); - - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } - - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); - - /* Telemeterize the module states to a Mechanism2d */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - } -} diff --git a/src/main/java/frc/robot/commands/ArmHold.java b/src/main/java/frc/robot/commands/ArmHold.java index 05ef2d9..e5e93ea 100644 --- a/src/main/java/frc/robot/commands/ArmHold.java +++ b/src/main/java/frc/robot/commands/ArmHold.java @@ -1,13 +1,13 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Arm; +import frc.robot.subsystems.arm.Arm; public class ArmHold extends Command { + private Arm arm; - private Arm arm = Arm.getInstance(); - - public ArmHold() { + public ArmHold(Arm arm) { + this.arm = arm; addRequirements(arm); } diff --git a/src/main/java/frc/robot/commands/AutoAlign.java b/src/main/java/frc/robot/commands/AutoAlign.java index 57650b5..8f3666e 100644 --- a/src/main/java/frc/robot/commands/AutoAlign.java +++ b/src/main/java/frc/robot/commands/AutoAlign.java @@ -1,371 +1,82 @@ package frc.robot.commands; -import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.Constants.AlignConstants; import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.VisionConstants; -import frc.robot.RobotContainer; -import frc.robot.util.vision.LimelightHelpers; -import java.util.HashMap; -import java.util.Map; +import frc.robot.subsystems.drive.Drive; +import frc.robot.util.RobotIdentity; import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; +import lombok.Setter; +import org.littletonrobotics.junction.AutoLogOutput; -/** Add your docs here. */ public class AutoAlign { + private Pose2d targetPose = Pose2d.kZero; - private Supplier poseSupplier; + private Debouncer isAlignedDebouncer = new Debouncer(0.2); - private PIDController reefStrafeSpeedController = - new PIDController(AlignConstants.REEF_kP, AlignConstants.REEF_kI, AlignConstants.REEF_kD); - private PIDController reefForwardSpeedController = - new PIDController(AlignConstants.REEF_Forward_kP, AlignConstants.REEF_kI, AlignConstants.REEF_kD); - private PIDController reefRotationSpeedController = - new PIDController(AlignConstants.ROT_REEF_kP, AlignConstants.ROT_REEF_kI, AlignConstants.ROT_REEF_kD); + @Setter + private Supplier robotSupplier; - private double alignSpeedStrafe = 0; - private double alignSpeedRotation = 0; - private double alignSpeedForward = 0; - private double horizDisplacement = 0; - private int currentReefAlignTagID = 18; // -1 - private int currentCSAlignTagID = 12; // -1 - private Map tagPoses3d = getTagPoses(); + public AutoAlign(Supplier robotPoseSupplier) { + this.robotSupplier = robotPoseSupplier; - public AutoAlign(Supplier poseSupplier) { - this.poseSupplier = poseSupplier; - reefRotationSpeedController.enableContinuousInput(-180, 180); - } - - public Map getTagPoses() { - Map tagPoses = new HashMap(); - for (int tag : FieldConstants.RED_REEF_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); - } - for (int tag : FieldConstants.BLUE_REEF_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); - } - for (int tag : FieldConstants.RED_CORAL_STATION_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); - } - for (int tag : FieldConstants.BLUE_CORAL_STATION_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); + if (Constants.IDENTITY == RobotIdentity.EVE) { + isAlignedDebouncer.setDebounceTime(1); } - return tagPoses; } - private Rotation2d getTagAngle(int tagID) { - - if (tagPoses3d.containsKey(tagID)) { - Pose3d tagPose = tagPoses3d.get(tagID); - return new Rotation2d(tagPose.getRotation().getZ()); - } else return new Rotation2d(0); + public Command reefAlignLeft(Drive drive) { + return reefAlign(drive, AlignConstants.LEFT_BRANCH_OFFSET); } - private Pose2d getTagPose(int tagID) { - - if (tagPoses3d.containsKey(tagID)) { - Pose3d tagPose = tagPoses3d.get(tagID); - return tagPose.toPose2d(); - } else return new Pose2d(); + public Command reefAlignMid(Drive drive) { + return reefAlign(drive, AlignConstants.MID_OFFSET); } - public Rotation2d getAlignAngleReef() { - if (!DriverStation.isAutonomous()) { - setReefAlignTagIDtoClosest(); - } - - return getTagAngle(currentReefAlignTagID); + public Command reefAlignRight(Drive drive) { + return reefAlign(drive, AlignConstants.RIGHT_BRANCH_OFFSET); } - public Rotation2d getAlignAngleAlgaeReef() { - currentReefAlignTagID = getClosestAprilTag( - RobotContainer.isRedAlliance() ? FieldConstants.RED_REEF_TAG_IDS : FieldConstants.BLUE_REEF_TAG_IDS, - poseSupplier.get()); - - return new Rotation2d(getTagAngle(currentReefAlignTagID).getRadians() + Units.degreesToRadians(180)); + private Command reefAlign(Drive drive, Translation2d offset) { + return new DriveToPose(drive, () -> findReefTargetPose(robotSupplier.get(), offset), robotSupplier); } - public Rotation2d getAlignAngleStation() { - currentCSAlignTagID = getClosestAprilTag( - RobotContainer.isRedAlliance() - ? FieldConstants.RED_CORAL_STATION_TAG_IDS - : FieldConstants.BLUE_CORAL_STATION_TAG_IDS, - poseSupplier.get()); + private Pose2d findReefTargetPose(Pose2d currentPose, Translation2d offset) { + Pose2d tagPose = currentPose.nearest(FieldConstants.REEF_TAGS); - return new Rotation2d(getTagAngle(currentCSAlignTagID).getRadians() + Units.degreesToRadians(180)); - } - - private int getClosestAprilTag(int[] tagIDs, Pose2d robotPose) { - double minDistance = Double.POSITIVE_INFINITY; - int closestTagID = -1; - - // iterates through all tag IDs - for (int i : tagIDs) { - if (tagPoses3d.containsKey(i)) { - Pose3d tagPose = tagPoses3d.get(i); + boolean isForwards = false; - // distance between robot pose and april tag - double distance = tagPose.getTranslation() - .toTranslation2d() - .minus(robotPose.getTranslation()) - .getNorm(); - - if (distance < minDistance) { - closestTagID = i; - minDistance = distance; - } - } - } - - return closestTagID; + return targetPose = + tagPose.transformBy(new Transform2d(offset, isForwards ? Rotation2d.k180deg : Rotation2d.kZero)); } - public double getTagDist() { - Transform2d offset = poseSupplier.get().minus(getTagPose(currentReefAlignTagID)); - - return Math.sqrt(Math.pow(offset.getX(), 2) + Math.pow(offset.getY(), 2)); + public Command stationAlign(Drive drive) { + return new DriveToPose(drive, this::findStationTargetPose, robotSupplier); } - public double getCageDist(Translation2d CageTranslation2d) { - Translation2d offset = poseSupplier.get().getTranslation().minus(CageTranslation2d); - - return Math.sqrt(Math.pow(offset.getX(), 2) + Math.pow(offset.getY(), 2)); - } - - public double getAlignStrafeSpeedPercent(double setPoint, int tagID, String llName) { - // 3D transform of the robot in the coordinate system of the primary in-view AprilTag - // (array (6)) [tx, ty, tz, pitch, yaw, roll] (meters, degrees) - double[] targetRelativeRobotPose = LimelightHelpers.getBotPose_TargetSpace(llName); - double tx = targetRelativeRobotPose[0]; - double txError = tx - setPoint; - - Transform2d offset = poseSupplier.get().minus(getTagPose(tagID)); - - if (!llIsValid(llName, tagID)) { - alignSpeedStrafe = reefStrafeSpeedController.calculate(offset.getY(), setPoint); - alignSpeedStrafe += AlignConstants.ALIGN_KS * Math.signum(alignSpeedStrafe); - } else { - alignSpeedStrafe = reefStrafeSpeedController.calculate(tx, setPoint); - alignSpeedStrafe += AlignConstants.ALIGN_KS * Math.signum(alignSpeedStrafe); - } - - // Logger.recordOutput("Align/Strafe Speed", alignSpeedStrafe); - // Logger.recordOutput("Align/Strafe Setpoint", setPoint); - // Logger.recordOutput("Align/Strafe Error", setPoint - offset.getY()); - // Logger.recordOutput("Align/tx", tx); - // Logger.recordOutput("Align/tx Error", txError); - - return alignSpeedStrafe; - } - - public double getAlignStrafeSpeedPercent(Translation2d targetTranslation2d) { - - Translation2d robotTranslation2d = poseSupplier.get().getTranslation(); - - alignSpeedStrafe = reefStrafeSpeedController.calculate(robotTranslation2d.getY(), targetTranslation2d.getY()); - alignSpeedStrafe += AlignConstants.ALIGN_KS * Math.signum(alignSpeedStrafe); - - // Logger.recordOutput("Align/Strafe Speed", alignSpeedStrafe); - // Logger.recordOutput("Align/Strafe/CageTranslation2d", targetTranslation2d); - // Logger.recordOutput("Align/Strafe/CageTargetY", targetTranslation2d.getY()); - // Logger.recordOutput("Align/Strafe Setpoint", setPoint); - // Logger.recordOutput("Align/Strafe Error", setPoint - offset.getY()); - - return alignSpeedStrafe; - } - - public double getAlignRotationSpeedPercent(Rotation2d targetAngle2d) { - double robotAngle = poseSupplier.get().getRotation().getDegrees(); - double targetAngle = targetAngle2d.getDegrees(); - double rotationError = robotAngle - targetAngle; - - alignSpeedRotation = reefRotationSpeedController.calculate(robotAngle, targetAngle); - - // Logger.recordOutput("Align/Rotation Speed", alignSpeedRotation); - // Logger.recordOutput("Align/Robot Angle", robotAngle); - // Logger.recordOutput("Align/Rotation Error", rotationError); - - return alignSpeedRotation; - } - - public double getAlignForwardSpeedPercent(double setPoint, int tagID, String llName) { - - double[] targetRelativeRobotPose = LimelightHelpers.getBotPose_TargetSpace(llName); - double tz = targetRelativeRobotPose[2]; - double tzError = tz - setPoint; - - Transform2d offset = poseSupplier.get().minus(getTagPose(tagID)); - this.horizDisplacement = offset.getX(); - - if (offset.getX() < 0.55 && Math.abs(reefStrafeSpeedController.getError()) > 0.05) { - return 0; - } - - if (!llIsValid(llName, tagID)) { - alignSpeedForward = reefForwardSpeedController.calculate(offset.getX(), setPoint); - } else { - if (tagID == currentCSAlignTagID) { - setPoint = 0; - } - alignSpeedForward = -reefForwardSpeedController.calculate(tz, setPoint); - } - - isAligned(); - isAlignedTest(); - - Logger.recordOutput("Align/Forward Speed", alignSpeedForward); - Logger.recordOutput("Align/ty", tz); - Logger.recordOutput("Align/ty Error", tzError); - Logger.recordOutput("Align/x", offset.getX()); - Logger.recordOutput("Align/y", offset.getY()); - // Logger.recordOutput("Align/Offset", offset); - Logger.recordOutput("Align/Fwd Error", offset.getX() - setPoint); - // Logger.recordOutput("Align/Fwd Setpoint", setPoint); - // Logger.recordOutput("Align/TagID", tagID); - // Logger.recordOutput("LL Valid", llIsValid(llName, tagID)); - // Logger.recordOutput("Align/Tag Pose", getTagPose(tagID)); - - return alignSpeedForward; - } - - public double getAlignForwardSpeedPercent(Translation2d targetTranslation2d) { - - Translation2d robotTranslation2d = poseSupplier.get().getTranslation(); - - alignSpeedForward = - reefStrafeSpeedController.calculate(robotTranslation2d.getX(), targetTranslation2d.getX()) / 2; - alignSpeedForward += AlignConstants.ALIGN_KS * Math.signum(alignSpeedStrafe); - - // Logger.recordOutput("Align/Forward Speed", alignSpeedForward); - // Logger.recordOutput("Align/Forward/CageTranslation2d", targetTranslation2d); - // Logger.recordOutput("Align/Strafe/CageTargetX", targetTranslation2d.getX()); - return alignSpeedForward; - } - - public int getReefAlignTag() { - return currentReefAlignTagID; - } - - public int getStationAlignTag() { - return currentCSAlignTagID; - } - - private boolean llIsValid(String llName, int tagID) { - LimelightHelpers.setPriorityTagID(llName, tagID); - boolean valid = LimelightHelpers.getTargetCount(llName) >= 1 && LimelightHelpers.getFiducialID(llName) == tagID; - Logger.recordOutput("Align/Valid", valid); - return valid; - } - - public ChassisSpeeds getFieldRelativeChassisSpeeds( - double tx, double ySpeed, Rotation2d gyroAngle, double maxSpeed, double maxAngularSpeed) { - Logger.recordOutput("Align/Timestamp", System.currentTimeMillis()); - return ChassisSpeeds.fromRobotRelativeSpeeds( - getAlignStrafeSpeedPercent(tx, currentReefAlignTagID, VisionConstants.LL_NAME) - * maxSpeed, // getAlignStrafeSpeedPercent(tx) * maxSpeed - ySpeed, - getAlignRotationSpeedPercent(getAlignAngleReef()) * maxAngularSpeed, - gyroAngle); - } - - // public Command getCSPathCommand(BooleanSupplier isProcessorSide) { - // try { - // if (isProcessorSide.getAsBoolean()) { // 2 & 12, processor side - // PathPlannerPath alignCSP = PathPlannerPath.fromPathFile("Align_CS_P"); - // // Since AutoBuilder is configured, we can use it to build pathfinding commands - // return AutoBuilder.pathfindThenFollowPath(alignCSP, AlignConstants.PATH_CONSTRAINTS); - // } else { - // PathPlannerPath alignCSNP = PathPlannerPath.fromPathFile("Align_CS_NP"); - // // Since AutoBuilder is configured, we can use it to build pathfinding commands - // return AutoBuilder.pathfindThenFollowPath(alignCSNP, AlignConstants.PATH_CONSTRAINTS); - // } - // } catch (Exception e) { - // e.printStackTrace(); - // return Commands.print("align csp not found"); - // } - // } - - public void setReefAlignTagIDtoClosest() { - currentReefAlignTagID = getClosestAprilTag( - RobotContainer.isRedAlliance() ? FieldConstants.RED_REEF_TAG_IDS : FieldConstants.BLUE_REEF_TAG_IDS, - poseSupplier.get()); + private Pose2d findStationTargetPose() { + return targetPose = robotSupplier + .get() + .nearest(FieldConstants.CORAL_STATIONS) + .transformBy(new Transform2d(AlignConstants.STATION_OFFSET, Rotation2d.k180deg)); } + @AutoLogOutput public boolean isAligned() { - boolean isAligned = Math.abs(reefForwardSpeedController.getError()) < 0.6 - && reefRotationSpeedController.getError() < AlignConstants.ALIGN_ROT_TOLERANCE_DEGREES; - - // + Math.abs(reefStrafeSpeedController.getError()) + Transform2d error = targetPose.minus(robotSupplier.get()); - Logger.recordOutput("Align/Error/IsAligned", isAligned); - Logger.recordOutput("Align/Error/fwd", reefForwardSpeedController.getError()); - Logger.recordOutput("Align/Error/strafe", reefStrafeSpeedController.getError()); - Logger.recordOutput("Align/Error/rot", reefRotationSpeedController.getError()); - - return isAligned; + return error.getTranslation().getNorm() < AlignConstants.ALIGN_TRANSLATION_TOLERANCE + && error.getRotation().getRadians() < AlignConstants.ALIGN_ROT_TOLERANCE; } - public boolean isAlignedTest() { - boolean isAligned = Math.abs(reefForwardSpeedController.getError()) < 0.49 // 0.49 - && reefRotationSpeedController.getError() < AlignConstants.ALIGN_ROT_TOLERANCE_DEGREES; - - Logger.recordOutput("Align/Error/isAlignedTest", isAligned); - - return isAligned; + @AutoLogOutput + public boolean isAlignedDebounced() { + return isAlignedDebouncer.calculate(isAligned()); } - - // private double getArmAngleRads() { - // Logger.recordOutput("Align/Math/tz", horizDisplacement); - // double deltaX = Math.abs(horizDisplacement) + AlignConstants.BRANCH_OFFSET_BEHIND_APRILTAG; - // Logger.recordOutput("Align/Math/deltaX", deltaX); - // return Math.acos(deltaX / AlignConstants.PIVOT_TO_CORAL_RADIUS); - // } - - // public double getArmAngleRots() { - // double armAngleRads = getArmAngleRads(); - // double armAngleRots; - // if (Double.isNaN(armAngleRads)) { - // armAngleRots = ArmConstants.ARM_L4_BEHIND_CORAL; - // } else { - // armAngleRots = Units.radiansToRotations(armAngleRads - // + AlignConstants.ARM_TO_CORAL_ANGULAR_OFFSET - // - AlignConstants.ARM_STARTING_ANGLE) - // * ArmConstants.ARM_GEAR_RATIO; - // } - // Logger.recordOutput("Align/Math/Arm Rads from x-axis", armAngleRads); - // Logger.recordOutput("Align/Math/Arm Degs from x-axis", Units.radiansToDegrees(armAngleRads)); - // Logger.recordOutput("Align/Math/Arm Rots", armAngleRots); - // return armAngleRots; - // } - - // private double getElevatorHeightMeters() { - // double armAngleRads = getArmAngleRads(); - // if (Double.isNaN(armAngleRads)) { - // return Double.NaN; - // } else { - // return AlignConstants.L4_HEIGHT - AlignConstants.PIVOT_TO_CORAL_RADIUS * Math.sin(armAngleRads); - // } - // } - - // public double getElevatorHeightRots() { - // double heightMeters = getElevatorHeightMeters(); - // if (Double.isNaN(heightMeters)) { - // return ElevatorConstants.BARGE_ROT; - // } - - // double height = Units.metersToInches(heightMeters - AlignConstants.ELEVATOR_STARTING_HEIGHT); - // double rots = height * ElevatorConstants.GEAR_RATIO / (Math.PI * ElevatorConstants.PULLEY_DIAMETER); - - // Logger.recordOutput("Align/Math/Elev Height (m)", height); - // Logger.recordOutput("Align/Math/Elev Rots", rots); - // return rots; - // } } diff --git a/src/main/java/frc/robot/commands/ClimbCommand.java b/src/main/java/frc/robot/commands/ClimbCommand.java index 45a9b6c..a221a80 100644 --- a/src/main/java/frc/robot/commands/ClimbCommand.java +++ b/src/main/java/frc/robot/commands/ClimbCommand.java @@ -5,15 +5,15 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Climber; +import frc.robot.subsystems.climber.Climber; // Command to run the roller with joystick inputs public class ClimbCommand extends Command { // private final CANRollerSubsystem rollerSubsystem; - private Climber climber = Climber.getInstance(); - - public ClimbCommand() { + private Climber climber; + public ClimbCommand(Climber climber) { + this.climber = climber; addRequirements(climber); } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java new file mode 100644 index 0000000..ea716fa --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -0,0 +1,276 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +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.kinematics.ChassisSpeeds; +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 edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.Drive; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveCommands { + private static final double DEADBAND = 0.15; + private static final double ANGLE_KP = 5.0; + private static final double ANGLE_KD = 0.4; + private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_MAX_ACCELERATION = 20.0; + private static final double FF_START_DELAY = 2.0; // Secs + private static final double FF_RAMP_RATE = 0.1; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + + private DriveCommands() {} + + private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), DEADBAND); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + + // Square magnitude for more precise control + // linearMagnitude = linearMagnitude * linearMagnitude; + + // Return new linear velocity + return new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + } + + /** Field relative drive command using two joysticks (controlling linear and angular velocities). */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier, + boolean fieldOriented) { + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Apply rotation deadband + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + + // Square rotation value for more precise control + omega = Math.copySign(omega * omega, omega); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec()); + + if (fieldOriented) { + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); + } else { + drive.runVelocity(speeds); + } + }, + drive); + } + + /** + * Field relative drive command using joystick for linear control and PID for angular control. Possible use cases + * include snapping to an angle, aiming at a vision target, or controlling absolute rotation with a joystick. + */ + public static Command joystickDriveAtAngle( + Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier rotationSupplier) { + + // Create PID controller + ProfiledPIDController angleController = new ProfiledPIDController( + ANGLE_KP, 0.0, ANGLE_KD, new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Calculate angular speed + double omega = angleController.calculate( + drive.getRotation().getRadians(), + rotationSupplier.get().getRadians()); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(Drive drive) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce(() -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + drive.runCharacterization(0.0); + }, + drive) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + drive) + + // When cancelled, calculate and print results + .finallyDo(() -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public static Command wheelRadiusCharacterization(Drive drive) { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce(() -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + drive)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce(() -> { + state.positions = drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run(() -> { + var rotation = drive.getRotation(); + state.gyroDelta += Math.abs( + rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo(() -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = (state.gyroDelta * Drive.DRIVE_BASE_RADIUS) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println("********** Wheel Radius Characterization Results **********"); + System.out.println("\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println("\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = new Rotation2d(); + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java new file mode 100644 index 0000000..70f3f5a --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -0,0 +1,141 @@ +// 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.commands; + +import edu.wpi.first.math.controller.ProfiledPIDController; +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.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.AlignConstants; +import frc.robot.subsystems.drive.Drive; +import frc.robot.util.LoggedTunableNumber; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class DriveToPose extends Command { + private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("Align/kP", AlignConstants.DRIVE_kP); + private static final LoggedTunableNumber drivekI = new LoggedTunableNumber("Align/kI", AlignConstants.DRIVE_kI); + private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("Align/kD", AlignConstants.DRIVE_kD); + private static final LoggedTunableNumber driveMaxVel = + new LoggedTunableNumber("Align/Max Vel", AlignConstants.MAX_DRIVE_VELOCITY); + private static final LoggedTunableNumber driveMaxAcc = + new LoggedTunableNumber("Align/Max Acc", AlignConstants.MAX_DRIVE_ACCELERATION); + + private static final LoggedTunableNumber rotkP = new LoggedTunableNumber("Align/Rot kP", AlignConstants.ROT_kP); + private static final LoggedTunableNumber rotkI = new LoggedTunableNumber("Align/Rot kI", AlignConstants.ROT_kI); + private static final LoggedTunableNumber rotkD = new LoggedTunableNumber("Align/Rot kD", AlignConstants.ROT_kD); + private static final LoggedTunableNumber rotMaxVel = + new LoggedTunableNumber("Align/Rot Max Vel", AlignConstants.MAX_ROT_VELOCITY); + private static final LoggedTunableNumber rotMaxAcc = + new LoggedTunableNumber("Align/Rot Max Acc", AlignConstants.MAX_ROT_ACCELERATION); + + private ProfiledPIDController translationController; + private ProfiledPIDController rotationController; + + private Drive drive; + private Supplier targetSupplier; + private Supplier robotSupplier; + + /** Creates a new DriveToPose. */ + public DriveToPose(Drive drive, Supplier target, Supplier robot) { + this.drive = drive; + this.targetSupplier = target; + this.robotSupplier = robot; + + translationController = new ProfiledPIDController( + drivekP.get(), + drivekI.get(), + drivekD.get(), + new TrapezoidProfile.Constraints(driveMaxVel.get(), driveMaxAcc.get())); + + rotationController = new ProfiledPIDController( + rotkP.get(), + rotkI.get(), + rotkD.get(), + new TrapezoidProfile.Constraints(rotMaxVel.get(), rotMaxAcc.get())); + + rotationController.enableContinuousInput(-Math.PI, Math.PI); + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + Pose2d targetPose = targetSupplier.get(); + Pose2d currentPose = robotSupplier.get(); + ChassisSpeeds currentSpeeds = drive.getChassisSpeeds(); + + Translation2d translationError = targetPose.minus(currentPose).getTranslation(); + Rotation2d directionToTarget = translationError.getAngle(); + double velocityTowardsTarget = currentSpeeds.vxMetersPerSecond * directionToTarget.getCos() + + currentSpeeds.vyMetersPerSecond * directionToTarget.getSin(); + + translationController.reset(translationError.getNorm(), -velocityTowardsTarget); + rotationController.reset(robotSupplier.get().getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + updatePID(); + + Pose2d targetPose = targetSupplier.get(); + Pose2d currentPose = robotSupplier.get(); + + Translation2d translationError = targetPose.minus(currentPose).getTranslation(); + Rotation2d directionToTarget = translationError.getAngle(); + + double translationOutput = translationController.calculate(translationError.getNorm(), 0); + double rotationOutput = rotationController.calculate( + currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + + Translation2d translationVelocity = new Translation2d(-translationOutput, directionToTarget); + + drive.runVelocity(new ChassisSpeeds(translationVelocity.getX(), translationVelocity.getY(), rotationOutput)); + + Logger.recordOutput("DriveToPose/Target", targetPose); + Logger.recordOutput("DriveToPose/Translation Output", translationOutput); + Logger.recordOutput("DriveToPose/Rotation Output", rotationOutput); + Logger.recordOutput("DriveToPose/Translation Error", translationError.getNorm()); + Logger.recordOutput( + "DriveToPose/Rotation Error", + targetPose.getRotation().minus(currentPose.getRotation()).getDegrees()); + Logger.recordOutput("DriveToPose/Translation Velocity", translationVelocity); + Logger.recordOutput("DriveToPose/Direction to Target", directionToTarget); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drive.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } + + private void updatePID() { + if (drivekP.hasChanged(hashCode()) || drivekI.hasChanged(hashCode()) || drivekD.hasChanged(hashCode())) { + translationController.setPID(drivekP.get(), drivekI.get(), drivekD.get()); + } + if (rotkP.hasChanged(hashCode()) || rotkI.hasChanged(hashCode()) || rotkD.hasChanged(hashCode())) { + rotationController.setPID(rotkP.get(), rotkI.get(), rotkD.get()); + } + if (driveMaxVel.hasChanged(hashCode()) || driveMaxAcc.hasChanged(hashCode())) { + translationController.setConstraints( + new TrapezoidProfile.Constraints(driveMaxVel.get(), driveMaxAcc.get())); + } + if (rotMaxVel.hasChanged(hashCode()) || rotMaxAcc.hasChanged(hashCode())) { + rotationController.setConstraints(new TrapezoidProfile.Constraints(rotMaxVel.get(), rotMaxAcc.get())); + } + } +} diff --git a/src/main/java/frc/robot/commands/ElevatorHold.java b/src/main/java/frc/robot/commands/ElevatorHold.java index bd72e84..07d6a8e 100644 --- a/src/main/java/frc/robot/commands/ElevatorHold.java +++ b/src/main/java/frc/robot/commands/ElevatorHold.java @@ -5,15 +5,16 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class ElevatorHold extends Command { - private Elevator elevator = Elevator.getInstance(); + private Elevator elevator; /** Creates a new ElevatorHold. */ - public ElevatorHold() { + public ElevatorHold(Elevator elevator) { // Use addRequirements() here to declare subsystem dependencies. + this.elevator = elevator; addRequirements(elevator); } diff --git a/src/main/java/frc/robot/commands/InverseKinematics.java b/src/main/java/frc/robot/commands/InverseKinematics.java new file mode 100644 index 0000000..6b86bac --- /dev/null +++ b/src/main/java/frc/robot/commands/InverseKinematics.java @@ -0,0 +1,99 @@ +package frc.robot.commands; + +import edu.wpi.first.math.util.Units; +import frc.robot.Constants.AlignConstants; +import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.Arm.ArmMode; +import org.littletonrobotics.junction.Logger; + +public class InverseKinematics { + public InverseKinematics() { + // nothing + } + + private double getElevatorHeightMeters(double branchX, double branchY) { + double elevatorHeightMeters = branchY + - Math.sqrt(Math.pow(AlignConstants.PIVOT_TO_CORAL_RADIUS, 2) + - Math.pow(branchX + AlignConstants.BRANCH_OFFSET_BEHIND_APRILTAG, 2)); + + if (Double.isNaN(elevatorHeightMeters)) { + return Double.NaN; + } + + return elevatorHeightMeters; + } + + public double getElevatorHeightRots(double branchX, double branchY) { + double elevatorHeightMeters = getElevatorHeightMeters(branchX, branchY); + Logger.recordOutput("Elevator/AlignElevatorHeight", elevatorHeightMeters); + Logger.recordOutput("Elevator/AlignElevatorIsNaN", Double.isNaN(elevatorHeightMeters)); + double rots = 0; + + if (Double.isNaN(elevatorHeightMeters)) { + return ElevatorConstants.LEVEL_FOUR_ROT; + } + + if (Arm.getArmMode() == ArmMode.L2) { + elevatorHeightMeters = elevatorHeightMeters + + 2 * AlignConstants.PIVOT_TO_CORAL_RADIUS * Math.sin(getArmAngleRads(branchX, branchY)); + } + + rots = Units.metersToInches(elevatorHeightMeters - AlignConstants.ELEVATOR_STARTING_HEIGHT) + * ElevatorConstants.GEAR_RATIO + / (Math.PI * ElevatorConstants.PULLEY_DIAMETER); + + return rots; + } + + private double getArmAngleRads(double branchX, double branchY) { + double armAngle = Math.acos( + (branchX + AlignConstants.BRANCH_OFFSET_BEHIND_APRILTAG) / AlignConstants.PIVOT_TO_CORAL_RADIUS); + + if (Double.isNaN(armAngle)) { + return Double.NaN; + } + + return armAngle; + } + + public double getArmAngleRots(double branchX, double branchY) { + double armAngleRads = getArmAngleRads(branchX, branchY); + Logger.recordOutput( + "Arm/AlignDegreesFromHorizontal", + Units.radiansToDegrees(armAngleRads + AlignConstants.ARM_TO_CORAL_ANGULAR_OFFSET)); + Logger.recordOutput("Arm/AlignIsNaN", Double.isNaN(armAngleRads)); + double rots = 0; + + if (Double.isNaN(armAngleRads)) { + return ArmConstants.ARM_LEVEL_4_ROT; + } + + if (Arm.getArmMode() == ArmMode.L2) { + armAngleRads = armAngleRads - 2 * armAngleRads; + } + + rots = Units.radiansToRotations((armAngleRads + + (-1 * AlignConstants.ARM_STARTING_ANGLE) + + AlignConstants.ARM_TO_CORAL_ANGULAR_OFFSET)) + * ArmConstants.ARM_GEAR_RATIO; + + return rots; + } + + // public double getTZForArmSpacing(Arm arm) { + // ArmMode armMode = arm.getArmMode(); + + // switch (armMode) { + // case L2: + // return AlignConstants.SPACING_TZ; + // case L3: + // return 0.0; + // case L4: + // return 0.0; + // default: + // return 0.0; + // } + // } +} diff --git a/src/main/java/frc/robot/generated/EveTunerConstants.java b/src/main/java/frc/robot/generated/EveTunerConstants.java new file mode 100644 index 0000000..8510f5f --- /dev/null +++ b/src/main/java/frc/robot/generated/EveTunerConstants.java @@ -0,0 +1,69 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; + +public class EveTunerConstants extends TunerConstants { + private static final Slot0Configs STEER_GAINS = new Slot0Configs() + .withKP(50) + .withKI(0) + .withKD(0.24539) + .withKS(0.19817) + .withKV(2.4066) + .withKA(0.040542) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + private static final Slot0Configs DRIVE_GAINS = new Slot0Configs() + .withKP(0.62183) // 0.62183 + .withKI(0) + .withKD(0) + .withKS(0.10765) + .withKV(0.82491) + .withKA(0.0040132); + + private static final CANBus CAN_BUS = new CANBus("rio", "./logs/example.hoot"); + + private static final double COUPLE_RATIO = 3.5714285714285716; + private static final double DRIVE_RATIO = 6.746031746031747; + private static final double TURN_RATIO = 21.428571428571427; + + private static final Distance WHEEL_RADIUS = Inches.of(2); + + private static final boolean LEFT_INVERTED = false; + private static final boolean RIGHT_INVERTED = true; + + private static final int PIGEON_ID = 15; + + private static final Angle FL_ENCODER_OFFSET = Rotations.of(-0.30517578125); + private static final Angle FR_ENCODER_OFFSET = Rotations.of(-0.04052734375); + private static final Angle BL_ENCODER_OFFSET = Rotations.of(-0.23486328125); + private static final Angle BR_ENCODER_OFFSET = Rotations.of(0.353271484375); + + private static final Distance FL_X_POS = Inches.of(12.375); + private static final Distance FL_Y_POS = Inches.of(11.375); + + public EveTunerConstants() { + super( + STEER_GAINS, + DRIVE_GAINS, + CAN_BUS, + COUPLE_RATIO, + DRIVE_RATIO, + TURN_RATIO, + WHEEL_RADIUS, + LEFT_INVERTED, + RIGHT_INVERTED, + PIGEON_ID, + FL_ENCODER_OFFSET, + FR_ENCODER_OFFSET, + BL_ENCODER_OFFSET, + BR_ENCODER_OFFSET, + FL_X_POS, + FL_Y_POS); + } +} diff --git a/src/main/java/frc/robot/generated/PearracudaTunerConstants.java b/src/main/java/frc/robot/generated/PearracudaTunerConstants.java new file mode 100644 index 0000000..30ea9d9 --- /dev/null +++ b/src/main/java/frc/robot/generated/PearracudaTunerConstants.java @@ -0,0 +1,69 @@ +package frc.robot.generated; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; + +public class PearracudaTunerConstants extends TunerConstants { + private static final Slot0Configs STEER_GAINS = new Slot0Configs() + .withKP(55) + .withKI(0) + .withKD(0.24539) + .withKS(0.12644) + .withKV(0.81228) + .withKA(0.040542) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + private static final Slot0Configs DRIVE_GAINS = new Slot0Configs() + .withKP(0.6) + .withKI(0) + .withKD(0) + .withKS(0.13158) // 0.12644 + .withKV(0.81887) // 0.81228 + .withKA(0.0040132); + + private static final CANBus CAN_BUS = new CANBus("Drivetrain", "./logs/example.hoot"); + + private static final double COUPLE_RATIO = 3.5714285714285716; + private static final double DRIVE_RATIO = 6.746031746031747; + private static final double TURN_RATIO = 21.428571428571427; + + private static final Distance WHEEL_RADIUS = Inches.of(2); + + private static final boolean LEFT_INVERTED = false; + private static final boolean RIGHT_INVERTED = true; + + private static final int PIGEON_ID = 15; + + private static final Angle FL_ENCODER_OFFSET = Rotations.of(-0.447021484375); + private static final Angle FR_ENCODER_OFFSET = Rotations.of(0.1220703125); + private static final Angle BL_ENCODER_OFFSET = Rotations.of(0.349853515625); + private static final Angle BR_ENCODER_OFFSET = Rotations.of(0.008056640625); + + private static final Distance FL_X_POS = Inches.of(11.5); + private static final Distance FL_Y_POS = Inches.of(12.25); + + public PearracudaTunerConstants() { + super( + STEER_GAINS, + DRIVE_GAINS, + CAN_BUS, + COUPLE_RATIO, + DRIVE_RATIO, + TURN_RATIO, + WHEEL_RADIUS, + LEFT_INVERTED, + RIGHT_INVERTED, + PIGEON_ID, + FL_ENCODER_OFFSET, + FR_ENCODER_OFFSET, + BL_ENCODER_OFFSET, + BR_ENCODER_OFFSET, + FL_X_POS, + FL_Y_POS); + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 47d1dcb..f29ff19 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -5,226 +5,235 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; -import frc.robot.subsystems.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(18.53) - .withKI(0) - .withKD(0.24539) - .withKS(0.19817) - .withKV(2.4066) - .withKA(0.040542) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.062183) - .withKI(0) - .withKD(0) - .withKS(0.15202) - .withKV(0.1141) - .withKA(0.0040132); +public abstract class TunerConstants { + protected TunerConstants( + Slot0Configs steerGains, + Slot0Configs driveGains, + CANBus canBus, + double coupleRatio, + double driveRatio, + double turnRatio, + Distance wheelRadius, + boolean leftInverted, + boolean rightInverted, + int pigeonID, + Angle flOffset, + Angle frOffset, + Angle blOffset, + Angle brOffset, + Distance flXPos, + Distance flYPos) { + this.steerGains = steerGains; + this.driveGains = driveGains; + this.kCANBus = canBus; + this.kCoupleRatio = coupleRatio; + this.kDriveGearRatio = driveRatio; + this.kSteerGearRatio = turnRatio; + this.kWheelRadius = wheelRadius; + this.kInvertLeftSide = leftInverted; + this.kInvertRightSide = rightInverted; + this.kPigeonId = pigeonID; + this.kFrontLeftEncoderOffset = flOffset; + this.kFrontRightEncoderOffset = frOffset; + this.kBackLeftEncoderOffset = blOffset; + this.kBackRightEncoderOffset = brOffset; + this.kFrontLeftXPos = flXPos; + this.kFrontLeftYPos = flYPos; + + DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + ConstantCreator = new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + FrontLeft = ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + + FrontRight = ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos.unaryMinus(), + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + + BackLeft = ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kFrontLeftXPos.unaryMinus(), + kFrontLeftXPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + + BackRight = ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kFrontLeftXPos.unaryMinus(), + kFrontLeftYPos.unaryMinus(), + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + } + + protected Slot0Configs steerGains; + + protected Slot0Configs driveGains; // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + private final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + private final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + private final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + private final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; // The remote sensor feedback type to use for the steer motors; // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + private final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private final Current kSlipCurrent = Amps.of(120.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + private final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits(new CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + private final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; + private final Pigeon2Configuration pigeonConfigs = null; // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("Drivetrain", "./logs/example.hoot"); + public CANBus kCANBus; // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); + public final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; + protected double kCoupleRatio; - private static final double kDriveGearRatio = 6.746031746031747; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); + protected double kDriveGearRatio; + protected double kSteerGearRatio; + protected Distance kWheelRadius; - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; + protected boolean kInvertLeftSide; + protected boolean kInvertRightSide; - private static final int kPigeonId = 15; + protected int kPigeonId; // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + private final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory - ConstantCreator = new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + private final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public final SwerveDrivetrainConstants DrivetrainConstants; + + private final SwerveModuleConstantsFactory + ConstantCreator; // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 5; - private static final int kFrontLeftEncoderId = 11; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.447021484375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; + private final int kFrontLeftDriveMotorId = 1; + private final int kFrontLeftSteerMotorId = 5; + private final int kFrontLeftEncoderId = 11; + protected Angle kFrontLeftEncoderOffset; + private final boolean kFrontLeftSteerMotorInverted = true; + private final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(11.5); - private static final Distance kFrontLeftYPos = Inches.of(12.25); + protected Distance kFrontLeftXPos; + protected Distance kFrontLeftYPos; // Front Right - private static final int kFrontRightDriveMotorId = 2; - private static final int kFrontRightSteerMotorId = 6; - private static final int kFrontRightEncoderId = 12; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.1220703125); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(11.5); - private static final Distance kFrontRightYPos = Inches.of(-12.25); + private final int kFrontRightDriveMotorId = 2; + private final int kFrontRightSteerMotorId = 6; + private final int kFrontRightEncoderId = 12; + protected Angle kFrontRightEncoderOffset; + private final boolean kFrontRightSteerMotorInverted = true; + private final boolean kFrontRightEncoderInverted = false; // Back Left - private static final int kBackLeftDriveMotorId = 3; - private static final int kBackLeftSteerMotorId = 7; - private static final int kBackLeftEncoderId = 13; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.349853515625); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-11.5); - private static final Distance kBackLeftYPos = Inches.of(12.25); + private final int kBackLeftDriveMotorId = 3; + private final int kBackLeftSteerMotorId = 7; + private final int kBackLeftEncoderId = 13; + protected Angle kBackLeftEncoderOffset; + private final boolean kBackLeftSteerMotorInverted = true; + private final boolean kBackLeftEncoderInverted = false; // Back Right - private static final int kBackRightDriveMotorId = 4; - private static final int kBackRightSteerMotorId = 8; - private static final int kBackRightEncoderId = 14; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.008056640625); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-11.5); - private static final Distance kBackRightYPos = Inches.of(-12.25); - - public static final SwerveModuleConstants - FrontLeft = ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants - FrontRight = ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants - BackLeft = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants - BackRight = ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot program,. */ - public static CommandSwerveDrivetrain createDrivetrain() { - return new CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - } + private final int kBackRightDriveMotorId = 4; + private final int kBackRightSteerMotorId = 8; + private final int kBackRightEncoderId = 14; + protected Angle kBackRightEncoderOffset; + private final boolean kBackRightSteerMotorInverted = true; + private final boolean kBackRightEncoderInverted = false; + + public final SwerveModuleConstants FrontLeft; + public final SwerveModuleConstants FrontRight; + public final SwerveModuleConstants BackLeft; + public final SwerveModuleConstants BackRight; /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + public class TunerSwerveDrivetrain extends SwerveDrivetrain { /** * Constructs a CTRE SwerveDrivetrain using the specified constants. * diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java deleted file mode 100644 index 9dad099..0000000 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ /dev/null @@ -1,268 +0,0 @@ -// 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.subsystems; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.lib.drivers.PearadoxTalonFX; -import frc.robot.Constants.ArmConstants; -import frc.robot.util.SmarterDashboard; - -public class Arm extends SubsystemBase { - private PearadoxTalonFX pivot; - private MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0); - - private ArmMode armMode = ArmMode.Stowed; - private TalonFXConfiguration talonFXConfigs; - - private VoltageOut voltageRequest = new VoltageOut(0); - - private boolean isCoral = true; - private boolean isAligning = false; - private double lastAngle = 0; - - public enum ArmMode { - Intake, - L2, - L3, - L4, - Stowed, - LOLLICLIMB - } - - private double armAdjust = 0.0; - - public static final Arm arm = new Arm(); - - public static Arm getInstance() { - return arm; - } - - private final VoltageOut m_voltReq = new VoltageOut(0.0); - - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism((volts) -> pivot.setControl(m_voltReq.withOutput(volts.in(Volts))), null, this)); - - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineTranslation.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineTranslation.dynamic(direction); - } - - public Arm() { - pivot = new PearadoxTalonFX( - ArmConstants.ARM_KRAKEN_ID, - NeutralModeValue.Brake, - ArmConstants.CURRENT_LIMIT, - ArmConstants.CURRENT_LIMIT, - false); - - BaseStatusSignal.setUpdateFrequencyForAll( - ArmConstants.UPDATE_FREQ, - pivot.getPosition(), - pivot.getVelocity(), - pivot.getDutyCycle(), - pivot.getMotorVoltage(), - pivot.getTorqueCurrent(), - pivot.getSupplyCurrent(), - pivot.getStatorCurrent()); - - pivot.optimizeBusUtilization(); - - talonFXConfigs = new TalonFXConfiguration(); - - talonFXConfigs.Voltage.PeakForwardVoltage = 4.5; - talonFXConfigs.Voltage.PeakReverseVoltage = -4.5; - - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kG = 0; // add enough Gravity Gain just before motor starts moving - slot0Configs.kS = 0.15; // Add x output to overcome static friction - slot0Configs.kV = 7.2; // A velocity target of 1 rps results in x output - slot0Configs.kA = 0; // An acceleration of 1 rps/s requires x output - slot0Configs.kP = 0.4; // A position error of x rotations results in 12 V output - slot0Configs.kI = 0; // no output for integrated error - slot0Configs.kD = 0; // A velocity error of 1 rps results in x output - slot0Configs.GravityType = GravityTypeValue.Arm_Cosine; - - var slot1Configs = talonFXConfigs.Slot1; // configs for game piece - slot1Configs.kG = 0; - slot1Configs.kS = 0; - slot1Configs.kV = 0; - slot1Configs.kA = 0; - slot1Configs.kP = 0; - slot1Configs.kI = 0; - slot1Configs.kD = 0; - - var motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = ArmConstants.MM_MAX_CRUISE_VELOCITY; - motionMagicConfigs.MotionMagicAcceleration = ArmConstants.MM_MAX_CRUISE_ACCELERATION; - - pivot.getConfigurator().apply(talonFXConfigs); - } - - @Override - public void periodic() { - SmarterDashboard.putNumber("Arm/Raw Position", pivot.getPosition().getValueAsDouble()); - SmarterDashboard.putNumber("Arm/Angle degrees", getArmAngleDegrees()); - SmarterDashboard.putNumber("Arm/Velocity rot/sec", pivot.getVelocity().getValueAsDouble()); - SmarterDashboard.putNumber("Arm/Voltage", pivot.getMotorVoltage().getValueAsDouble()); - SmarterDashboard.putNumber( - "Arm/Supply Current", pivot.getSupplyCurrent().getValueAsDouble()); - SmarterDashboard.putNumber( - "Arm/Stator Current", pivot.getStatorCurrent().getValueAsDouble()); - SmarterDashboard.putNumber("Arm/Adjust", armAdjust); - SmarterDashboard.putString("Arm/Mode", armMode.toString()); - SmarterDashboard.putBoolean("Arm/IsCoral", isCoral); - SmarterDashboard.putNumber("Arm/DeltaAngle", deltaArmAngle()); - SmarterDashboard.putNumber("Arm/kG", 0.35 * Math.cos(-1 * Units.degreesToRadians(getArmAngleDegrees() - 96))); - - // setAligning(!RobotContainer.align.isAligned()); - } - - public void armHold() { - double setpoint = ArmConstants.ARM_STOWED_ROT + armAdjust; - - if (isCoral) { - if (armMode == ArmMode.Stowed) { - setpoint = ArmConstants.ARM_STOWED_ROT + armAdjust; - } else if (armMode == ArmMode.Intake) { - if (isAligning) { - setpoint = ArmConstants.ARM_STATION_BEHIND_CORAL + armAdjust; - - } else { - setpoint = ArmConstants.ARM_INTAKE_ROT + armAdjust; - } - } else if (armMode == ArmMode.L2) { - setpoint = ArmConstants.ARM_LEVEL_2_ROT + armAdjust; - } else if (armMode == ArmMode.L3) { - setpoint = ArmConstants.ARM_LEVEL_3_ROT + armAdjust; - } else if (armMode == ArmMode.L4) { - if (isAligning) { - // setpoint = RobotContainer.align.getArmAngleRots() + armAdjust; - setpoint = ArmConstants.ARM_L4_BEHIND_CORAL + armAdjust; - - } else { - setpoint = ArmConstants.ARM_LEVEL_4_ROT + armAdjust; - } - } - } else if (!isCoral) { - if (armMode == ArmMode.Stowed) { - setpoint = ArmConstants.ARM_LOLLIPOP + armAdjust; - } else if (armMode == ArmMode.L2) { - setpoint = ArmConstants.ARM_ALGAE_LOW + armAdjust; - } else if (armMode == ArmMode.L3) { - setpoint = ArmConstants.ARM_ALGAE_HIGH + armAdjust; - } else if (armMode == ArmMode.L4) { - setpoint = ArmConstants.ARM_BARGE + armAdjust; - } - } - - // Logger.recordOutput("Arm/Align Setpoint", RobotContainer.align.getArmAngleRots() + armAdjust); - - // pivot.setControl(motionMagicRequest.withPosition(setpoint)); - pivot.setControl(new PositionVoltage(-setpoint) - .withFeedForward(0.35 * Math.cos(-1 * Units.degreesToRadians(getArmAngleDegrees() - 96)))); - // pivot.setControl(new PositionVoltage(-setpoint).withFeedForward(ArmConstants.kG * - // Math.cos(getArmAngleDegrees() - 96))); - SmarterDashboard.putNumber("Arm/Cur Setpoint", -setpoint); - } - - public double deltaArmAngle() { - var temp = lastAngle; - lastAngle = getArmAngleDegrees(); - return lastAngle - temp; - } - - public void setArmIntake() { - armMode = ArmMode.Intake; - } - - public void setArmL2() { - armMode = ArmMode.L2; - } - - public void setArmL3() { - armMode = ArmMode.L3; - } - - public void setArmL4() { - armMode = ArmMode.L4; - } - - public void setStowed() { - armMode = ArmMode.Stowed; - } - - public void setLollipopOrClimb() { - armMode = ArmMode.LOLLICLIMB; - } - - public void setCoral() { - isCoral = true; - } - - public void setAlgae() { - isCoral = false; - } - - public ArmMode getArmMode() { - return armMode; - } - - public boolean getIsCoral() { - return isCoral; - } - - public void setAligning(boolean flag) { - isAligning = flag; - } - - public double getPivotPosition() { - return pivot.getPosition().getValueAsDouble() / ArmConstants.ARM_GEAR_RATIO; - } - - public double getArmAngleDegrees() { - return Units.rotationsToDegrees(getPivotPosition()); - } - - // public void zeroArm() { - // pivot.setPosition(ArmConstants.ARM_STOWED_ROT * ArmConstants.ARM_GEAR_RATIO); - // } - - public void armAdjust(double adjustBy) { - armAdjust += adjustBy; - } - - public void resetAdjust() { - armAdjust = 0; - } - - // public double getAutoArmAngle(){ - // double dist = RobotContainer.align.getTagDist(); //this doesnt work maybe make AutoAlign a subsystem - - // return Math.acos((dist + ArmConstants.ARM_LENGTH * Math.cos(0)) / ArmConstants.ARM_LENGTH); //TODO Current - // Angle Setpoint - // } -} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java deleted file mode 100644 index fc5bc93..0000000 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ /dev/null @@ -1,154 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.drivers.PearadoxTalonFX; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ClimbConstants; -import frc.robot.util.SmarterDashboard; -import org.littletonrobotics.junction.Logger; - -/** Class to run the rollers over CAN */ -public class Climber extends SubsystemBase { - // private final SparkMax climbMotor; - private final PearadoxTalonFX climbMotor; - - private static final Climber CLIMBER = new Climber(); - private TalonFXConfiguration talonFXConfigs; - - private static final Elevator ELEVATOR = Elevator.getInstance(); - private static final Arm ARM = Arm.getInstance(); - - private double setpoint = ClimbConstants.CLIMB_SETPOINT; - private double climberOffset = 0; - - private int climbState = 1; - private String climbStateString = "unpowered"; - - public static Climber getInstance() { - return CLIMBER; - } - - public Climber() { - climbMotor = new PearadoxTalonFX(ClimbConstants.CLIMB_MOTOR_ID, NeutralModeValue.Brake, 50, false); - - BaseStatusSignal.setUpdateFrequencyForAll( - ArmConstants.UPDATE_FREQ, - climbMotor.getPosition(), - climbMotor.getMotorVoltage(), - climbMotor.getTorqueCurrent(), - climbMotor.getSupplyCurrent(), - climbMotor.getStatorCurrent()); - - climbMotor.optimizeBusUtilization(); - - talonFXConfigs = new TalonFXConfiguration(); - - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kG = 0; - slot0Configs.kS = 0; - slot0Configs.kV = 0; - slot0Configs.kA = 0; - slot0Configs.kP = 0.25; - slot0Configs.kI = 0; - slot0Configs.kD = 0; - - climbMotor.getConfigurator().apply(slot0Configs); - } - - @Override - public void periodic() { - - updateClimStateString(); - - SmarterDashboard.putNumber("Climber/Position", climbMotor.getPosition().getValueAsDouble()); - Logger.recordOutput("Climber/Pos", climbMotor.getPosition().getValueAsDouble()); - Logger.recordOutput("Climber/Volts", climbMotor.getMotorVoltage().getValueAsDouble()); - Logger.recordOutput( - "Climber/Supply Current", climbMotor.getSupplyCurrent().getValueAsDouble()); - Logger.recordOutput( - "Climber/Supply Current", climbMotor.getStatorCurrent().getValueAsDouble()); - - SmarterDashboard.putNumber("Climber/State", climbState); - SmarterDashboard.putString("Climber/StateStr", climbStateString); - } - - public void climberAdjustUp() { - climberOffset += 10; // in rotations - } - - public void climberAdjustDown() { - climberOffset -= 10; - } - - public void prepClimber() { - if (climbState != 0) { - ELEVATOR.setElevatorStowedMode(); - ARM.setAlgae(); - ARM.setStowed(); - } - } - - public void zeroClimber() { - climbMotor.setPosition(0); - climberOffset = 0; - } - - public void stop() { - climbMotor.set(0); - } - - public void updateClimStateString() { - if (climbState == 2) { - climbStateString = "Climbing!"; - } else if (climbState == 1) { - climbStateString = "Deployed (Vertical)"; - } else if (climbState == 0) { - climbStateString = "Retracted (Stowed)"; - } - } - - public void setClimbPosition() { - if (climbState == 2) { - climbClimber(); - } else if (climbState == 1) { - deployClimber(); - } else if (climbState == 0) { - retractClimber(); - } - } - - public void retractClimber() { - climbMotor.setControl(new PositionVoltage(-290 + climberOffset)); // -230 // -150 // -270 - } - - public void climberDown() { - climbMotor.set(-0.8); - } - - public void climberUp() { - climbMotor.set(0.8); - } - - public void deployClimber() { - climbMotor.setControl(new PositionVoltage(0 + climberOffset)); - } - - public void climbClimber() { - climbMotor.setControl(new PositionVoltage(-195 + climberOffset)); - } - - public void incrementClimbState() { - climbState = Math.min(2, climbState + 1); - prepClimber(); - setClimbPosition(); - } - - public void decrementClimbState() { - climbState = Math.max(0, climbState - 1); - setClimbPosition(); - } -} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java deleted file mode 100644 index c0ec7c0..0000000 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ /dev/null @@ -1,431 +0,0 @@ -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.VisionConstants; -import frc.robot.RobotContainer; -import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; -import frc.robot.util.SmarterDashboard; -import java.util.HashMap; -import java.util.Map; -import java.util.function.Supplier; - -/** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily be used in - * command-based projects. - */ -public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - public boolean isFieldOriented = true; - private double speedMultiplier = 1.0; - private boolean isSlowMode = false; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /** Swerve request to apply during robot-centric path following */ - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = - new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism(volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this)); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer; - - public final SlewRateLimiter frontLimiter = new SlewRateLimiter(4); - public final SlewRateLimiter sideLimiter = new SlewRateLimiter(4); - public final SlewRateLimiter turnLimiter = new SlewRateLimiter(4); - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices themselves. If they - * need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices themselves. If they - * need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to 0 Hz, this is 250 - * Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices themselves. If they - * need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to 0 Hz, this is 250 - * Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the form [x, y, theta]ᵀ, with - * units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, y, theta]ᵀ, with - * units in meters and radians - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); - } - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl(m_pathApplyRobotSpeeds - .withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(10, 0, 0), - // PID constants for rotation - new PIDConstants(7, 0, 0)), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError( - "Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine specified by - * {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation); - m_hasAppliedOperatorPerspective = true; - }); - } - - Transform2d offset = getState().Pose.minus(getTagPose(currentCSAlignTagID)); - - SmarterDashboard.putNumber("Drive/X", offset.getX()); - SmarterDashboard.putNumber("Drive/Y", offset.getY()); - SmarterDashboard.putBoolean("Drive/Slow Mode", isSlowMode); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still - * accounting for measurement noise. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still - * accounting for measurement noise. - * - *

Note that the vision measurement standard deviations passed into this method will continue to apply to future - * measurements until a subsequent call to {@link #setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement in the form [x, y, theta]ᵀ, - * with units in meters and radians. - */ - @Override - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - super.addVisionMeasurement( - visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); - } - - public void changeRobotOrientation() { - isFieldOriented = !isFieldOriented; - } - - public void changeSpeedMultiplier() { - if (speedMultiplier <= DriveConstants.SLOW_MODE_SPEED + 1e-6) { - speedMultiplier = 1; - isSlowMode = false; - } else { - speedMultiplier = DriveConstants.SLOW_MODE_SPEED; - isSlowMode = true; - } - } - - public double getSpeedMultipler() { - return speedMultiplier; - } - - private int currentReefAlignTagID = 18; // -1 - private int currentCSAlignTagID = 12; // -1 - private Map tagPoses3d = getTagPoses(); - - public Map getTagPoses() { - Map tagPoses = new HashMap(); - for (int tag : FieldConstants.RED_REEF_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); - } - for (int tag : FieldConstants.BLUE_REEF_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); - } - for (int tag : FieldConstants.RED_CORAL_STATION_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); - } - for (int tag : FieldConstants.BLUE_CORAL_STATION_TAG_IDS) { - tagPoses.put(tag, VisionConstants.aprilTagLayout.getTagPose(tag).get()); - } - return tagPoses; - } - - private Rotation2d getTagAngle(int tagID) { - - if (tagPoses3d.containsKey(tagID)) { - Pose3d tagPose = tagPoses3d.get(tagID); - return new Rotation2d(tagPose.getRotation().getZ()); - } else return new Rotation2d(0); - } - - private Pose2d getTagPose(int tagID) { - - if (tagPoses3d.containsKey(tagID)) { - Pose3d tagPose = tagPoses3d.get(tagID); - return tagPose.toPose2d(); - } else return new Pose2d(); - } - - public Rotation2d getAlignAngleReef() { - currentReefAlignTagID = getClosestAprilTag( - RobotContainer.isRedAlliance() ? FieldConstants.RED_REEF_TAG_IDS : FieldConstants.BLUE_REEF_TAG_IDS, - getState().Pose); - - return getTagAngle(currentReefAlignTagID); - } - - public Rotation2d getAlignAngleAlgaeReef() { - currentReefAlignTagID = getClosestAprilTag( - RobotContainer.isRedAlliance() ? FieldConstants.RED_REEF_TAG_IDS : FieldConstants.BLUE_REEF_TAG_IDS, - getState().Pose); - - return new Rotation2d(getTagAngle(currentReefAlignTagID).getRadians() + Units.degreesToRadians(180)); - } - - public Rotation2d getAlignAngleStation() { - currentCSAlignTagID = getClosestAprilTag( - RobotContainer.isRedAlliance() - ? FieldConstants.RED_CORAL_STATION_TAG_IDS - : FieldConstants.BLUE_CORAL_STATION_TAG_IDS, - getState().Pose); - - return new Rotation2d(getTagAngle(currentCSAlignTagID).getRadians() + +Units.degreesToRadians(180)); - } - - private int getClosestAprilTag(int[] tagIDs, Pose2d robotPose) { - double minDistance = Double.POSITIVE_INFINITY; - int closestTagID = -1; - - // iterates through all tag IDs - for (int i : tagIDs) { - if (tagPoses3d.containsKey(i)) { - Pose3d tagPose = tagPoses3d.get(i); - - // distance between robot pose and april tag - double distance = tagPose.getTranslation() - .toTranslation2d() - .minus(robotPose.getTranslation()) - .getNorm(); - - if (distance < minDistance) { - closestTagID = i; - minDistance = distance; - } - } - } - - return closestTagID; - } -} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java deleted file mode 100644 index 66da8e1..0000000 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ /dev/null @@ -1,427 +0,0 @@ -// 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.subsystems; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.signals.GravityTypeValue; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.lib.drivers.PearadoxTalonFX; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.util.SmarterDashboard; - -public class Elevator extends SubsystemBase { - - private PearadoxTalonFX elevator; - private PearadoxTalonFX elevatorFollower; - private MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0); - private double elevatorOffset = 0.0; - private TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); - - private boolean isCoral = true; - private boolean isAligning = false; - private boolean isZeroing = false; - - public static enum ElevatorMode { - STOWED, - STATION, - LEVEL_TWO, - LEVEL_THREE, - LEVEL_FOUR, - ALGAE_LOW, - ALGAE_HIGH, - BARGE, - } - - private ElevatorMode elevatorMode = ElevatorMode.STOWED; - // private double lowest_rot = ElevatorConstants.STOWED_ROT; - // private double highest_rot = ElevatorConstants.MAX_ELEVATOR_ROT; - - private boolean isLowering = false; - - private static Elevator ELEVATOR = new Elevator(); - - public static Elevator getInstance() { - return ELEVATOR; - } - - private final VoltageOut m_voltReq = new VoltageOut(0.0); - - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> elevator.setControl(m_voltReq.withOutput(volts.in(Volts))), null, this)); - - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineTranslation.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineTranslation.dynamic(direction); - } - - /** Creates a new Elevator. */ - public Elevator() { - elevator = new PearadoxTalonFX( - ElevatorConstants.ELEVATOR_ID, - ElevatorConstants.MODE, - ElevatorConstants.CURRENT_LIMIT, - ElevatorConstants.IS_INVERTED); - - elevatorFollower = new PearadoxTalonFX( - ElevatorConstants.ELEVATOR_FOLLOWER_ID, - ElevatorConstants.MODE, - ElevatorConstants.CURRENT_LIMIT, - ElevatorConstants.IS_INVERTED); - - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kG = ElevatorConstants.kG; // add enough Gravity Gain just before motor starts moving - slot0Configs.kS = ElevatorConstants.kS; // Add 0.1 V output to overcome static friction - slot0Configs.kV = ElevatorConstants.kV; // A velocity target of 1 rps results in 0.1 V output - slot0Configs.kA = ElevatorConstants.kA; // An acceleration of 1 rps/s requires 0.01 V output - slot0Configs.kP = ElevatorConstants.kP; // A position error of 2.5 rotations results in 12 V output, prev 4.8 - slot0Configs.kI = ElevatorConstants.kI; // no output for integrated error - slot0Configs.kD = ElevatorConstants.kD; // A velocity error of 1 rps results in 0.1 V output - - var slot1Configs = talonFXConfigs.Slot1; - slot1Configs.kG = ElevatorConstants.kG; // add enough Gravity Gain just before motor starts moving - slot1Configs.kS = ElevatorConstants.kS; // Add 0.1 V output to overcome static friction - slot1Configs.kV = ElevatorConstants.kV; // A velocity target of 1 rps results in 0.1 V output - slot1Configs.kA = ElevatorConstants.kA; // An acceleration of 1 rps/s requires 0.01 V output - slot1Configs.kP = ElevatorConstants.kP; // A position error of 2.5 rotations results in 12 V output, prev 4.8 - slot1Configs.kI = ElevatorConstants.kI; // no output for integrated error - slot1Configs.kD = ElevatorConstants.kD; // A velocity error of 1 rps results in 0.1 V output - - var motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = - ElevatorConstants.MM_CRUISE_VELCOCITY_UP; // Target cruise velocity of 80 rps - motionMagicConfigs.MotionMagicAcceleration = - ElevatorConstants.MM_ACCELERATION_UP; // Target acceleration of 160 rps/s (0.5 seconds) - // (not sure if needed - > ) motionMagicConfigs.MotionMagicJerk = 1600; // Target jerk of 1600 rps/s/s (0.1 - // seconds) - - talonFXConfigs.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - talonFXConfigs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - 33 * ElevatorConstants.GEAR_RATIO / (Math.PI * ElevatorConstants.PULLEY_DIAMETER); - talonFXConfigs.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - talonFXConfigs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; - talonFXConfigs.Slot0.GravityType = GravityTypeValue.Elevator_Static; - - elevator.getConfigurator().apply(talonFXConfigs); - - // elevator.getPosition().setUpdateFrequency(ElevatorConstants.UPDATE_FREQ); - // elevator.getVelocity().setUpdateFrequency(ElevatorConstants.UPDATE_FREQ); - - // // These are needed for the follower motor to work - // elevator.getDutyCycle().setUpdateFrequency(ElevatorConstants.UPDATE_FREQ); - // elevator.getMotorVoltage().setUpdateFrequency(ElevatorConstants.UPDATE_FREQ); - // elevator.getTorqueCurrent().setUpdateFrequency(ElevatorConstants.UPDATE_FREQ); - // elevator.getSupplyCurrent().setUpdateFrequency(ElevatorConstants.UPDATE_FREQ); - // elevator.getStatorCurrent().setUpdateFrequency(ElevatorConstants.UPDATE_FREQ); - - BaseStatusSignal.setUpdateFrequencyForAll( - ArmConstants.UPDATE_FREQ, - elevator.getPosition(), - elevator.getVelocity(), - elevator.getDutyCycle(), - elevator.getMotorVoltage(), - elevator.getTorqueCurrent(), - elevator.getSupplyCurrent(), - elevator.getStatorCurrent()); - - elevator.optimizeBusUtilization(); - - elevatorFollower.getConfigurator().apply(talonFXConfigs); - elevatorFollower.optimizeBusUtilization(); - elevatorFollower.setControl(new Follower(ElevatorConstants.ELEVATOR_ID, true)); - - // SmarterDashboard.putNumber("Elevator kG", ElevatorConstants.kG); - // SmarterDashboard.putNumber("Elevator kS", ElevatorConstants.kS); - // SmarterDashboard.putNumber("Elevator kV", ElevatorConstants.kV); - // SmarterDashboard.putNumber("Elevator kA", ElevatorConstants.kA); - // SmarterDashboard.putNumber("Elevator kP", ElevatorConstants.kP); - // SmarterDashboard.putNumber("Elevator kI", ElevatorConstants.kI); - // SmarterDashboard.putNumber("Elevator kD", ElevatorConstants.kD); - } - - @Override - public void periodic() { - SmarterDashboard.putNumber("Elevator/Position Inches", getElevatorPositionInches()); - SmarterDashboard.putNumber("Elevator/Velocity (in/sec)", getElevatorVelocityInchesPerSecond()); - SmarterDashboard.putNumber("Elevator/Position Rots", getElevatorPositionRots()); - SmarterDashboard.putNumber("Elevator/Velocity (rot/sec)", getElevatorVelocity_RotsPerSecond()); - SmarterDashboard.putNumber( - "Elevator/Supply Current A", elevator.getSupplyCurrent().getValueAsDouble()); - SmarterDashboard.putNumber( - "Elevator/Stator Current A", elevator.getStatorCurrent().getValueAsDouble()); - SmarterDashboard.putNumber( - "Elevator/Voltage V", elevator.getMotorVoltage().getValueAsDouble()); - SmarterDashboard.putNumber("Elevator/Offset", elevatorOffset); - SmarterDashboard.putString("Elevator Mode", elevatorMode.toString()); - SmarterDashboard.putBoolean("Elevator/IsCoral", isCoral); - - // setAligning(!RobotContainer.align.isAligned()); - } - - public void setElevatorPosition() { - double setpoint = ElevatorConstants.STOWED_ROT + elevatorOffset; - // if (elevatorMode == ElevatorMode.STATION) { - // if (isCoral) { - // setpoint = ElevatorConstants.STATION_ROT + elevatorOffset; - // } else { - // setpoint = ElevatorConstants.STOWED_ROT + elevatorOffset; - // } - // } else if (elevatorMode == ElevatorMode.LEVEL_TWO) { - // if (isCoral) { - // setpoint = ElevatorConstants.LEVEL_TWO_ROT + elevatorOffset; - // } else { - // setpoint = ElevatorConstants.ALGAE_LOW_ROT + elevatorOffset; - // } - // } else if (elevatorMode == ElevatorMode.LEVEL_THREE) { - // if (isCoral) { - // setpoint = ElevatorConstants.LEVEL_THREE_ROT + elevatorOffset; - // } else { - // setpoint = ElevatorConstants.ALGAE_HIGH_ROT + elevatorOffset; - // } - // } else if (elevatorMode == ElevatorMode.LEVEL_FOUR) { - // setpoint = ElevatorConstants.LEVEL_FOUR_ROT + elevatorOffset; - // } else if (elevatorMode == ElevatorMode.ALGAE_LOW) { - // setpoint = ElevatorConstants.ALGAE_LOW_HEIGHT + elevatorOffset; - // } else if (elevatorMode == ElevatorMode.ALGAE_HIGH) { - // setpoint = ElevatorConstants.ALGAE_HIGH_HEIGHT + elevatorOffset; - // } else if (elevatorMode == ElevatorMode.BARGE) { - // setpoint = ElevatorConstants.BARGE_ROT + elevatorOffset; - // } - - if (elevatorMode == ElevatorMode.STOWED) { - // if (isCoral) { - setpoint = ElevatorConstants.STOWED_ROT + elevatorOffset; - // } else { - // setpoint = ElevatorConstants.STATION_ROT + elevatorOffset; - // ; - // } - } else if (elevatorMode == ElevatorMode.STATION) { - if (isAligning) { - setpoint = ElevatorConstants.OBSTRUCTED_STATION_ROT + elevatorOffset; - } else { - setpoint = ElevatorConstants.STATION_ROT + elevatorOffset; - } - } - if (isCoral) { - if (elevatorMode == ElevatorMode.LEVEL_TWO) { - setpoint = ElevatorConstants.LEVEL_TWO_ROT + elevatorOffset; - - } else if (elevatorMode == ElevatorMode.LEVEL_THREE) { - setpoint = ElevatorConstants.LEVEL_THREE_ROT + elevatorOffset; - } else if (elevatorMode == ElevatorMode.LEVEL_FOUR) { - if (isAligning) { - // setpoint = RobotContainer.align.getElevatorHeightRots() + elevatorOffset; - // setpoint = ElevatorConstants.BARGE_ROT + elevatorOffset; - setpoint = ElevatorConstants.LEVEL_FOUR_ROT + elevatorOffset; - - } else { - if (DriverStation.isAutonomous()) { - setpoint = ElevatorConstants.LEVEL_FOUR_ROT + elevatorOffset - 0.4; - } else { - setpoint = ElevatorConstants.LEVEL_FOUR_ROT + elevatorOffset; - } - } - } - } else if (!isCoral) { - if (elevatorMode == ElevatorMode.LEVEL_TWO) { - setpoint = ElevatorConstants.ALGAE_LOW_ROT + elevatorOffset; - } else if (elevatorMode == ElevatorMode.LEVEL_THREE) { - setpoint = ElevatorConstants.ALGAE_HIGH_ROT + elevatorOffset; - } else if (elevatorMode == ElevatorMode.LEVEL_FOUR) { - setpoint = ElevatorConstants.BARGE_ROT + elevatorOffset; - } - } - - // } else { // stowed - // setpoint = Math.max(lowest_rot, Math.min((ElevatorConstants.STOWED_ROT + elevatorOffset), highest_rot)); - // } - - // switch (elevatorMode) { - // case LEVEL_FOUR: setpoint = ElevatorConstants.LEVEL_FOUR_ROT + elevatorOffset; break; - // case LEVEL_THREE: setpoint = ElevatorConstants.LEVEL_THREE_ROT + elevatorOffset; break; - // case LEVEL_TWO: setpoint = ElevatorConstants.LEVEL_TWO_ROT + elevatorOffset; break; - // case STATION: setpoint = ElevatorConstants.STATION_ROT + elevatorOffset; break; - // default: setpoint = Math.max(lowest_rot, Math.min((ElevatorConstants.STOWED_ROT + elevatorOffset), - // highest_rot)); - // } - - isLowering = elevator.getPosition().getValueAsDouble() > setpoint; - - if (!isZeroing) { - elevator.setControl(motionMagicRequest.withPosition(setpoint).withSlot(isLowering ? 1 : 0)); - // elevator.setControl(new VoltageOut(elevatorOffset)); - } else { - homeElevator(); - } - - // // only reapply configs when this changes - // if (isLowering != (elevator.getPosition().getValueAsDouble() > setpoint)) { - // isLowering = elevator.getPosition().getValueAsDouble() > setpoint; - - // if (isLowering) { - // talonFXConfigs.MotionMagic.MotionMagicCruiseVelocity = ElevatorConstants.MM_CRUISE_VELCOCITY_DOWN; - // talonFXConfigs.MotionMagic.MotionMagicAcceleration = ElevatorConstants.MM_ACCELERATION_DOWN; - // } else { - // talonFXConfigs.MotionMagic.MotionMagicCruiseVelocity = ElevatorConstants.MM_CRUISE_VELCOCITY_UP; - // talonFXConfigs.MotionMagic.MotionMagicAcceleration = ElevatorConstants.MM_ACCELERATION_UP; - // } - - // elevator.getConfigurator().refresh(talonFXConfigs); - // elevator.getConfigurator().apply(talonFXConfigs); - - // SmarterDashboard.putBoolean("Elevator/IsLowering", isLowering); - // } - SmarterDashboard.putBoolean("Elevator/IsLowering", isLowering); - - SmarterDashboard.putNumber("Elevator/Setpoint", setpoint); - // Logger.recordOutput("Elevator/Align Setpoint", RobotContainer.align.getElevatorHeightRots() + - // elevatorOffset); - } - - public void homeElevator() { - elevator.set(ElevatorConstants.HOMING_SPEED); - } - - public void zeroElevator() { - SmarterDashboard.putNumber( - "Elevator/New Zero Diff", 0 - elevator.getPosition().getValueAsDouble()); - elevator.setPosition(0); - elevatorOffset = 0; - } - - public void stopElevator() { - elevator.set(0); - } - - // public double getAutoHeightAdjust(){ - // double angle = RobotContainer.arm.getAutoArmAngle(); - - // return - // } - - public double getElevatorPositionRots() { - return elevator.getPosition().getValueAsDouble(); - } - - public double getElevatorVelocity_RotsPerSecond() { - return elevator.getVelocity().getValueAsDouble(); - } - - public double getElevatorPositionInches() { - return getElevatorPositionRots() * ElevatorConstants.kRotationToInches; - } - - public double getElevatorVelocityInchesPerSecond() { - return getElevatorVelocity_RotsPerSecond() * ElevatorConstants.kRotationToInches; - } - - public ElevatorMode getElevatorMode() { - return elevatorMode; - } - - public void changeElevatorOffset(double value) { - elevatorOffset += value; - } - - public void setElevatorStowedMode() { - elevatorMode = ElevatorMode.STOWED; - } - - public void setElevatorStationMode() { - elevatorMode = ElevatorMode.STATION; - } - - public void setElevatorLevelTwoMode() { - elevatorMode = ElevatorMode.LEVEL_TWO; - } - - public void setElevatorLevelThreeMode() { - elevatorMode = ElevatorMode.LEVEL_THREE; - } - - public void setElevatorLevelFourMode() { - elevatorMode = ElevatorMode.LEVEL_FOUR; - } - - public void setElevatorAlgaeLow() { - elevatorMode = ElevatorMode.ALGAE_LOW; - } - - public void setElevatorAlgaeHigh() { - elevatorMode = ElevatorMode.ALGAE_HIGH; - } - - public void setElevatorBarge() { - elevatorMode = ElevatorMode.BARGE; - } - - public void setCoral() { - isCoral = true; - } - - public void setAlgae() { - isCoral = false; - } - - public boolean getIsCoral() { - return isCoral; - } - - public void setAligning(boolean flag) { - isAligning = flag; - } - - public void setZeroing(boolean flag) { - isZeroing = flag; - } - - public void resetAdjust() { - elevatorOffset = 0; - } - - public void setPID() { - - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kG = SmartDashboard.getNumber("Elevator kG", ElevatorConstants.kG); - slot0Configs.kS = SmartDashboard.getNumber("Elevator kS", ElevatorConstants.kS); - slot0Configs.kV = SmartDashboard.getNumber("Elevator kV", ElevatorConstants.kV); - slot0Configs.kA = SmartDashboard.getNumber("Elevator kA", ElevatorConstants.kA); - slot0Configs.kP = SmartDashboard.getNumber("Elevator kP", ElevatorConstants.kP); - slot0Configs.kI = SmartDashboard.getNumber("Elevator kI", ElevatorConstants.kI); - slot0Configs.kD = SmartDashboard.getNumber("Elevator kD", ElevatorConstants.kD); - - var motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = - ElevatorConstants.MM_CRUISE_VELCOCITY_UP; // Target cruise velocity of 80 rps - motionMagicConfigs.MotionMagicAcceleration = - ElevatorConstants.MM_ACCELERATION_UP; // Target acceleration of 160 rps/s (0.5 seconds) - // (not sure if needed - > ) motionMagicConfigs.MotionMagicJerk = 1600; // Target jerk of 1600 rps/s/s (0.1 - // seconds) - - elevator.getConfigurator().refresh(talonFXConfigs); - elevator.getConfigurator().apply(talonFXConfigs); - } -} diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java deleted file mode 100644 index 8376b16..0000000 --- a/src/main/java/frc/robot/subsystems/EndEffector.java +++ /dev/null @@ -1,244 +0,0 @@ -// 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.subsystems; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.drivers.PearadoxTalonFX; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.EndEffectorConstants; -import frc.robot.RobotContainer; -import frc.robot.subsystems.Arm.ArmMode; -import frc.robot.util.SmarterDashboard; - -public class EndEffector extends SubsystemBase { - - private static final EndEffector END_EFFECTOR = new EndEffector(); - - private static final LEDStrip leds = LEDStrip.getInstance(); - - private static final Arm ARM = Arm.getInstance(); - - public static EndEffector getInstance() { - return END_EFFECTOR; - } - - private PearadoxTalonFX endEffector; - private DigitalInput endSensor; - private Debouncer debouncer; - - private boolean rumbled = false; - private boolean isIntaking = false; - private boolean isCoral = true; // TODO: integrate with arm - private boolean isHoldingCoral = true; - private boolean isHoldingAlgae = false; - private boolean holdSpeed = false; - - private double lastRot = 0; - - private static enum EEMode { - INTAKEMODE, - OUTTAKEMODE - } - - public EndEffector() { - endEffector = new PearadoxTalonFX(EndEffectorConstants.END_EFFECTOR_ID, NeutralModeValue.Brake, 60, false); - endSensor = new DigitalInput(EndEffectorConstants.END_SENSOR_CHANNEL); - debouncer = new Debouncer(0.125, DebounceType.kRising); - - BaseStatusSignal.setUpdateFrequencyForAll( - ArmConstants.UPDATE_FREQ, - endEffector.getPosition(), - endEffector.getVelocity(), - endEffector.getDutyCycle(), - endEffector.getMotorVoltage(), - endEffector.getTorqueCurrent(), - endEffector.getSupplyCurrent(), - endEffector.getStatorCurrent()); - SmarterDashboard.putNumber("EE/EE Speed", isCoral ? -0.15 : 0.1); - - var slot0Configs = new Slot0Configs(); - slot0Configs.kP = 0.2; - slot0Configs.kI = 0; - slot0Configs.kD = 0; - - endEffector.getConfigurator().apply(slot0Configs); - } - - @Override - public void periodic() { - collectGamePiece(); - - SmarterDashboard.putBoolean("EE/Holding Coral", isHoldingCoral); - SmarterDashboard.putBoolean("EE/Holding Algae", isHoldingAlgae); - SmarterDashboard.putBoolean("EE/Has Coral", hasCoral()); - SmarterDashboard.putBoolean("EE/Holding Speed", holdSpeed); - - SmarterDashboard.putNumber( - "EE/Stator Current", endEffector.getStatorCurrent().getValueAsDouble()); - SmarterDashboard.putNumber( - "EE/Supply Current", endEffector.getSupplyCurrent().getValueAsDouble()); - SmarterDashboard.putNumber("EE/Volts", endEffector.getMotorVoltage().getValueAsDouble()); - SmarterDashboard.putNumber( - "EE/Angular Velocity", endEffector.getVelocity().getValueAsDouble()); - } - - // public void collectCoral() { - // if (DriverStation.isTeleop()) { - // if (RobotContainer.opController.getRightTriggerAxis() >= 0.25) { - // coralIn(); - // isHolding = false; - // } else if (RobotContainer.opController.getLeftTriggerAxis() >= 0.25) { - // coralOut(); - // isHolding = false; - // } else if (hasCoral() && isCoral) { - // stop(); - // } else if (!isHolding) { - // holdCoral(); - // } - // } - // } - public void collectGamePiece() { - if (DriverStation.isTeleop()) { - // if(isCoral) { - if (RobotContainer.driverController.getLeftBumperButton()) { - if (isCoral) { - coralIn(); - holdSpeed = false; - } else if (!isCoral) { - algaeIn(); - holdSpeed = true; - } - isHoldingCoral = false; - } else if (RobotContainer.driverController.getRightBumperButton()) { - if (isCoral) { - coralOut(); - } else if (!isCoral) { - algaeOut(); - } - - holdSpeed = false; - isHoldingCoral = false; - } else { - if (!holdSpeed) { - stop(); - } - if (isCoral) { - holdCoral(); - } - } - - // if (endEffector.getStatorCurrent().getValueAsDouble() > 35.0) { - // leds.setLEDsFlash(); - // leds.handleFlashing(); - // } else leds.stopFlashing(); - } - } - - // else if(!isCoral) { - // if(RobotContainer.opController.getRightTriggerAxis() >= 0.25) { - // algaeIn(); - // isHoldingAlgae = true; - // } - // else if(RobotContainer.opController.getLeftTriggerAxis() >= 0.25) { - // algaeOut(); - // isHoldingAlgae = false; - // } - // else if(isHoldingAlgae) { - // stopAlgae(); - // } - // } - // } - - // scores - public void coralIn() { - endEffector.set(EndEffectorConstants.PULL_SPEED); - setLastRot(); - } - - public void algaeIn() { - endEffector.set(EndEffectorConstants.ALGAE_PULL_SPEED); - } - - public void algaeOut() { - endEffector.set(EndEffectorConstants.ALGAE_PUSH_SPEED); - } - - // intakes - public void coralOut() { - if (ARM.getArmMode() == ArmMode.L2 || ARM.getArmMode() == ArmMode.Stowed) { - endEffector.set(0.1); - } else if (ARM.getArmMode() == ArmMode.L3) { - endEffector.set(0.3); - } else endEffector.set(EndEffectorConstants.PUSH_SPEED); - setLastRot(); - } - - public void holdCoral() { - // endEffector.set(SmartDashboard.getNumber("EE/EE Speed", isCoral ? -0.15 : 0.1)); - endEffector.set(EndEffectorConstants.HOLD_SPEED); - setLastRot(); - } - - public void holdAlgae() { - endEffector.set(EndEffectorConstants.ALGAE_PULL_SPEED); - } - - public void stopCoral() { - stop(); - isHoldingCoral = true; - } - - public void stopAlgae() { - stop(); - isHoldingAlgae = true; - } - - public void stop() { - endEffector.set(0); - } - - public void passiveCoral() { - PositionVoltage request = new PositionVoltage(0).withSlot(0); - - endEffector.setControl(request.withPosition( - endEffector.getPosition().getValueAsDouble() + (RobotContainer.arm.deltaArmAngle() / 360))); - } - - public boolean hasCoral() { - return debouncer.calculate(endEffector.getStatorCurrent().getValueAsDouble() > 30); - } - - public boolean getHolding() { - return isHoldingCoral; - } - - public void setHolding(boolean hold) { - isHoldingCoral = hold; - } - - public void setLastRot() { - lastRot = endEffector.getPosition().getValueAsDouble(); - } - - public void setCoral() { - isCoral = true; - } - - public void setAlgae() { - isCoral = false; - } - - public boolean isCoral() { - return isCoral; - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..270ac47 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,187 @@ +// 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.subsystems.arm; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.SimulationConstants; +import frc.robot.subsystems.elevator.MechVisualizer; +import frc.robot.util.RobotIdentity; +import frc.robot.util.SmarterDashboard; +import org.littletonrobotics.junction.Logger; + +public class Arm extends SubsystemBase { + // private InverseKinematics ik = new InverseKinematics(); + + private boolean isCoral = true; + private boolean isAligning = false; + private double lastAngle = 0.0; + private double armAdjust = Constants.IDENTITY == RobotIdentity.EVE ? -0.71 : 0.0; + + public enum ArmMode { + Intake, + L2, + L3, + L4, + Stowed, + LOLLICLIMB + } + + private static ArmMode armMode = ArmMode.Stowed; + + private ArmIO io; + private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + + public Arm(ArmIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Arm", inputs); + + MechVisualizer.getInstance().updateArmAngle(getArmAngleRadsToHorizontal()); + + SmarterDashboard.putNumber("Arm/Angle degrees", getArmAngleDegrees()); + SmarterDashboard.putNumber("Arm/Adjust", armAdjust); + SmarterDashboard.putString("Arm/Mode", armMode.toString()); + SmarterDashboard.putBoolean("Arm/IsCoral", isCoral); + SmarterDashboard.putNumber("Arm/DeltaAngle", deltaArmAngle()); + SmarterDashboard.putNumber("Arm/kG", 0.35 * Math.cos(-1 * Units.degreesToRadians(getArmAngleDegrees() - 96))); + + // setAligning(!RobotContainer.align.isAligned()); + } + + public void armHold() { + double setpoint = ArmConstants.ARM_STOWED_ROT + armAdjust; + + if (isCoral) { + if (armMode == ArmMode.Stowed) { + setpoint = ArmConstants.ARM_STOWED_ROT + armAdjust; + } else if (armMode == ArmMode.Intake) { + if (isAligning) { + setpoint = ArmConstants.ARM_STATION_BEHIND_CORAL + armAdjust; + + } else { + setpoint = ArmConstants.ARM_INTAKE_ROT + armAdjust; + } + } else if (armMode == ArmMode.L2) { + // setpoint = ik.getArmAngleRots(AlignConstants.REEF_ALIGN_TZ, AlignConstants.L2_HEIGHT) + armAdjust; + setpoint = ArmConstants.ARM_LEVEL_2_ROT + armAdjust; + } else if (armMode == ArmMode.L3) { + // setpoint = ik.getArmAngleRots(AlignConstants.REEF_ALIGN_TZ, AlignConstants.L3_HEIGHT) + armAdjust - + // 2.5; + setpoint = ArmConstants.ARM_LEVEL_3_ROT + armAdjust; + + if (Constants.currentMode == Constants.Mode.SIM) { + setpoint += 1; + } + if (Constants.IDENTITY == RobotIdentity.EVE) { + setpoint += 2; // 12 degrees + } + } else if (armMode == ArmMode.L4) { + // setpoint = ik.getArmAngleRots(AlignConstants.REEF_ALIGN_TZ, AlignConstants.L4_HEIGHT) + armAdjust; + setpoint = ArmConstants.ARM_LEVEL_4_ROT + armAdjust; + } + } else if (!isCoral) { + if (armMode == ArmMode.Stowed) { + setpoint = ArmConstants.ARM_LOLLIPOP + armAdjust; + } else if (armMode == ArmMode.L2) { + setpoint = ArmConstants.ARM_ALGAE_LOW + armAdjust; + } else if (armMode == ArmMode.L3) { + setpoint = ArmConstants.ARM_ALGAE_HIGH + armAdjust; + } else if (armMode == ArmMode.L4) { + setpoint = ArmConstants.ARM_BARGE + armAdjust; + } + } + + // Logger.recordOutput("Arm/Align Setpoint", RobotContainer.align.getArmAngleRots() + armAdjust); + + // pivot.setControl(motionMagicRequest.withPosition(setpoint)); + io.runPosition(-setpoint, getFeedforwardVolts()); + + // pivot.setControl(new PositionVoltage(-setpoint).withFeedForward(ArmConstants.kG * + // Math.cos(getArmAngleDegrees() - 96))); + SmarterDashboard.putNumber("Arm/Cur Setpoint", -setpoint); + } + + public double deltaArmAngle() { + var temp = lastAngle; + lastAngle = getArmAngleDegrees(); + return lastAngle - temp; + } + + public void setArmIntake() { + armMode = ArmMode.Intake; + } + + public void setArmL2() { + armMode = ArmMode.L2; + } + + public void setArmL3() { + armMode = ArmMode.L3; + } + + public void setArmL4() { + armMode = ArmMode.L4; + } + + public void setStowed() { + armMode = ArmMode.Stowed; + } + + public void setLollipopOrClimb() { + armMode = ArmMode.LOLLICLIMB; + } + + public void setCoral() { + isCoral = true; + } + + public void setAlgae() { + isCoral = false; + } + + public static ArmMode getArmMode() { + return armMode; + } + + public boolean getIsCoral() { + return isCoral; + } + + public void setAligning(boolean flag) { + isAligning = flag; + } + + public double getPivotPosition() { + return inputs.positionRots / ArmConstants.ARM_GEAR_RATIO; + } + + public double getArmAngleDegrees() { + return Units.rotationsToDegrees(getPivotPosition()); + } + + public double getArmAngleRadsToHorizontal() { + return Units.rotationsToRadians(inputs.positionRots / ArmConstants.ARM_GEAR_RATIO) + + SimulationConstants.STARTING_ANGLE; + } + + public double getFeedforwardVolts() { + return 0.35 * Math.cos(-1 * Units.degreesToRadians(getArmAngleDegrees() - 96)); + } + + public void armAdjust(double adjustBy) { + armAdjust += adjustBy; + } + + public void resetAdjust() { + armAdjust = 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java new file mode 100644 index 0000000..578b716 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.arm; + +import org.littletonrobotics.junction.AutoLog; + +public interface ArmIO { + @AutoLog + public static class ArmIOInputs { + public double positionRots = 0.0; + public double velocityRps = 0.0; + + public double appliedVolts = 0.0; + + public double statorCurrent = 0.0; + public double supplyCurrent = 0.0; + } + + public default void updateInputs(ArmIOInputsAutoLogged inputs) {} + + public default void runPosition(double setpoint, double feedforward) {} +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java new file mode 100644 index 0000000..d3f2874 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.arm; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.lib.drivers.PearadoxTalonFX; +import frc.robot.Constants.ArmConstants; + +public class ArmIOReal implements ArmIO { + private PearadoxTalonFX pivot; + private TalonFXConfiguration talonFXConfigs; + + public ArmIOReal() { + pivot = new PearadoxTalonFX( + ArmConstants.ARM_KRAKEN_ID, + NeutralModeValue.Brake, + ArmConstants.CURRENT_LIMIT, + ArmConstants.CURRENT_LIMIT, + false); + + BaseStatusSignal.setUpdateFrequencyForAll( + ArmConstants.UPDATE_FREQ, + pivot.getPosition(), + pivot.getVelocity(), + pivot.getDutyCycle(), + pivot.getMotorVoltage(), + pivot.getTorqueCurrent(), + pivot.getSupplyCurrent(), + pivot.getStatorCurrent()); + + pivot.optimizeBusUtilization(); + + talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigs.Voltage.PeakForwardVoltage = 4.5; + talonFXConfigs.Voltage.PeakReverseVoltage = -4.5; + + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kG = 0; // add enough Gravity Gain just before motor starts moving + slot0Configs.kS = 0.15; // Add x output to overcome static friction + slot0Configs.kV = 7.2; // A velocity target of 1 rps results in x output + slot0Configs.kA = 0; // An acceleration of 1 rps/s requires x output + slot0Configs.kP = 0.4; // A position error of x rotations results in 12 V output + slot0Configs.kI = 0; // no output for integrated error + slot0Configs.kD = 0; // A velocity error of 1 rps results in x output + slot0Configs.GravityType = GravityTypeValue.Arm_Cosine; + + pivot.getConfigurator().apply(talonFXConfigs); + } + + @Override + public void updateInputs(ArmIOInputsAutoLogged inputs) { + inputs.positionRots = pivot.getPosition().getValueAsDouble(); + inputs.velocityRps = pivot.getVelocity().getValueAsDouble(); + + inputs.appliedVolts = pivot.getMotorVoltage().getValueAsDouble(); + + inputs.statorCurrent = pivot.getStatorCurrent().getValueAsDouble(); + inputs.supplyCurrent = pivot.getSupplyCurrent().getValueAsDouble(); + } + + @Override + public void runPosition(double setpoint, double feedforward) { + pivot.setControl(new PositionVoltage(setpoint).withFeedForward(feedforward)); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java new file mode 100644 index 0000000..fdafdd9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -0,0 +1,104 @@ +package frc.robot.subsystems.arm; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.lib.drivers.PearadoxTalonFX; +import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.SimulationConstants; + +public class ArmIOSim implements ArmIO { + private SingleJointedArmSim armSim = new SingleJointedArmSim( + DCMotor.getKrakenX60(1), + ArmConstants.ARM_GEAR_RATIO, + SimulationConstants.ARM_MOI, + SimulationConstants.ARM_LENGTH, + SimulationConstants.MIN_ANGLE, + SimulationConstants.MAX_ANGLE, + SimulationConstants.SIMULATE_GRAVITY, + SimulationConstants.STARTING_ANGLE); + + private PearadoxTalonFX pivot; + private TalonFXConfiguration talonFXConfigs; + private TalonFXSimState pivotMotorSimState; + + public ArmIOSim() { + pivot = new PearadoxTalonFX( + ArmConstants.ARM_KRAKEN_ID, + NeutralModeValue.Brake, + ArmConstants.CURRENT_LIMIT, + ArmConstants.CURRENT_LIMIT, + false); + + BaseStatusSignal.setUpdateFrequencyForAll( + ArmConstants.UPDATE_FREQ, + pivot.getPosition(), + pivot.getVelocity(), + pivot.getDutyCycle(), + pivot.getMotorVoltage(), + pivot.getTorqueCurrent(), + pivot.getSupplyCurrent(), + pivot.getStatorCurrent()); + + pivot.optimizeBusUtilization(); + + talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigs.Voltage.PeakForwardVoltage = 4.5; + talonFXConfigs.Voltage.PeakReverseVoltage = -4.5; + + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kG = 0; // add enough Gravity Gain just before motor starts moving + slot0Configs.kS = 0.15; // Add x output to overcome static friction + slot0Configs.kV = 7.2; // A velocity target of 1 rps results in x output + slot0Configs.kA = 0; // An acceleration of 1 rps/s requires x output + slot0Configs.kP = 0.4; // A position error of x rotations results in 12 V output + slot0Configs.kI = 0; // no output for integrated error + slot0Configs.kD = 0; // A velocity error of 1 rps results in x output + slot0Configs.GravityType = GravityTypeValue.Arm_Cosine; + + pivot.getConfigurator().apply(talonFXConfigs); + + pivotMotorSimState = pivot.getSimState(); + } + + @Override + public void updateInputs(ArmIOInputsAutoLogged inputs) { + updateSim(); + + inputs.positionRots = pivot.getPosition().getValueAsDouble(); + inputs.velocityRps = pivot.getVelocity().getValueAsDouble(); + + inputs.appliedVolts = pivot.getMotorVoltage().getValueAsDouble(); + + inputs.statorCurrent = pivot.getStatorCurrent().getValueAsDouble(); + inputs.supplyCurrent = pivot.getSupplyCurrent().getValueAsDouble(); + } + + @Override + public void runPosition(double setpoint, double feedforward) { + pivot.setControl(new PositionVoltage(setpoint).withFeedForward(feedforward)); + } + + private void updateSim() { + pivotMotorSimState.setSupplyVoltage(12); + + armSim.setInputVoltage(pivotMotorSimState.getMotorVoltage()); + armSim.update(0.02); + + pivotMotorSimState.setRawRotorPosition( + (Units.radiansToRotations(armSim.getAngleRads() - SimulationConstants.STARTING_ANGLE)) + * ArmConstants.ARM_GEAR_RATIO); + + pivotMotorSimState.setRotorVelocity( + Units.radiansToRotations(armSim.getVelocityRadPerSec() * ArmConstants.ARM_GEAR_RATIO)); + + // MechVisualizer.getInstance().updateArmAngle(armSim.getAngleRads()); + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..fcda04d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,112 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ClimbConstants; +import frc.robot.subsystems.elevator.MechVisualizer; +import frc.robot.util.SmarterDashboard; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Climber extends SubsystemBase { + @AutoLogOutput + private double climberOffset = 0; + + @AutoLogOutput + private int climbState = 0; + + @AutoLogOutput + private String climbStateString = "unpowered"; + + private ClimberIO io; + private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); + + public Climber(ClimberIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Climber", inputs); + + MechVisualizer.getInstance().updateClimberRoll(getAngleRads()); + + updateClimbStateString(); + + SmarterDashboard.putNumber("Climber/State", climbState); + SmarterDashboard.putString("Climber/StateStr", climbStateString); + SmarterDashboard.putNumber("Climber/ClimberOffset", climberOffset); + } + + public void climberAdjustUp() { + climberOffset += 20; // in rotations + } + + public void climberAdjustDown() { + climberOffset -= 15; + } + + public void zeroClimber() { + io.zeroPosition(); + climberOffset = 0; + } + + public void stop() { + io.setSpeed(0); + } + + public void updateClimbStateString() { + if (climbState == 2) { + climbStateString = "Climbing!"; + } else if (climbState == 1) { + climbStateString = "Deployed (Vertical)"; + } else if (climbState == 0) { + climbStateString = "Retracted (Stowed)"; + } + } + + public void setClimbPosition() { + if (climbState == 2) { + climbClimber(); + } else if (climbState == 1) { + deployClimber(); + } else if (climbState == 0) { + retractClimber(); + } + } + + public void retractClimber() { + io.runPosition(-290 + climberOffset); + } + + public void climberDown() { + io.setSpeed(-0.8); + } + + public void climberUp() { + io.setSpeed(0.8); + } + + public void deployClimber() { + io.runPosition(0 + climberOffset); + } + + public void climbClimber() { + io.runPosition(-195 + climberOffset); + } + + public void incrementClimbState() { + climbState = Math.min(2, climbState + 1); + setClimbPosition(); + } + + public void decrementClimbState() { + climbState = Math.max(0, climbState - 1); + setClimbPosition(); + } + + public double getAngleRads() { + return Units.rotationsToRadians(inputs.positionRots / ClimbConstants.GEAR_RATIO) + ClimbConstants.ZERO_ANGLE; + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..db4b903 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs { + public double positionRots = 0.0; + public double velocityRps = 0.0; + public double appliedVolts = 0.0; + public double statorCurrent = 0.0; + public double supplyCurrent = 0.0; + } + + default void updateInputs(ClimberIOInputs inputs) {} + + default void runPosition(double setpoint) {} + + default void setSpeed(double speed) {} + + default void zeroPosition() {} +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java new file mode 100644 index 0000000..c82723c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.lib.drivers.PearadoxTalonFX; +import frc.robot.Constants.ClimbConstants; + +public class ClimberIOReal implements ClimberIO { + protected PearadoxTalonFX climbMotor; + + public ClimberIOReal() { + climbMotor = new PearadoxTalonFX(ClimbConstants.CLIMB_MOTOR_ID, NeutralModeValue.Brake, 50, false); + + TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); + + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kG = 0; + slot0Configs.kS = 0; + slot0Configs.kV = 0; + slot0Configs.kA = 0; + slot0Configs.kP = 0.25; + slot0Configs.kI = 0; + slot0Configs.kD = 0; + + climbMotor.getConfigurator().apply(slot0Configs); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + inputs.positionRots = climbMotor.getPosition().getValueAsDouble(); + inputs.velocityRps = climbMotor.getVelocity().getValueAsDouble(); + inputs.appliedVolts = climbMotor.getMotorVoltage().getValueAsDouble(); + inputs.statorCurrent = climbMotor.getStatorCurrent().getValueAsDouble(); + inputs.supplyCurrent = climbMotor.getSupplyCurrent().getValueAsDouble(); + } + + @Override + public void runPosition(double setpoint) { + climbMotor.setControl(new PositionVoltage(setpoint)); + } + + @Override + public void setSpeed(double speed) { + climbMotor.set(speed); + } + + @Override + public void zeroPosition() { + climbMotor.setPosition(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java new file mode 100644 index 0000000..4e907d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Constants.ClimbConstants; + +public class ClimberIOSim extends ClimberIOReal { + private SingleJointedArmSim climberSim = new SingleJointedArmSim( + DCMotor.getKrakenX60(1), + ClimbConstants.GEAR_RATIO, + ClimbConstants.MOI, + ClimbConstants.CLIMBER_LENGTH, + ClimbConstants.MIN_ANGLE, + ClimbConstants.MAX_ANGLE, + false, + ClimbConstants.STARTING_ANGLE); + + private TalonFXSimState climberSimState = climbMotor.getSimState(); + + @Override + public void updateInputs(ClimberIOInputs inputs) { + super.updateInputs(inputs); + + climberSimState.setSupplyVoltage(12); + + climberSim.setInputVoltage(climberSimState.getMotorVoltage()); + climberSim.update(0.02); + + climberSimState.setRawRotorPosition( + Units.radiansToRotations(climberSim.getAngleRads() - ClimbConstants.ZERO_ANGLE) + * ClimbConstants.GEAR_RATIO); + + climberSimState.setRotorVelocity( + Units.radiansToRotations(climberSim.getVelocityRadPerSec() * ClimbConstants.GEAR_RATIO)); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java new file mode 100644 index 0000000..66b8929 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -0,0 +1,409 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.pathplanner.lib.auto.AutoBuilder; +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.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.subsystems.vision.Vision; +import frc.robot.util.LocalADStarAK; +import frc.robot.util.SmarterDashboard; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Drive extends SubsystemBase implements Vision.VisionConsumer { + // Constants.TUNER_CONSTANTS doesn't include these constants, so they are declared locally + static final double ODOMETRY_FREQUENCY = + new CANBus(Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; + public static final double DRIVE_BASE_RADIUS = Math.max( + Math.max( + Math.hypot( + Constants.TUNER_CONSTANTS.FrontLeft.LocationX, + Constants.TUNER_CONSTANTS.FrontLeft.LocationY), + Math.hypot( + Constants.TUNER_CONSTANTS.FrontRight.LocationX, + Constants.TUNER_CONSTANTS.FrontRight.LocationY)), + Math.max( + Math.hypot( + Constants.TUNER_CONSTANTS.BackLeft.LocationX, Constants.TUNER_CONSTANTS.BackLeft.LocationY), + Math.hypot( + Constants.TUNER_CONSTANTS.BackRight.LocationX, + Constants.TUNER_CONSTANTS.BackRight.LocationY))); + + private double driveMultiplier = 1; + private double turnMultiplier = 1; + private boolean useMultiplier = false; + + // PathPlanner config constants + private static final double ROBOT_MASS_KG = Pounds.of(114 + 18).in(Kilograms); + private static final double ROBOT_MOI = 6.883; + private static final double WHEEL_COF = COTS.WHEELS.VEX_GRIP_V2.cof; + private static final RobotConfig PP_CONFIG = new RobotConfig( + ROBOT_MASS_KG, + ROBOT_MOI, + new ModuleConfig( + Constants.TUNER_CONSTANTS.FrontLeft.WheelRadius, + Constants.TUNER_CONSTANTS.kSpeedAt12Volts.in(MetersPerSecond), + WHEEL_COF, + DCMotor.getKrakenX60Foc(1).withReduction(Constants.TUNER_CONSTANTS.FrontLeft.DriveMotorGearRatio), + Constants.TUNER_CONSTANTS.FrontLeft.SlipCurrent, + 1), + getModuleTranslations()); + + public static final DriveTrainSimulationConfig mapleSimConfig = DriveTrainSimulationConfig.Default() + .withRobotMass(Kilograms.of(ROBOT_MASS_KG)) + .withCustomModuleTranslations(getModuleTranslations()) + .withGyro(COTS.ofPigeon2()) + .withBumperSize(Inches.of(30 + 3.25 * 2), Inches.of(28 + 3.25 * 2)) + .withSwerveModule(new SwerveModuleSimulationConfig( + DCMotor.getKrakenX60(1), + DCMotor.getFalcon500(1), + Constants.TUNER_CONSTANTS.FrontLeft.DriveMotorGearRatio, + Constants.TUNER_CONSTANTS.FrontLeft.SteerMotorGearRatio, + Volts.of(Constants.TUNER_CONSTANTS.FrontLeft.DriveFrictionVoltage), + Volts.of(Constants.TUNER_CONSTANTS.FrontLeft.SteerFrictionVoltage), + Inches.of(2), + KilogramSquareMeters.of(Constants.TUNER_CONSTANTS.FrontLeft.SteerInertia), + WHEEL_COF)); + + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + private final Alert gyroDisconnectedAlert = + new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + + private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = new Rotation2d(); + private final SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private final SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + private final Consumer resetSimulationPoseCallBack; + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO, + Consumer resetSimulationPoseCallBack) { + this.gyroIO = gyroIO; + this.resetSimulationPoseCallBack = resetSimulationPoseCallBack; + modules[0] = new Module(flModuleIO, 0, Constants.TUNER_CONSTANTS.FrontLeft); + modules[1] = new Module(frModuleIO, 1, Constants.TUNER_CONSTANTS.FrontRight); + modules[2] = new Module(blModuleIO, 2, Constants.TUNER_CONSTANTS.BackLeft); + modules[3] = new Module(brModuleIO, 3, Constants.TUNER_CONSTANTS.BackRight); + + // Usage reporting for swerve template + HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); + + // Start odometry thread + PhoenixOdometryThread.getInstance().start(); + + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( + this::getPose, + this::resetOdometry, + this::getChassisSpeeds, + this::runVelocity, + new PPHolonomicDriveController(new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + PP_CONFIG, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback((activePath) -> { + Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback((targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + // Configure SysId + sysId = new SysIdRoutine( + new SysIdRoutine.Config( + null, null, null, (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + odometryLock.unlock(); + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + + // Update gyro alert + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); + + SmarterDashboard.putBoolean("Drive/Slow Mode", useMultiplier); + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + speeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, Constants.TUNER_CONSTANTS.kSpeedAt12Volts); + + // Log unoptimized setpoints and setpoint speeds + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds); + + // Send setpoints to modules + for (int i = 0; i < 4; i++) { + modules[i].runSetpoint(setpointStates[i]); + } + + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + } + + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); + } + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will return to their + * normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void resetOdometry(Pose2d pose) { + resetSimulationPoseCallBack.accept(pose); + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** Adds a new timestamped vision measurement. */ + @Override + public void accept(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return Constants.TUNER_CONSTANTS.kSpeedAt12Volts.in(MetersPerSecond); + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d( + Constants.TUNER_CONSTANTS.FrontLeft.LocationX, Constants.TUNER_CONSTANTS.FrontLeft.LocationY), + new Translation2d( + Constants.TUNER_CONSTANTS.FrontRight.LocationX, Constants.TUNER_CONSTANTS.FrontRight.LocationY), + new Translation2d( + Constants.TUNER_CONSTANTS.BackLeft.LocationX, Constants.TUNER_CONSTANTS.BackLeft.LocationY), + new Translation2d( + Constants.TUNER_CONSTANTS.BackRight.LocationX, Constants.TUNER_CONSTANTS.BackRight.LocationY) + }; + } + + public void changeSpeedMultiplier() { + useMultiplier = !useMultiplier; + if (useMultiplier) { + driveMultiplier = 0.4; + turnMultiplier = 0.6; + } else { + driveMultiplier = 1; + turnMultiplier = 1; + } + } + + public double getDriveMultiplier() { + return driveMultiplier; + } + + public double getTurnMultiplier() { + return turnMultiplier; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 0000000..5dbbae0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,30 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..0714655 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,61 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants; +import java.util.Queue; + +/** IO implementation for Pigeon 2. */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2( + Constants.TUNER_CONSTANTS.DrivetrainConstants.Pigeon2Id, + Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); + yawVelocity.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOSim.java b/src/main/java/frc/robot/subsystems/drive/GyroIOSim.java new file mode 100644 index 0000000..8b12b65 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOSim.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import edu.wpi.first.math.util.Units; +import frc.robot.util.PhoenixUtil; +import org.ironmaple.simulation.drivesims.GyroSimulation; + +public class GyroIOSim implements GyroIO { + private final GyroSimulation gyroSimulation; + + public GyroIOSim(GyroSimulation gyroSimulation) { + this.gyroSimulation = gyroSimulation; + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = true; + inputs.yawPosition = gyroSimulation.getGyroReading(); + inputs.yawVelocityRadPerSec = Units.degreesToRadians( + gyroSimulation.getMeasuredAngularVelocity().in(RadiansPerSecond)); + + inputs.odometryYawTimestamps = PhoenixUtil.getSimulationOdometryTimeStamps(); + inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..19dab59 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,139 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import org.littletonrobotics.junction.Logger; + +public class Module { + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + private final SwerveModuleConstants constants; + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module( + ModuleIO io, + int index, + SwerveModuleConstants constants) { + this.io = io; + this.index = index; + this.constants = constants; + driveDisconnectedAlert = + new Alert("Disconnected drive motor on module " + Integer.toString(index) + ".", AlertType.kError); + turnDisconnectedAlert = + new Alert("Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + turnEncoderDisconnectedAlert = + new Alert("Disconnected turn encoder on module " + Integer.toString(index) + ".", AlertType.kError); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * constants.WheelRadius; + Rotation2d angle = inputs.odometryTurnPositions[i]; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + } + + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + public void runSetpoint(SwerveModuleState state) { + // Optimize velocity setpoint + state.optimize(getAngle()); + state.cosineScale(inputs.turnPosition); + + // Apply setpoints + io.setDriveVelocity(state.speedMetersPerSecond / constants.WheelRadius); + io.setTurnPosition(state.angle); + } + + /** Runs the module with the specified output while controlling to zero degrees. */ + public void runCharacterization(double output) { + io.setDriveOpenLoop(output); + io.setTurnPosition(new Rotation2d()); + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setDriveOpenLoop(0.0); + io.setTurnOpenLoop(0.0); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + return inputs.turnPosition; + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * constants.WheelRadius; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * constants.WheelRadius; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.drivePositionRad; + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return Units.radiansToRotations(inputs.driveVelocityRadPerSec); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java new file mode 100644 index 0000000..089cd28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -0,0 +1,55 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public boolean driveConnected = false; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; + + public boolean turnConnected = false; + public boolean turnEncoderConnected = false; + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} + + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} + + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} + + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java new file mode 100644 index 0000000..b14ddd5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -0,0 +1,137 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.util.PhoenixUtil; +import java.util.Arrays; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; + +/** + * Physics sim implementation of module IO. The sim models are configured using a set of module constants from Phoenix. + * Simulation is always based on voltage control. + */ +public class ModuleIOSim implements ModuleIO { + // Constants.TUNER_CONSTANTS doesn't support separate sim constants, so they are declared locally + private static final double DRIVE_KS = 0.03; + private static final double DRIVE_KV_ROT = + 0.91035; // Same units as Constants.TUNER_CONSTANTS: (volt * secs) / rotation + private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + + private final SwerveModuleSimulation moduleSimulation; + private final SimulatedMotorController.GenericMotorController driveMotor; + private final SimulatedMotorController.GenericMotorController turnMotor; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private final PIDController driveController; + private final PIDController turnController; + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + public ModuleIOSim(SwerveModuleSimulation moduleSimulation) { + this.moduleSimulation = moduleSimulation; + this.driveMotor = moduleSimulation + .useGenericMotorControllerForDrive() + .withCurrentLimit(Amps.of(Constants.TUNER_CONSTANTS.FrontLeft.SlipCurrent)); + this.turnMotor = moduleSimulation.useGenericControllerForSteer().withCurrentLimit(Amps.of(20)); + + this.driveController = new PIDController(0.05, 0.0, 0.0); + this.turnController = new PIDController(8.0, 0.0, 0.0); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = driveFFVolts + + driveController.calculate( + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond)); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate( + moduleSimulation.getSteerAbsoluteFacing().getRadians()); + } else { + turnController.reset(); + } + + // Update simulation state + driveMotor.requestVoltage(Volts.of(driveAppliedVolts)); + turnMotor.requestVoltage(Volts.of(turnAppliedVolts)); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = moduleSimulation.getDriveWheelFinalPosition().in(Radians); + inputs.driveVelocityRadPerSec = + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = + Math.abs(moduleSimulation.getDriveMotorStatorCurrent().in(Amps)); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnPosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnVelocityRadPerSec = + moduleSimulation.getSteerAbsoluteEncoderSpeed().in(RadiansPerSecond); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = + Math.abs(moduleSimulation.getSteerMotorStatorCurrent().in(Amps)); + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = PhoenixUtil.getSimulationOdometryTimeStamps(); + inputs.odometryDrivePositionsRad = Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositions()) + .mapToDouble(angle -> angle.in(Radians)) + .toArray(); + inputs.odometryTurnPositions = moduleSimulation.getCachedSteerAbsolutePositions(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = DRIVE_KS * Math.signum(velocityRadPerSec) + DRIVE_KV * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java new file mode 100644 index 0000000..71511a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -0,0 +1,250 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import java.util.Queue; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and CANcoder. + * Configured using a set of module constants from Phoenix. + * + *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. + */ +public class ModuleIOTalonFX implements ModuleIO { + private final SwerveModuleConstants constants; + + // Hardware objects + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = new VelocityTorqueCurrentFOC(0.0); + + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5); + + public ModuleIOTalonFX( + SwerveModuleConstants constants) { + this.constants = constants; + driveTalon = new TalonFX(constants.DriveMotorId, Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName); + turnTalon = new TalonFX(constants.SteerMotorId, Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName); + cancoder = new CANcoder(constants.EncoderId, Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName); + + // Configure drive motor + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new TalonFXConfiguration(); + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = constants.SteerMotorGains; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + turnConfig.Feedback.FeedbackSensorSource = switch (constants.FeedbackSource) { + case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; + case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; + case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; + default -> throw new RuntimeException( + "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers.");}; + turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicAcceleration = turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + turnConfig.MotorOutput.Inverted = constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + + // Configure CANCoder + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cancoder.getConfigurator().apply(cancoderConfig); + + // Create timestamp queue + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + // Create drive status signals + drivePosition = driveTalon.getPosition(); + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + + // Create turn status signals + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon.getPosition()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll(Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Refresh all signals + var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + // Update odometry inputs + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setTurnOpenLoop(double output) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + }); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); + case TorqueCurrentFOC -> positionTorqueCurrentRequest.withPosition(rotation.getRotations()); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java new file mode 100644 index 0000000..fe001ab --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -0,0 +1,163 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using a CANivore, the + * thread uses the "waitForAll" blocking method to enable more consistent sampling. This also allows Phoenix Pro users + * to benefit from lower latency between devices using CANivore time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0]; + private final List genericSignals = new ArrayList<>(); + private final List> phoenixQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private static boolean isCANFD = new CANBus(Constants.TUNER_CONSTANTS.DrivetrainConstants.CANBusName).isNetworkFD(); + private static PhoenixOdometryThread instance = null; + + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } + + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } + + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } + } + + /** Registers a Phoenix signal to be read from the thread. */ + public Queue registerSignal(StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; + System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); + newSignals[phoenixSignals.length] = signal; + phoenixSignals = newSignals; + phoenixQueues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + genericSignals.add(signal); + genericQueues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD && phoenixSignals.length > 0) { + BaseStatusSignal.waitForAll(2.0 / Drive.ODOMETRY_FREQUENCY, phoenixSignals); + } else { + // "waitForAll" does not support blocking on multiple signals with a bus + // that is not CAN FD, regardless of Pro licensing. No reasoning for this + // behavior is provided by the documentation. + Thread.sleep((long) (1000.0 / Drive.ODOMETRY_FREQUENCY)); + if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + + // Save new data to queues + Drive.odometryLock.lock(); + try { + // Sample timestamp is current FPGA time minus average CAN latency + // Default timestamps from Phoenix are NOT compatible with + // FPGA timestamps, this solution is imperfect but close + double timestamp = RobotController.getFPGATime() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : phoenixSignals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (phoenixSignals.length > 0) { + timestamp -= totalLatency / phoenixSignals.length; + } + + // Add new samples to queues + for (int i = 0; i < phoenixSignals.length; i++) { + phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble()); + } + for (int i = 0; i < genericSignals.size(); i++) { + genericQueues.get(i).offer(genericSignals.get(i).getAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..01de209 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,223 @@ +// 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.subsystems.elevator; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.util.RobotIdentity; +// import frc.robot.commands.InverseKinematics; +import frc.robot.util.SmarterDashboard; +import org.littletonrobotics.junction.Logger; + +public class Elevator extends SubsystemBase { + // private InverseKinematics ik = new InverseKinematics(); + + private double elevatorOffset = Constants.IDENTITY == RobotIdentity.EVE ? 0.6814 : 0.0; + + private boolean isCoral = true; + private boolean isAligning = false; + private boolean isZeroing = false; + + public static enum ElevatorMode { + STOWED, + STATION, + LEVEL_TWO, + LEVEL_THREE, + LEVEL_FOUR, + ALGAE_LOW, + ALGAE_HIGH, + BARGE, + } + + private ElevatorMode elevatorMode = ElevatorMode.STOWED; + + private ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + /** Creates a new Elevator. */ + public Elevator(ElevatorIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Elevator", inputs); + + MechVisualizer.getInstance().updateElevatorHeight(getElevatorPositionMeters()); + + SmarterDashboard.putNumber("Elevator/Offset", elevatorOffset); + SmarterDashboard.putString("Elevator Mode", elevatorMode.toString()); + SmarterDashboard.putBoolean("Elevator/IsCoral", isCoral); + SmarterDashboard.putNumber("Elevator/Position Inches", getElevatorPositionInches()); + SmarterDashboard.putNumber("Elevator/Velocity (in/sec)", getElevatorVelocityInchesPerSecond()); + SmarterDashboard.putNumber("Elevator/Position Rots", getElevatorPositionRots()); + SmarterDashboard.putNumber("Elevator/Velocity (rot/sec)", getElevatorVelocity_RotsPerSecond()); + } + + public void setElevatorPosition() { + double setpoint = ElevatorConstants.STOWED_ROT + elevatorOffset; + + if (elevatorMode == ElevatorMode.STOWED) { + setpoint = ElevatorConstants.STOWED_ROT + elevatorOffset; + } else if (elevatorMode == ElevatorMode.STATION) { + if (isAligning) { + setpoint = ElevatorConstants.OBSTRUCTED_STATION_ROT + elevatorOffset; + } else { + setpoint = ElevatorConstants.STATION_ROT + elevatorOffset; + } + if (Constants.IDENTITY == RobotIdentity.EVE) { + double eveAdjustInches = -2; + setpoint += + eveAdjustInches * ElevatorConstants.GEAR_RATIO / (Math.PI * ElevatorConstants.PULLEY_DIAMETER); + } + } + if (isCoral) { + if (elevatorMode == ElevatorMode.LEVEL_TWO) { + // setpoint = ik.getElevatorHeightRots(AlignConstants.REEF_ALIGN_TZ, AlignConstants.L2_HEIGHT) + // + elevatorOffset; + setpoint = ElevatorConstants.LEVEL_TWO_ROT + elevatorOffset; + + } else if (elevatorMode == ElevatorMode.LEVEL_THREE) { + // setpoint = + // Math.max(-1, ik.getElevatorHeightRots(AlignConstants.REEF_ALIGN_TZ, + // AlignConstants.L3_HEIGHT)) + // + elevatorOffset; + setpoint = ElevatorConstants.LEVEL_THREE_ROT + elevatorOffset; + + } else if (elevatorMode == ElevatorMode.LEVEL_FOUR) { + // if (DriverStation.isAutonomous()) { + // setpoint = ik.getElevatorHeightRots(AlignConstants.REEF_ALIGN_TZ, AlignConstants.L4_HEIGHT) + // + elevatorOffset + // - 0.4; + // } else { + // setpoint = ik.getElevatorHeightRots(AlignConstants.REEF_ALIGN_TZ, AlignConstants.L4_HEIGHT) + // + elevatorOffset + // + 0.6; + // } + + setpoint = ElevatorConstants.LEVEL_FOUR_ROT + elevatorOffset; + } + } else if (!isCoral) { + if (elevatorMode == ElevatorMode.LEVEL_TWO) { + setpoint = ElevatorConstants.ALGAE_LOW_ROT + elevatorOffset; + } else if (elevatorMode == ElevatorMode.LEVEL_THREE) { + setpoint = ElevatorConstants.ALGAE_HIGH_ROT + elevatorOffset; + } else if (elevatorMode == ElevatorMode.LEVEL_FOUR) { + setpoint = ElevatorConstants.BARGE_ROT + elevatorOffset; + } + } + + if (!isZeroing) { + io.reachGoal(setpoint); + } else { + homeElevator(); + } + + SmarterDashboard.putNumber("Elevator/Setpoint", setpoint); + // Logger.recordOutput("Elevator/Align Setpoint", RobotContainer.align.getElevatorHeightRots() + + // elevatorOffset); + } + + public void homeElevator() { + io.setSpeed(0); + } + + public void zeroElevator() { + SmarterDashboard.putNumber("Elevator/New Zero Diff", 0 - inputs.positionRots); + io.setPosition(0); + elevatorOffset = 0; + } + + public void stopElevator() { + io.setSpeed(0); + } + + public double getElevatorPositionRots() { + return inputs.positionRots; + } + + public double getElevatorVelocity_RotsPerSecond() { + return inputs.velocityRps; + } + + public double getElevatorPositionInches() { + return getElevatorPositionRots() * ElevatorConstants.kRotationToInches; + } + + public double getElevatorPositionMeters() { + return Units.inchesToMeters(getElevatorPositionInches()); + } + + public double getElevatorVelocityInchesPerSecond() { + return getElevatorVelocity_RotsPerSecond() * ElevatorConstants.kRotationToInches; + } + + public ElevatorMode getElevatorMode() { + return elevatorMode; + } + + public void changeElevatorOffset(double value) { + elevatorOffset += value; + } + + public void setElevatorStowedMode() { + elevatorMode = ElevatorMode.STOWED; + } + + public void setElevatorStationMode() { + elevatorMode = ElevatorMode.STATION; + } + + public void setElevatorLevelTwoMode() { + elevatorMode = ElevatorMode.LEVEL_TWO; + } + + public void setElevatorLevelThreeMode() { + elevatorMode = ElevatorMode.LEVEL_THREE; + } + + public void setElevatorLevelFourMode() { + elevatorMode = ElevatorMode.LEVEL_FOUR; + } + + public void setElevatorAlgaeLow() { + elevatorMode = ElevatorMode.ALGAE_LOW; + } + + public void setElevatorAlgaeHigh() { + elevatorMode = ElevatorMode.ALGAE_HIGH; + } + + public void setElevatorBarge() { + elevatorMode = ElevatorMode.BARGE; + } + + public void setCoral() { + isCoral = true; + } + + public void setAlgae() { + isCoral = false; + } + + public boolean getIsCoral() { + return isCoral; + } + + public void setAligning(boolean flag) { + isAligning = flag; + } + + public void setZeroing(boolean flag) { + isZeroing = flag; + } + + public void resetAdjust() { + elevatorOffset = 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..54bdbb4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + public static class ElevatorIOInputs { + public double positionRots = 0.0; + public double velocityRps = 0.0; + + public double appliedVolts = 0.0; + + public double statorCurrent = 0.0; + public double supplyCurrent = 0.0; + } + + public default void updateInputs(ElevatorIOInputs inputs) {} + + public default void reachGoal(double setpoint) {} + + public default void setSpeed(double speed) {} + + public default void setPosition(double position) {} +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java new file mode 100644 index 0000000..bf36d45 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -0,0 +1,98 @@ +package frc.robot.subsystems.elevator; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.signals.GravityTypeValue; +import frc.lib.drivers.PearadoxTalonFX; +import frc.robot.Constants.ElevatorConstants; + +public class ElevatorIOReal implements ElevatorIO { + private PearadoxTalonFX elevatorMotor; + private PearadoxTalonFX elevatorFollower; + private MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0); + private TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); + + public ElevatorIOReal() { + elevatorMotor = new PearadoxTalonFX( + ElevatorConstants.ELEVATOR_ID, + ElevatorConstants.MODE, + ElevatorConstants.CURRENT_LIMIT, + ElevatorConstants.IS_INVERTED); + + elevatorFollower = new PearadoxTalonFX( + ElevatorConstants.ELEVATOR_FOLLOWER_ID, + ElevatorConstants.MODE, + ElevatorConstants.CURRENT_LIMIT, + ElevatorConstants.IS_INVERTED); + + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kG = ElevatorConstants.kG; // add enough Gravity Gain just before motor starts moving + slot0Configs.kS = ElevatorConstants.kS; // Add 0.1 V output to overcome static friction + slot0Configs.kV = ElevatorConstants.kV; // A velocity target of 1 rps results in 0.1 V output + slot0Configs.kA = ElevatorConstants.kA; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kP = ElevatorConstants.kP; // A position error of 2.5 rotations results in 12 V output, prev 4.8 + slot0Configs.kI = ElevatorConstants.kI; // no output for integrated error + slot0Configs.kD = ElevatorConstants.kD; // A velocity error of 1 rps results in 0.1 V output + + var motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = + ElevatorConstants.MM_CRUISE_VELCOCITY_UP; // Target cruise velocity of 80 rps + motionMagicConfigs.MotionMagicAcceleration = + ElevatorConstants.MM_ACCELERATION_UP; // Target acceleration of 160 rps/s (0.5 seconds) + // (not sure if needed - > ) motionMagicConfigs.MotionMagicJerk = 1600; // Target jerk of 1600 rps/s/s (0.1 + // seconds) + + talonFXConfigs.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + talonFXConfigs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + 33 * ElevatorConstants.GEAR_RATIO / (Math.PI * ElevatorConstants.PULLEY_DIAMETER); + talonFXConfigs.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + talonFXConfigs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + talonFXConfigs.Slot0.GravityType = GravityTypeValue.Elevator_Static; + + elevatorMotor.getConfigurator().apply(talonFXConfigs); + + BaseStatusSignal.setUpdateFrequencyForAll( + ElevatorConstants.UPDATE_FREQ, + elevatorMotor.getPosition(), + elevatorMotor.getVelocity(), + elevatorMotor.getDutyCycle(), + elevatorMotor.getMotorVoltage(), + elevatorMotor.getTorqueCurrent(), + elevatorMotor.getSupplyCurrent(), + elevatorMotor.getStatorCurrent()); + + elevatorMotor.optimizeBusUtilization(); + + elevatorFollower.getConfigurator().apply(talonFXConfigs); + elevatorFollower.optimizeBusUtilization(); + elevatorFollower.setControl(new Follower(ElevatorConstants.ELEVATOR_ID, true)); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.positionRots = elevatorMotor.getPosition().getValueAsDouble(); + inputs.velocityRps = elevatorMotor.getVelocity().getValueAsDouble(); + + inputs.appliedVolts = elevatorMotor.getMotorVoltage().getValueAsDouble(); + + inputs.statorCurrent = elevatorMotor.getStatorCurrent().getValueAsDouble(); + inputs.supplyCurrent = elevatorMotor.getSupplyCurrent().getValueAsDouble(); + } + + @Override + public void reachGoal(double setpoint) { + elevatorMotor.setControl(motionMagicRequest.withPosition(setpoint)); + } + + @Override + public void setSpeed(double speed) { + elevatorMotor.set(speed); + } + + @Override + public void setPosition(double position) { + elevatorMotor.setPosition(position); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java new file mode 100644 index 0000000..7271fc8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -0,0 +1,118 @@ +package frc.robot.subsystems.elevator; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import frc.lib.drivers.PearadoxTalonFX; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.SimulationConstants; + +public class ElevatorIOSim implements ElevatorIO { + private final ElevatorSim elevatorSim = new ElevatorSim( + DCMotor.getKrakenX60(2), + ElevatorConstants.GEAR_RATIO, + SimulationConstants.CARRIAGE_MASS_KG, + SimulationConstants.DRUM_RADIUS_METERS, + SimulationConstants.MIN_HEIGHT, + SimulationConstants.MAX_HEIGHT, + SimulationConstants.SIMULATE_GRAVITY, + SimulationConstants.STARTING_HEIGHT); + + private PearadoxTalonFX elevatorMotor; + private TalonFXSimState elevatorMotorSimState; + private TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); + private MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0); + + public ElevatorIOSim() { + elevatorMotor = new PearadoxTalonFX( + ElevatorConstants.ELEVATOR_ID, + ElevatorConstants.MODE, + ElevatorConstants.CURRENT_LIMIT, + !ElevatorConstants.IS_INVERTED); + + elevatorMotorSimState = elevatorMotor.getSimState(); + + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kG = ElevatorConstants.kG; + slot0Configs.kS = ElevatorConstants.kS; + slot0Configs.kV = ElevatorConstants.kV; + slot0Configs.kA = ElevatorConstants.kA; + slot0Configs.kP = ElevatorConstants.kP; + slot0Configs.kI = ElevatorConstants.kI; + slot0Configs.kD = ElevatorConstants.kD; + + var motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = + ElevatorConstants.MM_CRUISE_VELCOCITY_UP; // Target cruise velocity of 80 rps + motionMagicConfigs.MotionMagicAcceleration = + ElevatorConstants.MM_ACCELERATION_UP; // Target acceleration of 160 rps/s (0.5 seconds) + // (not sure if needed - > ) motionMagicConfigs.MotionMagicJerk = 1600; // Target jerk of 1600 rps/s/s (0.1 + // seconds) + + talonFXConfigs.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + talonFXConfigs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + 33 * ElevatorConstants.GEAR_RATIO / (Math.PI * ElevatorConstants.PULLEY_DIAMETER); + talonFXConfigs.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + talonFXConfigs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + talonFXConfigs.Slot0.GravityType = GravityTypeValue.Elevator_Static; + + elevatorMotor.getConfigurator().apply(talonFXConfigs); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + updateSim(); + + inputs.positionRots = elevatorMotor.getPosition().getValueAsDouble(); + inputs.velocityRps = elevatorMotor.getVelocity().getValueAsDouble(); + + inputs.appliedVolts = elevatorMotor.getMotorVoltage().getValueAsDouble(); + + inputs.statorCurrent = elevatorMotor.getStatorCurrent().getValueAsDouble(); + inputs.supplyCurrent = elevatorMotor.getSupplyCurrent().getValueAsDouble(); + } + + @Override + public void reachGoal(double setpoint) { + elevatorMotor.setControl(motionMagicRequest.withPosition(setpoint)); + } + + @Override + public void setSpeed(double speed) { + elevatorMotor.set(speed); + } + + @Override + public void setPosition(double position) { + elevatorMotor.setPosition(position); + } + + private void updateSim() { + elevatorMotorSimState.setSupplyVoltage(12); + + elevatorSim.setInput(elevatorMotorSimState.getMotorVoltage()); + elevatorSim.update(0.02); + + elevatorMotorSimState.setRawRotorPosition(getMotorRotations(elevatorSim.getPositionMeters())); + + // angular velocity = linear velocity / radius + elevatorMotorSimState.setRotorVelocity( + ((elevatorSim.getVelocityMetersPerSecond() / SimulationConstants.DRUM_RADIUS_METERS) + // radians/sec to rotations/sec + / (2.0 * Math.PI)) + * ElevatorConstants.GEAR_RATIO); + + // MechVisualizer.getInstance().updateElevatorHeight(elevatorSim.getPositionMeters()); + } + + private static double getMotorRotations(double linearDisplacement) { + // angular displacement in radians = linear displacement / radius + return Units.radiansToRotations(linearDisplacement / SimulationConstants.DRUM_RADIUS_METERS) + // multiply by gear ratio + * ElevatorConstants.GEAR_RATIO; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/MechVisualizer.java b/src/main/java/frc/robot/subsystems/elevator/MechVisualizer.java new file mode 100644 index 0000000..d44541e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/MechVisualizer.java @@ -0,0 +1,107 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import frc.robot.Constants.AlignConstants; +import frc.robot.Constants.ClimbConstants; +import frc.robot.Constants.SimulationConstants; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; + +public class MechVisualizer { + private final double elevatorAngle = 90; // straight up + + private final LoggedMechanism2d mech2d = new LoggedMechanism2d(Units.inchesToMeters(45), Units.inchesToMeters(90)); + private final LoggedMechanismRoot2d elevatorRoot = mech2d.getRoot("Elevator Root", Units.inchesToMeters(22.5), 0); + private final LoggedMechanismLigament2d elevator2d = elevatorRoot.append( + new LoggedMechanismLigament2d("Elevator", SimulationConstants.MIN_HEIGHT, elevatorAngle)); + private LoggedMechanismLigament2d arm = elevator2d.append( + new LoggedMechanismLigament2d("Arm", Units.inchesToMeters(16.5), 0, 5, new Color8Bit(Color.kYellow))); + + private LoggedMechanismRoot2d climbRoot = mech2d.getRoot( + "Climb Root", + Units.inchesToMeters(22.5) + SimulationConstants.CLIMBER_CAD_ZERO_Y, + SimulationConstants.CLIMBER_CAD_ZERO_Z); + private LoggedMechanismLigament2d climber = climbRoot.append(new LoggedMechanismLigament2d( + "Climber", + ClimbConstants.CLIMBER_LENGTH, + ClimbConstants.STARTING_ANGLE, + 3, + new Color8Bit(Color.kLightSteelBlue))); + + // End effector segments (derived from CAD) + private LoggedMechanismLigament2d ee23a = arm.append(new LoggedMechanismLigament2d( + "EE23a", Units.inchesToMeters(5.6102), 180 - 109.295, 7, new Color8Bit(Color.kDarkSeaGreen))); + private LoggedMechanismLigament2d ee34a = ee23a.append(new LoggedMechanismLigament2d( + "EE34a", Units.inchesToMeters(2), -(180 - 163.914), 7, new Color8Bit(Color.kMediumSeaGreen))); + private LoggedMechanismLigament2d ee45a1 = ee34a.append(new LoggedMechanismLigament2d( + "EE45a1", Units.inchesToMeters(5.1181 / 2), -(180 - 151.395), 7, new Color8Bit(Color.kLightSeaGreen))); + private LoggedMechanismLigament2d ee45a2 = ee45a1.append(new LoggedMechanismLigament2d( + "EE45a2", Units.inchesToMeters(5.1181 / 2), 0, 7, new Color8Bit(Color.kPaleGreen))); + + private LoggedMechanismLigament2d ee23b = arm.append(new LoggedMechanismLigament2d( + "EE23b", Units.inchesToMeters(5.6102), 180 + 109.295, 7, new Color8Bit(Color.kDarkBlue))); + private LoggedMechanismLigament2d ee34b = ee23b.append(new LoggedMechanismLigament2d( + "EE34b", Units.inchesToMeters(2), (180 - 163.914), 7, new Color8Bit(Color.kRoyalBlue))); + private LoggedMechanismLigament2d ee45b1 = ee34b.append(new LoggedMechanismLigament2d( + "EE45b1", Units.inchesToMeters(5.1181 / 2), (180 - 151.395), 7, new Color8Bit(Color.kDeepSkyBlue))); + private LoggedMechanismLigament2d ee45b2 = ee45b1.append(new LoggedMechanismLigament2d( + "EE45b2", Units.inchesToMeters(5.1181 / 2), 0, 7, new Color8Bit(Color.kSkyBlue))); + + private LoggedMechanismLigament2d coral1a = ee45a1.append(new LoggedMechanismLigament2d( + "CORAL1a", SimulationConstants.CORAL_LENGTH / 2.0, 90, 8, new Color8Bit(Color.kPapayaWhip))); + private LoggedMechanismLigament2d coral2a = ee45a1.append(new LoggedMechanismLigament2d( + "CORAL2a", SimulationConstants.CORAL_LENGTH / 2.0, -90, 8, new Color8Bit(Color.kAntiqueWhite))); + + private double heightMeters = 0.0; + private double armAngleRads = 0.0; + private double climbAngRads = 0.0; + + private static final MechVisualizer instance = new MechVisualizer(); + + public static MechVisualizer getInstance() { + return instance; + } + + private MechVisualizer() {} + + public void periodic() { + SmartDashboard.putData("Elevator Sim", mech2d); + Logger.recordOutput("FieldSimulation/Mechanism Visualizer", mech2d); + + Logger.recordOutput("FieldSimulation/Components", new Transform3d[] { + new Transform3d(0, 0, heightMeters, new Rotation3d()), + new Transform3d( + 0, + 0, + SimulationConstants.ARM_CAD_ZERO_Z + heightMeters, + new Rotation3d(0, -armAngleRads + SimulationConstants.STARTING_ANGLE, 0)), + new Transform3d( + 0, + SimulationConstants.CLIMBER_CAD_ZERO_Y, + SimulationConstants.CLIMBER_CAD_ZERO_Z, + new Rotation3d(-climbAngRads + SimulationConstants.CLIMBER_CAD_ANG_OFFSET, 0, 0)) + }); + } + + public void updateElevatorHeight(double heightMeters) { + this.heightMeters = heightMeters; + elevator2d.setLength(heightMeters + AlignConstants.ELEVATOR_STARTING_HEIGHT); + } + + public void updateArmAngle(double angleRads) { + this.armAngleRads = angleRads; + arm.setAngle(Units.radiansToDegrees(angleRads) - elevatorAngle); + } + + public void updateClimberRoll(double angleRads) { + this.climbAngRads = angleRads; + climber.setAngle(180 - Units.radiansToDegrees(climbAngRads)); + } +} diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffector.java new file mode 100644 index 0000000..7680c0d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffector.java @@ -0,0 +1,153 @@ +// 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.subsystems.endeffector; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.EndEffectorConstants; +import frc.robot.RobotContainer; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.Arm.ArmMode; +import frc.robot.util.SmarterDashboard; +import org.littletonrobotics.junction.Logger; + +public class EndEffector extends SubsystemBase { + private boolean isCoralMode = true; + private boolean isHoldingCoral = true; + private boolean isHoldingAlgae = false; + private boolean needsHoldSpeed = false; + + private EndEffectorIO io; + private final EndEffectorIOInputsAutoLogged inputs = new EndEffectorIOInputsAutoLogged(); + + public EndEffector(EndEffectorIO io) { + this.io = io; + + SmarterDashboard.putNumber("EE/EE Speed", isCoralMode ? -0.15 : 0.1); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + // inputs.hasCoral = true; // this was for testing autos on eve + + Logger.processInputs("EE", inputs); + + collectGamePiece(); + + SmarterDashboard.putBoolean("EE/Holding Coral", isHoldingCoral); + SmarterDashboard.putBoolean("EE/Holding Algae", isHoldingAlgae); + SmarterDashboard.putBoolean("EE/Has Coral", hasCoral()); + SmarterDashboard.putBoolean("EE/Holding Speed", needsHoldSpeed); + } + + public void collectGamePiece() { + if (DriverStation.isAutonomous()) return; + + boolean isIntaking = RobotContainer.driverController.getLeftBumperButton(); + boolean isOuttaking = RobotContainer.driverController.getRightBumperButton(); + + if (isIntaking) { + if (isCoralMode) { + intakeCoral(); + needsHoldSpeed = false; + } else { + intakeAlgae(); + needsHoldSpeed = true; + } + isHoldingCoral = false; + return; + } + + if (isOuttaking) { + if (isCoralMode) { + outtakeCoral(); + } else { + outtakeAlgae(); + } + needsHoldSpeed = false; + isHoldingCoral = false; + return; + } + + // Idle behavior + if (!needsHoldSpeed) stopEE(); + if (isCoralMode) holdCoral(); + } + + public void intakeCoral() { + io.setSpeed(EndEffectorConstants.PULL_SPEED); + setLastRot(); + } + + public void intakeAlgae() { + io.setSpeed(EndEffectorConstants.ALGAE_PULL_SPEED); + } + + public void outtakeAlgae() { + io.setSpeed(EndEffectorConstants.ALGAE_PUSH_SPEED); + } + + public void outtakeCoral() { + if (Arm.getArmMode() == ArmMode.L2 || Arm.getArmMode() == ArmMode.Stowed) { + io.setSpeed(EndEffectorConstants.L2_PUSH_SPEED); + } else if (Arm.getArmMode() == ArmMode.L3) { + io.setSpeed(EndEffectorConstants.L3_PUSH_SPEED); + } else { + io.setSpeed(EndEffectorConstants.PUSH_SPEED); + } + setLastRot(); + } + + public void holdCoral() { + // endEffector.set(SmartDashboard.getNumber("EE/EE Speed", isCoral ? -0.15 : 0.1)); + io.setSpeed(EndEffectorConstants.HOLD_SPEED); + setLastRot(); + } + + public void holdAlgae() { + io.setSpeed(EndEffectorConstants.ALGAE_PULL_SPEED); + } + + public void stopCoral() { + stopEE(); + isHoldingCoral = true; + } + + public void stopAlgae() { + stopEE(); + isHoldingAlgae = true; + } + + public void stopEE() { + io.setSpeed(0); + } + + public boolean hasCoral() { + return inputs.hasCoral; + } + + public boolean getHolding() { + return isHoldingCoral; + } + + public void setHolding(boolean hold) { + isHoldingCoral = hold; + } + + public void setLastRot() {} + + public void setCoralMode() { + isCoralMode = true; + } + + public void setAlgaeMode() { + isCoralMode = false; + } + + public boolean getIsCoralMode() { + return isCoralMode; + } +} diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIO.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIO.java new file mode 100644 index 0000000..8be9e3e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.endeffector; + +import org.littletonrobotics.junction.AutoLog; + +public interface EndEffectorIO { + @AutoLog + public static class EndEffectorIOInputs { + public double positionRots = 0.0; + public double velocityRps = 0.0; + public double appliedVolts = 0.0; + public double statorCurrent = 0.0; + public double supplyCurrent = 0.0; + + public boolean hasCoral = false; + } + + public default void updateInputs(EndEffectorIOInputs inputs) {} + + public default void setSpeed(double speed) {} +} diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOReal.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOReal.java new file mode 100644 index 0000000..58bdce3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOReal.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.endeffector; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import frc.lib.drivers.PearadoxTalonFX; +import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.EndEffectorConstants; + +public class EndEffectorIOReal implements EndEffectorIO { + private PearadoxTalonFX endEffector; + + private Debouncer debouncer; + + public EndEffectorIOReal() { + endEffector = new PearadoxTalonFX(EndEffectorConstants.END_EFFECTOR_ID, NeutralModeValue.Brake, 60, false); + + BaseStatusSignal.setUpdateFrequencyForAll( + ArmConstants.UPDATE_FREQ, + endEffector.getPosition(), + endEffector.getVelocity(), + endEffector.getDutyCycle(), + endEffector.getMotorVoltage(), + endEffector.getTorqueCurrent(), + endEffector.getSupplyCurrent(), + endEffector.getStatorCurrent()); + + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.2; + slot0Configs.kI = 0; + slot0Configs.kD = 0; + + endEffector.getConfigurator().apply(slot0Configs); + + debouncer = new Debouncer(0.125, DebounceType.kRising); + } + + @Override + public void updateInputs(EndEffectorIOInputs inputs) { + inputs.positionRots = endEffector.getPosition().getValueAsDouble(); + inputs.velocityRps = endEffector.getVelocity().getValueAsDouble(); + + inputs.appliedVolts = endEffector.getMotorVoltage().getValueAsDouble(); + + inputs.statorCurrent = endEffector.getStatorCurrent().getValueAsDouble(); + inputs.supplyCurrent = endEffector.getSupplyCurrent().getValueAsDouble(); + + inputs.hasCoral = debouncer.calculate(inputs.statorCurrent > EndEffectorConstants.CORAL_CURRENT); + } + + @Override + public void setSpeed(double speed) { + endEffector.set(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOSim.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOSim.java new file mode 100644 index 0000000..965f48d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorIOSim.java @@ -0,0 +1,335 @@ +package frc.robot.subsystems.endeffector; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import frc.lib.drivers.PearadoxTalonFX; +import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.EndEffectorConstants; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.SimulationConstants; +import frc.robot.util.simulation.AlgaeHandler; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly; +import org.littletonrobotics.junction.Logger; + +public class EndEffectorIOSim implements EndEffectorIO { + private PearadoxTalonFX endEffector; + private TalonFXSimState endEffectorSimState; + + // must be within 6 inches of the intake + private static final double TRANSLATIONAL_TOLERANCE_METERS = Units.inchesToMeters(16); + + private boolean hasCoral = true; + private boolean hasAlgae = false; + + // interpolates the game piece position from its original location to the robot + private Timer intakingTimer = new Timer(); + private static final double INTAKING_TIME = 0.5; + + // cooldown between shooting and rerunning the intake + private Timer shootingTimer = new Timer(); + private static final double SHOOTING_TIME = 1.25; + + // cooldown for dropping coral at the coral station + private Timer droppingTimer = new Timer(); + private static final double DROP_COOLDOWN = 1.5; + + private Pose3d intookGamePiecePrevPose = Pose3d.kZero; + + private Supplier poseSupplier; + private Supplier chassisSpeedsSupplier; + private DoubleSupplier elevatorHeightSupplier; + private DoubleSupplier armAngleSupplier; + + public EndEffectorIOSim( + Supplier poseSupplier, + Supplier chassisSpeedsSupplier, + DoubleSupplier elevatorHeightSupplier, + DoubleSupplier armAngleSupplier) { + + this.poseSupplier = poseSupplier; + this.chassisSpeedsSupplier = chassisSpeedsSupplier; + this.elevatorHeightSupplier = elevatorHeightSupplier; + this.armAngleSupplier = armAngleSupplier; + + endEffector = new PearadoxTalonFX(EndEffectorConstants.END_EFFECTOR_ID, NeutralModeValue.Brake, 60, false); + + BaseStatusSignal.setUpdateFrequencyForAll( + ArmConstants.UPDATE_FREQ, + endEffector.getPosition(), + endEffector.getVelocity(), + endEffector.getDutyCycle(), + endEffector.getMotorVoltage(), + endEffector.getTorqueCurrent(), + endEffector.getSupplyCurrent(), + endEffector.getStatorCurrent()); + + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.2; + slot0Configs.kI = 0; + slot0Configs.kD = 0; + + endEffector.getConfigurator().apply(slot0Configs); + + endEffectorSimState = endEffector.getSimState(); + } + + @Override + public void updateInputs(EndEffectorIOInputs inputs) { + endEffectorSimState.setSupplyVoltage(12); + + inputs.positionRots = endEffector.getPosition().getValueAsDouble(); + inputs.velocityRps = endEffector.getVelocity().getValueAsDouble(); + + inputs.appliedVolts = endEffector.getMotorVoltage().getValueAsDouble(); + + inputs.statorCurrent = endEffector.getStatorCurrent().getValueAsDouble(); + inputs.supplyCurrent = endEffector.getSupplyCurrent().getValueAsDouble(); + + inputs.hasCoral = this.hasCoral || this.hasAlgae; + + if (inputs.appliedVolts <= -0.5) { // rollers spinning clockwise + intakeCoralProjectiles(); + autoDropNearCS(); + shootAlgae(); + } else if (inputs.appliedVolts >= 0.5) { // rollers spinning counterclockwise + shootCoral(); + intakeAlgae(); + } + + visualizeHeldGamePiece(); + } + + @Override + public void setSpeed(double speed) { + endEffector.set(speed); + } + + private void intakeCoralProjectiles() { + // cooldown between shooting and restarting the intake + // so you don't immediately reintake what you just launched + if (!checkAndResetTimer(shootingTimer, SHOOTING_TIME)) return; + + if (hasCoral) return; // max 1 coral + + Pose3d intakePose = getHeldCoralPose(); // the end effector is the intake + + var iterator = SimulatedArena.getInstance().gamePieceLaunched().iterator(); + while (iterator.hasNext()) { + var gamePiece = iterator.next(); + if (gamePiece instanceof ReefscapeCoralOnFly) { + if (checkTolerance(intakePose.minus(gamePiece.getPose3d()))) { + iterator.remove(); + intookGamePiecePrevPose = gamePiece.getPose3d(); + hasCoral = true; + intakingTimer.restart(); + break; + } + } + } + } + + private static boolean checkTolerance(Transform3d difference) { + return difference.getTranslation().getNorm() < TRANSLATIONAL_TOLERANCE_METERS; + } + + private void visualizeHeldGamePiece() { + Logger.recordOutput( + "FieldSimulation/Held Coral", hasCoral ? interpolateHeldPose(getHeldCoralPose()) : Pose3d.kZero); + Logger.recordOutput( + "FieldSimulation/Held Algae", hasAlgae ? interpolateHeldPose(getHeldAlgaePose()) : Pose3d.kZero); + } + + private Pose3d interpolateHeldPose(Pose3d targetPose) { + if (intakingTimer.isRunning()) { + if (intakingTimer.get() > INTAKING_TIME) { + intakingTimer.stop(); + intakingTimer.reset(); + } else { + return intookGamePiecePrevPose.interpolate(targetPose, intakingTimer.get() / INTAKING_TIME); + } + } + return targetPose; + } + + private Transform3d getHeldCoralTransform() { + double armAngle = armAngleSupplier.getAsDouble(); + double elevatorHeight = elevatorHeightSupplier.getAsDouble(); + + Translation3d pivotToEE = new Translation3d(SimulationConstants.PIVOT_TO_MIDDLE_OF_CORAL_RADIUS, 0, 0) + .rotateBy(new Rotation3d(0, -armAngle - SimulationConstants.PIVOT_TO_MIDDLE_OF_CORAL_ANG_OFFSET, 0)) + .plus(new Translation3d( + 0, SimulationConstants.CORAL_Y_OFFSET, elevatorHeight + SimulationConstants.ARM_CAD_ZERO_Z)); + + Rotation3d rotation = + new Rotation3d(0, -Math.PI - armAngle - SimulationConstants.PIVOT_ANGLE_TO_CORAL_ANGLE, 0); + + return new Transform3d(pivotToEE, rotation); + } + + private Pose3d getHeldCoralPose() { + Pose3d robotPose = new Pose3d(poseSupplier.get()); + return robotPose.transformBy(getHeldCoralTransform()); + } + + private void shootCoral() { + if (!hasCoral) return; + + Transform3d eeTransform = getHeldCoralTransform(); + + SimulatedArena.getInstance() + .addGamePieceProjectile(new ReefscapeCoralOnFly( + // Obtain robot position from drive simulation + poseSupplier.get().getTranslation(), + // The scoring mechanism is installed at this position on the robot + eeTransform.getTranslation().toTranslation2d(), + // Obtain robot speed from drive simulation + chassisSpeedsSupplier.get(), + // Obtain robot facing from drive simulation + poseSupplier.get().getRotation(), + // The height at which the coral is ejected + eeTransform.getMeasureZ(), + // The initial speed of the coral + MetersPerSecond.of(-2), + // The coral is ejected at this angle + eeTransform.getRotation().getMeasureAngle().unaryMinus())); + + hasCoral = false; + shootingTimer.restart(); + } + + // Drops a coral projectile from the coral station + private void autoDropNearCS() { + // don't drop if the robot has a coral + if (hasCoral) return; + + // don't drop if the robot is moving + ChassisSpeeds speeds = chassisSpeedsSupplier.get(); + if (Math.abs(speeds.vxMetersPerSecond + speeds.vyMetersPerSecond + speeds.omegaRadiansPerSecond) > 0.1) { + return; + } + + // don't drop if the robot is too far from the station + Pose3d eePose = getHeldCoralPose(); + Pose2d nearestCS = eePose.toPose2d().nearest(FieldConstants.CORAL_STATIONS); + Transform2d diff = eePose.toPose2d().minus(nearestCS); + + if (diff.getTranslation().getNorm() > TRANSLATIONAL_TOLERANCE_METERS) return; + + // don't drop if it's been too soon since the last drop + if (!checkAndResetTimer(droppingTimer, DROP_COOLDOWN)) return; + + droppingTimer.restart(); + + SimulatedArena.getInstance() + .addGamePieceProjectile(new ReefscapeCoralOnFly( + // Coral Station Translation + nearestCS.getTranslation(), + // No additional offset + Translation2d.kZero, + // No additional velocity (the coral station isn't moving) + new ChassisSpeeds(), + // Coral station yaw + nearestCS.getRotation(), + // The height at which the coral is ejected + Meters.of(1), + // The initial speed of the coral + MetersPerSecond.of(1), + // The coral is ejected at this angle (the chute's pitch) + Degrees.of(-55))); + } + + private Transform3d getHeldAlgaeTransform() { + double armAngle = armAngleSupplier.getAsDouble(); + double elevatorHeight = elevatorHeightSupplier.getAsDouble(); + + Translation3d pivotToEE = new Translation3d(SimulationConstants.PIVOT_TO_MIDDLE_OF_CORAL_RADIUS, 0, 0) + .rotateBy(new Rotation3d(0, -armAngle, 0)) + .plus(new Translation3d(0, 0, elevatorHeight + SimulationConstants.ARM_CAD_ZERO_Z)); + + Rotation3d rotation = new Rotation3d(0, -armAngle, 0); + + return new Transform3d(pivotToEE, rotation); + } + + private Pose3d getHeldAlgaePose() { + Pose3d robotPose = new Pose3d(poseSupplier.get()); + return robotPose.transformBy(getHeldAlgaeTransform()); + } + + private void intakeAlgae() { + // cooldown between shooting and restarting the intake + // so you don't immediately reintake what you just launched + // if (shootingTimer.get() > SHOOTING_TIME) { + // shootingTimer.stop(); + // shootingTimer.reset(); + // disableIntake = false; + // } else if (shootingTimer.get() > 0) { + // disableIntake = true; + // } + + if (!checkAndResetTimer(shootingTimer, SHOOTING_TIME)) return; + + if (hasAlgae) return; // max 1 algae + + AlgaeHandler.getInstance().intake(getHeldAlgaePose()).ifPresent(algae -> { + intookGamePiecePrevPose = + new Pose3d(algae.getTranslation(), getHeldAlgaePose().getRotation()); + hasAlgae = true; + intakingTimer.restart(); + }); + } + + private void shootAlgae() { + if (!hasAlgae) return; + Transform3d eeTransform = getHeldAlgaeTransform(); + + SimulatedArena.getInstance() + .addGamePieceProjectile(new ReefscapeAlgaeOnFly( + // Obtain robot position from drive simulation + poseSupplier.get().getTranslation(), + // The scoring mechanism is installed at this position on the robot + eeTransform.getTranslation().toTranslation2d(), + // Obtain robot speed from drive simulation + chassisSpeedsSupplier.get(), + // Obtain robot facing from drive simulation + poseSupplier.get().getRotation(), + // The height at which the coral is ejected + eeTransform.getMeasureZ(), + // The initial speed of the coral + MetersPerSecond.of(2.910), + // The coral is ejected at this angle + eeTransform.getRotation().getMeasureAngle().unaryMinus())); + + hasAlgae = false; + shootingTimer.restart(); + } + + private static boolean checkAndResetTimer(Timer timer, double duration) { + if (timer.get() > duration) { + timer.stop(); + timer.reset(); + return true; + } else if (timer.get() > 0) { + return false; + } + return true; // Timer not started + } +} diff --git a/src/main/java/frc/robot/subsystems/LEDStrip.java b/src/main/java/frc/robot/subsystems/led/LEDStrip.java similarity index 91% rename from src/main/java/frc/robot/subsystems/LEDStrip.java rename to src/main/java/frc/robot/subsystems/led/LEDStrip.java index ad8cdcd..f79a501 100644 --- a/src/main/java/frc/robot/subsystems/LEDStrip.java +++ b/src/main/java/frc/robot/subsystems/led/LEDStrip.java @@ -1,8 +1,7 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.led; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -27,12 +26,15 @@ public LEDStrip() { led.setLength(ledBuffer.getLength()); led.setData(ledBuffer); led.start(); + + // can this be run periodically? + setGradient(Color.kGreen, Color.kGold); } public void periodic() { - if (DriverStation.isDisabled()) { - setGradient(Color.kGreen, Color.kGold); - } + // if (DriverStation.isDisabled()) { + // setGradient(Color.kLightGreen, Color.kLightYellow); + // } led.setData(ledBuffer); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..103346c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,179 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionIO.PoseObservationType; +import java.util.LinkedList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +public class Vision extends SubsystemBase { + private final VisionConsumer consumer; + private final VisionIO[] io; + private final VisionIOInputsAutoLogged[] inputs; + private final Alert[] disconnectedAlerts; + + public Vision(VisionConsumer consumer, VisionIO... io) { + this.consumer = consumer; + this.io = io; + + // Initialize inputs + this.inputs = new VisionIOInputsAutoLogged[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new VisionIOInputsAutoLogged(); + } + + // Initialize disconnected alerts + this.disconnectedAlerts = new Alert[io.length]; + for (int i = 0; i < inputs.length; i++) { + disconnectedAlerts[i] = + new Alert("Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + } + } + + /** + * Returns the X angle to the best target, which can be used for simple servoing with vision. + * + * @param cameraIndex The index of the camera to use. + */ + public Rotation2d getTargetX(int cameraIndex) { + return inputs[cameraIndex].latestTargetObservation.tx(); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + } + + // Initialize logging values + List allTagPoses = new LinkedList<>(); + List allRobotPoses = new LinkedList<>(); + List allRobotPosesAccepted = new LinkedList<>(); + List allRobotPosesRejected = new LinkedList<>(); + + // Loop over cameras + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { + // Update disconnected alert + disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + + // Initialize logging values + List tagPoses = new LinkedList<>(); + List robotPoses = new LinkedList<>(); + List robotPosesAccepted = new LinkedList<>(); + List robotPosesRejected = new LinkedList<>(); + + // Add tag poses + for (int tagId : inputs[cameraIndex].tagIds) { + var tagPose = aprilTagLayout.getTagPose(tagId); + if (tagPose.isPresent()) { + tagPoses.add(tagPose.get()); + } + } + + // Loop over pose observations + for (var observation : inputs[cameraIndex].poseObservations) { + // Check whether to reject pose + boolean rejectPose = observation.tagCount() == 0 // Must have at least one tag + || (observation.tagCount() == 1 + && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity + || Math.abs(observation.pose().getZ()) > maxZError // Must have realistic Z coordinate + + // Must be within the field boundaries + || observation.pose().getX() < 0.0 + || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + + // Add pose to log + robotPoses.add(observation.pose()); + if (rejectPose) { + robotPosesRejected.add(observation.pose()); + } else { + robotPosesAccepted.add(observation.pose()); + } + + // Skip if rejected + if (rejectPose) { + continue; + } + + // Calculate standard deviations + double stdDevFactor = Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double angularStdDev = angularStdDevBaseline * stdDevFactor; + if (observation.type() == PoseObservationType.MEGATAG_2) { + linearStdDev *= linearStdDevMegatag2Factor; + angularStdDev *= angularStdDevMegatag2Factor; + } + if (cameraIndex < cameraStdDevFactors.length) { + linearStdDev *= cameraStdDevFactors[cameraIndex]; + angularStdDev *= cameraStdDevFactors[cameraIndex]; + } + + // Send vision observation + consumer.accept( + observation.pose().toPose2d(), + observation.timestamp(), + VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); + } + + // Log camera datadata + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", + tagPoses.toArray(new Pose3d[tagPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", + robotPoses.toArray(new Pose3d[robotPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()])); + allTagPoses.addAll(tagPoses); + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + allRobotPosesRejected.addAll(robotPosesRejected); + } + + // Log summary data + Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); + Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[allRobotPoses.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesAccepted", + allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesRejected", + allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()])); + } + + @FunctionalInterface + public interface VisionConsumer { + void accept(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java new file mode 100644 index 0000000..22ed42f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -0,0 +1,54 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Transform3d; + +public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "limelight-back"; + public static String camera1Name = "limelight-front"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + // TODO: find robot to camera + public static Transform3d robotToCamera0 = + Transform3d.kZero; // new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = Transform3d.kZero; + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..749d095 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,49 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionIO { + @AutoLog + class VisionIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d()); + public PoseObservation[] poseObservations = new PoseObservation[0]; + public int[] tagIds = new int[0]; + } + + /** Represents the angle to a simple target, not used for pose estimation. */ + record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + /** Represents a robot pose sample used for pose estimation. */ + record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + PoseObservationType type) {} + + enum PoseObservationType { + MEGATAG_1, + MEGATAG_2, + PHOTONVISION, + QUESTNAV + } + + default void updateInputs(VisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java new file mode 100644 index 0000000..4372bff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -0,0 +1,150 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + +/** IO implementation for real Limelight hardware. */ +public class VisionIOLimelight implements VisionIO { + private final Supplier rotationSupplier; + private final DoubleArrayPublisher orientationPublisher; + + private final DoubleSubscriber latencySubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleArraySubscriber megatag1Subscriber; + private final DoubleArraySubscriber megatag2Subscriber; + + /** + * Creates a new VisionIOLimelight. + * + * @param name The configured name of the Limelight. + * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. + */ + public VisionIOLimelight(String name, Supplier rotationSupplier) { + var table = NetworkTableInstance.getDefault().getTable(name); + this.rotationSupplier = rotationSupplier; + orientationPublisher = + table.getDoubleArrayTopic("robot_orientation_set").publish(); + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); + tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + megatag2Subscriber = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + // Update connection status based on whether an update has been seen in the last 250ms + inputs.connected = ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + + // Update target observation + inputs.latestTargetObservation = new TargetObservation( + Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); + + // Update orientation for MegaTag 2 + orientationPublisher.accept(new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + NetworkTableInstance.getDefault().flush(); // Increases network traffic but recommended by Limelight + + // Read new pose observations from NetworkTables + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add(new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_1)); + } + for (var rawSample : megatag2Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add(new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, zeroed because the pose is already disambiguated + 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_2)); + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } + + /** Parses the 3D pose from a Limelight botpose array. */ + private static Pose3d parsePose(double[] rawLLArray) { + return new Pose3d( + rawLLArray[0], + rawLLArray[1], + rawLLArray[2], + new Rotation3d( + Units.degreesToRadians(rawLLArray[3]), + Units.degreesToRadians(rawLLArray[4]), + Units.degreesToRadians(rawLLArray[5]))); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java new file mode 100644 index 0000000..bcee749 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -0,0 +1,129 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import org.photonvision.PhotonCamera; + +/** IO implementation for real PhotonVision hardware. */ +public class VisionIOPhotonVision implements VisionIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + + /** + * Creates a new VisionIOPhotonVision. + * + * @param name The configured name of the camera. + * @param rotationSupplier The 3D position of the camera relative to the robot. + */ + public VisionIOPhotonVision(String name, Transform3d robotToCamera) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + inputs.connected = camera.isConnected(); + + // Read new camera observations + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var result : camera.getAllUnreadResults()) { + // Update latest target observation + if (result.hasTargets()) { + inputs.latestTargetObservation = new TargetObservation( + Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + } else { + inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d()); + } + + // Add pose observation + if (result.multitagResult.isPresent()) { // Multitag result + var multitagResult = result.multitagResult.get(); + + // Calculate robot pose + Transform3d fieldToCamera = multitagResult.estimatedPose.best; + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Calculate average tag distance + double totalTagDistance = 0.0; + for (var target : result.targets) { + totalTagDistance += + target.bestCameraToTarget.getTranslation().getNorm(); + } + + // Add tag IDs + tagIds.addAll(multitagResult.fiducialIDsUsed); + + // Add observation + poseObservations.add(new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + multitagResult.estimatedPose.ambiguity, // Ambiguity + multitagResult.fiducialIDsUsed.size(), // Tag count + totalTagDistance / result.targets.size(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + + // Calculate robot pose + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = new Transform3d( + tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Add tag ID + tagIds.add((short) target.fiducialId); + + // Add observation + poseObservations.add(new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } + } + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java new file mode 100644 index 0000000..680b743 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -0,0 +1,59 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** IO implementation for physics sim using PhotonVision simulator. */ +public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { + private static VisionSystemSim visionSim; + + private final Supplier poseSupplier; + private final PhotonCameraSim cameraSim; + + /** + * Creates a new VisionIOPhotonVisionSim. + * + * @param name The name of the camera. + * @param poseSupplier Supplier for the robot pose to use in simulation. + */ + public VisionIOPhotonVisionSim(String name, Transform3d robotToCamera, Supplier poseSupplier) { + super(name, robotToCamera); + this.poseSupplier = poseSupplier; + + // Initialize vision sim + if (visionSim == null) { + visionSim = new VisionSystemSim("main"); + visionSim.addAprilTags(aprilTagLayout); + } + + // Add sim camera + var cameraProperties = new SimCameraProperties(); + cameraSim = new PhotonCameraSim(camera, cameraProperties); + visionSim.addCamera(cameraSim, robotToCamera); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + visionSim.update(poseSupplier.get()); + super.updateInputs(inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOQuestNav.java b/src/main/java/frc/robot/subsystems/vision/VisionIOQuestNav.java new file mode 100644 index 0000000..b139af1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOQuestNav.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.vision; +// package frc.robot.subsystems.Vision; + +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Pose3d; +// import frc.robot.util.QuestNav; + +// public class VisionIOQuestNav implements VisionIO { +// @Override +// public void updateInputs(VisionIOInputs inputs) { +// QuestNav.periodic(); + +// if (inputs.connected = QuestNav.isConnected()) { +// Pose2d pose = QuestNav.getRobotPose(); + +// double displacement = Math.sqrt(Math.pow(pose.getX(), 2) + Math.pow(pose.getY(), 2)); + +// inputs.poseObservations = new PoseObservation[] { +// new PoseObservation( +// QuestNav.getTimestamp(), new Pose3d(pose), 0.0, 1, displacement, +// PoseObservationType.QUESTNAV) +// }; +// } +// } +// } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOQuestNavSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOQuestNavSim.java new file mode 100644 index 0000000..1cbbdd0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOQuestNavSim.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import java.util.function.Supplier; + +public class VisionIOQuestNavSim implements VisionIO { + Supplier poseSupplier; + Pose2d resetPose; + + public VisionIOQuestNavSim(Supplier poseSupplier) { + this.poseSupplier = poseSupplier; + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + inputs.connected = true; + Pose2d pose = updatePose(); + if (pose == null) { + inputs.poseObservations = new PoseObservation[0]; + } else { + inputs.poseObservations = new PoseObservation[] { + new PoseObservation( + System.currentTimeMillis() * 1e-6, new Pose3d(), 0.1, 1, 1, PoseObservationType.QUESTNAV) + }; + } + } + + // TODO + private Pose2d updatePose() { + return resetPose = poseSupplier.get(); + } +} diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java new file mode 100644 index 0000000..8eb24b2 --- /dev/null +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -0,0 +1,151 @@ +package frc.robot.util; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +// NOTE: This file is available at +// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be moved to the nearest + * non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved to the nearest + * non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents opposite corners + * of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start position of the path + * to properly avoid obstacles + */ + @Override + public void setDynamicObstacles(List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java new file mode 100644 index 0000000..29d0df4 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -0,0 +1,123 @@ +// 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 java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or value not in + * dashboard. + */ +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "/Tuning"; + private static final boolean tuningMode = true; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (tuningMode) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple objects. Recommended + * approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * objects. Recommended + * approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable numbers in order + * inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged(int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers) + .mapToDouble(LoggedTunableNumber::get) + .toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); + } +} diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java index 53e4027..c95d0f4 100644 --- a/src/main/java/frc/robot/util/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -1,4 +1,4 @@ -// Copyright 2021-2025 FRC 6328 +// Copyright 2021-2024 FRC 6328 // http://github.com/Mechanical-Advantage // // This program is free software; you can redistribute it and/or @@ -13,10 +13,28 @@ package frc.robot.util; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.motorsims.SimulatedBattery; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; -public class PhoenixUtil { +public final class PhoenixUtil { /** Attempts to run the command until no error is produced. */ public static void tryUntilOk(int maxAttempts, Supplier command) { for (int i = 0; i < maxAttempts; i++) { @@ -24,4 +42,140 @@ public static void tryUntilOk(int maxAttempts, Supplier command) { if (error.isOK()) break; } } + + /** Signals for synchronized refresh. */ + private static BaseStatusSignal[] canivoreSignals = new BaseStatusSignal[0]; + + private static BaseStatusSignal[] rioSignals = new BaseStatusSignal[0]; + + /** Registers a set of signals for synchronized refresh. */ + public static void registerSignals(boolean canivore, BaseStatusSignal... signals) { + if (canivore) { + BaseStatusSignal[] newSignals = new BaseStatusSignal[canivoreSignals.length + signals.length]; + System.arraycopy(canivoreSignals, 0, newSignals, 0, canivoreSignals.length); + System.arraycopy(signals, 0, newSignals, canivoreSignals.length, signals.length); + canivoreSignals = newSignals; + } else { + BaseStatusSignal[] newSignals = new BaseStatusSignal[rioSignals.length + signals.length]; + System.arraycopy(rioSignals, 0, newSignals, 0, rioSignals.length); + System.arraycopy(signals, 0, newSignals, rioSignals.length, signals.length); + rioSignals = newSignals; + } + } + + /** Refresh all registered signals. */ + public static void refreshAll() { + if (canivoreSignals.length > 0) { + BaseStatusSignal.refreshAll(canivoreSignals); + } + if (rioSignals.length > 0) { + BaseStatusSignal.refreshAll(rioSignals); + } + } + + public static class TalonFXMotorControllerSim implements SimulatedMotorController { + private static int instances = 0; + public final int id; + + private final TalonFXSimState talonFXSimState; + + public TalonFXMotorControllerSim(TalonFX talonFX) { + this.id = instances++; + + this.talonFXSimState = talonFX.getSimState(); + } + + @Override + public Voltage updateControlSignal( + Angle mechanismAngle, + AngularVelocity mechanismVelocity, + Angle encoderAngle, + AngularVelocity encoderVelocity) { + talonFXSimState.setRawRotorPosition(encoderAngle); + talonFXSimState.setRotorVelocity(encoderVelocity); + talonFXSimState.setSupplyVoltage(SimulatedBattery.getBatteryVoltage()); + return talonFXSimState.getMotorVoltageMeasure(); + } + } + + public static class TalonFXMotorControllerWithRemoteCancoderSim extends TalonFXMotorControllerSim { + private final CANcoderSimState remoteCancoderSimState; + + public TalonFXMotorControllerWithRemoteCancoderSim(TalonFX talonFX, CANcoder cancoder) { + super(talonFX); + this.remoteCancoderSimState = cancoder.getSimState(); + } + + @Override + public Voltage updateControlSignal( + Angle mechanismAngle, + AngularVelocity mechanismVelocity, + Angle encoderAngle, + AngularVelocity encoderVelocity) { + remoteCancoderSimState.setRawPosition(mechanismAngle); + remoteCancoderSimState.setVelocity(mechanismVelocity); + + return super.updateControlSignal(mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity); + } + } + + public static double[] getSimulationOdometryTimeStamps() { + final double[] odometryTimeStamps = new double[SimulatedArena.getSimulationSubTicksIn1Period()]; + for (int i = 0; i < odometryTimeStamps.length; i++) { + odometryTimeStamps[i] = Timer.getFPGATimestamp() + - 0.02 + + i * SimulatedArena.getSimulationDt().in(Seconds); + } + + return odometryTimeStamps; + } + + /** + * + * + *

Regulates the {@link SwerveModuleConstants} for a single module.

+ * + *

This method applies specific adjustments to the {@link SwerveModuleConstants} for simulation purposes. These + * changes have no effect on real robot operations and address known simulation bugs: + * + *

    + *
  • Inverted Drive Motors: Prevents drive PID issues caused by inverted configurations. + *
  • Non-zero CanCoder Offsets: Fixes potential module state optimization issues. + *
  • Steer Motor PID: Adjusts PID values tuned for real robots to improve simulation + * performance. + *
+ * + *

Note:This function is skipped when running on a real robot, ensuring no impact on constants used on real + * robot hardware.

+ */ + public static SwerveModuleConstants regulateModuleConstantForSimulation( + SwerveModuleConstants moduleConstants) { + // Skip regulation if running on a real robot + if (RobotBase.isReal()) return moduleConstants; + + // Apply simulation-specific adjustments to module constants + return moduleConstants + // Disable encoder offsets + .withEncoderOffset(0) + // Disable motor inversions for drive and steer motors + .withDriveMotorInverted(false) + .withSteerMotorInverted(false) + // Disable CanCoder inversion + .withEncoderInverted(false) + // Adjust steer motor PID gains for simulation + .withSteerMotorGains(new Slot0Configs() + .withKP(70) + .withKI(0) + .withKD(4.5) + .withKS(0) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)) + .withSteerMotorGearRatio(16.0) + // Adjust friction voltages + .withDriveFrictionVoltage(Volts.of(0.1)) + .withSteerFrictionVoltage(Volts.of(0.05)) + // Adjust steer inertia + .withSteerInertia(KilogramSquareMeters.of(0.05)); + } } diff --git a/src/main/java/frc/robot/util/RobotIdentity.java b/src/main/java/frc/robot/util/RobotIdentity.java new file mode 100644 index 0000000..5b76cf9 --- /dev/null +++ b/src/main/java/frc/robot/util/RobotIdentity.java @@ -0,0 +1,43 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.generated.EveTunerConstants; +import frc.robot.generated.PearracudaTunerConstants; +import frc.robot.generated.TunerConstants; + +public enum RobotIdentity { + PEARRACUDA(new PearracudaTunerConstants()), // Competition Bot (5414) + EVE(new EveTunerConstants()); // Practice Bot (9994) + + public final TunerConstants tunerConstants; + + private RobotIdentity(TunerConstants driveConstants) { + this.tunerConstants = driveConstants; + } + + public static RobotIdentity getRobotIdentity() { + String rioSerial = RobotController.getSerialNumber(); + + if (rioSerial.equals("032B4B61")) { + return EVE; + } + + return PEARRACUDA; + } + + public static String getRobotIdentityString() { + String rioSerial = RobotController.getSerialNumber(); + + if (rioSerial.equals("032B4B61")) { + return "EVE"; + } else if (rioSerial.equals("032B4B64")) { + return "PEARRACUDA"; + } else { + return "UNKNOWN"; + } + } + + public static String getRoboRioSerial() { + return RobotController.getSerialNumber(); + } +} diff --git a/src/main/java/frc/robot/util/simulation/AlgaeHandler.java b/src/main/java/frc/robot/util/simulation/AlgaeHandler.java new file mode 100644 index 0000000..6dbc04b --- /dev/null +++ b/src/main/java/frc/robot/util/simulation/AlgaeHandler.java @@ -0,0 +1,81 @@ +package frc.robot.util.simulation; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.Constants.FieldConstants; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; + +public class AlgaeHandler { + private final List stagedAlgae = new ArrayList<>(); + + private static AlgaeHandler INSTANCE; + + public static AlgaeHandler getInstance() { + if (INSTANCE == null) { + INSTANCE = new AlgaeHandler(); + } + return INSTANCE; + } + + private AlgaeHandler() { + reset(); + } + + public void reset() { + stagedAlgae.clear(); + stagedAlgae.addAll(List.of(FieldConstants.REEF_ALGAE_POSES)); + } + + public Pose3d[] periodic() { + simulateNet(); + return getPose3ds(); + } + + public Optional intake(Pose3d intakePose) { + var iterator = stagedAlgae.iterator(); + while (iterator.hasNext()) { + Pose3d algae = iterator.next(); + if (checkTolerance(intakePose.minus(algae))) { + iterator.remove(); + return Optional.of(algae); + } + } + return Optional.empty(); + } + + // collect launched algae projectiles + public void simulateNet() { + var iterator = SimulatedArena.getInstance().gamePieceLaunched().iterator(); + while (iterator.hasNext()) { + var gamePiece = iterator.next(); + if (gamePiece instanceof ReefscapeAlgaeOnFly) { + Pose3d algaePose = gamePiece.getPose3d(); + + if (gamePiece.getVelocity3dMPS().getZ() < 0 + && Math.abs(algaePose.getX() - FieldConstants.BARGE_X) < FieldConstants.BARGE_WIDTH + && Math.abs(algaePose.getZ() - FieldConstants.BARGE_HEIGHT) + < FieldConstants.BARGE_HEIGHT_TOLERANCE) { + stagedAlgae.add(new Pose3d( + FieldConstants.BARGE_X, + algaePose.getY() + gamePiece.getVelocity3dMPS().getY() / 2.0, + FieldConstants.BARGE_HEIGHT, + Rotation3d.kZero)); + iterator.remove(); + } + } + } + } + + public Pose3d[] getPose3ds() { + return stagedAlgae.toArray(Pose3d[]::new); + } + + private static boolean checkTolerance(Transform3d difference) { + return difference.getTranslation().getNorm() < FieldConstants.EE_TOLERANCE; + } +} diff --git a/src/main/java/frc/robot/util/vision/LimelightBackend.java b/src/main/java/frc/robot/util/vision/LimelightBackend.java deleted file mode 100644 index 26a16d2..0000000 --- a/src/main/java/frc/robot/util/vision/LimelightBackend.java +++ /dev/null @@ -1,120 +0,0 @@ -// 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.vision; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.*; -import frc.robot.util.SmarterDashboard; -import java.util.Arrays; -import java.util.Optional; - -public class LimelightBackend extends VisionBackend { - private String llName; - - private boolean megaTag2; - - private final DoubleArraySubscriber botPose; - private final DoubleArraySubscriber botPose2; - private final DoubleArraySubscriber hw; - private final DoubleSubscriber cl; - private final DoubleSubscriber tl; - private final DoubleSubscriber ta; - - public LimelightBackend(String llName, boolean megaTag2) { - this.llName = llName; - this.megaTag2 = megaTag2; - - botPose = NetworkTableInstance.getDefault() - .getTable(llName) - .getDoubleArrayTopic("botpose_wpiblue") - .subscribe(null); - botPose2 = NetworkTableInstance.getDefault() - .getTable(llName) - .getDoubleArrayTopic("botpose_orb_wpiblue") - .subscribe(null); - hw = NetworkTableInstance.getDefault() - .getTable(llName) - .getDoubleArrayTopic("hw") - .subscribe(null); - cl = NetworkTableInstance.getDefault() - .getTable(llName) - .getDoubleTopic("cl") - .subscribe(0); - tl = NetworkTableInstance.getDefault() - .getTable(llName) - .getDoubleTopic("tl") - .subscribe(0); - - ta = NetworkTableInstance.getDefault() - .getTable(llName) - .getDoubleTopic("ta") - .subscribe(0); - } - - @Override - public Optional getMeasurement() { - TimestampedDoubleArray[] updates; - if (megaTag2) { - updates = botPose.readQueue(); - } else { - updates = botPose2.readQueue(); - } - - if (updates.length == 0) { - return Optional.empty(); - } - - TimestampedDoubleArray update = updates[updates.length - 1]; - - if (Arrays.equals(update.value, new double[11])) { - return Optional.empty(); - } - - double x = update.value[0]; - double y = update.value[1]; - double z = update.value[2]; - double roll = Units.degreesToRadians(update.value[3]); - double pitch = Units.degreesToRadians(update.value[4]); - double yaw = Units.degreesToRadians(update.value[5]); - - double latency = cl.get() + tl.get(); - - double timestamp = (update.timestamp * 1e-6) - (latency * 1e-3); - Pose3d pose = new Pose3d(new Translation3d(x, y, z), new Rotation3d(roll, pitch, yaw)); - - return Optional.of(new Measurement(timestamp, pose, getStdDevs())); - } - - public boolean isValid() { - double[] botpose_orb_wpiblue = NetworkTableInstance.getDefault() - .getTable(llName) - .getEntry("botpose_orb_wpiblue") - .getDoubleArray(new double[11]); - - double tagCount = botpose_orb_wpiblue[7]; - double avgDist = botpose_orb_wpiblue[9]; - - return tagCount > 0 && avgDist < 4.5; - } - - public Vector getStdDevs() { - double xyStdDev = -0.002468 * Math.pow(ta.get(), 3) + 0.046 * Math.pow(ta.get(), 2) - 0.29 * (ta.get()) + 1; - if (Double.isNaN(xyStdDev)) xyStdDev = 0.1; // ta >= 7 - xyStdDev = Math.max(xyStdDev, 0.1); - xyStdDev = Math.min(xyStdDev, 0.9); - - double loggedTa = ta.get(); - - SmarterDashboard.putNumber("Vision/" + llName + "/StdDev", xyStdDev); - SmarterDashboard.putNumber("Vision/" + llName + "/ta", loggedTa); - return VecBuilder.fill(xyStdDev, xyStdDev, 0.999999); - } -} diff --git a/src/main/java/frc/robot/util/vision/LimelightHelpers.java b/src/main/java/frc/robot/util/vision/LimelightHelpers.java deleted file mode 100644 index 5b9bcd7..0000000 --- a/src/main/java/frc/robot/util/vision/LimelightHelpers.java +++ /dev/null @@ -1,1602 +0,0 @@ -// LimelightHelpers v1.10 (REQUIRES LLOS 2024.9.1 OR LATER) - -package frc.robot.util.vision; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -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.util.vision.LimelightHelpers.LimelightResults; -import frc.robot.util.vision.LimelightHelpers.PoseEstimate; -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.Map; -import java.util.concurrent.CompletableFuture; -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") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @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; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @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; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - /** 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") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() {} - } - - /** Represents a Neural Detector Pipeline Result extracted from JSON Output */ - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @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() {} - } - - /** Limelight Results object, parsed from a Limelight's JSON results output. */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public LimelightResults() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - } - } - - /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ - public static class RawFiducial { - 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) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - /** 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; - public double latency; - public int tagCount; - 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, - boolean isMegaTag2) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - this.isMegaTag2 = isMegaTag2; - } - } - - private static ObjectMapper mapper; - - /** Print JSON Parse time to the console in milliseconds */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; - } - - /** - * 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!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d( - Units.degreesToRadians(inData[3]), - Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - /** - * 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!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - /** - * 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; - } - return inData[position]; - } - - 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 = 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; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for (int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int) poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate( - pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); - } - - /** - * 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; - } - - /** - * 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; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static 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); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static 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; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// - - /** - * 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"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - 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); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * 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", false); - } - - /** - * 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", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired", false); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * 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); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - 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); - } - - /** - * 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]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - 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; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - if (flush) { - Flush(); - } - } - - /** - * 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++) { - validIDsDouble[i] = validIDs[i]; - } - 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; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** Asynchronously take snapshot. */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync(() -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * 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) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} diff --git a/src/main/java/frc/robot/util/vision/PoseEstimation.java b/src/main/java/frc/robot/util/vision/PoseEstimation.java deleted file mode 100644 index 443a54e..0000000 --- a/src/main/java/frc/robot/util/vision/PoseEstimation.java +++ /dev/null @@ -1,175 +0,0 @@ -// 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.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants.VisionConstants; -import frc.robot.Robot; -import frc.robot.RobotContainer; -import frc.robot.util.SmarterDashboard; - -public class PoseEstimation { - // private final SwerveDrivePoseEstimator poseEstimator; - - private final LimelightBackend[] backends; - private final boolean[] backendToggles; - - private final TimeInterpolatableBuffer poseHistory = TimeInterpolatableBuffer.createBuffer(2); - - private static final double DIFFERENTIATION_TIME = Robot.defaultPeriodSecs; - - private GenericEntry robotPoseEntry = Shuffleboard.getTab("Swerve") - .add("Robot Pose", new Pose2d().toString()) - .withSize(4, 1) - .withPosition(4, 0) - .getEntry(); - - private static final NetworkTable llTable = - NetworkTableInstance.getDefault().getTable(VisionConstants.LL_NAME); - - public PoseEstimation(/*SwerveDrivePoseEstimator poseEstimator*/ ) { - // this.poseEstimator = poseEstimator; - - backends = new LimelightBackend[2]; - backendToggles = new boolean[2]; - - backends[0] = new LimelightBackend(VisionConstants.LL_NAME, true); - backends[1] = new LimelightBackend(VisionConstants.LL_B_NAME, true); - backendToggles[0] = true; - backendToggles[1] = true; - } - - public void periodic(double angularSpeed) { - boolean rejectUpdate = false; - - // setRobotOrientation( - // VisionConstants.LL_NAME, - // poseEstimator.getEstimatedPosition().getRotation().getDegrees(), - // 0, - // 0, - // 0, - // 0, - // 0); - - if (Math.abs(angularSpeed) > 720) { - rejectUpdate = true; - } - - for (int i = 0; i < backends.length; i++) { - if (backendToggles[i]) { - // this is a hack to get around an issue in `SwerveDrivePoseEstimator` - // where two measurements cannot share the same timestamp - double timestampOffset = 1e-9 * i; - - if (backends[i].isValid() && !rejectUpdate) { - backends[i] - .getMeasurement() - .map((measurement) -> { - measurement.timestamp += timestampOffset; - return measurement; - }) - .ifPresent(this::addVisionMeasurement); - - backends[i] - .getMeasurement() - .map((measurement) -> { - measurement.timestamp += timestampOffset; - return measurement; - }) - .ifPresent(this::loggingPose); - } - } - } - - // QuestNav.periodic(); - // if (QuestNav.isConnected()) { - // addVisionMeasurement(QuestNav.getVisionMeasurement()); - // } - - double[] llStats = llTable.getEntry("hw").getDoubleArray(new double[4]); - - // note: docs are different from ip address dashboard - // fps, temp, cpu usage, ram usage; not fps, cpu temp, ram usage, temp - SmarterDashboard.putNumber("Limelight FPS", llStats[0], "Vision"); - SmarterDashboard.putNumber("Limelight Temp (C)", llStats[1], "Vision"); - SmarterDashboard.putNumber("Limelight CPU Usage", llStats[2], "Vision"); - SmarterDashboard.putNumber("Limelight Ram Usage", llStats[3], "Vision"); - - // robotPoseEntry.setString(getEstimatedPose().toString()); - // poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition()); - } - - // public void updateOdometry(Rotation2d gyro, SwerveModulePosition[] modulePositions) { - // poseEstimator.update(gyro, modulePositions); - // } - - // public void updateWithTime(double timestamp, Rotation2d gyro, SwerveModulePosition[] modulePositions) { - // poseEstimator.updateWithTime(timestamp, gyro, modulePositions); - // } - - // public Pose2d getEstimatedPose() { - // return poseEstimator.getEstimatedPosition(); - // } - - // public Translation2d getEstimatedVelocity() { - // double now = Timer.getFPGATimestamp(); - - // Translation2d current = - // poseHistory.getSample(now).orElseGet(Pose2d::new).getTranslation(); - // Translation2d previous = poseHistory - // .getSample(now - DIFFERENTIATION_TIME) - // .orElseGet(Pose2d::new) - // .getTranslation(); - - // return current.minus(previous).div(DIFFERENTIATION_TIME); - // } - - // public void resetPose(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) { - // poseEstimator.resetPosition(gyroAngle, modulePositions, pose); - // QuestNav.resetPose(pose); - // } - - private void addVisionMeasurement(VisionBackend.Measurement measurement) { - // poseEstimator.addVisionMeasurement( - // measurement.pose.toPose2d(), measurement.timestamp, measurement.stdDeviation); - loggingPose(measurement); - RobotContainer.drivetrain.addVisionMeasurement( - measurement.pose.toPose2d(), measurement.timestamp, measurement.stdDeviation); - } - - private void loggingPose(VisionBackend.Measurement measurement) { - SmartDashboard.putString("Vision Pose", measurement.pose.toPose2d().toString()); - // Logger.recordOutput("Drivetrain/Vision Pose", measurement.pose.toPose2d()); - } - - public void toggleBackends(int index) { - backendToggles[index] = !backendToggles[index]; - } - - // private void setRobotOrientation( - // String limelightName, - // double yaw, - // double yawRate, - // double pitch, - // double pitchRate, - // double roll, - // double rollRate) { - // double[] entries = new double[6]; - // entries[0] = yaw; - // entries[1] = yawRate; - // entries[2] = pitch; - // entries[3] = pitchRate; - // entries[4] = roll; - // entries[5] = rollRate; - - // llTable.getEntry("robot_orientation_set").setDoubleArray(entries); - // } -} diff --git a/src/main/java/frc/robot/util/vision/VisionBackend.java b/src/main/java/frc/robot/util/vision/VisionBackend.java deleted file mode 100644 index 4f8425a..0000000 --- a/src/main/java/frc/robot/util/vision/VisionBackend.java +++ /dev/null @@ -1,28 +0,0 @@ -// 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.vision; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import java.util.Optional; - -// From FRC Team 3636 Generals -public abstract class VisionBackend { - public abstract Optional getMeasurement(); - - public static class Measurement { - public double timestamp; - public Pose3d pose; - public Matrix stdDeviation; - - public Measurement(double timestamp, Pose3d pose, Matrix stdDeviation) { - this.timestamp = timestamp; - this.pose = pose; - this.stdDeviation = stdDeviation; - } - } -}