PID Velocity DC Motor Controller

Project Introduction

For my entry to the Trossen Robotics DIY Project Contest #8 I have decided to create and document the design of a simple PID velocity DC motor controller. While this idea is not new or novel I feel that many people can take what I have learned from this project and implement it into their own designs. Also this is a project that I can use myself as a component in future robotic projects.

 PID Velocity DC Motor Controller Measuring Velocity of 50 RPM

Project Goals

The goals of my project included:
  • Control 5-12 volt dc motors.
  • Interface for quadrature encoders with A/B output.
  • Implement PID velocity control.
  • Serial USART interface allowing a master computer or microcontroller offload motor control to the controller.
  • Develop a GUI application for PID tuning.

Background information

So DC motor control is a snap right? To move I simply drive my robot's motors forward or backwards in various combinations to move forward, backwards or to turn left and right. If I want to run the motors at different speeds it is just a matter of increasing or decreasing the motors voltage to either speed it up and slow it down.

While all the above is true it is not exactly what I am aiming for with this project. Described above is what would be called open loop motor control. Pictured below you can see an open loop motor control system consisting of a microcontroller, H-Bridge and DC Motor. The microcontroller provides the H-bridge with direction and speed information. The H-Bridge in turn drives the DC motor.

Open Loop Motor Control

That is all great but what I want is accurate speed control. How do I know that a motor is rotating at the desired speed? Or even more important how do I know that both of a robot's wheels are spinning at their desired speed to produce accurate movements?

For this I need to close the open loop system by adding some feedback. My feed back will come in the form of a rotary encoder. With an encoder attached to the dc motor's and the encoders outputs being decoded by the microcontroller it will be able to calculate the rotational velocity of the motor forming the closed loop system shown below.

Closed Loop Motor Control

The complete system will control two motors and be controlled by an optional master that can either be another microcontroller or computer.

Full System

Micro Controller

To simplify the prototyping of this project I repurposed a MINI Robocontroller from Vanadium Labs that was originally a Mech Warfare targeting system on my mech Second Amendment.

The MINI Robocontroller use the ATMEGA 168 Atmel AVR 8-bit microcontroller. It also has an interface to both an Xbee wireless module and a SN754410 1A quadruple half H-hbridge driver making it ideal for prototyping this project.

To program the ATMEGA 168 on the MINI Robocontroller I have installed the AVR-GCC toolchain under Ubuntu. The tools that make up the AVR-GCC toolchain include:

Tool Description
GCC
The C/C++ Compiler
binutils
Binary tools that include an assembler and linker.
libc-avr
Standard C Library with AVR specific functions.
Gdb
Debugger.
avrdude
Download, upload, and manipulate the ROM and EEPROM of AVR micro controllers.

All of these items were installed from the terminal with the following command:
'sudo apt-get install gcc-avr binutils-avr avr-libc gdb-avr avrdude'

Motor And Motor Driver

The DC motor that I have available to use with this project is a 50:1 Metal Gearmotor from Pololu Robotics and Electronics. This motor is 12V with 300mA draw at free run. It also has an integrated quadrature encoder that I will talk about in the next section.

As mentioned earlier in the Microcontroller section the MINI Robocontroller has an interface to a SN754410 quad half H-brdige driver. The quad half drivers can be combined to make two full H-bridge drives perfect for driving DC motors bidirectionally.

Here is a little truth table that outlines the response of a motor connected the the output of two of the SN754410's quad drivers:
Enable 1A, 2A 1A Input 2A Input Motor Output
Low X X Stop
High Low Low Stop
High High High Stop
High Low High Clockwise
High High Low Counter Clockwise

Motor Output Logic

To adjust the speed of rotation either the 1A or 2A input can controlled via a pwm signal with a varying duty cycle.

We can configure the ATMEGA168's 8-bit timer/counter to output a pwm signal.

