I would like to have accurate motor control in the robot I am planning. Accordingly, I have been experimenting with encoders attached to the motors that provide feedback to a Proportional Integral controller. This post is a demonstration of the progress and what looks to be a promising start.
Introduction
The final robot will probably have a Raspberry Pi as the "brain" since I would also like to have facial recognition. To unload the Pi, microcontrollers will be used for some tasks such as motor control. Encoders will be used to provide feedback on motor speed and the individual components were tested in the last post: Simple Arduino DC Motor Control with Encoder, Part 1. An I2C template for controlling an Arduino from a Raspberry Pi was described in this post: Creating Multi-Purpose I2C Devices with Arduino for use with a Raspberry Pi.
In the motor control tests Part 1 it was seen that the two motors did not rotate at the same speed when driven by the same voltage. This is not uncommon and the datasheet for the motors has plus / minus 10% no-load speed recorded in the specifications. To assure variable accurate speeds under all conditions a control system will be designed that uses the encoders to provide feedback. The more general form of the controller to be used is known as a PID Controller (Proportional-Integral-Derivative Controller) but as will be seen shortly the derivative portion is not necessary here and without additional work would actually inhibit control.
Some background... I remember many years ago sitting through a course where PID control was taught but being glad at the end of the day to pass the test and move on to a different subject. And while the facilities in my field of employment were full of complex PID controllers I never actually worked directly with them. We had professionals that did that :-). So although I have familiarity with the topic I am not an expert and rather than telling you how to do it, I will just tell you what I did. If there are experts who can comment further please do so.
I would like to acknowledge work by Brett Beauregard in his blogs and Arduino library which are linked below. His work and the documentation he provides are excellent examples of how it should be done.
Why Use PID Here?
Sure the motors turn at different speeds but why can't we just measure the difference and apply a factor to slow or speed one of the motors up? Turns out it is not that easy much of the time. All mechanical things are inherently different and change as they progress through their lifetime. The conditions they encounter will also differ from test conditions. For example:
- The load on the motors could change due to going up hill or a different payload
- The battery may influence the motors differently as it discharges and voltage drops
- The motors may wear differently as they age
- And so on....
PID Control
PID control continuously evaluates the error between a setpoint and the variable being controlled and applies a correction based on proportional, integral, and derivative terms. The control loop looks like this for our brushed motors (the derivative term is crossed out since it won't be used here but some explanation is provided):
The user provided set point is the speed of the motor. The output speed of the motor from the encoder is compared to the setpoint and fed to the controller. The controller uses the PID control algorithm to determine a new output (PWM) if needed to reduce the error and a new output from the encoder starts the loop over again.
This is what the PID equation looks like:
If you haven't used calculus in a while, don't worry. It's not that bad. In simplified form it turns into this when written as a C algorithm
/*working variables*/
unsigned long lastTime;
double Input, Output, Setpoint;
double errSum, lastErr;
double kp, ki, kd;
void Compute()
{
/*How long since we last calculated*/
unsigned long now = millis();
double timeChange = (double)(now - lastTime);
/*Compute all the working error variables*/
double error = Setpoint - Input;
errSum += (error * timeChange);
double dErr = (error - lastErr) / timeChange;
/*Compute PID Output*/
Output = kp * error + ki * errSum + kd * dErr;
/*Remember some variables for next time*/
lastErr = error;
lastTime = now;
}
void SetTunings(double Kp, double Ki, double Kd)
{
kp = Kp;
ki = Ki;
kd = Kd;
}
This is incomplete code but I am amazed that the guts can be condensed down to that. See the series of blogs at this link for a detailed rundown on a professional PID library. So what are the terms doing? The calculation in the algorithm is simple but getting an intuition for how the terms behave takes familiarity with the system. At a high level, the following is helpful:
- Proportional adjustment is proportional to the error at that point in time. It acts immediately when a change occurs and reduces as the error reduces.
- Integral adjustment is sensitive not only to the error but also to the time in which it has existed. It is small at the beginning of a change and builds.
- Derivative adjustment is sensitive to the rate of change of the error and attempts to flatten the trajectory and thus avoid overshoot. In a noisy system (like our motor) the derivative may provide confusing information and not be helpful.
Poor choices of Kp, Ki, and Kd result in poor performance or even instability. So, how are Kp, Ki, and Kd determined? More on how I did it below but there are tuning methods described in the Wikipedia article and some control software has "automatic" tuners. These days it is also possible to develop a system model on a computer.
System Model
Process control has advanced tremendously in recent years and it is possible to develop sophisticated computer models for steady state and transient behavior of very complicated systems. See for example this video demonstrating PID development for a motor using MATLAB and Simulink. Unfortunately I no longer have an active account but I thought I would try developing a simple model and estimate the tuning factors with a spreadsheet. All done with no idea of whether it would work of course...
Voltage, current, and frequency of the encoder output from one of the motors was recorded as PWM was increased:
This looks promising, the frequency of the encoder output is very linear after 20% duty cycle is obtained with the power supply set at 6V. The linear equation for the frequency is:
frequency = 7.11 * duty cycle - 71.1
At this point it should be noted that this is an unloaded motor unlike the eventual robot. The rough assumption is that friction, inertia, inductance, back EMF, and so are all captured adequately in a single line. Time is not handled properly and the PID equation is pretty "how ya doin'" but here we go...
Remember that the derivative term is not being used. For the model we start at time 0 and the motor starts up. The error and error sums are calculated. From this we get the proportional and integral terms and add them together. This is then the output. The output is plugged into the motor model and this gives the new input. We then go to the next time interval.
After about 50 time intervals when everything has smoothed out a change is made to see whether it is stable in another range. Then two more changes are made at 100 time intervals and 150 time intervals. I fooled with the terms quite a bit and found they were fairly sensitive. It was quite easy to make the system unstable. The final "tuned" parameters from this crude model were:
Kp = 0.05
Ki = 0.03
As will be seen shortly, these parameters were stable in the actual system and allowed me to tune it further fairly quickly.
PI Controller
The PI Controller uses the same Adafruit M4 Feather Express and setup as the previous post. The main addition is the PID Library. Here is the code:
/* Robot_SimpleMotor_Drive_V0 with encoders
*
* Adafruit Feather M4 using Pololu TB6612FNG motor controller
* Drives two motors at fixed speed with PI control
*
* Motor Control Table
* XIN1 XIN2 Effect
* Low Low Brake
* Low High Forward
* High Low Reverse
*
* Free to use for all
* F Milburn, January 2020
*/
#include <PID_v1.h>
// Output pins used to control motors
const uint16_t PWMA = 5; // Motor A PWM control Orange
const uint16_t AIN2 = 6; // Motor A input 2 Brown
const uint16_t AIN1 = 9; // Motor A input 1 Green
const uint16_t BIN1 = 10; // Motor B input 1 Yellow
const uint16_t BIN2 = 11; // Motor B input 2 Purple
const uint16_t PWMB = 12; // Motor B PWM control White
const uint16_t STBY = 13; // Standby Brown
// Motor encoder external interrupt pins
const uint16_t ENCA = A3; // Encoder A input Yellow
const uint16_t ENCB = A2; // Encoder B input Green
// PWM
const uint16_t ANALOG_WRITE_BITS = 8;
const uint16_t MAX_PWM = pow(2, ANALOG_WRITE_BITS)-1;
const uint16_t MIN_PWM = MAX_PWM / 4; // Make sure motor turns
// Motor timing
unsigned long nowTime = 0; // updated on every loop
unsigned long startTimeA = 0; // start timing A interrupts
unsigned long startTimeB = 0; // start timing B interrupts
unsigned long countIntA = 0; // count the A interrupts
unsigned long countIntB = 0; // count the B interrupts
double periodA = 0; // motor A period
double periodB = 0; // motor B period
// PID
const unsigned long SAMPLE_TIME = 100; // time between PID updates
const unsigned long INT_COUNT = 20; // sufficient interrupts for accurate timing
double setpointA = 150; // setpoint is rotational speed in Hz
double inputA = 0; // input is PWM to motors
double outputA = 0; // output is rotational speed in Hz
double setpointB = 150; // setpoint is rotational speed in Hz
double inputB = 0; // input is PWM to motors
double outputB = 0; // output is rotational speed in Hz
double KpA = 0.20, KiA = 0.20, KdA = 0;
double KpB = 0.20, KiB = 0.20, KdB = 0;
PID motorA(&inputA, &outputA, &setpointA, KpA, KiA, KdA, DIRECT);
PID motorB(&inputB, &outputB, &setpointB, KpB, KiB, KdB, DIRECT);
double storeB = 0; // used for debug print
void setup(){
initMotors();
initEncoders();
initPWM();
Serial.begin(115200);
while(!Serial){
// wait for serial to start
}
}
void loop(){
nowTime = millis();
motorA.Compute();
motorB.Compute();
forwardA((int)outputA);
forwardB((int)outputB);
if (storeB != outputB){
storeB = outputB;
Serial.println("inputA, inputB, errorA, errorB");
Serial.print(inputA); Serial.print(" ");
Serial.print(inputB); Serial.print(" ");
Serial.print(100*(setpointA-inputA)/setpointA); Serial.print(" ");
Serial.print(100*(setpointB-inputB)/setpointB); Serial.println("\n");
}
}
void forwardA(uint16_t pwm){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(PWMA, pwm);
}
void forwardB(uint16_t pwm){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
analogWrite(PWMB, pwm);
}
void reverseA(uint16_t pwm){
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
analogWrite(PWMA, pwm);
}
void reverseB(uint16_t pwm){
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
analogWrite(PWMB, pwm);
}
void brakeA(){
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
}
void brakeB(){
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}
void standbyMotors(bool standby){
if (standby == true){
digitalWrite(STBY, LOW);
}
else{
digitalWrite(STBY, HIGH);
}
}
void initMotors(){
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(STBY, OUTPUT);
analogWriteResolution(ANALOG_WRITE_BITS);
standbyMotors(false);
}
void initEncoders(){
pinMode(ENCA, INPUT_PULLUP);
pinMode(ENCB, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCA), isr_A, RISING);
attachInterrupt(digitalPinToInterrupt(ENCB), isr_B, RISING);
}
void initPWM(){
startTimeA = millis();
startTimeB = millis();
motorA.SetOutputLimits(MIN_PWM, MAX_PWM);
motorB.SetOutputLimits(MIN_PWM, MAX_PWM);
motorA.SetSampleTime(SAMPLE_TIME);
motorB.SetSampleTime(SAMPLE_TIME);
motorA.SetMode(AUTOMATIC);
motorB.SetMode(AUTOMATIC);
}
void isr_A(){
// count sufficient interrupts to get accurate timing
// inputX is the encoder frequency in Hz
countIntA++;
if (countIntA == INT_COUNT){
inputA = (float) INT_COUNT * 1000 / (float)(nowTime - startTimeA);
startTimeA = nowTime;
countIntA = 0;
}
}
void isr_B(){
// count sufficient interrupts to get accurate timing
// inputX is the encoder frequency in Hz
countIntB++;
if (countIntB == INT_COUNT){
inputB = (float) INT_COUNT * 1000 / (float)(nowTime - startTimeB);
startTimeB = nowTime;
countIntB = 0;
}
}The PID Library is this one and appears to be quite good. Documentation is excellent extraordinary.
The input from the encoders is captured in interrupts. I found that I needed to capture a number of interrupts over a period of time to get an accurate frequency and settled on 20. The PID controller is updated every 100 milliseconds. This may not be optimal and needs reexamination. Good practice would be to reduce time in the ISR by getting rid of the floats and division.
Only one channel of the encoders is being captured on the rising edge - this is partially because 2 of the pins did not seem to work with the interrupts although the Adafruit documentation implied that they should. It may be possible to move things around and get this working on both channels, both rising and falling, which would increase resolution. Debounce was not implemented and no problems were noticed but this should also be examined further.
Using the library is straightforward and the author explains it much better than I can.
System Test
The test setup is the same as the last blog. As noted above the tuning parameters developed with my simple model worked right away. I will assume this wasn't dumb luck but my knowledge and skillful application of motor systems and use of numerical methods. It was noticed that the motors were ramping up slowly so Kp and Ki were incrementally increased until the system became unstable and then backed off. The values in the code above may not be optimal but seem to be in the ballpark and a good start.
As can be seen in the screenshot below, the errors when running at low speeds in particular are minimal.
Error from the setpoint of the two motors is shown to the right. The error is centered and for the most part less than one percent on each reading.
In the following test the motors are started from a dead stop and accelerate to a setpoint of approximately 25% of full speed.
And in this next test the motors are shown operating at approximately 100% of full speed.
The error is quite acceptable for my use but I expect it could be improved somewhat with the use of PWM with more resolution. I also expect the ramp up and down of speed could be improved through further tuning (probably at the expense of some over / undershoot) but is acceptable for my use.
Summary and Next Steps
I am quite pleased with the results and getting this to work was easier than I thought it would be. The PID Library by Brett Beauregard is highly recommended as is the blog he wrote on PID in general. I am going to put this aside for a bit in order to do a RoadTest but hope to return quickly unlike my other incomplete projects.
As always comments, suggestions, and corrections are appreciated.
Useful Links
Simple Arduino DC Motor Control with Encoder, Part 1
Creating Multi-Purpose I2C Devices with Arduino to use with a Raspberry Pi
Raspberry P and Arduino I2C Communication
Brett Beauregard Project Blog - PID Introduction (additional useful links embedded in this blog)
Brett Beauregard Arduino PID Library
Top Comments