Skip to content

Commit

Permalink
PIDController Merge
Browse files Browse the repository at this point in the history 
Merged all PIDControllers  & Constants to one class.
  • Loading branch information
@Iooob
Iooob committed Nov 9, 2023
1 parent 051ad1c commit 789c13c
Show file tree
Hide file tree
Showing 3 changed files with 132 additions and 98 deletions.
34 changes: 34 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,40 @@ public static final class CANConstants {
public static final int MOTORBACKLEFTID = 14;
}

public final class PIDConstants {
public final class TurnPIDConstants {
// Angle PID / RotateToAngle
public static final double turn_P = 0.1;
public static final double turn_I = 0.00;
public static final double turn_D = 0.00;
public static final double MaxTurnRateDegPerS = 100;
public static final double MaxTurnAccelerationDegPerSSquared = 300;
public static final double TurnToleranceDeg = 3; // max diff in degrees
public static final double TurnRateToleranceDegPerS = 10; // degrees per second
}

public final class DistancePIDConstants {
// Distance PID / MoveDistance
public static final double distance_P = 0.1;
public static final double distance_I = 0.00;
public static final double distance_D = 0.00;
public static final double distanceMaxSpeed = 1; // m/s
public static final double distanceMaxAcceleration = 2; // m/s^2
public static final double DistanceTolerance = 0.01; // max diff in meters
public static final double DistanceSpeedTolerance = 0.1; // ignore if velocity is below. (m)
}

public final class BalancePIDConstants {
// Balance PID / AutoBalance
public static final double balance_P = 0.0625; // 1/16
public static final double balance_I = 0.00;
public static final double balance_D = 0.00;
public static final double MaxBalanceRateDegPerS = 10;
public static final double MaxBalanceAccelerationDegPerSSquared = 20;
public static final double BalanceToleranceDeg = 2; // max diff in degrees
}
}

public static final double MAX_SPEED = 0.8;

// USB Devices
Expand Down
47 changes: 47 additions & 0 deletions src/main/java/frc/robot/PIDControllers.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
package frc.robot;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;

public class PIDControllers {

// false when inactive, true when active / a target is set.
public static boolean turnControllerEnabled = false;
public static double turnRotateToAngleRate; // This value will be updated by the PID Controller
// pid controller for "RotateToAngle"
public static final ProfiledPIDController m_balanceController =
new ProfiledPIDController(
Constants.PIDConstants.BalancePIDConstants.balance_P,
Constants.PIDConstants.BalancePIDConstants.balance_I,
Constants.PIDConstants.BalancePIDConstants.balance_D,
new TrapezoidProfile.Constraints(
Constants.PIDConstants.BalancePIDConstants.MaxBalanceRateDegPerS,
Constants.PIDConstants.BalancePIDConstants.MaxBalanceAccelerationDegPerSSquared));

// false when inactive, true when active / a target is set.
public static boolean distanceControllerEnabled = false;
public static double distanceThrottleRate; // This value will be updated by the PID Controller
// pid controller for "MoveDistance"
public static final ProfiledPIDController m_distanceController =
new ProfiledPIDController(
Constants.PIDConstants.DistancePIDConstants.distance_P,
Constants.PIDConstants.DistancePIDConstants.distance_I,
Constants.PIDConstants.DistancePIDConstants.distance_D,
new TrapezoidProfile.Constraints(
Constants.PIDConstants.DistancePIDConstants.distanceMaxSpeed,
Constants.PIDConstants.DistancePIDConstants.distanceMaxAcceleration));

// false when inactive, true when active / a target is set.
public static boolean balanceControllerEnabled = false;
public static double balanceThrottleRate; // This value will be updated by the PID Controller
// pid controller for balanceCorrection

public static final ProfiledPIDController m_turnController =
new ProfiledPIDController(
Constants.PIDConstants.TurnPIDConstants.turn_P,
Constants.PIDConstants.TurnPIDConstants.turn_I,
Constants.PIDConstants.TurnPIDConstants.turn_D,
new TrapezoidProfile.Constraints(
Constants.PIDConstants.TurnPIDConstants.MaxTurnRateDegPerS,
Constants.PIDConstants.TurnPIDConstants.MaxTurnAccelerationDegPerSSquared));
}
149 changes: 51 additions & 98 deletions src/main/java/frc/robot/subsystems/DriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,10 @@
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
Expand All @@ -21,7 +19,9 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.Constants.CANConstants;
import frc.robot.Constants.PIDConstants;
import frc.robot.DriveConstants;
import frc.robot.PIDControllers;