// Fast PWM Initializations
TCCR0A |= (1 << WGM00);				// Set timer/counter 0 waveform generation mode to fast pwm.
TCCR0A |= (1 << WGM01);

TCCR0A |= (1 << COM0A0);			// Set timer/counter 0 compare output mode.
TCCR0A |= (1 << COM0A1);

TCCR0B |= (1 << CS00);				// Set timer/counter 0 clock source.
TCCR0B &= ~(1 << CS01);
TCCR0B &= ~(1 << CS02);

Then by adjusting the timer/counter's compare register we vary the duty cycle of the pwm and in turn adjust the speed of the motor.

// Preform PID and set output each motor.
OCR0A = motor_output(paramaters[PARAM_RIGHT_DIRECTION], OCR0A, do_pid(pRMotor));


Quadrature Encoder

A quadrature encoder has two outputs that are 90 deg out of phase of each other. To decode the output from the encoder it is only a matter of interpreting on the rising and falling edges of the encoder's outputs and reading the logical level of the opposite output to determine direction.

In this project I am interrupting off of the logical changes of the encoders Input A.

Input A Input B Count Direction
Rising Edge High + Clockwise
Rising Edge Low - Counter Clockwise
Falling Edge High - Counter Clockwise
Falling Edge Low + Clockwise

Quadrature Encoder Decoding

To implement encoder decoding in my project I set up the ATMEGA168's external interrupts to trigger on the rising and falling edges of just one of the encoders outputs.

// Encoder Initializations
pencoder pREncoder = &REncoder;		// Pointer to right encoder structure.
DDRC |= ~(1 << PC0);				// Right encoder A input.
DDRD |= ~(1 << PD3);				// Right encoder B input.
PORTC |= (1 << PC0);				// Right encoder A pullup resistor enabled.
PORTD |= (1 << PD3);				// Right encoder B pullup resistor enabled.
EICRA |= (1 << ISC10);				// Any logical change on right encoder output A generates an interrupt request.
EIMSK |= (1 << INT1);				// Enable the external interrupt.


Then an Interrupt service routine for the external interrupt is created. Here whenever any logical change occurs on the external interrupt pin of the micro a function is called to update the encoders count.

ISR(INT1_vect){
	encoder_update(&REncoder, (PIND & 8) >> 3, (PINC & 1));
}


The encoders count is updated with the following function. By simply knowing the logic levels of the encoders A and B outputs we can determine what direction the encoder is turning and update the count accordingly.

void encoder_update(encoder *e, int A, int B){
	// Determine direction and update encoder count from the logic levels of the encoder's A and B outputs.
	if(A == 1){
		// Rising edge of A.
		if(B == 1){
			e->direction = 1;
			e->count ++;
		}
		else{
			e->direction = 0;
			e->count --;
		}
	}
	else{
		// Falling edge of A.
		if(B == 1){
			e->direction = 0;
			e->count --;
		}
		else{
			e->direction = 1;
			e->count ++;
		}
	}
}


On each iteration of the program's main loop this following function is called to determine the encoders velocity. By simply taking the absolute value of the encoder count we get the velocity of the motor in 'encoder counts per control loop time'. This can be converted to RPM elsewhere when needed.

int encoder_velocity(encoder *e){
	int velocity = abs(e->count);	// Velocity in encoder counts per control loop.
	e->count = 0;					// Reset encoder's count variable.
	return velocity;
}


PID Algorithm

A PID algorithm attempts to calculate an error between a desired setpoint and a measured value. In this case of this project we are measuring the velocity of a spinning motor.

The PID algorithm uses three terms known as the proportional, integral and derivative values.
Term Description
Proportional The difference between the desired value (setpoint) and current value.
Integral The summation of errors over time.
Derivative The difference between the previous proportional error and the current proportional error.

Each of these terms are multiplied by a gain to tune the response of the PID Algorithm.
Gain Response Adjustment
Proportional Increases response but increases steady state error.
Integral Reduces steady state error but increases overshoot.
Derivative Decreases overshoot but slows response.

