PID Controller
A PID controller (Proportional-Integral-Derivative controller) is a feedback control mechanism widely used in robotics and automation. It continuously calculates an error value as the difference between a desired setpoint and a measured process variable, then adjusts the control output to minimize this error and maintain the system at the desired state.
The PID controller uses three main components:
Proportional (P) term: The output is proportional to the current error value. It provides an immediate response to the error and helps reduce steady-state error. The P term alone can lead to overshooting or oscillations.
Integral (I) term: The output is proportional to the accumulated past error over time. It helps correct for steady-state errors and eliminates offset. The I term alone can lead to overshooting or instability.
Derivative (D) term: The output is proportional to the rate of change of the error. It helps dampen the response, reducing overshoot and oscillations. The D term alone can be sensitive to noise.
The PID controller combines these three terms in different proportions to achieve a balance between stability, responsiveness, and accuracy, depending on the specific application.
Five year old explanation
Integral (I) term: This term helps the robot correct for any errors that accumulate over time. Imagine you're riding a bicycle and there's a slight slope. You start pedaling, but the slope makes you go slower than you want to. The integral term looks at how much you've fallen behind over time and helps you pedal a bit harder to catch up. It helps you correct for any long-term errors and get back on track.
Derivative (D) term: This term helps the robot react to changes in the environment. Imagine you're driving a car and the road suddenly becomes bumpy. The derivative term helps you react quickly to the bumps by adjusting your steering. It looks at how fast things are changing and helps you make the right adjustments to stay on course.
Here's a sample PID controller code in Java that can be used as a starting point for implementing PID control in an FTC robot:
public class PIDController {
private double kp; // Proportional gain
private double ki; // Integral gain
private double kd; // Derivative gain
private double target; // Setpoint
private double integral; // Integral term accumulation
private double previousError; // Previous error value
public PIDController(double kp, double ki, double kd) {
this.kp = kp;
this.ki = ki;
this.kd = kd;
this.target = 0.0;
this.integral = 0.0;
this.previousError = 0.0;
}
public void setTarget(double target) {
this.target = target;
this.integral = 0.0;
this.previousError = 0.0;
}
public double calculateOutput(double currentValue, double deltaTime) {
double error = target - currentValue;
// Proportional term
double proportionalTerm = kp * error;
// Integral term
integral += error * deltaTime;
double integralTerm = ki * integral;
// Derivative term
double derivativeTerm = kd * ((error - previousError) / deltaTime);
// Calculate the output value
double output = proportionalTerm + integralTerm + derivativeTerm;
// Update previous error value
previousError = error;
return output;
}
}
To use the PIDController class, you would typically perform the following steps:
1. Create an instance of the PIDController and specify the desired values for the proportional, integral, and derivative gains (kp, ki, kd).
2. Set the target setpoint using the setTarget() method.
3. Inside a control loop, provide the current value of the process variable and the time elapsed since the last iteration (deltaTime) to the calculateOutput() method. It will calculate the appropriate control output based on the PID algorithm.
4. The output value can then be used to control a motor, servo, or any other component of the robot.
Keep in mind that PID tuning is often an iterative process to find the optimal values for your specific system. The gains (kp, ki, kd) need to be adjusted experimentally to achieve the desired response, considering factors such as the system dynamics, desired stability, and response time.
--- code --- PID and Mecanum
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
public class MecanumDriveWithPID extends LinearOpMode {
private MecanumDriveTrain driveTrain;
private PIDController pidController;
private final double TARGET_ANGLE = 0.0; // Desired angle
@Override
public void runOpMode() throws InterruptedException {
initializeHardware();
waitForStart();
while (opModeIsActive()) {
// Calculate PID output
double pidOutput = pidController.calculateOutput(getCurrentAngle(), getRuntime());
// Apply PID output to adjust motor speeds
driveTrain.setSpeeds(
gamepad1.left_stick_x,
gamepad1.left_stick_y,
gamepad1.right_stick_x + pidOutput
);
telemetry.addData("PID Output", pidOutput);
telemetry.update();
idle();
}
}
private void initializeHardware() {
// Initialize mecanum drive train and other hardware
driveTrain = new MecanumDriveTrain(hardwareMap);
// Initialize PID controller
double kp = 0.03; // Proportional gain
double ki = 0.001; // Integral gain
double kd = 0.001; // Derivative gain
pidController = new PIDController(kp, ki, kd);
pidController.setTarget(TARGET_ANGLE);
}
private double getCurrentAngle() {
// Get current angle using gyro sensor or other orientation sensor
// Return the current angle in degrees
}
}
class MecanumDriveTrain {
private DcMotor frontLeftMotor;
private DcMotor frontRightMotor;
private DcMotor rearLeftMotor;
private DcMotor rearRightMotor;
public MecanumDriveTrain(HardwareMap hardwareMap) {
// Initialize motors using hardwareMap
frontLeftMotor = hardwareMap.dcMotor.get("frontLeft");
frontRightMotor = hardwareMap.dcMotor.get("frontRight");
rearLeftMotor = hardwareMap.dcMotor.get("rearLeft");
rearRightMotor = hardwareMap.dcMotor.get("rearRight");
// Set motor directions and zero power behavior
frontLeftMotor.setDirection(DcMotor.Direction.REVERSE);
rearLeftMotor.setDirection(DcMotor.Direction.REVERSE);
frontRightMotor.setDirection(DcMotor.Direction.FORWARD);
rearRightMotor.setDirection(DcMotor.Direction.FORWARD);
setMotorZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
public void setSpeeds(double x, double y, double rotation) {
double frontLeftPower = y + x + rotation;
double frontRightPower = y - x - rotation;
double rearLeftPower = y - x + rotation;
double rearRightPower = y + x - rotation;
// Scale the motor powers to ensure they are within the acceptable range of -1 to 1
double maxPower = Math.max(
Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)),
Math.max(Math.abs(rearLeftPower), Math.abs(rearRightPower))
);
if (maxPower > 1.0) {
frontLeftPower /= maxPower;
frontRightPower /= maxPower;
rearLeftPower /= maxPower;
rearRightPower /= maxPower;
}
// Set motor powers
frontLeftMotor.setPower(frontLeftPower);
frontRightMotor.setPower(frontRightPower);
rearLeftMotor.setPower(rearLeftPower);
rearRightMotor.setPower(rearRightPower);
}
class PIDController {
private double kp;
private double ki;
private double kd;
private double target;
private double integral;
private double previousError;
private double previousTime;
public PIDController(double kp, double ki, double kd) {
this.kp = kp;
this.ki = ki;
this.kd = kd;
target = 0.0;
integral = 0.0;
previousError = 0.0;
previousTime = 0.0;
}
public void setTarget(double target) {
this.target = target;
}
public double calculateOutput(double current, double time) {
double error = target - current;
double deltaTime = time - previousTime;
double derivative = (error - previousError) / deltaTime;
integral += error * deltaTime;
double output = kp * error + ki * integral + kd * derivative;
previousError = error;
previousTime = time;
return output;
}
}
In this example, the code is organized into three classes: `MecanumDriveWithPID` for the main control loop, `MecanumDriveTrain` for controlling the mecanum drive train motors, and `PIDController` for implementing the PID control algorithm.
The `MecanumDriveWithPID` class extends `LinearOpMode` and contains the main control loop inside the `runOpMode()` method. It initializes the hardware, including the mecanum drive train and PID controller, and runs the control loop to adjust the motor speeds based on user input and the PID output.
The `MecanumDriveTrain` class handles the motor control for the mecanum drive train. It initializes the motors using the `hardwareMap` and provides a method `setSpeeds()` to set the motor powers based on input from the gamepad.
The `PIDController` class implements the PID control algorithm. It takes in the proportional gain (`kp`), integral gain (`ki`), and derivative gain (`kd`) during initialization. The `setTarget()` method sets the desired target value for the controller. The `calculateOutput()` method calculates the PID output based on the current value, time, and the target value.
Remember to update the hardware initialization code, motor names, and adjust the PID gains according to your specific robot setup and requirements.
I hope this provides a clear example of a mecanum drive train with PID control integrated.
