27/10/2016
Upgrayd: PID Velocity DC Motor Controller 0
mais Próximo blog»
[email protected] Painel Sair
Upgrayd Blog of Team Upgrayd and Upgrayd Labs
PID Velocity DC Motor Controller
Blog Archive ▼ 2012 (15)
Project Introduction
▼ May (2)
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.
Blog Has Moved! BeagleBone Dynamixel AX12 Interface Part 2 ► April (1) ► March (7) ► February (2) ► January (3) ► 2011 (21) ► 2010 (45) ► 2009 (3)
Labels AVR (15) PID Velocity DC Motor Controller Measuring Velocity of 50 RPM Project Goals
AX12 (9) Dynamixel (8)
The goals of my project included:
Gait (7)
Control 512 volt dc motors.
Gold Rush (9)
Interface for quadrature encoders with A/B output.
IPMechCam (4)
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.
Mech Warfare (17) Prototype ATmega644p Breakout Board (2) Python (12)
Background information
Robogames 2010 (1)
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, HBridge and DC Motor. The microcontroller provides the H bridge with direction and speed information. The HBridge in turn drives the DC motor.
Second Amendment (10) TRENDnet (4) UpgraydMech (1) Wireless (3) Xbee (7)
Search This Blog Search 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.
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
1/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller
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 8bit microcontroller. It also has an interface to both an Xbee wireless module and a SN754410 1A quadruple half Hhbridge driver making it ideal for prototyping this project. To program the ATMEGA 168 on the MINI Robocontroller I have installed the AVRGCC toolchain under Ubuntu. The tools that make up the AVRGCC toolchain include: Tool
Description
GCC
The C/C++ Compiler
binutils
Binary tools that include an assembler and linker.
libcavr
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 aptget install gccavr binutilsavr avrlibc gdbavr 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 Hbrdige driver. The quad half drivers can be combined to make two full Hbridge 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
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
2/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller
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 8bit timer/counter to output a pwm signal.
// Fast PWM Initializations TCCR0A |= (1 << WGM00);
// Set timer/counter 0 waveform generation mode to fast pwm.
// Set timer/counter 0 compare output mode.
// Set timer/counter 0 clock source.
TCCR0A |= (1 << WGM01); TCCR0A |= (1 << COM0A0); TCCR0A |= (1 << COM0A1); TCCR0B |= (1 << CS00); 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
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
3/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller
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 ++;
}
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
4/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller }
}
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;
return velocity;
// Reset encoder's count variable.
}
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;
// 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;
// The sum of the proportional, integral and derivative terms.
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
5/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller
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){
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
6/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller
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;
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;
}
}
}
// Reading the paramaters after the header.
else{
//
if(index == length){
// Calculate the checksum.
if(255 ‐ ((checksum) % 256) == buffer_read()){
if(instruction == INST_READ)
if(instruction == INST_WRITE)
// Reset the value of index.
// Add the length to the checksum.
param_get(length, temp_params);
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
7/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller
if(instruction == INST_RESET)
param_set(length, temp_params);
index = ‐1;
}
else
}
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
}
}
}
buffer_flush();
param_init();
index = ‐1;
index = ‐1;
}
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 crossplatform 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.
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
8/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller
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 23 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
HBridge
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 (23 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
Home
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
9/10
27/10/2016
Upgrayd: PID Velocity DC Motor Controller Subscribe to: Posts (Atom)
Awesome Inc. template. Powered by Blogger.
http://upgrayd.blogspot.com.br/p/pidvelocitymotorcontroller.html
10/10
