Lesson 24: Applying PID Controllers to the Drive Train

Drive trains are one of the best places to use feedback. Encoders can help the robot drive a distance, and a gyro can help it drive straight or turn to an angle.

Driving a Distance

To drive a distance, compare the target distance to the encoder distance. That gives you an error. A proportional controller turns that error into motor output.

double targetDistance = 60.0;
double currentDistance = leftEncoder.getDistance();
double error = targetDistance - currentDistance;
double speed = error * 0.04;

Clamp the speed so the robot does not jump to full power.

speed = Math.max(-0.6, Math.min(0.6, speed));
leftMotor.set(speed);
rightMotor.set(speed);

Driving Straight

Even if both sides receive the same speed, a robot may drift. Use the gyro to add a turn correction.

double angleError = 0.0 - gyro.getAngle();
double correction = angleError * 0.03;

leftMotor.set(speed - correction);
rightMotor.set(speed + correction);

Turning to an Angle

Turning is similar, but the gyro error becomes the main value.

double targetAngle = 90.0;
double turnError = targetAngle - gyro.getAngle();
double turnSpeed = turnError * 0.025;

leftMotor.set(-turnSpeed);
rightMotor.set(turnSpeed);

Know When You Are Done

A controller needs an exit condition. For example, stop when the error is small enough for several cycles, or stop when a timeout is reached. Never assume the robot will always reach the target perfectly.

Practice

Write pseudocode for autonomous: reset sensors, drive 72 inches while holding zero degrees, stop, then turn to 90 degrees. Include the error calculation for each step.