/** This Subsystem is what allows the code to interact with the drivetrain of the robot. */
public class DriveSubsystem extends SubsystemBase {
Expand All @@ -46,63 +46,6 @@ public class DriveSubsystem extends SubsystemBase {
// Odometry class for tracking robot pose (position on field)
private final DifferentialDriveOdometry m_driveOdometry;

// Angle PID / RotateToAngle
static final double turn_P = 0.1;
static final double turn_I = 0.00;
static final double turn_D = 0.00;
static final double MaxTurnRateDegPerS = 100;
static final double MaxTurnAccelerationDegPerSSquared = 300;
static final double TurnToleranceDeg = 3; // max diff in degrees
static final double TurnRateToleranceDegPerS = 10; // degrees per second
// false when inactive, true when active / a target is set.
private boolean turnControllerEnabled = false;
private double turnRotateToAngleRate; // This value will be updated by the PID Controller
// pid controller for "RotateToAngle"
private final ProfiledPIDController m_turnController =
new ProfiledPIDController(
turn_P,
turn_I,
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;
static final double balance_D = 0.00;
static final double MaxBalanceRateDegPerS = 10;
static final double MaxBalanceAccelerationDegPerSSquared = 20;
static final double BalanceToleranceDeg = 2; // max diff in degrees
// false when inactive, true when active / a target is set.
private boolean balanceControllerEnabled = false;
private double balanceThrottleRate; // This value will be updated by the PID Controller
// pid controller for balanceCorrection
private final ProfiledPIDController m_balanceController =
new ProfiledPIDController(
balance_P,
balance_I,
balance_D,
new TrapezoidProfile.Constraints(
MaxBalanceRateDegPerS, MaxBalanceAccelerationDegPerSSquared));

/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
// Init gyro
Expand Down Expand Up @@ -144,13 +87,18 @@ public DriveSubsystem() {
getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance());

// config turn pid controller.
m_turnController.enableContinuousInput(-180.0f, 180.0f);
m_turnController.setTolerance(TurnToleranceDeg, TurnRateToleranceDegPerS);
PIDControllers.m_turnController.enableContinuousInput(-180.0f, 180.0f);
PIDControllers.m_turnController.setTolerance(
PIDConstants.TurnPIDConstants.TurnToleranceDeg,
PIDConstants.TurnPIDConstants.TurnRateToleranceDegPerS);
// config distance pid controller
m_distanceController.setTolerance(DistanceTolerance, DistanceSpeedTolerance);
PIDControllers.m_distanceController.setTolerance(
PIDConstants.DistancePIDConstants.DistanceTolerance,
PIDConstants.DistancePIDConstants.DistanceSpeedTolerance);
// this is the target pitch/ tilt error.
m_balanceController.setGoal(0);
m_balanceController.setTolerance(BalanceToleranceDeg); // max error in degrees
PIDControllers.m_balanceController.setGoal(0);
PIDControllers.m_balanceController.setTolerance(
PIDConstants.BalancePIDConstants.BalanceToleranceDeg); // max error in degrees
}

/**
Expand All @@ -174,46 +122,47 @@ public void tankDrive(double leftSpeed, double rightSpeed) {

public void balanceResetPID() {
/** This should be run when stopping a pid command. */
balanceControllerEnabled = false;
PIDControllers.balanceControllerEnabled = false;
}

public void balanceCorrection(double gyroPitchAngle) {
if (!balanceControllerEnabled) {
m_balanceController.reset(gyroPitchAngle);
balanceControllerEnabled = true;
if (!PIDControllers.balanceControllerEnabled) {
PIDControllers.m_balanceController.reset(gyroPitchAngle);
PIDControllers.balanceControllerEnabled = true;
}
balanceThrottleRate = MathUtil.clamp(m_balanceController.calculate(gyroPitchAngle), -1.0, 1.0);
if (!m_balanceController.atGoal()) {
PIDControllers.balanceThrottleRate =
MathUtil.clamp(PIDControllers.m_balanceController.calculate(gyroPitchAngle), -1.0, 1.0);
if (!PIDControllers.m_balanceController.atGoal()) {
// we reverse the values beacuse the robot was balancing in the wrong direction.
this.tankDrive(-balanceThrottleRate, -balanceThrottleRate);
System.out.println(balanceThrottleRate);
this.tankDrive(-PIDControllers.balanceThrottleRate, -PIDControllers.balanceThrottleRate);
System.out.println(PIDControllers.balanceThrottleRate);
}
}

public void distanceResetPID() {
/** This should be run when stopping a pid command. */
distanceControllerEnabled = false;
PIDControllers.distanceControllerEnabled = false;
}

public void distanceSetGoal(double targetDistance) {
m_distanceController.setGoal(AverageDistance() + targetDistance);
PIDControllers.m_distanceController.setGoal(AverageDistance() + targetDistance);
}

private void calculateDistanceRate(double targetDistance) {
if (!distanceControllerEnabled) {
m_distanceController.reset(AverageDistance());
m_distanceController.setGoal(AverageDistance() + targetDistance);
distanceControllerEnabled = true;
if (!PIDControllers.distanceControllerEnabled) {
PIDControllers.m_distanceController.reset(AverageDistance());
PIDControllers.m_distanceController.setGoal(AverageDistance() + targetDistance);
PIDControllers.distanceControllerEnabled = true;
}
distanceThrottleRate =
MathUtil.clamp(m_distanceController.calculate(AverageDistance()), -1.0, 1.0);
PIDControllers.distanceThrottleRate =
MathUtil.clamp(PIDControllers.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()) {
double leftStickValue = PIDControllers.turnRotateToAngleRate;
double rightStickValue = PIDControllers.turnRotateToAngleRate;
if (!PIDControllers.m_distanceController.atGoal()) {
this.tankDrive(leftStickValue, rightStickValue);
}
}
Expand All @@ -225,30 +174,34 @@ public void driveAndTurn(double gyroYawAngle, double TargetAngleDegrees, double
*/
this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees);
this.calculateDistanceRate(targetDistance);
double leftStickValue = distanceThrottleRate + turnRotateToAngleRate;
double rightStickValue = distanceThrottleRate - turnRotateToAngleRate;
if (!m_distanceController.atGoal() || !m_turnController.atGoal()) {
double leftStickValue =
PIDControllers.distanceThrottleRate + PIDControllers.turnRotateToAngleRate;
double rightStickValue =
PIDControllers.distanceThrottleRate - PIDControllers.turnRotateToAngleRate;
if (!PIDControllers.m_distanceController.atGoal()
|| !PIDControllers.m_turnController.atGoal()) {
this.tankDrive(leftStickValue, rightStickValue);
}
}

// these next 4 functions are for turning a set radius while using the gyro.
public void turnResetPID() {
/** This should be run when stopping a pid command. */
turnControllerEnabled = false;
PIDControllers.turnControllerEnabled = false;
}

public void turnSetGoal(double targetAngleDegrees) {
m_turnController.setGoal(targetAngleDegrees);
PIDControllers.m_turnController.setGoal(targetAngleDegrees);
}

private void calcuateAngleRate(double gyroYawAngle, double targetAngleDegrees) {
if (!turnControllerEnabled) {
m_turnController.reset(gyroYawAngle);
m_turnController.setGoal(targetAngleDegrees);
turnControllerEnabled = true;
if (!PIDControllers.turnControllerEnabled) {
PIDControllers.m_turnController.reset(gyroYawAngle);
PIDControllers.m_turnController.setGoal(targetAngleDegrees);
PIDControllers.turnControllerEnabled = true;
}
turnRotateToAngleRate = MathUtil.clamp(m_turnController.calculate(gyroYawAngle), -1.0, 1.0);
PIDControllers.turnRotateToAngleRate =
MathUtil.clamp(PIDControllers.m_turnController.calculate(gyroYawAngle), -1.0, 1.0);
}

public void turnToAngle(double gyroYawAngle, double TargetAngleDegrees) {
Expand All @@ -258,9 +211,9 @@ public void turnToAngle(double gyroYawAngle, double TargetAngleDegrees) {
* is ignored until this button is released.
*/
this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees);
double leftStickValue = turnRotateToAngleRate;
double rightStickValue = -turnRotateToAngleRate;
if (!m_turnController.atGoal()) {
double leftStickValue = PIDControllers.turnRotateToAngleRate;
double rightStickValue = -PIDControllers.turnRotateToAngleRate;
if (!PIDControllers.m_turnController.atGoal()) {
this.tankDrive(leftStickValue, rightStickValue);
}
}
Expand All @@ -276,8 +229,8 @@ public void driveStraight(
*/
this.calcuateAngleRate(gyroYawAngle, gyroAccumYawAngle);
double angleRate;
if (!m_turnController.atGoal()) {
angleRate = turnRotateToAngleRate;
if (!PIDControllers.m_turnController.atGoal()) {
angleRate = PIDControllers.turnRotateToAngleRate;
} else {
angleRate = 0;
}
Expand Down

0 comments on commit 789c13c

Please sign in to comment.