Doing this project really turned me on to systems and controls theory. A friend and I implemented a fully functioning PID motor speed and positioning controller onto a PIC MCU. This is a pretty thorough guide to building this very hand robotics peripheral.
As part of the IEEE hardware team I’ve done a lot of projects, but I’m proudest of this one. This was a PIC based system that implemented a closed-loop PID controller for position and speed control of two DC motors. In addition, the control loop could be adjusted to induce arcs.
The system was designed as a peripheral for another processor so it also had an SPI command interface that allowed it to be communicated with on a high level while it controlled the motors on a low level. The peripheral had a serial console that allowed you to tune, test, and drive the robot.
In order to complete its task, the robot needed to have precise control of its speed and position. Because much of the way finding of this robot was accomplished through dead reckoning, it was also especially important to maintain accurate information regarding the distance traveled by the robot.
In order to give the robot all of these features, we designed a special sub-processor that was devoted to the low-level control of the two primary motors. We implemented this system on a Microchip PIC18F252. This processor was housed on the same board as the motor driver which took control signals from this controller and used them to actually drive the motors.
Our sub-processor monitored the motors, and applied an appropriate pulse width modulated (PWM) signals to allow them to reach their goal. This controller also had the ability to communicate with the primary controller to receive speed and distance commands as well as report accomplished speed and distance traveled back to the commanding processor.
In order to get speed and distance information from the motors, we purchased motors with built in encoders that output a quatrature encoder interface (QEI) signal. This signal is comprised of two pulse trains that tell when the wheel travels a part of a rotation, and the direction of motion. When channel A makes a low to high transition, the value of channel B is checked to determine the direction. A sample QEI output can be seen below.
The motion controller was able to interrupt on low to high transitions from the QEI on channel A and create an estimate of instantaneous speed and direction by monitoring the number of counts obtained during a specified time interval. We used this information as inputs to the control loop that determines and compensates for errors in speed or distance.
The Control Loop
We implemented a PID (Proportional, Integral, and Differential) control loop to control the speed of the motors. The speed of the motors is manipulated by altering the duty cycle of a PWM signal generated by the processor. The duty cycle of this signal is known as the control value. below is a functional diagram of the control loop.
This algorithm first determines the difference between the commanded speed and the actual speed. This value is known as error. The integral error is determined by adding the error to an accumulator, and the derivative error is calculated by subtracting the error from the previous error.
Every time the control loop executes, each of these errors are multiplied by a gain constant that has been tuned to optimize response time and accuracy. These errors, multiplied by their gain constants are then be added to the control value to compute the new control value.
In this way, the control value is continuously updated based on the response of the motors. This ensured that the motors are moving at the desired speed despite drag, obstacles, or other unexpected track conditions.
A final compensation factor is summed along with the P, I, and D errors. This is an error generated by inconsistencies in the two wheels. This helps to ensure that the robot goes straight by compensating for factors that slow one wheel but not the other.
This was accomplished by controlling the speed based on the robotâ€™s proximity to the desired position. The motor controller could take commands from the main processor that told it how many â€˜ticksâ€™ to travel. This value corresponded exactly to the number of QEI pulses that the robot should move.
The exact distance represented by each tick was determined by the resolution of the encoders and the diameter of the wheel. Our control system was capable of responding to 2944 ticks per revolution. With wheels that were 2.2 inches in diameter, the total distance per tick was 2.346×10-3 inches. Thus, very detailed motor control was possible.
The position control algorithm allowed the robot to approach its target distance at any speed. Then, as the target approached, it would slow its speed so that it could stop accurately when desired. This was achieved with a simple algorithm diagramed below.
The motors position was monitored and continuously updated in local variables. Once the motors reached a distance a certain threshold before the desired position, the position control section of the algorithm would command the motors to stop. This would ensure that as the motors slowed down and achieved a speed of zero the robot would be at the desired position. These two systems were independent for the left and right wheel so that they could be commanded to go different distances for turning purposes.
These abilities provided the robot with the tools needed to control the speed and distance of travel. However, in order to be truly useful, this sub processor needed to be able to receive commands from the master processor, and provide information regarding the motor status back.
We accomplished this by using an SPI (Serial Peripheral Interface) communication protocol. The communication was message-based. The main processor could send eight-byte messages to the PID controller. These messages began with a one byte op-code telling the sub processor what type of command the message contained. The next seven bytes held data such as the desired speed, or the desired distance to be traveled.
The sub processor could simultaneously send back eight bytes that held instantaneous speed, and total distance traveled information. With the ability to control speed, position, and communicate with the main processor completed, the motion control system was almost done.
Tuning the Control
The final step was to tune the PID control algorithm. This is accomplished by adjusting the gain constants by which each error is multiplied by before summing it with the previous control effort. This is a complicated process that is mostly achieved through trial and error.
Another tool that helped was using GNUplot to make graphs of debug data output by the PID controller. This allowed us to see the interaction of different errors. This interaction can be seen below.
The work of tuning the processor was simplified by developing a user interface on the PID controller that was separate from the main processor. This allowed the control system to be tuned over serial by adjusting gain constants stored in EEPROM.
With the motor controller fully tuned, it performed very well and provided excellent speed and position control for the robot without bothering the main processor with this low level task. Full code for this motion controller is below.
The Motor Driver
All of this fluffy code stuff is great, but you always need some good ol’ power electronics too. The motor driver was required to step up logic level PWM signals to 9.7V motor driving PWM at up to 3A. In order to accomplish this, we used an L298 H-bridge chip and some glue logic. In addition, it was a nice place to house the PIC chip that implemented the PID controller. We added some handy plugs for programming, connecting to other processors, and attaching motors and power. Just because I’m a nice guy, I’ve included a schematic and layout for the board as well as the code below.
I was really happy with this project. Below you can see me and Jacques giving our ‘gang sign.’ In case you can’t read it that says “PIC” … as in the PIC chip we wrote the PID controller on.
But more seriously, there were some valuable lessons learned. I really got the hang of SPI. It may have been better to use I2C for this, but I’ll have to save that for next time. I also got really turned on to systems and controls. The idea of a closed loop control system that uses its error to correct itself is very clever and elegant. I strongly recommend taking a class on systems and controls if you haven’t yet.
Another thing I’d like to share is *ALWAYS* program in C. Assembly is sooo much more annoying. We started the project in assembly and quickly realized that things were getting out of hand so we switched to C (God’s own language) and everything started going very smoothly.
If I had it to do over again, I might also make the position control better. As it was, you really needed to tweak constants every time you changed the default speed of the system. It was highly accurate, but it would be a little more robust if I had implemented a PID position control algorithm in addition to the PID speed control algorithm.