From 73b468e608a09b96fb058698910fe6d26749c3df Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Fri, 12 Jan 2024 17:10:43 -0500 Subject: [PATCH] Fully Finish Autonomous Code + Migrate PID --- src/main/java/frc/robot/DriveConstants.java | 9 + .../java/frc/robot/commands/AimCommand.java | 3 +- .../frc/robot/subsystems/DriveSubsystem.java | 170 +++++++++--------- 3 files changed, 99 insertions(+), 83 deletions(-) diff --git a/src/main/java/frc/robot/DriveConstants.java b/src/main/java/frc/robot/DriveConstants.java index 13fe510..219ba47 100644 --- a/src/main/java/frc/robot/DriveConstants.java +++ b/src/main/java/frc/robot/DriveConstants.java @@ -37,10 +37,19 @@ public final class DriveConstants { public static final double kIDriveVel = 0.0; public static final double kDDriveVel = 0.0; public static final double kIzDriveVel = 0.0; // error before integral takes effect + + public static final double kPDrivePos = 0.1; + public static final double kIDrivePos = 0.0; + public static final double kDDrivePos = 0.0; + public static final double kIzDrivePos = 0.0; // error before integral takes effect // Helper class that converts a chassis velocity (dx and dtheta components) to left and right // wheel velocities for a differential drive. public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters); // Default path replanning config. See the API for the options public static final ReplanningConfig autoReplanningConfig = new ReplanningConfig(); + + // Motor Controller PID Slots + public static final int kDrivetrainVelocityPIDSlot = 0; + public static final int kDrivetrainPositionPIDSlot = 1; } diff --git a/src/main/java/frc/robot/commands/AimCommand.java b/src/main/java/frc/robot/commands/AimCommand.java index fe6a50d..900ac71 100644 --- a/src/main/java/frc/robot/commands/AimCommand.java +++ b/src/main/java/frc/robot/commands/AimCommand.java @@ -56,7 +56,6 @@ public void execute() { m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget); // we reset the angle everytime as the target could change / move. m_driveSubsystem.turnSetGoal(angleGoal); - m_driveSubsystem.distanceSetGoal(distanceFromTarget); } else { SmartDashboard.putBoolean("CameraTargetDetected", false); SmartDashboard.putNumber("CameraTargetPitch", 0.0); @@ -68,7 +67,7 @@ public void execute() { @Override public void end(boolean interrupted) { m_driveSubsystem.turnResetPID(); // we clear the PID turn controller. - m_driveSubsystem.distanceResetPID(); // we clear the distance PID contoller too. + m_driveSubsystem.stop(); // end execution of on board PID. } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 9721067..6cddabb 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -74,25 +74,6 @@ public class DriveSubsystem extends SubsystemBase { turn_D, new TrapezoidProfile.Constraints(MaxTurnRateDegPerS, MaxTurnAccelerationDegPerSSquared)); - // Distance PID / MoveDistance - static final double distance_P = 0.1; - static final double distance_I = 0.00; - static final double distance_D = 0.00; - static final double distanceMaxSpeed = 1; // m/s - static final double distanceMaxAcceleration = 2; // m/s^2 - static final double DistanceTolerance = 0.01; // max diff in meters - static final double DistanceSpeedTolerance = 0.1; // ignore if velocity is below. (m) - // false when inactive, true when active / a target is set. - private boolean distanceControllerEnabled = false; - private double distanceThrottleRate; // This value will be updated by the PID Controller - // pid controller for "MoveDistance" - private final ProfiledPIDController m_distanceController = - new ProfiledPIDController( - distance_P, - distance_I, - distance_D, - new TrapezoidProfile.Constraints(distanceMaxSpeed, distanceMaxAcceleration)); - // Balance PID / AutoBalance static final double balance_P = 0.0625; // 1/16 static final double balance_I = 0.00; @@ -164,19 +145,57 @@ public DriveSubsystem() { resetGyro(); // setup PID controllers - // setup feed forward - m_backLeftPIDController.setP(DriveConstants.kPDriveVel); - m_backRightPIDController.setP(DriveConstants.kPDriveVel); - m_backLeftPIDController.setI(DriveConstants.kIDriveVel); - m_backRightPIDController.setI(DriveConstants.kIDriveVel); - m_backLeftPIDController.setD(DriveConstants.kDDriveVel); - m_backRightPIDController.setD(DriveConstants.kDDriveVel); - m_backLeftPIDController.setIZone(DriveConstants.kIzDriveVel); - m_backRightPIDController.setIZone(DriveConstants.kIzDriveVel); - m_backLeftPIDController.setFF(DriveConstants.kDriveFeedForward); - m_backRightPIDController.setFF(DriveConstants.kDriveFeedForward); - // m_backLeftPIDController.setOutputRange(kMinOutput, kMaxOutput); - // m_backRightPIDController.setOutputRange(kMinOutput, kMaxOutput); + // setup velocity PID controllers (used by auto) + m_backLeftPIDController.setP( + DriveConstants.kPDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backRightPIDController.setP( + DriveConstants.kPDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backLeftPIDController.setI( + DriveConstants.kIDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backRightPIDController.setI( + DriveConstants.kIDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backLeftPIDController.setD( + DriveConstants.kDDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backRightPIDController.setD( + DriveConstants.kDDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backLeftPIDController.setIZone( + DriveConstants.kIzDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backRightPIDController.setIZone( + DriveConstants.kIzDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backLeftPIDController.setFF( + DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backRightPIDController.setFF( + DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainVelocityPIDSlot); + // m_backLeftPIDController.setOutputRange(kMinOutput, kMaxOutputVel, + // DriveConstants.kDrivetrainVelocityPIDSlot); + // m_backRightPIDController.setOutputRange(kMinOutput, kMaxOutputVel, + // DriveConstants.kDrivetrainVelocityPIDSlot); + + // setup position PID controllers (used when we manually path find) + m_backLeftPIDController.setP( + DriveConstants.kPDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backRightPIDController.setP( + DriveConstants.kPDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backLeftPIDController.setI( + DriveConstants.kIDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backRightPIDController.setI( + DriveConstants.kIDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backLeftPIDController.setD( + DriveConstants.kDDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backRightPIDController.setD( + DriveConstants.kDDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backLeftPIDController.setIZone( + DriveConstants.kIzDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backRightPIDController.setIZone( + DriveConstants.kIzDrivePos, DriveConstants.kDrivetrainPositionPIDSlot); + m_backLeftPIDController.setFF( + DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainPositionPIDSlot); + m_backRightPIDController.setFF( + DriveConstants.kDriveFeedForward, DriveConstants.kDrivetrainPositionPIDSlot); + // m_backLeftPIDController.setOutputRange(kMinOutput, kMaxOutputPos, + // DriveConstants.kDrivetrainPositionPIDSlot); + // m_backRightPIDController.setOutputRange(kMinOutput, kMaxOutputPos, + // DriveConstants.kDrivetrainPositionPIDSlot); // configure Odemetry m_driveOdometry = @@ -190,8 +209,6 @@ public DriveSubsystem() { // config turn pid controller. m_turnController.enableContinuousInput(-180.0f, 180.0f); m_turnController.setTolerance(TurnToleranceDeg, TurnRateToleranceDegPerS); - // config distance pid controller - m_distanceController.setTolerance(DistanceTolerance, DistanceSpeedTolerance); // this is the target pitch/ tilt error. m_balanceController.setGoal(0); m_balanceController.setTolerance(BalanceToleranceDeg); // max error in degrees @@ -220,22 +237,6 @@ public DriveSubsystem() { SmartDashboard.putData("Field", field); // add field to dashboard } - public double getVelocityLeft() { - return (m_encoderBackLeft.getVelocity() + m_encoderFrontLeft.getVelocity()) / 2; - } - - public double getVelocityRight() { - return (m_encoderBackRight.getVelocity() + m_encoderFrontRight.getVelocity()) / 2; - } - - public double getPositionLeft() { - return (m_encoderBackLeft.getPosition() + m_encoderFrontLeft.getPosition()) / 2; - } - - public double getPositionRight() { - return (m_encoderBackRight.getPosition() + m_encoderFrontRight.getPosition()) / 2; - } - // default tank drive function // **tank drive = specific control style where two parallel forces of motion are controlled to // create linear and rotational motion @@ -261,31 +262,10 @@ public void balanceCorrection(double gyroPitchAngle) { } } - public void distanceResetPID() { - /** This should be run when stopping a pid command. */ - distanceControllerEnabled = false; - } - - public void distanceSetGoal(double targetDistance) { - m_distanceController.setGoal(AverageDistance() + targetDistance); - } - - private void calculateDistanceRate(double targetDistance) { - if (!distanceControllerEnabled) { - m_distanceController.reset(AverageDistance()); - m_distanceController.setGoal(AverageDistance() + targetDistance); - distanceControllerEnabled = true; - } - distanceThrottleRate = - MathUtil.clamp(m_distanceController.calculate(AverageDistance()), -1.0, 1.0); - } - - public void driveToDistance(double targetDistance) { - this.calculateDistanceRate(targetDistance); - double leftStickValue = turnRotateToAngleRate; - double rightStickValue = turnRotateToAngleRate; - if (!m_distanceController.atGoal()) { - this.tankDrive(leftStickValue, rightStickValue); + public void driveToRelativePosition(double targetPosition) { + if (targetPosition < 0.1) { // less then 0.1 meters, do nothing, pervents feedback loop + double totalPosition = this.AverageDistance() + targetPosition; + this.driveToPosition(totalPosition); } } @@ -295,11 +275,12 @@ public void driveAndTurn(double gyroYawAngle, double TargetAngleDegrees, double this should not be used in auto mode. */ this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees); - this.calculateDistanceRate(targetDistance); - double leftStickValue = distanceThrottleRate + turnRotateToAngleRate; - double rightStickValue = distanceThrottleRate - turnRotateToAngleRate; - if (!m_distanceController.atGoal() || !m_turnController.atGoal()) { + double leftStickValue = turnRotateToAngleRate; + double rightStickValue = turnRotateToAngleRate; + if (!m_turnController.atGoal()) { this.tankDrive(leftStickValue, rightStickValue); + } else { + this.driveToRelativePosition(targetDistance); } } @@ -387,8 +368,19 @@ public void setWheelVelocities(DifferentialDriveWheelSpeeds speeds) { double leftSpeed = speeds.leftMetersPerSecond; double rightSpeed = speeds.rightMetersPerSecond; // set to position of motors - m_backLeftPIDController.setReference(leftSpeed, CANSparkBase.ControlType.kVelocity); - m_backRightPIDController.setReference(rightSpeed, CANSparkBase.ControlType.kVelocity); + m_backLeftPIDController.setReference( + leftSpeed, CANSparkBase.ControlType.kVelocity, DriveConstants.kDrivetrainVelocityPIDSlot); + m_backRightPIDController.setReference( + rightSpeed, CANSparkBase.ControlType.kVelocity, DriveConstants.kDrivetrainVelocityPIDSlot); + } + + // in meters, use averageDistance() to get average distance traveled, as an offset to set this + // function. + public void driveToPosition(double NewPosition) { + m_backLeftPIDController.setReference( + NewPosition, CANSparkBase.ControlType.kPosition, DriveConstants.kDrivetrainPositionPIDSlot); + m_backRightPIDController.setReference( + NewPosition, CANSparkBase.ControlType.kPosition, DriveConstants.kDrivetrainPositionPIDSlot); } public Pose2d getPose() { @@ -460,6 +452,22 @@ public void resetGyro() { m_Gyro.reset(); } + public double getVelocityLeft() { + return (m_encoderBackLeft.getVelocity() + m_encoderFrontLeft.getVelocity()) / 2; + } + + public double getVelocityRight() { + return (m_encoderBackRight.getVelocity() + m_encoderFrontRight.getVelocity()) / 2; + } + + public double getPositionLeft() { + return (m_encoderBackLeft.getPosition() + m_encoderFrontLeft.getPosition()) / 2; + } + + public double getPositionRight() { + return (m_encoderBackRight.getPosition() + m_encoderFrontRight.getPosition()) / 2; + } + @Override public void periodic() { // This method will be called once per scheduler run