From 56688d03cdc82265843c2c353d5469e57e6d783b Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 27 Oct 2024 18:15:38 +0800 Subject: [PATCH] fine tuned the drivetrain pids --- .../deploy/choreo/grab sixth and shoot.traj | 3 +- .../deploy/choreo/shoot third and fourth.traj | 3 +- .../commands/drive/ChassisFaceToRotation.java | 2 +- .../frc/robot/commands/drive/DriveToPose.java | 2 +- .../robot/commands/drive/JoystickDrive.java | 6 +-- .../drive/JoystickDriveAndAimAtTarget.java | 43 ++++++++++++------- .../robot/constants/DriveControlLoops.java | 10 ++--- .../robot/constants/DriveTrainConstants.java | 2 +- .../frc/robot/constants/TunerConstants.java | 4 +- .../frc/robot/constants/VisionConstants.java | 4 +- .../drive/HolonomicDriveSubsystem.java | 27 ++++++------ .../robot/subsystems/drive/SwerveDrive.java | 11 ++--- .../utils/CustomPIDs/MaplePIDController.java | 15 ++++--- 13 files changed, 72 insertions(+), 60 deletions(-) diff --git a/src/main/deploy/choreo/grab sixth and shoot.traj b/src/main/deploy/choreo/grab sixth and shoot.traj index e325f9f..415005f 100644 --- a/src/main/deploy/choreo/grab sixth and shoot.traj +++ b/src/main/deploy/choreo/grab sixth and shoot.traj @@ -82,6 +82,5 @@ {"t":2.6072148262330437, "x":3.900952100753784, "y":5.60628080368042, "heading":3.141592653589793, "vx":0.0, "vy":0.0, "omega":0.0, "ax":11.241231668708654, "ay":7.457031618684653, "alpha":-0.20225908819860608, "fx":[111.57544211985864,112.58500129044012,113.23875308269538,112.250070255352], "fy":[75.82418489615657,74.31544288372957,73.3176641188492,74.82397284865081]}], "splits":[] }, - "events":[], - "pplib_commands":[] + "events":[] } \ No newline at end of file diff --git a/src/main/deploy/choreo/shoot third and fourth.traj b/src/main/deploy/choreo/shoot third and fourth.traj index 82f7668..02dd989 100644 --- a/src/main/deploy/choreo/shoot third and fourth.traj +++ b/src/main/deploy/choreo/shoot third and fourth.traj @@ -77,6 +77,5 @@ {"t":2.2229604277583737, "x":3.002295970916748, "y":6.939965724945068, "heading":-2.704135164645048, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.785672543246733, "ay":-4.301998901297384, "alpha":0.006409431260137176, "fx":[-127.84703842576408,-127.86883944817907,-127.8664088501156,-127.84461500581048], "fy":[-43.048701920816214,-42.98391881232034,-42.99129291253959,-43.056042406219184]}], "splits":[] }, - "events":[], - "pplib_commands":[] + "events":[] } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java b/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java index f0774c2..44fe706 100644 --- a/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java +++ b/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java @@ -36,7 +36,7 @@ public void initialize() { @Override public void execute() { final double rotationFeedBack = chassisRotationController.calculate(driveSubsystem.getFacing().getRadians(), targetRotationSupplier.get().getRadians()); - driveSubsystem.runRobotCentricChassisSpeeds(new ChassisSpeeds(0, 0, rotationFeedBack)); + driveSubsystem.runRobotCentricChassisSpeeds(new ChassisSpeeds(0, 0, rotationFeedBack), false); } @Override diff --git a/src/main/java/frc/robot/commands/drive/DriveToPose.java b/src/main/java/frc/robot/commands/drive/DriveToPose.java index 6b205dd..842169b 100644 --- a/src/main/java/frc/robot/commands/drive/DriveToPose.java +++ b/src/main/java/frc/robot/commands/drive/DriveToPose.java @@ -56,7 +56,7 @@ public void execute() { feedBackSpeeds.vxMetersPerSecond *= speedConstrainMPS / feedBackSpeedMagnitude; feedBackSpeeds.vyMetersPerSecond *= speedConstrainMPS / feedBackSpeedMagnitude; } - driveSubsystem.runRobotCentricChassisSpeeds(feedBackSpeeds); + driveSubsystem.runRobotCentricChassisSpeeds(feedBackSpeeds, false); } @Override diff --git a/src/main/java/frc/robot/commands/drive/JoystickDrive.java b/src/main/java/frc/robot/commands/drive/JoystickDrive.java index d0c425f..2502ea7 100644 --- a/src/main/java/frc/robot/commands/drive/JoystickDrive.java +++ b/src/main/java/frc/robot/commands/drive/JoystickDrive.java @@ -49,7 +49,7 @@ public JoystickDrive(MapleJoystickDriveInput input, BooleanSupplier useDriverSta this.previousRotationalInputTimer.start(); this.rotationProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints( - driveSubsystem.getChassisMaxAngularVelocity(), + driveSubsystem.getChassisMaxAngularVelocity() * 0.7, driveSubsystem.getChassisMaxAngularAccelerationRadPerSecSq() )); this.chassisRotationController = new MaplePIDController(DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP); @@ -139,9 +139,9 @@ public void execute() { } if (useDriverStationCentricSwitch.getAsBoolean()) - driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance); + driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance, true); else - driveSubsystem.runRobotCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance); + driveSubsystem.runRobotCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance, true); Logger.recordOutput("rotationMaintain", new Pose2d( driveSubsystem.getPose().getTranslation(), diff --git a/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java b/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java index f67e851..0e58cbe 100644 --- a/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java +++ b/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java @@ -1,5 +1,6 @@ package frc.robot.commands.drive; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -9,6 +10,7 @@ import frc.robot.Robot; import frc.robot.constants.DriveControlLoops; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; +import frc.robot.subsystems.drive.SwerveDrive; import frc.robot.utils.MapleJoystickDriveInput; import frc.robot.utils.MapleShooterOptimization; import frc.robot.utils.CustomPIDs.MaplePIDController; @@ -50,6 +52,7 @@ public JoystickDriveAndAimAtTarget(MapleJoystickDriveInput input, HolonomicDrive public void initialize() { this.chassisRotationController.calculate(driveSubsystem.getRawGyroYaw().getRadians()); this.chassisRotationInPosition = false; + SwerveDrive.acceptRotationalMeasurement = false; } @Override @@ -62,11 +65,11 @@ public void execute() { getRotationalCorrectionVelocityRadPerSec() )); - driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeeds); + driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeeds, false); super.execute(); } - public static double FEED_FORWARD_RATE = 0.5, ROTATION_TOLERANCE_DEGREES = 2.5; + public static double FEED_FORWARD_RATE = 1; public double getRotationalCorrectionVelocityRadPerSec() { final Translation2d robotPosition = driveSubsystem.getPose().getTranslation(); final ChassisSpeeds robotVelocityFieldRelative = driveSubsystem.getMeasuredChassisSpeedsFieldRelative(); @@ -90,23 +93,28 @@ public double getRotationalCorrectionVelocityRadPerSec() { final double targetedFacingChangeRateRadPerSec = targetedFacingAfterDT.minus(targetedFacing).getRadians() / Robot.defaultPeriodSecs; - Logger.recordOutput("Drive/Face To Target Rotation (Deg)", targetedFacing.getDegrees()); final double feedBackRotationalSpeed = chassisRotationController.calculate( driveSubsystem.getFacing().getRadians(), - targetedFacing.getRadians()), - feedForwardRotationalSpeed = targetedFacingChangeRateRadPerSec - * FEED_FORWARD_RATE; - - final double chassisRotationalError = Math.abs( - targetedFacing - .minus(driveSubsystem.getFacing()) - .getDegrees() - ); - Logger.recordOutput("Drive/Aim At Target Rational Error (Deg)", chassisRotationalError); - this.chassisRotationInPosition = chassisRotationalError < ROTATION_TOLERANCE_DEGREES; + targetedFacing.getRadians() + ), + feedForwardRotationalSpeed = MathUtil.applyDeadband( + targetedFacingChangeRateRadPerSec * FEED_FORWARD_RATE, + Math.toRadians(30), + Double.POSITIVE_INFINITY + ); + + final Rotation2d chassisRotationalError = targetedFacing + .minus(driveSubsystem.getFacing()); + this.chassisRotationInPosition = Math.abs(chassisRotationalError.getRadians()) + < DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP.errorTolerance; + SmartDashboard.putBoolean("Chassis Rotation Aiming Target Reached", chassisRotationInPosition); - SmartDashboard.putNumber("Chassis Rotation Aiming Error", chassisRotationalError); + SmartDashboard.putNumber("Chassis Rotation Aiming Error", chassisRotationalError.getDegrees()); + Logger.recordOutput("DriveAndAimAtTarget/Aim At Target Rational Error (Deg)", chassisRotationalError.getDegrees()); + Logger.recordOutput("DriveAndAimAtTarget/Rotation Target (Deg)", targetedFacing.getDegrees()); + Logger.recordOutput("DriveAndAimAtTarget/FeedForwardSpeed (Deg per Sec)", Math.toDegrees(feedForwardRotationalSpeed)); + Logger.recordOutput("DriveAndAimAtTarget/FeedBackSpeed (Deg per Sec)", Math.toDegrees(feedBackRotationalSpeed)); return feedForwardRotationalSpeed + feedBackRotationalSpeed; } @@ -115,4 +123,9 @@ public double getRotationalCorrectionVelocityRadPerSec() { public boolean chassisRotationInPosition() { return chassisRotationInPosition; } + + @Override + public void end(boolean interrupted) { + SwerveDrive.acceptRotationalMeasurement = true; + } } diff --git a/src/main/java/frc/robot/constants/DriveControlLoops.java b/src/main/java/frc/robot/constants/DriveControlLoops.java index db45872..9e4fab0 100644 --- a/src/main/java/frc/robot/constants/DriveControlLoops.java +++ b/src/main/java/frc/robot/constants/DriveControlLoops.java @@ -7,11 +7,11 @@ public class DriveControlLoops { public static final MaplePIDController.MaplePIDConfig CHASSIS_ROTATION_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( - Math.toRadians(540), - Math.toRadians(45), - 0.01, + Math.toRadians(400), + Math.toRadians(90), + 0.03, Math.toRadians(3), - 0.07, + 0.04, true, 0 ); @@ -28,7 +28,7 @@ public class DriveControlLoops { public static final MaplePIDController.MaplePIDConfig STEER_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( 0.5, Math.toRadians(90), - 0.02, + 0, Math.toRadians(1.5), 0, true, diff --git a/src/main/java/frc/robot/constants/DriveTrainConstants.java b/src/main/java/frc/robot/constants/DriveTrainConstants.java index bcfc48e..e993d0e 100644 --- a/src/main/java/frc/robot/constants/DriveTrainConstants.java +++ b/src/main/java/frc/robot/constants/DriveTrainConstants.java @@ -81,7 +81,7 @@ public class DriveTrainConstants { MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG ), CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS, - CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS * 3; + CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS * 2; public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics(MODULE_TRANSLATIONS); diff --git a/src/main/java/frc/robot/constants/TunerConstants.java b/src/main/java/frc/robot/constants/TunerConstants.java index cb9f5e1..912078d 100644 --- a/src/main/java/frc/robot/constants/TunerConstants.java +++ b/src/main/java/frc/robot/constants/TunerConstants.java @@ -94,8 +94,8 @@ public class TunerConstants { public static final double kSteerInertia = 0.00001; public static final double kDriveInertia = 0.001; // Simulated voltage necessary to overcome friction - public static final double kSteerFrictionVoltage = 0.25; - public static final double kDriveFrictionVoltage = 0.25; + public static final double kSteerFrictionVoltage = 0.15; + public static final double kDriveFrictionVoltage = 0.2; public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withCANbusName(kCANbusName) diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 4a7b349..204e7ed 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -23,8 +23,8 @@ public class VisionConstants { ROTATIONAL_STANDARD_DEVS_THRESHOLD_DISCARD_RESULT = Math.toRadians(20), /* standard deviation for odometry and gyros */ - ODOMETRY_TRANSLATIONAL_STANDARD_ERROR_METERS = 0.08, - GYRO_ROTATIONAL_STANDARD_ERROR_RADIANS = Math.toRadians(0.5); + ODOMETRY_TRANSLATIONAL_STANDARD_ERROR_METERS = 0.04, + GYRO_ROTATIONAL_STANDARD_ERROR_RADIANS = Math.toRadians(0.1); public static final List photonVisionCameras = List.of( diff --git a/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java index bbd997b..cd8c70b 100644 --- a/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java @@ -21,6 +21,7 @@ import frc.robot.Robot; import frc.robot.constants.DriveTrainConstants; import frc.robot.constants.FieldConstants; +import frc.robot.subsystems.vision.apriltags.MapleMultiTagPoseEstimator; import frc.robot.utils.LocalADStarAK; import org.ironmaple.utils.FieldMirroringUtils; import org.littletonrobotics.junction.Logger; @@ -54,11 +55,10 @@ public interface HolonomicDriveSubsystem extends Subsystem { /** * Adds a vision measurement to the pose estimator. * - * @param visionPose The pose of the robot as measured by the vision camera. + * @param poseEstimationResult the pose estimation result * @param timestamp The timestamp of the vision measurement in seconds. - * @param measurementStdDevs the standard deviation of the measurement */ - default void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix measurementStdDevs) {} + default void addVisionMeasurement(MapleMultiTagPoseEstimator.RobotPoseEstimationResult poseEstimationResult, double timestamp) {} /** * @return the measured(actual) velocities of the chassis, robot-relative @@ -87,40 +87,37 @@ default PathConstraints getChassisConstrains(double speedMultiplier) { * runs a driverstation-centric ChassisSpeeds * @param driverStationCentricSpeeds a continuous chassis speeds, driverstation-centric, normally from a gamepad * */ - default void runDriverStationCentricChassisSpeeds(ChassisSpeeds driverStationCentricSpeeds) { + default void runDriverStationCentricChassisSpeeds(ChassisSpeeds driverStationCentricSpeeds, boolean discretize) { final Rotation2d driverStationFacing = FieldConstants.getDriverStationFacing(); runRobotCentricChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( driverStationCentricSpeeds, getPose().getRotation().minus(driverStationFacing) - )); + ), discretize); } /** * runs a field-centric ChassisSpeeds * @param fieldCentricSpeeds a continuous chassis speeds, field-centric, normally from a pid position controller * */ - default void runFieldCentricChassisSpeeds(ChassisSpeeds fieldCentricSpeeds) { + default void runFieldCentricChassisSpeeds(ChassisSpeeds fieldCentricSpeeds, boolean discretize) { runRobotCentricChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( fieldCentricSpeeds, getPose().getRotation() - )); + ), discretize); } default void stop() { - runRobotCentricChassisSpeeds(new ChassisSpeeds()); + runRobotCentricChassisSpeeds(new ChassisSpeeds(), false); } /** * runs a ChassisSpeeds, pre-processed with ChassisSpeeds.discretize() * @param speeds a continuous chassis speed, robot-centric * */ - default void runRobotCentricChassisSpeeds(ChassisSpeeds speeds) { - final double PERCENT_DEADBAND = 0.02; - if (Math.abs(speeds.omegaRadiansPerSecond) < PERCENT_DEADBAND * getChassisMaxAngularVelocity() - && Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) < PERCENT_DEADBAND * getChassisMaxLinearVelocityMetersPerSec()) - speeds = new ChassisSpeeds(); - - runRawChassisSpeeds(ChassisSpeeds.discretize(speeds, Robot.defaultPeriodSecs)); + default void runRobotCentricChassisSpeeds(ChassisSpeeds speeds, boolean discretize) { + if (discretize) + speeds = ChassisSpeeds.discretize(speeds, Robot.defaultPeriodSecs); + runRawChassisSpeeds(speeds); } default void configHolonomicPathPlannerAutoBuilder() { diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 04f21b9..a454fc6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -4,7 +4,6 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -14,12 +13,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.constants.FieldConstants; import frc.robot.subsystems.MapleSubsystem; import frc.robot.subsystems.drive.IO.*; +import frc.robot.subsystems.vision.apriltags.MapleMultiTagPoseEstimator; import frc.robot.utils.Alert; import frc.robot.utils.MapleTimeUtils; import org.littletonrobotics.junction.AutoLogOutput; @@ -242,10 +240,13 @@ public ChassisSpeeds getMeasuredChassisSpeedsRobotRelative() { @Override public double getChassisMaxAngularVelocity() {return CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC;} @Override public double getChassisMaxAngularAccelerationRadPerSecSq() {return CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ;} + public static boolean acceptRotationalMeasurement = true; @Override - public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix measurementStdDevs) { - poseEstimator.addVisionMeasurement(visionPose, timestamp ,measurementStdDevs); + public void addVisionMeasurement(MapleMultiTagPoseEstimator.RobotPoseEstimationResult poseEstimationResult, double timestamp) { previousMeasurementTimeStamp = Math.max(timestamp, previousMeasurementTimeStamp); + if (!acceptRotationalMeasurement) + poseEstimationResult.rotationalStandardDeviationRadians = 100; + poseEstimator.addVisionMeasurement(poseEstimationResult.pointEstimation, timestamp, poseEstimationResult.getEstimationStandardError()); } private double previousMeasurementTimeStamp = -1; diff --git a/src/main/java/frc/robot/utils/CustomPIDs/MaplePIDController.java b/src/main/java/frc/robot/utils/CustomPIDs/MaplePIDController.java index f6a7aef..dc843ee 100644 --- a/src/main/java/frc/robot/utils/CustomPIDs/MaplePIDController.java +++ b/src/main/java/frc/robot/utils/CustomPIDs/MaplePIDController.java @@ -19,15 +19,18 @@ public MaplePIDController( @Override public double calculate(double measurement) { - return MathUtil.clamp( - MathUtil.applyDeadband(super.calculate(measurement), config.deadBand) - , -config.maximumPower, config.maximumPower); + final double constrainedCorrection = MathUtil.clamp(super.calculate(measurement), -config.maximumPower, config.maximumPower); + + if (Math.abs(constrainedCorrection) < config.deadBand) + return 0; + return constrainedCorrection; + } public static final class MaplePIDConfig { - final double maximumPower, errorStartDecelerate, deadBand, errorTolerance, timeThinkAhead; - final double Kp, Ki, Kd; - final boolean isCircularLoop; + public final double maximumPower, errorStartDecelerate, deadBand, errorTolerance, timeThinkAhead; + public final double Kp, Ki, Kd; + public final boolean isCircularLoop; public MaplePIDConfig(double maximumPower, double errorStartDecelerate, double percentDeadBand, double errorTolerance, double timeThinkAhead, boolean isCircularLoop, double ki) { this.maximumPower = maximumPower;