Months ago I wrote a post on the use of motor encoders with Arduino and promised to follow up with a post on speed control. In fact at the time I already had a mostly functional speed control class but I decided I had to do some major cleanup on it before presenting here. Then I started school I had no time for Colin or this blog for nine months.

Now, that I’m done with school I’ve had time to write a brand new motor control library! You can find the source code and an example sketch at github.com/1988kramer/motor_control.

In this post I’m going to briefly explain how my motor control program is structured, then give some background on the control theory that went into it, then I’ll explain how the theory is implemented in the actual code.

Program Structure

I structured my program to mirror the hardware as closely as possible so I made two classes, Motor and Encoder, to handle the low level functions of the motor and encoder. The Motor and Encoder classes are used by the SpeedControl class to precisely control the motor’s speed. See the diagram below for a detailed explanation of the the library’s structure.

GDE Error: Error retrieving file - if necessary turn off error checking (404:Not Found)

You’ll notice two classes in the diagram above that use the functionality of SpeedControl: PositionControl and DifferentialDrive. PositionControl uses an instance of SpeedControl to provide position control functions and DifferentialDrive uses two instances of PositionControl to provide high level control for a differential drive robot like Colin.

I’m just going to be discussing SpeedControl class today, but you can expect a post on PositionControl and DifferentialDrive in the next few weeks.

Control Loop Basics

In my earlier post on motor control I explained that simply using analogWrite does not actually set the speed of a motor. It only sets the voltage to the motor. Varying loads will cause a motor to run at different speeds with the same input voltage. How do we get a motor to run at a specific speed? This sounds like a job for a closed-loop controller!

In a closed-loop controller, we tell the controller how fast we want the motor to run. We call this the set point. The controller then measures the actual speed of the motor and calculates the difference between the actual speed and the set point. This is the error. The controller then adjusts the voltage to the motor to reduce the error.

Proportional Control

This sounds simple enough, but how much should the controller adjust for a given error? One could make the adjustment directly proportional to the error, i.e. large errors get large adjustments and small errors get small adjustments. The adjustment would be defined as

$A(t)=K_{{p}}e(t)$

where $A(t)$ is the adjustment, $e(t)$ is the error, and $K_{p}$ is a constant referred to as the gain. This is a decent solution but it has some problems. If $K_{p}$ is large the controller will be quick to respond to disturbances but it will tend to over-correct (known as overshooting). If $K_{p}$ is very large the motor speed will oscillate around the set point and never stabilize. If $K_{p}$ is small enough to avoid overshooting, however, the controller will respond very slowly to disturbances.

Integral and Derivative Terms

To compensate for this we can add a couple more terms to the adjustment equation. A term proportional to both the error and duration of the error, called the integral term, will correct for persistent small errors that won’t be corrected by the proportional term. It will also make the controller faster to reach the set point. However, if the integral term is too large it will cause overshoot and oscillation. The integral term is defined as

$I_{out}=K_{i}\int_{0}^{t}e(\tau)d\tau$

Lastly, the derivative (slope) of the error over time can be used to predict the system’s future behavior. For instance, a large derivative indicates the next error will likely be far away from the last whereas a small derivative indicates the next error will likely be close to the last. A term calculated from the derivative of past errors helps to minimize overshoot and improve the stability of the controller. The derivative term is defined as

$D_{out}=K_{d}\frac{de(t)}{dt}$

PID Adjustment Equation

So our final adjustment is the sum of the proportional, integral, and derivative terms:

$A(t)=P_{out}+I_{out}+D_{out}=K_{p}e(t)+K_{i}\int_{0}^{t}e(\tau)d\tau+K_{d}\frac{de(t)}{dt}$

Every time the controller adjusts the voltage output to the motor it must find the current speed of the motor, calculate the error between the current speed and the set point, then use the above equation to determine how much the output voltage to the motor should be adjusted to compensate for the error. In Colin’s case this is done 20 times per second.

The Code

Because posting my motor control code in its entirety would take far too much space and most of it isn’t very interesting anyway, I’m just going to post and explain the update function. It calculates the required adjustment to the PWM output to the Motor to maintain consistent speed.

My code draws heavily on Brett Beagregard’s PID library. I would encourage you to check out his blog for a more in-depth explanation of PID control. Thanks for your work, Brett!

