From d3e9bc1bcf96771886a8800ed74417e25b28044c Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla <49494444+BrownGenius@users.noreply.github.com> Date: Thu, 4 Apr 2024 09:26:22 -0400 Subject: [PATCH] Changes for District Comp (#102) * Arm Trapezoid Motion Profile * Fixed compilation error * Cleaned up Arm API. Removed redundant ArmIO input values * Cleaned up Arm Subsystem Interface * Fixed reported currentDraw in ArmIOStub * Updated camera location based on measurements * Add error to vision camera simulation * Add 2d based pose estimate * Adjust arm trapezoid profile constraints based on testing * Add vision pose estimates to drivetrain * Update gyro using vision * spotlessApply * "Drive to Amp" command (test) * Shooter Subsystem Cleanup (#104) * Added arm backlash compensation code (#103) * Added 3 degree backlash compensation * add side vision cameras (#106) * Add left/right vision cameras * Only run arm command when driver assist is actuating stick * Improved 4-Note (Center) v2.0 C231 * Added 4-Note (Center) v2.1 C231 M5 * Update TestShooteAngle command to only update subsystem if the shuffleboard value has changed * Fixed P to 8 Center Sweep Routine * Fixed "Drive to Amp" command * Updated "Drive to Amp" to correct yaw and decrease acceleration * Add support for a second intake sensor (#107) * Add support for a second intake sensor * Made changes to drive controls * Fixed drive to amp driver control * Do not control arm for prepare to score when in amp mode * Avoid using arm at all when aiming for amp * Split rightBumper to 4 separate triggers * Split ParallelCommands into separate scheduled commands * Set default scoring mode to vision-based * Recalibrate shooter camera location --------- Co-authored-by: joshuman * Update LedSystem.java changed color from hot pink to deep pink * Re-Measured left/right camera locations * Remove Arm Backlash Compensation * Updated getArmAngleFromDistance() to use 2nd-degree polynomial and latest distance/angle calibration data * Fixed typo * Practice Tweaks --------- Co-authored-by: Maciej Wiczynski Co-authored-by: joshuman Co-authored-by: Adi Prajapati <95832264+adipraj17@users.noreply.github.com> --- .../autos/4-Note (Center) v2.1 C231 M5.auto | 182 ++++++++++++++++++ src/main/deploy/pathplanner/paths/1 to 7.path | 8 +- src/main/deploy/pathplanner/paths/1 to C.path | 14 +- src/main/deploy/pathplanner/paths/2 to C.path | 6 +- src/main/deploy/pathplanner/paths/3 to 5.path | 8 +- .../pathplanner/paths/3 to Between 2-3.path | 8 +- src/main/deploy/pathplanner/paths/3 to C.path | 14 +- src/main/deploy/pathplanner/paths/5 to 3.path | 8 +- src/main/deploy/pathplanner/paths/7 to 1.path | 8 +- .../pathplanner/paths/Between 2-3 to 3.path | 8 +- src/main/deploy/pathplanner/paths/C to 1.path | 16 +- src/main/deploy/pathplanner/paths/C to 3.path | 22 +-- src/main/deploy/pathplanner/paths/C to 5.path | 68 +++++++ src/main/deploy/pathplanner/paths/P to 1.path | 10 +- .../paths/P to 8 Center Sweep.path | 4 +- .../frc/robot/commands/arm/ArmToPosition.java | 14 +- .../commands/auto/AutoNamedCommands.java | 14 +- .../commands/debug/TestShooterAngle.java | 27 ++- .../commands/shooter/SetShooterVelocity.java | 18 +- .../java/frc/robot/config/RobotConfig.java | 50 ++++- .../frc/robot/config/RobotConfigInferno.java | 78 ++++---- .../frc/robot/config/RobotConfigPhoenix.java | 11 +- .../frc/robot/config/RobotConfigSherman.java | 4 +- .../frc/robot/controls/DebugControls.java | 17 +- .../frc/robot/controls/DriverControls.java | 74 ++++--- .../java/frc/robot/controls/PitControls.java | 2 +- .../java/frc/robot/subsystems/arm/Arm.java | 42 +--- .../java/frc/robot/subsystems/arm/ArmIO.java | 13 +- .../robot/subsystems/arm/ArmIOSparkMax.java | 18 +- .../frc/robot/subsystems/arm/ArmIOStub.java | 12 +- .../robot/subsystems/arm/ArmSubsystem.java | 133 +++++++------ .../frc/robot/subsystems/drive/Drive.java | 6 + .../subsystems/drive/DriveSwerveYAGSL.java | 20 +- .../subsystems/intake/IntakeIOSparkMax.java | 6 +- .../frc/robot/subsystems/led/LedSystem.java | 6 +- .../frc/robot/subsystems/shooter/Shooter.java | 2 + .../subsystems/shooter/ShooterSubsystem.java | 37 ++-- .../frc/robot/subsystems/vision/Vision.java | 9 - .../subsystems/vision/VisionSubsystem.java | 62 ++++-- .../java/frc/robot/util/DevilBotState.java | 6 +- .../util/TrapezoidProfileSubsystem2876.java | 122 ++++++++++++ 41 files changed, 822 insertions(+), 365 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/4-Note (Center) v2.1 C231 M5.auto create mode 100644 src/main/deploy/pathplanner/paths/C to 5.path create mode 100644 src/main/java/frc/robot/util/TrapezoidProfileSubsystem2876.java diff --git a/src/main/deploy/pathplanner/autos/4-Note (Center) v2.1 C231 M5.auto b/src/main/deploy/pathplanner/autos/4-Note (Center) v2.1 C231 M5.auto new file mode 100644 index 00000000..3942d45b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4-Note (Center) v2.1 C231 M5.auto @@ -0,0 +1,182 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.35, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Score from C (Subwoofer Center)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C to 2" + } + }, + { + "type": "named", + "data": { + "name": "Intake Piece v2.0" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "2 to C" + } + }, + { + "type": "named", + "data": { + "name": "Prepare to Score from C (Subwoofer Center)" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score from C (Subwoofer Center)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C to 3" + } + }, + { + "type": "named", + "data": { + "name": "Intake Piece v2.0" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 to C" + } + }, + { + "type": "named", + "data": { + "name": "Prepare to Score from C (Subwoofer Center)" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score from C (Subwoofer Center)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C to 1" + } + }, + { + "type": "named", + "data": { + "name": "Intake Piece v2.0" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1 to C" + } + }, + { + "type": "named", + "data": { + "name": "Prepare to Score from C (Subwoofer Center)" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score from C (Subwoofer Center)" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C to 5" + } + }, + { + "type": "named", + "data": { + "name": "Intake Piece v2.0" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Turn off Shooter and Intake" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1 to 7.path b/src/main/deploy/pathplanner/paths/1 to 7.path index f5cb0901..aea5c966 100644 --- a/src/main/deploy/pathplanner/paths/1 to 7.path +++ b/src/main/deploy/pathplanner/paths/1 to 7.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.6, - "y": 3.9 + "x": 2.65, + "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 2.6, - "y": 2.9 + "x": 2.6500000000000004, + "y": 3.0 }, "isLocked": true, "linkedName": "\"1\" Wing Podium Note" diff --git a/src/main/deploy/pathplanner/paths/1 to C.path b/src/main/deploy/pathplanner/paths/1 to C.path index 28c1636b..201413c1 100644 --- a/src/main/deploy/pathplanner/paths/1 to C.path +++ b/src/main/deploy/pathplanner/paths/1 to C.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.6, - "y": 3.9 + "x": 2.65, + "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 2.246446609406726, - "y": 4.253553390593275 + "x": 2.2964466094067273, + "y": 4.353553390593277 }, "isLocked": true, "linkedName": "\"1\" Wing Podium Note" }, { "anchor": { - "x": 1.35, + "x": 1.75, "y": 5.55 }, "prevControl": { - "x": 1.7035533905932736, + "x": 2.103553390593273, "y": 5.196446609406726 }, "nextControl": null, "isLocked": true, - "linkedName": "\"C\" Subwoofer (Center Side)" + "linkedName": "\"C\" Subwoofer (Center Side) - No Sub Touch" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/2 to C.path b/src/main/deploy/pathplanner/paths/2 to C.path index 8fa5bd91..265f4b00 100644 --- a/src/main/deploy/pathplanner/paths/2 to C.path +++ b/src/main/deploy/pathplanner/paths/2 to C.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 1.35, + "x": 1.75, "y": 5.55 }, "prevControl": { - "x": 1.85, + "x": 2.25, "y": 5.55 }, "nextControl": null, "isLocked": true, - "linkedName": "\"C\" Subwoofer (Center Side)" + "linkedName": "\"C\" Subwoofer (Center Side) - No Sub Touch" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/3 to 5.path b/src/main/deploy/pathplanner/paths/3 to 5.path index 9bdb4cfd..74405bff 100644 --- a/src/main/deploy/pathplanner/paths/3 to 5.path +++ b/src/main/deploy/pathplanner/paths/3 to 5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.873688005440527, - "y": 5.777446393602466 + "x": 8.0, + "y": 5.75 }, "prevControl": { - "x": 6.967380218403877, - "y": 6.200064655343165 + "x": 7.09369221296335, + "y": 6.172618261740699 }, "nextControl": null, "isLocked": true, diff --git a/src/main/deploy/pathplanner/paths/3 to Between 2-3.path b/src/main/deploy/pathplanner/paths/3 to Between 2-3.path index f2b326c1..6fc3e6ee 100644 --- a/src/main/deploy/pathplanner/paths/3 to Between 2-3.path +++ b/src/main/deploy/pathplanner/paths/3 to Between 2-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.7, - "y": 6.8 + "x": 2.75, + "y": 6.75 }, "prevControl": null, "nextControl": { - "x": 2.1767288353915135, - "y": 6.669526014370095 + "x": 2.2267288353915133, + "y": 6.619526014370095 }, "isLocked": false, "linkedName": "\"3\" Wing Amp Note (45 degrees)" diff --git a/src/main/deploy/pathplanner/paths/3 to C.path b/src/main/deploy/pathplanner/paths/3 to C.path index 1183a422..38b144ce 100644 --- a/src/main/deploy/pathplanner/paths/3 to C.path +++ b/src/main/deploy/pathplanner/paths/3 to C.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.7, - "y": 6.8 + "x": 2.75, + "y": 6.75 }, "prevControl": null, "nextControl": { - "x": 2.3464466094067262, - "y": 6.446446609406726 + "x": 2.396446609406726, + "y": 6.396446609406726 }, "isLocked": true, "linkedName": "\"3\" Wing Amp Note (45 degrees)" }, { "anchor": { - "x": 1.35, + "x": 1.75, "y": 5.55 }, "prevControl": { - "x": 1.7035533905932738, + "x": 2.103553390593274, "y": 5.903553390593274 }, "nextControl": null, "isLocked": true, - "linkedName": "\"C\" Subwoofer (Center Side)" + "linkedName": "\"C\" Subwoofer (Center Side) - No Sub Touch" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/5 to 3.path b/src/main/deploy/pathplanner/paths/5 to 3.path index 5fcd65f3..5b3f736f 100644 --- a/src/main/deploy/pathplanner/paths/5 to 3.path +++ b/src/main/deploy/pathplanner/paths/5 to 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.873688005440527, - "y": 5.777446393602466 + "x": 8.0, + "y": 5.75 }, "prevControl": null, "nextControl": { - "x": 6.907762179151458, - "y": 6.036265438704987 + "x": 7.034074173710931, + "y": 6.008819045102522 }, "isLocked": true, "linkedName": "\"5\" Center Amp Mid Note" diff --git a/src/main/deploy/pathplanner/paths/7 to 1.path b/src/main/deploy/pathplanner/paths/7 to 1.path index f53f9dcc..d4c74558 100644 --- a/src/main/deploy/pathplanner/paths/7 to 1.path +++ b/src/main/deploy/pathplanner/paths/7 to 1.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 2.6, - "y": 3.9 + "x": 2.65, + "y": 4.0 }, "prevControl": { - "x": 2.6, - "y": 2.9 + "x": 2.6500000000000004, + "y": 3.0 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Between 2-3 to 3.path b/src/main/deploy/pathplanner/paths/Between 2-3 to 3.path index 15c00dc3..fce3de69 100644 --- a/src/main/deploy/pathplanner/paths/Between 2-3 to 3.path +++ b/src/main/deploy/pathplanner/paths/Between 2-3 to 3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.7, - "y": 6.8 + "x": 2.75, + "y": 6.75 }, "prevControl": { - "x": 2.3229504000992742, - "y": 6.5233044496623345 + "x": 2.372950400099274, + "y": 6.473304449662335 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C to 1.path b/src/main/deploy/pathplanner/paths/C to 1.path index a41a4233..da518821 100644 --- a/src/main/deploy/pathplanner/paths/C to 1.path +++ b/src/main/deploy/pathplanner/paths/C to 1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.35, + "x": 1.75, "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 1.4371557427476582, - "y": 4.5538053019082545 + "x": 1.75, + "y": 4.55 }, "isLocked": false, - "linkedName": "\"C\" Subwoofer (Center Side)" + "linkedName": "\"C\" Subwoofer (Center Side) - No Sub Touch" }, { "anchor": { - "x": 2.6, - "y": 3.9 + "x": 2.65, + "y": 4.0 }, "prevControl": { - "x": 1.104281392901949, - "y": 3.786748737840739 + "x": 1.65, + "y": 4.0 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C to 3.path b/src/main/deploy/pathplanner/paths/C to 3.path index 5fcb6384..f6c3006e 100644 --- a/src/main/deploy/pathplanner/paths/C to 3.path +++ b/src/main/deploy/pathplanner/paths/C to 3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.35, + "x": 1.75, "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 1.7035533905932738, - "y": 5.903553390593274 + "x": 2.12476659402887, + "y": 5.9247665940288705 }, - "isLocked": true, - "linkedName": "\"C\" Subwoofer (Center Side)" + "isLocked": false, + "linkedName": "\"C\" Subwoofer (Center Side) - No Sub Touch" }, { "anchor": { - "x": 2.7, - "y": 6.8 + "x": 2.75, + "y": 6.75 }, "prevControl": { - "x": 2.3464466094067262, - "y": 6.446446609406726 + "x": 2.396446609406726, + "y": 6.396446609406726 }, "nextControl": null, "isLocked": true, @@ -32,7 +32,7 @@ { "waypointRelativePos": 0.5, "rotationDegrees": 45.0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 44.999999999999964, + "rotation": 45.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C to 5.path b/src/main/deploy/pathplanner/paths/C to 5.path new file mode 100644 index 00000000..7ce670a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C to 5.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.75, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.4571067811865475, + "y": 6.257106781186548 + }, + "isLocked": false, + "linkedName": "\"C\" Subwoofer (Center Side) - No Sub Touch" + }, + { + "anchor": { + "x": 5.0, + "y": 6.5 + }, + "prevControl": { + "x": 4.0, + "y": 6.5 + }, + "nextControl": { + "x": 5.999999999999999, + "y": 6.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 5.75 + }, + "prevControl": { + "x": 7.55, + "y": 5.75 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "\"5\" Center Amp Mid Note" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P to 1.path b/src/main/deploy/pathplanner/paths/P to 1.path index f20da5dd..bca45248 100644 --- a/src/main/deploy/pathplanner/paths/P to 1.path +++ b/src/main/deploy/pathplanner/paths/P to 1.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 10.0, - "y": 2.0 + "x": 2.65, + "y": 4.0 }, "prevControl": { - "x": 9.0, - "y": 1.9999999999999998 + "x": 1.6500000000000004, + "y": 4.0 }, "nextControl": null, - "isLocked": true, + "isLocked": false, "linkedName": "\"1\" Wing Podium Note" } ], diff --git a/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path b/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path index 160e4686..b5c37383 100644 --- a/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path +++ b/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path @@ -25,10 +25,10 @@ }, "nextControl": { "x": 10.0, - "y": 3.0 + "y": 3.0000000000000004 }, "isLocked": false, - "linkedName": "\"1\" Wing Podium Note" + "linkedName": null }, { "anchor": { diff --git a/src/main/java/frc/robot/commands/arm/ArmToPosition.java b/src/main/java/frc/robot/commands/arm/ArmToPosition.java index cae7aed1..caf810ee 100644 --- a/src/main/java/frc/robot/commands/arm/ArmToPosition.java +++ b/src/main/java/frc/robot/commands/arm/ArmToPosition.java @@ -1,10 +1,8 @@ package frc.robot.commands.arm; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.config.RobotConfig.ArmConstants; import frc.robot.subsystems.arm.Arm; import java.util.function.DoubleSupplier; @@ -12,7 +10,6 @@ public class ArmToPosition extends Command { Arm arm; DoubleSupplier positionDegrees; double targetPositionDegrees; - Timer timer = new Timer(); public ArmToPosition(Arm arm, DoubleSupplier positionDegrees) { this.arm = arm; @@ -24,7 +21,6 @@ public ArmToPosition(Arm arm, DoubleSupplier positionDegrees) { @Override public void initialize() { targetPositionDegrees = positionDegrees.getAsDouble(); - timer.restart(); if (Constants.debugCommands) { System.out.println( @@ -39,19 +35,11 @@ public void execute() {} @Override public boolean isFinished() { - if (Math.abs(arm.getAngle() - targetPositionDegrees) <= ArmConstants.pidAngleErrorInDegrees) { - if (timer.get() >= ArmConstants.pidSettlingTimeInSeconds) { - return true; - } - } else { - timer.reset(); - } - return false; + return arm.isAtSetpoint(); } @Override public void end(boolean interrupted) { - // arm.runVoltage(0); if (interrupted) { System.err.println("INTERRUPTED: " + this.getClass().getSimpleName()); } diff --git a/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java b/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java index 8445807c..d137b249 100644 --- a/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java @@ -32,26 +32,26 @@ public static class AutoConstants { /* TODO: Fill in the values for scoring here */ public static AutoScoreConstants scoreFromASubwooferAmpSide = new AutoScoreConstants( - 0.0, ArmConstants.subwooferScoreAngleInDegrees, ShooterConstants.velocityInRPMs); + 0.0, ArmConstants.subwooferScoreAngleInDegrees, ShooterConstants.velocityInRPM); public static AutoScoreConstants scoreFromCSubwooferCenter = new AutoScoreConstants( - 0.0, ArmConstants.subwooferScoreAngleInDegrees, ShooterConstants.velocityInRPMs); + 0.0, ArmConstants.subwooferScoreAngleInDegrees, ShooterConstants.velocityInRPM); public static AutoScoreConstants scoreFromPSubwooferPodiumSide = new AutoScoreConstants( - 0.0, ArmConstants.subwooferScoreAngleInDegrees, ShooterConstants.velocityInRPMs); + 0.0, ArmConstants.subwooferScoreAngleInDegrees, ShooterConstants.velocityInRPM); public static AutoScoreConstants scoreFrom1WingPodiumNote = new AutoScoreConstants( - -28, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPMs); + -28, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPM); public static AutoScoreConstants scoreFrom2WingSpeakerNote = new AutoScoreConstants( - 0.0, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPMs); + 0.0, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPM); public static AutoScoreConstants scoreFrom3WingAmpNote = new AutoScoreConstants( - 28, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPMs); + 28, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPM); public static AutoScoreConstants scoreFromBetween2and3 = new AutoScoreConstants( - 21, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPMs); + 21, ArmConstants.noteScoreAngleInDegrees, ShooterConstants.velocityInRPM); } public static void configure() { diff --git a/src/main/java/frc/robot/commands/debug/TestShooterAngle.java b/src/main/java/frc/robot/commands/debug/TestShooterAngle.java index 9366cba1..cd2c2d08 100644 --- a/src/main/java/frc/robot/commands/debug/TestShooterAngle.java +++ b/src/main/java/frc/robot/commands/debug/TestShooterAngle.java @@ -15,6 +15,9 @@ public class TestShooterAngle extends Command { DoubleSupplier shooterVelocity; DoubleSupplier intakeVoltage; DoubleSupplier armAngle; + double currentShooterVelocity = -1; + double currentIntakeVoltage = -1; + double currentArmAngle = -1; public TestShooterAngle( Shooter shooter, @@ -35,11 +38,29 @@ public TestShooterAngle( addRequirements((Subsystem) arm); } + @Override + public void initialize() { + currentShooterVelocity = 0; + currentIntakeVoltage = 0; + currentArmAngle = 0; + } + @Override public void execute() { - shooter.runVelocity(shooterVelocity.getAsDouble()); - intake.runVoltage(intakeVoltage.getAsDouble()); - arm.setAngle(armAngle.getAsDouble()); + if (currentShooterVelocity != shooterVelocity.getAsDouble()) { + currentShooterVelocity = shooterVelocity.getAsDouble(); + shooter.runVelocity(shooterVelocity.getAsDouble()); + } + + if (currentIntakeVoltage != intakeVoltage.getAsDouble()) { + currentIntakeVoltage = intakeVoltage.getAsDouble(); + intake.runVoltage(currentIntakeVoltage); + } + + if (currentArmAngle != armAngle.getAsDouble()) { + currentArmAngle = armAngle.getAsDouble(); + arm.setAngle(currentArmAngle); + } } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java index 0f4f3a0b..e41b7c74 100644 --- a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java +++ b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java @@ -1,11 +1,8 @@ package frc.robot.commands.shooter; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.config.RobotConfig.ShooterConstants; import frc.robot.subsystems.shooter.Shooter; import java.util.function.DoubleSupplier; @@ -13,7 +10,6 @@ public class SetShooterVelocity extends Command { Shooter shooter; DoubleSupplier velocityRPM; double targetVelocityRPM; - Timer timer = new Timer(); public SetShooterVelocity(Shooter shooter, DoubleSupplier velocityRPM) { this.shooter = shooter; @@ -27,25 +23,13 @@ public void initialize() { System.out.println( "START: " + this.getClass().getSimpleName() + " velocity: " + velocityRPM.getAsDouble()); } - timer.restart(); targetVelocityRPM = velocityRPM.getAsDouble(); shooter.runVelocity(targetVelocityRPM); } @Override public boolean isFinished() { - if (Math.abs( - Units.radiansPerSecondToRotationsPerMinute(shooter.getCurrentSpeed()) - - targetVelocityRPM) - <= ShooterConstants.pidVelocityErrorInRPMS) { - if (timer.get() >= ShooterConstants.pidSettlingTimeInSeconds) { - return true; - } - } else { - timer.reset(); - } - - return false; + return shooter.isAtSetpoint(); } @Override diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 9ae0d817..d6a22c09 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -1,6 +1,7 @@ package frc.robot.config; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -79,8 +80,8 @@ public static class ArmConstants { public static double maxAngleInDegrees = 90.0; public static double minAngleInDegrees = 0.0; - public static double maxVelocityInDegreesPerSecond = 90; - public static double maxAccelerationInDegreesPerSecondSquared = 720; + public static double maxVelocityInDegreesPerSecond = 45; + public static double maxAccelerationInDegreesPerSecondSquared = 120; public static double intakeAngleInDegrees = 1; public static double ejectAngleInDegrees = 15; @@ -92,6 +93,17 @@ public static class ArmConstants { public static double matchStartArmAngle = 90; public static double defaultSpeedInVolts = 1.0; + + public static double maxBacklashDegrees = 0.0; + + // Arm Angle Calculations + // Polynomial: y = a*x^2 + b*x + c + // y = angle and x = distance + public static double minDistanceInMeters = 0; // min distance we can aim at + public static double maxDistanceInMeters = 3.0; // max distance we can aim at + public static double Ax2 = 0; + public static double Bx = (maxAngleInDegrees / 2) / maxDistanceInMeters; + public static double C = 0; } public static class ShooterConstants { @@ -104,7 +116,7 @@ public static class ShooterConstants { public static double ffKaBottom = 0.019246; /* PID */ - public static double pidVelocityErrorInRPMS = 300; + public static double pidVelocityErrorInRPM = 300; public static double pidSettlingTimeInSeconds = 0.1; public static double pidTimeoutInSeconds = 2.0; public static double pidKp = 0.043566; @@ -114,11 +126,11 @@ public static class ShooterConstants { public static double pidKiBottom = 0.0; public static double pidKdBottom = 0.0; - public static double velocityInRPMs = 3000; + public static double velocityInRPM = 3000; public static double defaultSpeedInVolts = 6.0; - public static double ampScoreVelocityInRPMs = 1000; - public static double maxVelocityInRPMs = 4000; - public static double maxAccelerationInRPMsSquared = maxVelocityInRPMs * 4; + public static double ampScoreVelocityInRPM = 1000; + public static double maxVelocityInRPM = 4000; + public static double maxAccelerationInRPMSquared = maxVelocityInRPM * 4; } public static class IntakeConstants { @@ -153,9 +165,29 @@ public static class LedConstants { public static int Led2Length = 60; } + public static class VisionConstants { + public static double visionDistanceOffsetInMeters = + 0; // Average difference between vision-calculated distance vs actual + } + public Optional getArmAngleFromDistance(double distanceInMeters) { - if (distanceInMeters > 3.0) return Optional.empty(); - return Optional.of(45 * distanceInMeters / 3.0); + // Calculated using https://stats.blue/Stats_Suite/polynomial_regression_calculator.html + // Based on empirical measurements done on 2024-04-03 + // (https://docs.google.com/spreadsheets/d/17Rh0MyVeME0KEAvkSqZKafnL0LPy9PoqWqOJ7ho49S8) + + distanceInMeters = + MathUtil.clamp( + distanceInMeters, ArmConstants.minDistanceInMeters, ArmConstants.maxDistanceInMeters); + + Optional angle = + Optional.of( + ArmConstants.Ax2 * Math.pow(distanceInMeters, 2) + + ArmConstants.Bx * distanceInMeters + + ArmConstants.C); + + // System.out.println("getArmAngleFromDistance(" + distanceInMeters + ") = " + angle.get()); + + return angle; } public RobotConfig() { diff --git a/src/main/java/frc/robot/config/RobotConfigInferno.java b/src/main/java/frc/robot/config/RobotConfigInferno.java index 76edd291..d0f75f8c 100644 --- a/src/main/java/frc/robot/config/RobotConfigInferno.java +++ b/src/main/java/frc/robot/config/RobotConfigInferno.java @@ -22,7 +22,6 @@ import frc.robot.subsystems.vision.VisionCamera; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.ArrayList; -import java.util.Optional; /* Override Inferno specific constants here */ public class RobotConfigInferno extends RobotConfig { @@ -66,12 +65,12 @@ public RobotConfigInferno() { ShooterConstants.pidKp = 0.0047154; ShooterConstants.pidKi = 0.0; ShooterConstants.pidKd = 0.0; - ShooterConstants.pidVelocityErrorInRPMS = 500; + ShooterConstants.pidVelocityErrorInRPM = 500; - ShooterConstants.maxVelocityInRPMs = 6000; - ShooterConstants.maxAccelerationInRPMsSquared = ShooterConstants.maxVelocityInRPMs * 4; - ShooterConstants.ampScoreVelocityInRPMs = 2500; - ShooterConstants.velocityInRPMs = 4500; + ShooterConstants.maxVelocityInRPM = 6000; + ShooterConstants.maxAccelerationInRPMSquared = ShooterConstants.maxVelocityInRPM * 4; + ShooterConstants.ampScoreVelocityInRPM = 2500; + ShooterConstants.velocityInRPM = 4500; shooter = new ShooterSubsystem(new ShooterIOSparkMax(2)); ArmConstants.absolutePositionOffset = @@ -87,17 +86,17 @@ public RobotConfigInferno() { ArmConstants.ffKv = 6.18; ArmConstants.ffKa = 0.04; - ArmConstants.maxVelocityInDegreesPerSecond = 90; - ArmConstants.maxAccelerationInDegreesPerSecondSquared = 720; + ArmConstants.maxVelocityInDegreesPerSecond = 120; + ArmConstants.maxAccelerationInDegreesPerSecondSquared = 120; ArmConstants.pidMaxOutput = 6.0; ArmConstants.pidMinOutput = -5.0; ArmConstants.pidAngleErrorInDegrees = 1.5; ArmConstants.maxAngleInDegrees = 89.0; - ArmConstants.minAngleInDegrees = -1.0; + ArmConstants.minAngleInDegrees = -1.5; - ArmConstants.intakeAngleInDegrees = 1.5; + ArmConstants.intakeAngleInDegrees = -1.5; ArmConstants.ampScoreAngleInDegrees = 89.0; ArmConstants.subwooferScoreAngleInDegrees = 9.80; ArmConstants.subwooferScoreFromPodiumAngleInDegrees = 33; // min/max = 33.24/34.37 @@ -107,6 +106,13 @@ public RobotConfigInferno() { ArmConstants.matchStartArmAngle = 90; ArmConstants.pidTimeoutInSeconds = 2.0; + ArmConstants.maxBacklashDegrees = 3.0; + + ArmConstants.minDistanceInMeters = Units.inchesToMeters(38); + ArmConstants.maxDistanceInMeters = 4.0; + ArmConstants.Ax2 = -3.2; + ArmConstants.Bx = 23.7; + ArmConstants.C = -10.3; arm = new ArmSubsystem(new ArmIOSparkMax(4, true)); ClimberConstants.minPositionInRadians = 0.01; @@ -131,8 +137,8 @@ public RobotConfigInferno() { "shooter", "1188", new Transform3d( - new Translation3d(Units.inchesToMeters(10.5), 0, Units.inchesToMeters(13)), - new Rotation3d(0, Units.degreesToRadians(-28), Units.degreesToRadians(180))))); + new Translation3d(-Units.inchesToMeters(9.5), 0, Units.inchesToMeters(14)), + new Rotation3d(0, Units.degreesToRadians(-32), Units.degreesToRadians(180))))); cameras.add( new VisionCamera( @@ -142,6 +148,26 @@ public RobotConfigInferno() { new Translation3d(0.3048, 0, 0.22), new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(0))))); + /* + cameras.add( + new VisionCamera( + "right", + "1196", + new Transform3d( + new Translation3d( + -Units.inchesToMeters(5.25), -Units.inchesToMeters(11.25), Units.inchesToMeters(7)), + new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(-90))))); + + cameras.add( + new VisionCamera( + "left", + "1190", + new Transform3d( + new Translation3d( + -Units.inchesToMeters(5.25), Units.inchesToMeters(11.25), Units.inchesToMeters(7)), + new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(90))))); + */ + VisionConstants.visionDistanceOffsetInMeters = -0.2; vision = new VisionSubsystem(cameras, AprilTagFields.k2024Crescendo.loadAprilTagLayoutField()); if (Robot.isSimulation()) { @@ -155,32 +181,4 @@ public RobotConfigInferno() { LedConstants.Led1Length = 34; led = new LedSystem(new LedIOWS121b()); } - - @Override - public Optional getArmAngleFromDistance(double distanceInMeters) { - /* TODO: Insert mapping of distance to arm angle for scoring in speaker */ - System.out.println("TODO: Inferno getArmAngleFromDistance(" + distanceInMeters + ")"); - // return Optional.empty(); - distanceInMeters -= Units.inchesToMeters(38); - Optional angle = Optional.empty(); - if (distanceInMeters < 0.6) { - angle = Optional.of(8.0 + 6.0 / 0.3 * distanceInMeters); - } else if (distanceInMeters < 1.2) { - angle = Optional.of(2.0 / 0.3 * distanceInMeters + 16.0); - } else if (distanceInMeters < 1.5) { - angle = Optional.of(25.0); - } else { - angle = Optional.of(1.0 / 0.3 * distanceInMeters + 21.0); - } - - if (angle.isPresent() && angle.get() < ArmConstants.subwooferScoreAngleInDegrees) { - angle = Optional.of(ArmConstants.subwooferScoreAngleInDegrees); - } - - return angle; - /* - if (distanceInMeters > 2.0) return Optional.empty(); - return Optional.of(30 * distanceInMeters / 3.0); - */ - } } diff --git a/src/main/java/frc/robot/config/RobotConfigPhoenix.java b/src/main/java/frc/robot/config/RobotConfigPhoenix.java index 699e12d7..794ea58f 100644 --- a/src/main/java/frc/robot/config/RobotConfigPhoenix.java +++ b/src/main/java/frc/robot/config/RobotConfigPhoenix.java @@ -18,6 +18,12 @@ public class RobotConfigPhoenix extends RobotConfig { public RobotConfigPhoenix() { super(false, true, true, true, false, true, false); + ArmConstants.minDistanceInMeters = Units.inchesToMeters(38); + ArmConstants.maxDistanceInMeters = 4.0; + ArmConstants.Ax2 = -3.2; + ArmConstants.Bx = 23.7; + ArmConstants.C = -10.3; + // Phoenix has a Swerve drive train DriveConstants.rotatePidKp = 0.025; DriveConstants.rotatePidKi = 0.0; @@ -31,9 +37,10 @@ public RobotConfigPhoenix() { "shooter", "1182", new Transform3d( - new Translation3d(-0.3048, 0, 0.22), - new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(180))))); + new Translation3d(-Units.inchesToMeters(10.75), 0, Units.inchesToMeters(8)), + new Rotation3d(0, Units.degreesToRadians(-33), Units.degreesToRadians(180))))); + VisionConstants.visionDistanceOffsetInMeters = -0.2; vision = new VisionSubsystem(cameras, AprilTagFields.k2024Crescendo.loadAprilTagLayoutField()); if (Robot.isSimulation()) { diff --git a/src/main/java/frc/robot/config/RobotConfigSherman.java b/src/main/java/frc/robot/config/RobotConfigSherman.java index 0f1acacd..d832acc8 100644 --- a/src/main/java/frc/robot/config/RobotConfigSherman.java +++ b/src/main/java/frc/robot/config/RobotConfigSherman.java @@ -39,9 +39,9 @@ public RobotConfigSherman() { ShooterConstants.pidKpBottom = 0.0001581; ShooterConstants.pidKiBottom = 0.0; ShooterConstants.pidKdBottom = 0.0; - ShooterConstants.pidVelocityErrorInRPMS = 20; + ShooterConstants.pidVelocityErrorInRPM = 20; - ShooterConstants.velocityInRPMs = 3000; + ShooterConstants.velocityInRPM = 3000; shooter = new ShooterSubsystem(new ShooterIOSparkMax(2), new ShooterIOSparkMax(1)); // ArmConstants.absolutePositionOffset = 0.426777535; diff --git a/src/main/java/frc/robot/controls/DebugControls.java b/src/main/java/frc/robot/controls/DebugControls.java index ebcd876c..7e22ab3a 100644 --- a/src/main/java/frc/robot/controls/DebugControls.java +++ b/src/main/java/frc/robot/controls/DebugControls.java @@ -1,5 +1,9 @@ package frc.robot.controls; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; @@ -127,7 +131,7 @@ public static void setupControls() { GenericEntry shooterVelocityEntry = debugTab - .add("Shooter: Velocity", ShooterConstants.velocityInRPMs) + .add("Shooter: Velocity", ShooterConstants.velocityInRPM) .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("min", 0, "max", 6000)) .withPosition(colIndex, rowIndex++) @@ -141,7 +145,7 @@ public static void setupControls() { RobotConfig.shooter, RobotConfig.intake, RobotConfig.arm, - () -> shooterVelocityEntry.getDouble(ShooterConstants.velocityInRPMs), + () -> shooterVelocityEntry.getDouble(ShooterConstants.velocityInRPM), () -> intakeVoltageEntry.getDouble(IntakeConstants.defaultSpeedInVolts), () -> armAngleEntry.getDouble(ArmConstants.subwooferScoreAngleInDegrees))) .withPosition(colIndex, rowIndex++) @@ -164,10 +168,17 @@ public static void setupControls() { "Shooter to Velocity", new SetShooterVelocity( RobotConfig.shooter, - () -> shooterVelocityEntry.getDouble(ShooterConstants.velocityInRPMs))) + () -> shooterVelocityEntry.getDouble(ShooterConstants.velocityInRPM))) .withPosition(colIndex, rowIndex++) .withSize(2, 1); + Command driveToAmp = + AutoBuilder.pathfindToPoseFlipped( + new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)), + new PathConstraints(4.0, 3.0, 2 * Math.PI, 3 * Math.PI)); + driveToAmp.setName("Drive To Amp"); + debugTab.add(driveToAmp).withPosition(colIndex, rowIndex++).withSize(2, 1); + ; /* colIndex += 2; rowIndex = 1; diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index 342b3362..956493b5 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -1,6 +1,10 @@ package frc.robot.controls; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -17,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SelectCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -198,14 +203,11 @@ private static void setupCommonControls(CommandXboxController controller) { /* Y Button = Prepare to Climb * X Button = Climber */ - controller - .y() - .onTrue( - new ParallelCommandGroup( - RobotConfig.intake.getTurnOffCommand(), - RobotConfig.shooter.getTurnOffCommand(), - RobotConfig.arm.getStowCommand(), - RobotConfig.climber.getExtendCommand())); + controller.y().onTrue(RobotConfig.intake.getTurnOffCommand()); + controller.y().onTrue(RobotConfig.shooter.getTurnOffCommand()); + controller.y().onTrue(RobotConfig.arm.getStowCommand()); + controller.y().onTrue(RobotConfig.climber.getExtendCommand()); + controller.x().onTrue(RobotConfig.climber.getRetractCommand()); /* Target Selection Controls */ @@ -288,30 +290,44 @@ public static void setupMainControls(CommandXboxController mainController) { .onTrue( new EjectPiece(RobotConfig.intake, RobotConfig.arm, RobotConfig.shooter)); // Eject Note + mainController.rightBumper().onTrue(RobotConfig.intake.getTurnOffCommand()); + mainController .rightBumper() .onTrue( - new ParallelCommandGroup( - RobotConfig.intake.getTurnOffCommand(), - new DriveToYaw(RobotConfig.drive, () -> DevilBotState.getVisionRobotYawToTarget()) - .withTimeout(DriveConstants.pidTimeoutInSeconds), - new SetShooterVelocity( - RobotConfig.shooter, () -> DevilBotState.getShooterVelocity()) - .withTimeout(ShooterConstants.pidTimeoutInSeconds), // turn on shooter - /* TODO: Use ArmToPositionTP instead of setting arm angle directly */ - new InstantCommand( + new SelectCommand<>( + Map.ofEntries( + Map.entry( + true, + AutoBuilder.pathfindToPoseFlipped( + new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)), + new PathConstraints(3.0, 2.0, 2 * Math.PI, 3 * Math.PI))), + Map.entry( + false, + new DriveToYaw( + RobotConfig.drive, () -> DevilBotState.getVisionRobotYawToTarget()) + .withTimeout(DriveConstants.pidTimeoutInSeconds))), + () -> DevilBotState.isAmpMode())); + + mainController + .rightBumper() + .onTrue( + new SetShooterVelocity(RobotConfig.shooter, () -> DevilBotState.getShooterVelocity()) + .withTimeout(ShooterConstants.pidTimeoutInSeconds) // turn on shooter + ); + + mainController + .rightBumper() + .onTrue( + new InstantCommand( () -> { - if (DevilBotState.isAmpMode()) { - RobotConfig.arm.setAngle(ArmConstants.ampScoreAngleInDegrees); - } else { - Optional armAngle = DevilBotState.getArmAngleToTarget(); - if (armAngle.isPresent()) { - RobotConfig.arm.setAngle((armAngle.get())); - } + Optional armAngle = DevilBotState.getArmAngleToTarget(); + if (armAngle.isPresent()) { + RobotConfig.arm.setAngle((armAngle.get())); } }, - RobotConfig.arm) // adjust arm angle based on vision's distance from target - )); // Aim + RobotConfig.arm) + .onlyIf(() -> !DevilBotState.isAmpMode())); mainController .rightTrigger() @@ -346,11 +362,11 @@ public static void setupMainControls(CommandXboxController mainController) { () -> Units.radiansPerSecondToRotationsPerMinute(RobotConfig.shooter.getCurrentSpeed()) >= DevilBotState.getShooterVelocity() - - ShooterConstants.pidVelocityErrorInRPMS + - ShooterConstants.pidVelocityErrorInRPM && Units.radiansPerSecondToRotationsPerMinute( RobotConfig.shooter.getCurrentSpeed()) <= DevilBotState.getShooterVelocity() - + ShooterConstants.pidVelocityErrorInRPMS); + + ShooterConstants.pidVelocityErrorInRPM); shooterRPMTrigger.onTrue( new SequentialCommandGroup( // Starts the controller rumble @@ -495,7 +511,7 @@ private static void setupSecondaryControls(CommandXboxController controller) { new BooleanEvent(eventLoop, () -> Math.abs(controller.getLeftY()) > 0.05); Trigger leftYPressedTrigger = leftYPressed.castTo(Trigger::new); - leftYPressedTrigger.onTrue( + leftYPressedTrigger.whileTrue( new ArmCommand( RobotConfig.arm, () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.05))); // Arm Up/Down diff --git a/src/main/java/frc/robot/controls/PitControls.java b/src/main/java/frc/robot/controls/PitControls.java index 0419b5ce..2c174b8d 100644 --- a/src/main/java/frc/robot/controls/PitControls.java +++ b/src/main/java/frc/robot/controls/PitControls.java @@ -224,7 +224,7 @@ private static int setupShooterControls(ShuffleboardTab tab, int col, int row, i "Velocity (RPMs)", () -> Units.radiansPerSecondToRotationsPerMinute(RobotConfig.shooter.getCurrentSpeed())) .withWidget(BuiltInWidgets.kNumberBar) - .withProperties(Map.of("min", 0, "max", ShooterConstants.maxVelocityInRPMs)) + .withProperties(Map.of("min", 0, "max", ShooterConstants.maxVelocityInRPM)) .withPosition(layoutColIndex, layoutRowIndex++); ShuffleboardLayout shooterCommandLayout = diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 51025520..a6740ef9 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -3,51 +3,25 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.config.RobotConfig.ArmConstants; public interface Arm extends Subsystem { // gets the angle of the arm - public default double getAngle() { - return 0; - } + public double getAngle(); - public default double getVelocity() { - return 0; - } + public double getTargetAngle(); - public default double getRelativeAngle() { - return getAngle(); - } + public boolean isAtMaxLimit(); - public default double getTargetAngle() { - return 0; - } + public boolean isAtMinLimit(); - // sets of the angle of the arm - public default void setAngle(double degrees) { - setAngle(degrees, 0); - } - - public default void setAngle(double degrees, double velocityDegreesPerSecond) {} - - public default boolean isAbsoluteEncoderConnected() { - return true; - } + public double getVelocity(); - public default boolean isAtMaxLimit() { - return false; - } - - public default boolean isAtMinLimit() { - return false; - } + // sets of the angle of the arm + public void setAngle(double degrees); - public default void stow() { - setAngle(ArmConstants.stowIntakeAngleInDegrees); - } + public boolean isAtSetpoint(); public Command getStowCommand(); public default void add2dSim(Mechanism2d mech2d) {} - ; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 42a42df1..2b8c7b7b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -5,15 +5,16 @@ public interface ArmIO { @AutoLog public static class ArmIOInputs { - public double positionRad = 0.0; - public double positionDegree = 0.0; - public double velocityInDegrees = 0.0; + public double appliedVolts = 0.0; + public double currentAmps; + public double positionDegrees = 0.0; + public double velocityDegrees = 0.0; public double absolutePositionRaw; - public boolean absoluteEncoderConnected = false; - public double current; - public double appliedVolts = 0.0; public double relativePositionDegrees = 0.0; + public double positionError = 0.0; + + public boolean absoluteEncoderConnected = false; public boolean limitHigh = false; public boolean limitLow = false; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index b16f5070..e4f6340a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -65,10 +65,10 @@ public ArmIOSparkMax(int id, boolean inverted) { // relEncoder.setPosition(0); // 60:1 gear box, 72 teeth on the arm cog and 14 teeth on the motor cog - double gearRatio = (60 * (72 / 14)); + double gearRatio = (60.0 * (72.0 / 14.0)); double rotationsToDegreesConversionFactor = 360.0 / gearRatio; relEncoder.setPositionConversionFactor(rotationsToDegreesConversionFactor); - relEncoder.setVelocityConversionFactor(rotationsToDegreesConversionFactor / 60); + relEncoder.setVelocityConversionFactor(rotationsToDegreesConversionFactor / 60.0); relEncoder.setPosition(Units.radiansToDegrees(getOffsetCorrectedAbsolutePositionInRadians())); lkP = RobotConfig.ArmConstants.pidKp; @@ -102,17 +102,15 @@ public ArmIOSparkMax(int id, boolean inverted) { /** Updates the set of loggable inputs. */ @Override public void updateInputs(ArmIOInputs inputs) { - inputs.positionRad = getOffsetCorrectedAbsolutePositionInRadians(); - inputs.positionDegree = Units.radiansToDegrees(inputs.positionRad); - inputs.absolutePositionRaw = absoluteEncoder.getAbsolutePosition(); - inputs.absoluteEncoderConnected = isAbsoluteEncoderConnected(); - inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.currentAmps = motor.getOutputCurrent(); + inputs.positionDegrees = Units.radiansToDegrees(getOffsetCorrectedAbsolutePositionInRadians()); + inputs.velocityDegrees = relEncoder.getVelocity(); - inputs.current = motor.getOutputCurrent(); - + inputs.absoluteEncoderConnected = isAbsoluteEncoderConnected(); + inputs.absolutePositionRaw = absoluteEncoder.getAbsolutePosition(); inputs.relativePositionDegrees = relEncoder.getPosition(); - inputs.velocityInDegrees = relEncoder.getVelocity(); + inputs.positionError = inputs.positionDegrees - inputs.relativePositionDegrees; // Code below allows PID to be tuned using SmartDashboard. And outputs extra data to // SmartDashboard. diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOStub.java b/src/main/java/frc/robot/subsystems/arm/ArmIOStub.java index bafe75d2..dc02637b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOStub.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOStub.java @@ -49,15 +49,19 @@ public class ArmIOStub implements ArmIO { /** Updates the set of loggable inputs. */ @Override public void updateInputs(ArmIOInputs inputs) { - inputs.positionRad = arm.getAngleRads(); - inputs.positionDegree = Units.radiansToDegrees(inputs.positionRad); - inputs.velocityInDegrees = Units.radiansToDegrees(arm.getVelocityRadPerSec()); inputs.appliedVolts = currentVoltage; + inputs.currentAmps = Math.abs(arm.getCurrentDrawAmps()); + inputs.positionDegrees = Units.radiansToDegrees(arm.getAngleRads()); + inputs.velocityDegrees = Units.radiansToDegrees(arm.getVelocityRadPerSec()); + inputs.relativePositionDegrees = + inputs + .positionDegrees; // In sim, the relative/absolute encoder are identical, so just it to + // the same value; if (softwarePidEnabled) { currentVoltage = feedForwardVolts - + pid.calculate(inputs.positionDegree, targetDegrees) + + pid.calculate(inputs.positionDegrees, targetDegrees) * RobotController.getBatteryVoltage(); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 8e20b4cd..11fc62e0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -2,33 +2,39 @@ import static edu.wpi.first.units.Units.Volts; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; 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.config.RobotConfig.ArmConstants; import frc.robot.util.DevilBotState; import frc.robot.util.DevilBotState.State; import frc.robot.util.LoggedTunableNumber; +import frc.robot.util.TrapezoidProfileSubsystem2876; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class ArmSubsystem extends SubsystemBase implements Arm { +public class ArmSubsystem extends TrapezoidProfileSubsystem2876 implements Arm { private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private ArmFeedforward feedforward; + private ArmFeedforward feedforward = + new ArmFeedforward( + ArmConstants.ffKs, ArmConstants.ffKg, ArmConstants.ffKv, ArmConstants.ffKa); private final SysIdRoutine sysId; private final double positionDegreeMax = ArmConstants.maxAngleInDegrees; private final double positionDegreeMin = ArmConstants.minAngleInDegrees; @AutoLogOutput private double targetVoltage; @AutoLogOutput private double targetDegrees; @AutoLogOutput private double targetRelativeDegrees; - @AutoLogOutput private double targetVelocityDegreesPerSecond; + @AutoLogOutput private double goalSetpointDegrees; + @AutoLogOutput private double currentSetpointDegrees; + private double backlashCompensationDirection = 0; // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm. private final double armAngle2dOffset = 0; @@ -55,6 +61,10 @@ public class ArmSubsystem extends SubsystemBase implements Arm { private double kG, kV, kA, kS; public ArmSubsystem(ArmIO io) { + super( + new TrapezoidProfile.Constraints( + ArmConstants.maxVelocityInDegreesPerSecond, + ArmConstants.maxAccelerationInDegreesPerSecondSquared)); this.io = io; armKp.initDefault(ArmConstants.pidKp); @@ -85,21 +95,41 @@ public ArmSubsystem(ArmIO io) { new SysIdRoutine.Mechanism((voltage) -> runVoltage(voltage.in(Volts)), null, this)); io.setBrakeMode(true); + disable(); } @Override - public double getAngle() { - return inputs.positionDegree; + public void useState(TrapezoidProfile.State setpoint) { + double ff = feedforward.calculate(setpoint.position, 0); + + // Use feedforward + HW velocity PID (ignore SW PID) + io.setPosition(setpoint.position, ff); + currentSetpointDegrees = setpoint.position; + + Logger.recordOutput("Arm/setAngle/setpointDegrees", setpoint.position); + Logger.recordOutput("Arm/setAngle/ffVolts", ff); + + // System.out.println("pos: " + setpoint.position); + // System.out.println("vel: " + setpoint.velocity); + } + + public double getRelativeAngle() { + return inputs.relativePositionDegrees; } @Override - public double getVelocity() { - return inputs.velocityInDegrees; + public TrapezoidProfile.State getMeasurement() { + return new TrapezoidProfile.State(getRelativeAngle(), getVelocity()); } @Override - public double getRelativeAngle() { - return inputs.relativePositionDegrees; + public double getAngle() { + return inputs.positionDegrees; + } + + @Override + public double getVelocity() { + return inputs.velocityDegrees; } @Override @@ -109,7 +139,10 @@ public double getTargetAngle() { // sets of the angle of the arm @Override - public void setAngle(double degrees, double velocityDegreesPerSecond) { + public void setAngle(double degrees) { + degrees = + MathUtil.clamp(degrees, ArmConstants.minAngleInDegrees, ArmConstants.maxAngleInDegrees); + Logger.recordOutput("Arm/setAngle/requestedAngleDegress", degrees); // Don't try to set position if absolute encoder is broken/missing. if (isAbsoluteEncoderConnected() == false) { @@ -118,44 +151,31 @@ public void setAngle(double degrees, double velocityDegreesPerSecond) { if (isAbsoluteEncoderReadingValid() == false) { return; } - // Check if the arm angle is within limits. Don't try to move the arm to new angle if it is - // already at limit. - // if (isHighLimit() || isLowLimit()) { - // return; - // } - // Check if the angle is below the minimum limit or above the maximum limit - // If it is the it is set to min/max - if (degrees < ArmConstants.minAngleInDegrees) { - this.targetDegrees = ArmConstants.minAngleInDegrees; // Set to the minimum angle - } else if (degrees > ArmConstants.maxAngleInDegrees) { - this.targetDegrees = ArmConstants.maxAngleInDegrees; // Set to the maximum angle - } else { - // The angle is within the range and is set - this.targetDegrees = degrees; - } - this.targetVelocityDegreesPerSecond = velocityDegreesPerSecond; + // Clamp the target degrees + this.targetDegrees = degrees; // We instantiate a new object here each time because constants can change when being tuned. feedforward = new ArmFeedforward(kS, kG, kV, kA); - // To account for differences in absolute encoder and relative encoder readings cause by - // backlash and other arm physics, - // we calculate the difference in current vs target absolute encoder value and then calculate - // the corrsponding relative - // angle - double deltaDegrees = this.targetDegrees - getAngle(); - this.targetRelativeDegrees = getRelativeAngle() + deltaDegrees; - double ff = feedforward.calculate(this.targetDegrees, this.targetVelocityDegreesPerSecond); + this.targetRelativeDegrees = this.targetDegrees; + + // If we are moving up, we need to account for backlash since the arm tends to bias down (due to + // gravity) + // if ((this.targetRelativeDegrees > inputs.relativePositionDegrees)) { + // backlashCompensationDirection = 1; + // } else { + // backlashCompensationDirection = 0; + // } + this.targetRelativeDegrees += (backlashCompensationDirection * ArmConstants.maxBacklashDegrees); Logger.recordOutput("Arm/setAngle/setpointDegrees", this.targetRelativeDegrees); - Logger.recordOutput("Arm/setAngle/ffVolts", ff); + this.goalSetpointDegrees = this.targetRelativeDegrees; - // Set the position reference with feedforward voltage - io.setPosition(this.targetDegrees, ff); + setGoal(this.goalSetpointDegrees); + enable(); } - @Override public boolean isAbsoluteEncoderConnected() { return io.isAbsoluteEncoderConnected(); } @@ -171,6 +191,7 @@ && getAngle() < ArmConstants.maxAngleInDegrees + 10) { // Sets the voltage to volts. the volts value is -12 to 12 public void runVoltage(double volts) { targetVoltage = voltageSafety(volts); + disable(); io.setVoltage(targetVoltage); } @@ -191,6 +212,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { + super.periodic(); if (armKp.hasChanged(hashCode()) || armKd.hasChanged(hashCode()) || armOutputMin.hasChanged(hashCode()) @@ -213,34 +235,19 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Arm", inputs); - // The relative encoder is initialized in the hw specific code/file. - // - // if (relEncoderInit) { - // io.resetRelativeEncoder( - // 0); // TODO: We need to figure out the mapping for the absolute encoder to relative - // // encoder - // relEncoderInit = false; - // } if (DevilBotState.getState() == State.DISABLED && io.isAbsoluteEncoderConnected()) { io.resetRelativeEncoder(getAngle()); } - if (Math.abs(inputs.velocityInDegrees) < 0.1) { - io.resetRelativeEncoder(getAngle()); - } - if (isLimitHigh() && inputs.appliedVolts > 0) { - // TODO: turn off voltage or stop pid io.setVoltage(0); } if (isLimitLow() && inputs.appliedVolts < 0) { - // TODO: turn off voltage or stop pid - // io.resetRelativeEncoder(0.0); io.setVoltage(0); } if (null != arm2d) { - arm2d.setAngle(inputs.positionDegree + armAngle2dOffset); + arm2d.setAngle(inputs.positionDegrees + armAngle2dOffset); } } @@ -249,7 +256,7 @@ private boolean isLimitHigh() { if (isAbsoluteEncoderConnected() == false) { return true; } - if (inputs.positionDegree >= positionDegreeMax) { + if (inputs.positionDegrees >= positionDegreeMax) { inputs.limitHigh = true; } else { inputs.limitHigh = false; @@ -262,7 +269,7 @@ private boolean isLimitLow() { if (isAbsoluteEncoderConnected() == false) { return true; } - if (inputs.positionDegree <= positionDegreeMin) { + if (inputs.positionDegrees <= positionDegreeMin) { inputs.limitLow = true; } else { @@ -291,6 +298,10 @@ public boolean isAtMinLimit() { return isLimitLow(); } + private void stow() { + setAngle(ArmConstants.stowIntakeAngleInDegrees); + } + @Override public Command getStowCommand() { return runOnce(() -> stow()); @@ -305,8 +316,14 @@ public void add2dSim(Mechanism2d mech2d) { new MechanismLigament2d( "Arm", 30, - inputs.positionDegree + armAngle2dOffset, + inputs.positionDegrees + armAngle2dOffset, 6, new Color8Bit(Color.kYellow))); } + + @Override + public boolean isAtSetpoint() { + return (Math.abs(currentSetpointDegrees - goalSetpointDegrees) + < ArmConstants.pidAngleErrorInDegrees); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 526e6d04..95ba6870 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.Command; public interface Drive { @@ -66,4 +69,7 @@ public default double getAngle() { public default void lockPose() {} ; + + public default void addVisionMeasurement( + Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java index 8c13b4f2..83a01eea 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java @@ -2,11 +2,13 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; @@ -17,7 +19,6 @@ import org.littletonrobotics.junction.Logger; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; -import swervelib.parser.PIDFConfig; import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; @@ -52,9 +53,6 @@ public DriveSwerveYAGSL(String configPath) { throw new RuntimeException(e); } - PIDFConfig drivePIDF = swerveDrive.getModules()[0].getDrivePIDF(); - PIDFConfig anglePIDF = swerveDrive.getModules()[0].getAnglePIDF(); - AutoBuilder.configureHolonomic( swerveDrive::getPose, // Robot pose supplier swerveDrive @@ -63,10 +61,7 @@ public DriveSwerveYAGSL(String configPath) { swerveDrive::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE swerveDrive::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE // ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in - // your Constants class - new PIDConstants(drivePIDF.p, drivePIDF.i, drivePIDF.d), // Translation PID constants - new PIDConstants(anglePIDF.p, anglePIDF.i, anglePIDF.d), // Rotation PID constants + new HolonomicPathFollowerConfig( swerveDrive.getMaximumVelocity(), // Max module speed, in m/s swerveDrive.swerveDriveConfiguration .getDriveBaseRadiusMeters(), // Drive base radius in meters. Distance from robot @@ -155,4 +150,11 @@ public void periodic() { io.updateInputs(inputs, swerveDrive); Logger.processInputs("Drive", inputs); } + + @Override + public void addVisionMeasurement( + Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) { + + swerveDrive.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 34225e1c..d3aeaef8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -15,6 +15,7 @@ public class IntakeIOSparkMax implements IntakeIO { // Gets the NEO encoder private final RelativeEncoder encoder; DigitalInput limitSwitchIntake = new DigitalInput(1); + DigitalInput limitSwitchIntakeSecondary = new DigitalInput(2); public IntakeIOSparkMax(int id, boolean inverted) { leader = new CANSparkMax(id, MotorType.kBrushless); @@ -31,7 +32,10 @@ public IntakeIOSparkMax(int id, boolean inverted) { @Override public void updateInputs(IntakeIOInputs inputs) { inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.limitSwitchIntake = !limitSwitchIntake.get(); + inputs.limitSwitchIntake = + !limitSwitchIntake.get() + || !limitSwitchIntakeSecondary + .get(); // Assume note in intake if either sensor is triggered inputs.current = leader.getOutputCurrent(); inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); diff --git a/src/main/java/frc/robot/subsystems/led/LedSystem.java b/src/main/java/frc/robot/subsystems/led/LedSystem.java index c9e2429a..8a994e0d 100644 --- a/src/main/java/frc/robot/subsystems/led/LedSystem.java +++ b/src/main/java/frc/robot/subsystems/led/LedSystem.java @@ -73,13 +73,13 @@ public Command getNoteDetectionCommand() { // Blink orange for (int i = 0; i < 2; i++) { commandGroup.addCommands( - new InstantCommand(() -> setColor(255, 105, 180)), + new InstantCommand(() -> setColor(231, 84, 128)), new WaitCommand(0.2), - new InstantCommand(() -> setColor(255, 105, 180)), + new InstantCommand(() -> setColor(231, 84, 128)), new WaitCommand(0.2)); } - new InstantCommand(() -> setColor(255, 105, 180)); + new InstantCommand(() -> setColor(231, 84, 128)); // Turn LED White commandGroup.addCommands(new InstantCommand(() -> setColor(255, 255, 255))); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f833ed7c..b3d40735 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -14,6 +14,8 @@ public default void runVoltage(double volts) {} /** Run closed loop at the specified velocity */ public default void runVelocity(double velocityRPM) {} + public boolean isAtSetpoint(); + public double getCurrentSpeed(); public double getVoltage(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b2954da8..8e0ef4b4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -31,6 +31,7 @@ public class ShooterSubsystem extends ProfiledPIDSubsystem implements Shooter { @AutoLogOutput private double targetVoltage; @AutoLogOutput private double targetVelocityRPM; @AutoLogOutput private double targetVelocityRadPerSec; + @AutoLogOutput private double currentVelocitySetpointRPM; // Mechanism2d display of a Shooter private List shooter2d = new ArrayList(); @@ -43,9 +44,9 @@ public ShooterSubsystem(ShooterIO io) { ShooterConstants.pidKi, ShooterConstants.pidKd, new TrapezoidProfile.Constraints( - Units.rotationsPerMinuteToRadiansPerSecond(ShooterConstants.maxVelocityInRPMs), + Units.rotationsPerMinuteToRadiansPerSecond(ShooterConstants.maxVelocityInRPM), Units.rotationsPerMinuteToRadiansPerSecond( - ShooterConstants.maxAccelerationInRPMsSquared)))); + ShooterConstants.maxAccelerationInRPMSquared)))); this.io = io; useSoftwarePidVelocityControl = !io.supportsHardwarePid(); @@ -91,6 +92,7 @@ public void useOutput(double output, TrapezoidProfile.State setpoint) { io.setVelocity(setpoint.position, ff); } + currentVelocitySetpointRPM = Units.radiansPerSecondToRotationsPerMinute(setpoint.position); // System.out.println(setpoint.position); } @@ -104,14 +106,14 @@ public double getMeasurement() { public void runVoltage(double volts) { targetVoltage = volts; targetVelocityRadPerSec = 0; - this.targetVelocityRPM = ShooterConstants.maxVelocityInRPMs * (volts / 12.0); + this.targetVelocityRPM = ShooterConstants.maxVelocityInRPM * (volts / 12.0); disable(); // disable PID control io.setVoltage(targetVoltage); } @Override public void runVelocity(double velocityRPM) { - velocityRPM = MathUtil.clamp(velocityRPM, 0, ShooterConstants.maxVelocityInRPMs); + velocityRPM = MathUtil.clamp(velocityRPM, 0, ShooterConstants.maxVelocityInRPM); targetVoltage = -1; this.targetVelocityRPM = velocityRPM; targetVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); @@ -136,17 +138,15 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); - if (targetVelocityRPM != 0) { - currentSimAngle -= - (inputs.velocityRadPerSec - / Units.rotationsPerMinuteToRadiansPerSecond(ShooterConstants.maxVelocityInRPMs)) - * 45; - - int angleOffset = 0; - for (MechanismLigament2d shooter : shooter2d) { - shooter.setAngle(angleOffset + currentSimAngle); - angleOffset += 90; - } + currentSimAngle -= + (inputs.velocityRadPerSec + / Units.rotationsPerMinuteToRadiansPerSecond(ShooterConstants.maxVelocityInRPM)) + * 45; + + int angleOffset = 0; + for (MechanismLigament2d shooter : shooter2d) { + shooter.setAngle(angleOffset + currentSimAngle); + angleOffset += 90; } } @@ -180,5 +180,10 @@ public void add2dSim(Mechanism2d mech2d) { intakePivot2d.append( new MechanismLigament2d("Wheel Spoke D", 5, 270, 12, new Color8Bit(Color.kBlue)))); } - ; + + @Override + public boolean isAtSetpoint() { + return (Math.abs(currentVelocitySetpointRPM - targetVelocityRPM) + < ShooterConstants.pidVelocityErrorInRPM); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index cc60d46b..89836582 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Pose2d; import java.text.DecimalFormat; -import java.util.List; import java.util.Optional; import java.util.function.Supplier; @@ -83,13 +82,5 @@ public default boolean setPrimaryCamera(String name) { */ public Optional getYawToAprilTag(int id); - /** - * Returns a list of vision-based estimated robot poses - * - * @return list of estimated poses. list length may be zero if updated pose estimation data is not - * available. - */ - public List getEstimatedRobotPoses(); - public default void enableSimulation(Supplier poseSupplier, boolean enableWireFrame) {} } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index d82bd4e3..abec06ce 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -1,10 +1,15 @@ package frc.robot.subsystems.vision; import edu.wpi.first.apriltag.AprilTagFieldLayout; +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.geometry.Transform3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.config.RobotConfig; +import frc.robot.config.RobotConfig.VisionConstants; import frc.robot.util.DevilBotState; import java.util.ArrayList; import java.util.List; @@ -17,6 +22,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.PhotonUtils; import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -87,8 +93,6 @@ private Transform3d getRobotToCamera() { private VisionCameraImpl primaryCamera = null; private final AprilTagFieldLayout fieldLayout; - private List estimatedPoses = new ArrayList(); - /* Debug Info */ @AutoLogOutput private int debugTargetsVisible; @AutoLogOutput private int debugCurrentTargetId = DevilBotState.getActiveTargetId(); @@ -99,6 +103,7 @@ private Transform3d getRobotToCamera() { @AutoLogOutput private double debugTargetDistance = 0; @AutoLogOutput private double debugTargetYaw = 0; @AutoLogOutput private Pose2d debugEstimatedPose; + @AutoLogOutput private Pose2d debugEstimatedPose2d; /* Simulation Support*/ private boolean simEnabled = false; @@ -108,7 +113,6 @@ private Transform3d getRobotToCamera() { public VisionSubsystem(List cameras, AprilTagFieldLayout fieldLayout) { for (VisionCamera camera : cameras) { this.cameras.add(new VisionCameraImpl(camera, fieldLayout)); - estimatedPoses.add(new VisionPose()); } this.fieldLayout = fieldLayout; if (0 != cameras.size()) { @@ -122,7 +126,13 @@ public void enableSimulation(Supplier poseSupplier, boolean enableWireFr simVision.addAprilTags(this.fieldLayout); for (VisionCameraImpl camera : cameras) { - camera.simCamera = new PhotonCameraSim(camera.camera); + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(800, 600, Rotation2d.fromDegrees(70)); + cameraProp.setCalibError(0.35, 0.10); + cameraProp.setFPS(120); + cameraProp.setAvgLatencyMs(50); + cameraProp.setLatencyStdDevMs(15); + camera.simCamera = new PhotonCameraSim(camera.camera, cameraProp); simVision.addCamera(camera.simCamera, camera.robotToCamera); camera.simCamera.enableDrawWireframe(enableWireFrame); } @@ -141,18 +151,36 @@ public void periodic() { camera.update(); if (camera == primaryCamera) { debugTargetsVisible = camera.result.getTargets().size(); + + if (debugTargetsVisible > 0) { + PhotonTrackedTarget target = camera.result.getBestTarget(); + Optional fieldToTarget = fieldLayout.getTagPose(target.getFiducialId()); + if (fieldToTarget.isPresent()) { + debugEstimatedPose2d = + PhotonUtils.estimateFieldToRobotAprilTag( + target.getBestCameraToTarget(), fieldToTarget.get(), camera.robotToCamera) + .toPose2d(); + } + } } Optional currentEstimatedRobotPose = camera.getEstimatedRobotPose(); if (currentEstimatedRobotPose.isPresent()) { - estimatedPoses.get(camera.getIndex()).robotPose = - currentEstimatedRobotPose.get().estimatedPose.toPose2d(); - estimatedPoses.get(camera.getIndex()).timestamp = - currentEstimatedRobotPose.get().timestampSeconds; - estimatedPoses.get(camera.getIndex()).cameraName = camera.getName(); + Optional distanceToBestTarget = + getDistanceToAprilTag(camera.getBestTarget().getFiducialId()); + if (distanceToBestTarget.isPresent()) { + double distance = distanceToBestTarget.get(); + + // Add vision measurement to the drivetrain. + // TODO: clean up this abstraction + RobotConfig.drive.addVisionMeasurement( + currentEstimatedRobotPose.get().estimatedPose.toPose2d(), + currentEstimatedRobotPose.get().timestampSeconds, + VecBuilder.fill(distance / 2, distance / 2, distance / 2)); + } if (camera == primaryCamera) { - debugEstimatedPose = estimatedPoses.get(camera.getIndex()).robotPose; + debugEstimatedPose = currentEstimatedRobotPose.get().estimatedPose.toPose2d(); } } } @@ -193,10 +221,11 @@ public Optional getDistanceToAprilTag(int id) { if ((target != null) && (primaryCamera != null)) { return Optional.of( PhotonUtils.calculateDistanceToTargetMeters( - primaryCamera.getRobotToCamera().getZ(), - fieldLayout.getTagPose(target.getFiducialId()).get().getZ(), - -primaryCamera.getRobotToCamera().getRotation().getY(), - Units.degreesToRadians(target.getPitch()))); + primaryCamera.getRobotToCamera().getZ(), + fieldLayout.getTagPose(target.getFiducialId()).get().getZ(), + -primaryCamera.getRobotToCamera().getRotation().getY(), + Units.degreesToRadians(target.getPitch())) + + VisionConstants.visionDistanceOffsetInMeters); } return Optional.empty(); } @@ -236,11 +265,6 @@ public Optional getYawToAprilTag(int id) { return Optional.empty(); } - @Override - public List getEstimatedRobotPoses() { - return estimatedPoses; - } - @Override public boolean setPrimaryCamera(String name) { boolean foundCamera = false; diff --git a/src/main/java/frc/robot/util/DevilBotState.java b/src/main/java/frc/robot/util/DevilBotState.java index a98bb328..68fb777c 100644 --- a/src/main/java/frc/robot/util/DevilBotState.java +++ b/src/main/java/frc/robot/util/DevilBotState.java @@ -101,9 +101,9 @@ public static String getTargetName(int id) { public static double getShooterVelocity() { if (isAmpMode()) { - return ShooterConstants.ampScoreVelocityInRPMs; + return ShooterConstants.ampScoreVelocityInRPM; } else { - return ShooterConstants.velocityInRPMs; + return ShooterConstants.velocityInRPM; } } @@ -137,7 +137,7 @@ public enum SpeakerShootingMode { SPEAKER_VISION_BASED, } - private static SpeakerShootingMode shootingMode = SpeakerShootingMode.SPEAKER_FROM_SUBWOOFER; + private static SpeakerShootingMode shootingMode = SpeakerShootingMode.SPEAKER_VISION_BASED; public static void setShootingMode(SpeakerShootingMode position) { DevilBotState.shootingMode = position; diff --git a/src/main/java/frc/robot/util/TrapezoidProfileSubsystem2876.java b/src/main/java/frc/robot/util/TrapezoidProfileSubsystem2876.java new file mode 100644 index 00000000..38217534 --- /dev/null +++ b/src/main/java/frc/robot/util/TrapezoidProfileSubsystem2876.java @@ -0,0 +1,122 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies + * how to use the current state of the motion profile by overriding the `useState` method. + * + *

This class is provided by the NewCommands VendorDep + */ +public abstract class TrapezoidProfileSubsystem2876 extends SubsystemBase { + private final double m_period; + private final TrapezoidProfile m_profile; + + private TrapezoidProfile.State m_state; + private TrapezoidProfile.State m_goal; + + private boolean m_enabled = true; + + /** + * Creates a new TrapezoidProfileSubsystem2876. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + * @param initialPosition The initial position of the controlled mechanism when the subsystem is + * constructed. + * @param period The period of the main robot loop, in seconds. + */ + public TrapezoidProfileSubsystem2876( + TrapezoidProfile.Constraints constraints, double initialPosition, double period) { + requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem2876"); + m_profile = new TrapezoidProfile(constraints); + m_state = new TrapezoidProfile.State(initialPosition, 0); + setGoal(initialPosition); + m_period = period; + } + + /** + * Creates a new TrapezoidProfileSubsystem2876. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + * @param initialPosition The initial position of the controlled mechanism when the subsystem is + * constructed. + */ + public TrapezoidProfileSubsystem2876( + TrapezoidProfile.Constraints constraints, double initialPosition) { + this(constraints, initialPosition, 0.02); + } + + /** + * Creates a new TrapezoidProfileSubsystem2876. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + */ + public TrapezoidProfileSubsystem2876(TrapezoidProfile.Constraints constraints) { + this(constraints, 0, 0.02); + } + + @Override + public void periodic() { + m_state = m_profile.calculate(m_period, m_state, m_goal); + if (m_enabled) { + useState(m_state); + } + } + + /** + * Sets the goal state for the subsystem. + * + * @param goal The goal state for the subsystem's motion profile. + */ + public final void setGoal(TrapezoidProfile.State goal) { + m_goal = goal; + } + + /** + * Sets the goal state for the subsystem. Goal velocity assumed to be zero. + * + * @param goal The goal position for the subsystem's motion profile. + */ + public final void setGoal(double goal) { + setGoal(new TrapezoidProfile.State(goal, 0)); + } + + /** Enable the TrapezoidProfileSubsystem2876's output. */ + public void enable() { + if (false == m_enabled) { + // DevilBotz 2876: Update the *current* state before starting the motion + // profile to ensure smooth motion to the target in case the + // position/velocity changed *outside* of this subsystem. + // + // E.g. robot was disabled and the arm position changed due to gravity + m_state = getMeasurement(); + } + m_enabled = true; + } + + /** Disable the TrapezoidProfileSubsystem2876's output. */ + public void disable() { + m_enabled = false; + } + + /** + * Users should override this to consume the current state of the motion profile. + * + * @param state The current state of the motion profile. + */ + protected abstract void useState(TrapezoidProfile.State state); + + /** + * Returns the measurement of the process variable used by the TrapezoidProfileSubsystem2876 + * + * @return the measurement of the process variable + */ + protected abstract TrapezoidProfile.State getMeasurement(); +}