The implementation of the PID algorithm is surprisingly very simple.

int do_pid(motor *m){

	float p_error;	// The difference between the desired velocity and current velocity.
	float i_error;	// The sum of errors over time.
	float d_error;	// The difference between the previous proportional error and the current proportional error.

	float p_output;	// The proportional component of the output.
	float i_output;	// The integral component of the output. This term dampens the output to prevent overshoot and oscillation.
	float d_output;	// The derivative component of the output. This term is responsible for accelerating the output if the error is large.

	float output;	// The sum of the proportional, integral and derivative terms.

	// Calculate the three errors.
	p_error = m->velocity_setpoint - m->velocity;
	i_error = m->integral_error;
	d_error = p_error - m->previous_error;

	// Calculate the three components of the PID output.
	p_output = m->kp * p_error;
	i_output = m->ki * i_error;
	d_output = m->kd * d_error;

	// Sum the three components of the PID output.
	output = p_output + i_output + d_output;

	// Update the previous error and the integral error.
	m->previous_error = p_error;
	m->integral_error += p_error;

	return (int) output;
}


Serial Communication

Communication to the motor controller is 8 bit asynchronous serial with 1 stop bit and no parity.

Instruction packets are sent by a master controller to the motor controller and are in the form of:
[0xFF] [0xFF] [id] [instruction] [length] [param 0] ... [param n] [checksum]

The meaning of each byte are:
Byte Description
[0xFF] [0xFF]
header bytes. Indicates the start of a new packet.
[id]
ID of the motor controller.
[instruction]
The instruction the motor controller is to preform.
[length]
The number of parameters included in the packet.
[param n]
Parameters of the packet's instruction.
[checksum]
Packet checksum.
Checksum = 255–((id+instruction+length+param 0+…+param n)%255)

The following instructions are available:
Instruction Function Value Parameters
READ Read values from parameters the control table. 0x01 1 * n
WRITE Write values to parameters in the control table. 0x02 2 * n
RESET Set all parameters in the control table back there initial value. 0x03 0

The parameters in the control table include:
Parameter Description Address Initial value
id The id of the motor controller. 0x00 1
Kp The proportional gain. 0x01 1
Ki The integral gain. 0x02 0
Kd The derivative gain. 0x03 0
Velocity Setpoint The desired motor velocity. 0x03 0
Velocity The current motor velocity. 0x04 0
Direction Motor rotational direction. 0x05 0

To make this all work first we do some initializations on the ATMEGA168's USART.

void xbee_init(long baud){
	UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8;
	UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1);
	UCSR0B |= (1 << RXEN0);
	UCSR0B |= (1 << TXEN0);
	UCSR0B |= (1 << RXCIE0);
}


Next we create a ring buffer to hold incoming instruction packets. Wikipedia has a great article about ring buffers if you would like a full description.

char buffer[BUFFER_SIZE];
int start = 0;
int end = 0;

void buffer_write(char c){
	if ((end + 1) % BUFFER_SIZE != start){
		buffer[end] = c;
		end = (end + 1) % BUFFER_SIZE;
	}
}

int buffer_read(){
	char temp = buffer[start];
	start = (start + 1) % BUFFER_SIZE;
	return temp;
}

int buffer_data_avaliable(){
	return (BUFFER_SIZE + start - end) % BUFFER_SIZE;
}

void buffer_flush(){
	start = end;
}


An interrupt service routine triggers on the receive complete flag. It simply writes the received byte to the buffer.

ISR(USART_RX_vect){
	buffer_write(UDR0);
}


Lastly we have a large routine that iterates through the entire buffer looking for complete instruction packets. When it finds a complete packet it calls the correct instruction routine (read, write, or rest) and then continues looking for more instruction packets until the buffer is empty.

