From 3b2afddeec5b85b32d4c4794a304be13ee31b43a Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 10 Mar 2024 14:37:18 -0400 Subject: [PATCH] initial auto and vision tuning --- .../deploy/pathplanner/autos/1M Auto.auto | 29 ++++++++++++- ...{1M Path.path => speakerToCenterNote.path} | 14 +++---- src/main/java/frc/robot/Constants.java | 28 ++++++------- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 18 +++++++- .../robot/commands/ShooterAutoCommand.java | 41 +++++++++++++++++++ .../subsystems/drive/DriveSubsystem.java | 9 ++-- .../drive/module/ModuleIOTalonFX.java | 4 +- .../subsystems/shooter/ShooterSubsystem.java | 18 +++++++- .../subsystems/vision/VisionSubsystem.java | 15 ++++--- src/main/java/lib/utils/FieldConstants.java | 7 ++++ vendordeps/playingwithfusion2024.json | 10 ++--- 12 files changed, 151 insertions(+), 44 deletions(-) rename src/main/deploy/pathplanner/paths/{1M Path.path => speakerToCenterNote.path} (77%) create mode 100644 src/main/java/frc/robot/commands/ShooterAutoCommand.java diff --git a/src/main/deploy/pathplanner/autos/1M Auto.auto b/src/main/deploy/pathplanner/autos/1M Auto.auto index 77afe835..f0521a12 100644 --- a/src/main/deploy/pathplanner/autos/1M Auto.auto +++ b/src/main/deploy/pathplanner/autos/1M Auto.auto @@ -6,9 +6,34 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "1M Path" + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "speakerToCenterNote" + } + }, + { + "type": "named", + "data": { + "name": "Run Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" } } ] diff --git a/src/main/deploy/pathplanner/paths/1M Path.path b/src/main/deploy/pathplanner/paths/speakerToCenterNote.path similarity index 77% rename from src/main/deploy/pathplanner/paths/1M Path.path rename to src/main/deploy/pathplanner/paths/speakerToCenterNote.path index c5ce33e3..340e0ca3 100644 --- a/src/main/deploy/pathplanner/paths/1M Path.path +++ b/src/main/deploy/pathplanner/paths/speakerToCenterNote.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.34, + "x": 2.4599653194866877, "y": 5.55 }, "prevControl": { - "x": 1.3399999999999999, + "x": 1.4599653194866877, "y": 5.55 }, "nextControl": null, @@ -32,21 +32,21 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.44451110511873265, + "rotation": -179.61351583273776, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 0.2420983632108403, + "rotation": 179.7976785591257, "velocity": 0 }, - "useDefaultConstraints": true + "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 311bae68..2a5f1693 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -86,24 +86,24 @@ private DriveConstants() { public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d( - Units.inchesToMeters(7.0351), // 11.0351 - Units.inchesToMeters(-6.523204), // 10.023204 - Units.inchesToMeters(6.1374)), // 4.1374 + Units.inchesToMeters(11.0351), // 11.0351 + Units.inchesToMeters(10.023204), // 10.023204 + Units.inchesToMeters(4.1374)), // 4.1374 new Rotation3d( Units.degreesToRadians(0.0), - Units.degreesToRadians(-30.0 + 1), // -120.0 + 91.0 - Units.degreesToRadians(-13.7)) // 165.3224 + 180 + Units.degreesToRadians(-30.0), // -120.0 + 91.0 + Units.degreesToRadians(-14.7)) // 165.3224 + 180 ); public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d( - Units.inchesToMeters(5.0351), //11.0351 - Units.inchesToMeters(-22.023204), //-10.023204 + Units.inchesToMeters(11.0351), //11.0351 + Units.inchesToMeters(-10.023204), //-10.023204 Units.inchesToMeters(4.1374)), // 7.1374 new Rotation3d( - Units.degreesToRadians(2.0), - Units.degreesToRadians(-30.0 + 3), // -30.0 - 1 - Units.degreesToRadians(5.25)) // 165.3224) + Units.degreesToRadians(0.0), + Units.degreesToRadians(-30.0), // -30.0 - 1 + Units.degreesToRadians(14.7)) // 165.3224) ); public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( @@ -123,7 +123,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.472168), // offset 0.457764 + Units.rotationsToDegrees(-0.017822) + 180, // offset 0.457764 true, // inversion ModuleConstants.GearRatios.L3_KRAKEN ); @@ -134,7 +134,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.046143), + Units.rotationsToDegrees(-0.453857) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); @@ -145,7 +145,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.073730), + Units.rotationsToDegrees(0.428467) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); @@ -156,7 +156,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.404297), + Units.rotationsToDegrees(-0.093750) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 828a504e..17979ee4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -80,7 +80,7 @@ public void robotPeriodic() { // the Command-based framework to work. CommandScheduler.getInstance().run(); TalonFXFactory.handleFaults(); -// DataLogUtil.updateTables(); + DataLogUtil.updateTables(); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae321201..6b26f431 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,7 @@ import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; import frc.robot.commands.IntakeControlCommand; +import frc.robot.commands.ShooterAutoCommand; import frc.robot.subsystems.arm.*; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOKraken; @@ -222,9 +223,13 @@ private void configureButtonBindings() { ampLineupTrigger.whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), - m_shooter)); + m_shooter) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); controller.pov(0).onTrue(Commands.runOnce(SignalLogger::stop)); + controller.pov(180).whileTrue(m_driveSubsystem.pathfollowFactory( + () -> AllianceFlipUtil.apply(FieldConstants.AMP_LINEUP) + )); m_driveSubsystem.setDefaultCommand( DriveCommands.joystickDrive( @@ -248,7 +253,16 @@ private void configureButtonBindings() { * Use this method to configure any named commands needed for PathPlanner autos */ private void configureNamedCommands() { - NamedCommands.registerCommand("Run Intake", Commands.run(() -> m_shooter.setIntakePower(0.5))); + NamedCommands.registerCommand("Run Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); + + NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter) + .raceWith(DriveCommands.alignmentDrive( + m_driveSubsystem, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) + ))); } private void configureDashboard() { diff --git a/src/main/java/frc/robot/commands/ShooterAutoCommand.java b/src/main/java/frc/robot/commands/ShooterAutoCommand.java new file mode 100644 index 00000000..cde298a7 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterAutoCommand.java @@ -0,0 +1,41 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.arm.ArmSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; + + +public class ShooterAutoCommand extends Command { + private final ArmSubsystem armSubsystem; + private final ShooterSubsystem shooterSubsystem; + + public ShooterAutoCommand(ArmSubsystem armSubsystem, ShooterSubsystem shooterSubsystem) { + this.armSubsystem = armSubsystem; + this.shooterSubsystem = shooterSubsystem; + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.armSubsystem, this.shooterSubsystem); + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + shooterSubsystem.runShooterVelocity(shooterSubsystem.atSpeed()).execute(); + armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); + } + + @Override + public boolean isFinished() { + return !shooterSubsystem.hasPiece(); + } + + @Override + public void end(boolean interrupted) { + shooterSubsystem.setShooterPowerFactory(0.0, 0.0, 0.0).execute(); + armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 5c26c657..8533b3fa 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -47,6 +47,7 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Supplier; import lib.logger.DataLogUtil; import lib.logger.DataLogUtil.DataLogTable; @@ -156,7 +157,9 @@ public DriveSubsystem( kinematics, new Rotation2d(), getModulePositions(), - new Pose2d() + new Pose2d(), + VecBuilder.fill(0.15, 0.15, 0.1), + VecBuilder.fill(1.0, 1.0, 1.25) ); // Configure AutoBuilder for PathPlanner @@ -375,8 +378,8 @@ public double getMaxAngularSpeedRadPerSec() { return DriveConstants.MAX_ANGULAR_SPEED; } - public Command pathfollowFactory(Pose2d pose) { - return AutoBuilder.pathfindToPose(pose, DriveConstants.DEFAULT_CONSTRAINTS); + public Command pathfollowFactory(Supplier pose) { + return AutoBuilder.pathfindToPose(pose.get(), DriveConstants.DEFAULT_CONSTRAINTS); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index a5a2e48b..3df18499 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -99,8 +99,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { // run configs on drive motor var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b0e22924..4a72ebd2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -26,6 +26,9 @@ public class ShooterSubsystem extends SubsystemBase { private final DataLogTable m_logTable = DataLogUtil.getTable("Shooter"); + private double m_leftSpeedSetpoint = 0.0; + private double m_rightSpeedSetpoint = 0.0; + private final GosDoubleProperty m_leftPower = new GosDoubleProperty(false, "Shooter/Left RPM", 3600); private final GosDoubleProperty m_rightPower = new @@ -76,8 +79,11 @@ public Command runShooterVelocity(boolean runKicker) { Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround); - m_io.setLeftVelocityRpm(AimbotUtils.getLeftSpeed(groundDistance)); - m_io.setRightVelocityRpm(AimbotUtils.getRightSpeed(groundDistance)); + m_leftSpeedSetpoint = AimbotUtils.getLeftSpeed(groundDistance); + m_rightSpeedSetpoint = AimbotUtils.getRightSpeed(groundDistance); + + m_io.setLeftVelocityRpm(m_leftSpeedSetpoint); + m_io.setRightVelocityRpm(m_rightSpeedSetpoint); if (runKicker) { m_io.setKickerVoltage(12.0); @@ -140,6 +146,9 @@ public void setupLogging() { m_logTable.addDouble("LeftVelocity", () -> m_inputs.tlVelocityRPM, true); m_logTable.addDouble("RightVelocity", () -> m_inputs.trVelocityRPM, true); + m_logTable.addDouble("LeftSetpoint", () -> m_leftSpeedSetpoint, true); + m_logTable.addDouble("RightSetpoint", () -> m_rightSpeedSetpoint, true); + m_logTable.addDouble("LeftAppliedVolts", () -> m_inputs.tlAppliedVolts, true); m_logTable.addDouble("RightAppliedVolts", () -> m_inputs.trAppliedVolts, true); @@ -151,4 +160,9 @@ public void setupLogging() { m_logTable.addDouble("IntakeCurrent", () -> m_inputs.intakeCurrentDraw, true); } + + public boolean atSpeed() { + return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 75 + && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 75; + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 2d8e6c13..a36cec64 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -31,8 +31,11 @@ public class VisionSubsystem { private final String m_name; private Pose3d m_lastEstimatedPose = new Pose3d(); - private final double xyStdDevCoefficient = 0.01; - private final double thetaStdDevCoefficient = 0.01; + private final double xyStdDevCoefficient = 0.03; + private final double thetaStdDevCoefficient = 0.05; + + private final double xyStdDevMultiTagCoefficient = 0.02; + private final double thetaStdDevMultiTagCoefficient = 0.04; private final PhotonVisionIOInputs inputs = new PhotonVisionIOInputs(); @@ -80,15 +83,15 @@ public Optional getPose(Pose2d prevEstima double distance = estPose.estimatedPose.toPose2d().getTranslation().getDistance( robotToCam.getTranslation().toTranslation2d()); - double xyStdDev = 0.0; - double thetaStdDev = 0.0; + double xyStdDev; + double thetaStdDev; if (estPose.strategy != PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) { xyStdDev = xyStdDevCoefficient * Math.pow(distance, 2.0); thetaStdDev = thetaStdDevCoefficient * Math.pow(distance, 2.0); } else { - xyStdDev = (xyStdDevCoefficient / 2.0) * Math.pow(distance, 2.0); - thetaStdDev = (thetaStdDevCoefficient / 2.0) * Math.pow(distance, 2.0); + xyStdDev = xyStdDevMultiTagCoefficient * Math.pow(distance, 2.0); + thetaStdDev = thetaStdDevMultiTagCoefficient * Math.pow(distance, 2.0); } return Optional.of(new PoseEstimator.TimestampedVisionUpdate(estPose.timestampSeconds, diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java index eaad9c4a..7e800aa2 100644 --- a/src/main/java/lib/utils/FieldConstants.java +++ b/src/main/java/lib/utils/FieldConstants.java @@ -29,6 +29,13 @@ private FieldConstants() {} public static final Translation2d AMP_CENTER = new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); + public static final Pose2d AMP_LINEUP = + new Pose2d( + AMP_CENTER.getX() - Units.inchesToMeters(10), + AMP_CENTER.getY(), + Rotation2d.fromDegrees(90) + ); + /** Staging locations for each note */ public static final class StagingLocations { public static final double CENTERLINE_X = FIELD_LENGTH / 2.0; diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2024.json index 07d47e82..9dde4f2e 100644 --- a/vendordeps/playingwithfusion2024.json +++ b/vendordeps/playingwithfusion2024.json @@ -1,7 +1,7 @@ { "fileName": "playingwithfusion2024.json", "name": "PlayingWithFusion", - "version": "2024.01.28", + "version": "2024.03.09", "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", "frcYear": "2024", "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json", @@ -12,14 +12,14 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-java", - "version": "2024.01.28" + "version": "2024.03.09" } ], "jniDependencies": [ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.28", + "version": "2024.03.09", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-cpp", - "version": "2024.01.28", + "version": "2024.03.09", "headerClassifier": "headers", "sharedLibrary": false, "libName": "PlayingWithFusion", @@ -53,7 +53,7 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.28", + "version": "2024.03.09", "headerClassifier": "headers", "sharedLibrary": true, "libName": "PlayingWithFusionDriver",