Sample Code - PID Controller

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.

Five year old explanation
Proportional (P) term: This term helps the robot move in the right direction. Imagine you're playing a game and trying to hit a target with a bow and arrow. The proportional term tells you how much to adjust your aim based on how far you are from the target. If you're far away, you need to aim more to the left or right. If you're close, you only need a small adjustment. The proportional term helps you make the right adjustments to get closer to the target.

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.

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.

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) { = kp; = ki;
        this.kd = kd; = 0.0;
        this.integral = 0.0;
        this.previousError = 0.0;

    public void setTarget(double 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

    public void runOpMode() throws InterruptedException {


        while (opModeIsActive()) {
            // Calculate PID output
            double pidOutput = pidController.calculateOutput(getCurrentAngle(), getRuntime());

            // Apply PID output to adjust motor speeds
                gamepad1.right_stick_x + pidOutput

            telemetry.addData("PID Output", pidOutput);


    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);

    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

    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

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) { = kp; = ki;
    this.kd = kd;
    target = 0.0;
    integral = 0.0;
    previousError = 0.0;
    previousTime = 0.0;

public void setTarget(double 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.

© Copyright CircuitBreakers - All Rights Reserved

Made with ‌

Easiest Website Builder