void param_read(){
	int index = -1;
	int checksum = 0;
	int id = 0;
	int length = 0;
	int instruction = 0;

	// While there is data in the buffer...
	while(buffer_data_avaliable() > 0){
		// Searching for the first two 0xFF header bytes.
		if(index == -1){
			// First header byte.
			if(buffer_read() == 0xff){
				// Second header byte.
				if(buffer_read() == 0xff){
					checksum = 0;					// Reset the value of checksum.
					index = 0;						// Reset the value of index.
					id = buffer_read();				// Set id to the next byte in the buffer.
					checksum += id;					// Add the id to the checksum.
					instruction = buffer_read();	// Set instruction to the next byte in the buffer.
					checksum += instruction;		// Add the instrction to the checksum.
					length = buffer_read();			// Set length to the next byte in the buffer.
					checksum += length;				// Add the length to the checksum.
				}
			}
		}
		// Reading the paramaters after the header.
		else{
			// 
			if(index == length){
				// Calculate the checksum.
				if(255 - ((checksum) % 256) == buffer_read()){
					
					if(instruction == INST_READ)
						param_get(length, temp_params);
					if(instruction == INST_WRITE)
						param_set(length, temp_params);
					if(instruction == INST_RESET)
						param_init();					
					index = -1;
				}
				else
					index = -1;
			}
			else{
				temp_params[index] = (unsigned char) buffer_read();
				// Paramaters should never be 0xFF.
				if(temp_params[index] != 0xff){
					checksum += (int) temp_params[index];
					index++;
				}
				else
					index = -1;
			}
		}
	}
	buffer_flush();
}


PID Tuning Application

The last project goal on my list is to develop a GUI application for tuning the PID parameters of the motor controller. The application is written in Python with the wxPython cross-platform toolkit

In its current sate it is very basic but so far it has been very helpful in debugging the project. Below is a screen shot of the application at the time of writing. On the left we have controls to open and close serial coms. In the middle we have controls to set, get or reset the motor controller's parameters. On the right we have controls to capture and view the step response of the system.

The PID Tune Application

Here is a rudimentary window to take a quick look at the captured step response of the system.

Step Response Displayed By The PID Tune Application

The step response data can be imported into a spreadsheet program such as Open Office Calc and analyzed further.
Step Response Imported Into Open Office Calc

Results

The results of this project have exceeded my expectations.

The controller is able to maintain the rotational velocity of the motor within 2-3 RPM as measured with a digital tachometer under varying loads and input voltages.

The serial communication instruction packets make it very easy to interface the motor controller to a larger system. So far I have been able to add motor control to both my PC and ATMEGA644P prototype board without issue.

Lastly the PID Tune application while still very much a work in progress has been invaluable in debugging the motor controller and analyzing a the step response of the system.

Future Improvements

This project is still very much a work in progress. Here is a list of improvements I have planned.

Topic Improvements
H-Bridge While the SN754410 driver chip was great for prototyping it is very undersized at only 1A output. An alternative will be needed for the final revision.
Encoder Decoding Double the resolution of velocity measurement by interrupting on the rising and falling edges of both encoder outputs.
PID Algorithm Look into the topic of integral windup.
Add acceleration control.
Possibly add Position control.
Serial Communication Many more parameters can be added to make the motor controller more general purpose.
Some parameters should be write only, some read only, some read/writable.
Ability to save parameter settings in EEPROM.
Timing The main program loop does not have accurate timing. This causes small errors (2-3 RPM) on the measured output of the motor.
General Overall the software is very tied to my specific hardware. Much work needs to be done to make the code more general purpose for a large variety of applications.
For the final revision of this project I will design a custom PCB.
PID Tune Program Flesh out the PID Tune Program to automate PID gain tuning.
Improved interface for viewing step response.

Downloads

The c code and makefile for this project can be downloaded here.
The python code for the PID Tune application can be downloaded here.

Thanks for reading!
- Upgrayd
Subscribe to: Posts (Atom)