// adjusts motor's PWM level to correct for any error between the
// set point and the actual speed of the motor's output shaft
// MUST be called regularly on the same deltaT used to calculate
// motor speed in the Encoder object
void SpeedControl::adjustPWM()
{
	int speed = _encoder->getSpeed(); // motor control returns vector speed
	if (speed < 0) speed *= -1;  // convert speed to scalar
	int error = _setPoint - speed;  // calculate error
	_iTerm += (_kI * (double)error); // calculate integral term
	double dInput = speed - _lastSpeed; // calculate derivative
	int adjustment = (_kP * (double)error) + _iTerm - (_kD * dInput);
	_pwm += adjustment;
	constrainPWM(); // limit _pwm to the range 0-255
	_motor->setPWM(_pwm);
	_lastSpeed = speed;
}

The code above basically performs the calculation discussed in the preceding section. In line 7 it fetches the current speed of the motor and calculates the error in line 9. It then calculates the proportional, integral, and derivative terms and uses them to calculate the adjustment in lines 10 through 12.

Derivative Kick

There is one deviation from the equations presented in the previous section: in lines 11 and 12 we’re using the speed, not the error to calculate the derivative term. Also, we’re subtracting it from the adjustment. What gives?

Basically, when the user changes the set point there is an instantaneous change in the error $e(t)$ . This means the derivative of the error $\frac{de(t)}{dt}$ is infinite at this point. So in theory the derivative term becomes infinite whenever the set point changes, a phenomenon called derivative kick. Because the code above calculates the approximate numerical derivative the phenomenon is not as pronounced, but it should still be addressed. Making the change above corrects for derivative kick. See Brett’s blog for a more in-depth explanation.

Example Sketch

So how do we actually implement this to control a motor? You could spend weeks or possibly months of your free time developing a full speed control program, or you could try using the one that I spent weeks of my free time on! The example sketch below demonstrates the use of my SpeedControl class, available on github.

#include <TimerOne.h>
#include <PositionControl.h>

const int dir1 = 9, dir 2 = 8, pwm = 6;
const int encoderA = 3, encoderB = 5;
const int deltaT = 50000;
const int encoderTicksPerRev = 309;
const double kP = 0.025, kI = 0.0008, kD = 0.0005;

Motor rhMotor(dir1, dir2, pwm);
Encoder rhEncoder(encoderA, encoderB, deltaT, encoderTicksPerRev);
SpeedControl rhSpeedControl(&rhMotor, &rhEncoder);

  
void setup() {
  Serial.begin(9600);
  Timer1.initialize(deltaT);
  Timer1.attachInterrupt(adjustPWM);
  attachInterrupt(1, readRHEncoder, CHANGE);
  rhSpeedControl.setGains(kP, kI, kD);
}

void loop() {
  
  rhSpeedControl.setSpeed(180);
  delay(5000);
  rhSpeedControl.setSpeed(0);
  Serial.println(rhSpeedControl.getDistance());
  delay(2000);
}

void readRHEncoder()
{
  rhEncoder.updateCount();
}

void adjustPWM()
{
  rhSpeedControl.adjustPWM();
}

Implementation Notes

A couple of notes on note on the sketch above:

First, the PID gains, kP, kI, and kD used in the example sketch work well for Colin but will need to be tuned for your robot. PID tuning is something of a black art; I haven’t been able to find much info on it online, anyway. The method presented on this wikipedia page worked fairly well for me though.

Second, the SpeedControl.adjustPWM() function must be called every deltaT microseconds using a timer interrupt. See this Arduino playground page for more info on timer interrupts. The deltaT value needs to be tuned just like the PID gains. A deltaT value that’s too large will result in the controller not adjusting frequently enough to accurately control the motor’s speed. A value that’s too small, will limit the accuracy of the speed measurement because fewer encoder ticks can elapse in a smaller amount of time. The maximum accuracy for a given deltaT, by the way, is

$Accuracy=\frac{D}{\Delta t}$

Where D is the number of degrees per encoder tick. I found a deltaT of 50 milliseconds works well for Colin, but you should play around with it for yourself.

That’s all I have for now! Stay tuned for a post on my PositionControl and DifferentialDrive classes in the next few weeks!

Bot Blog

building my robot army one sketch at a time

Monthly Archives: July 2016

PID Motor Control