Using motor encoders to control speed
This tutorial shows how to connect 2 DC motors with simple encoders to an Arduino for precise speed control and distance measuring. The sample code is written for a DAGU "Mini Driver" and "Simple Motor and Encoders Kit" but will work with any Arduino board with any gearbox / encoder combo.
When you build a robot using wheels or tracks then you quickly discover that it is impossible to make the robot travel in a straight line. This is because no 2 motors, wheels or gearboxes are identical and brushed DC motors tend to run slightly faster in one direction than another.
You will also discover than your robot will either stall or lurch a bit when trying to perform precise maneuvers at low speeds. This is because the motors need more power to overcome inertia when starting up than they need to maintain a set speed.
The best way to overcome these problems is with encoders. Encoders allow your processor to monitor the actual speed of the wheel and adjust the power to the motor if required to maintain the correct speed. As an added advantage, if you know the circumference of your wheels and resolution of your encoders then you can also measure distance travelled, which is very useful for mapping an environment or navigating by "dead reckoning".
Encoders basically come with either 1 or 2 sensors. The encoders that come with 1 sensor can measure speed and distance but cannot determine which direction the motor is spinning. Encoders with 2 sensors (often called quadrature encoders) have 2 sensors that are 90° degrees out of phase. These sensors can determine what direction the motor is spinning as well as measuring speed and distance. In many cases they also offer better resolution.
Typically most encoders in hobby robots use optical sensors however there are other types. In this tutorial I am using hall-effect sensors that detect a magnetic field but the type of sensor used should not matter for this tutorial. The only thing that matters is that the outputs of the sensors swing between digital high and digital low as the wheel turns.
How precisely you can control your motors depends largely on the resolution of your encoders. Higher resolution generally gives better control although it also depends a bit on the speed of your processor and how you write your code. The encoder's resolution is how many state changes the encoder produces per revolution of the wheel. A state change is when the sensors output changes from low to high or from high to low.
The resolution depends on where the encoder is within the gearbox as well as the number of sensors used and how they are triggered. In my example I only have 1 sensor on each motor. The sensor is triggered by a magnetic disc that has 4 north poles and 4 south poles. This means that for every revolution of the magnetic disc my sensor output will change state 8 times.
Because my encoder disc is on the motor shaft, my encoder resolution needs to be multiplied by the gear ratio of the gearbox. The gearbox has a ratio of 118.5:1 so my final resolution is 8 x 118.5 = 948 state changes per revolution of the wheel. My wheels have a diameter of 65mm so the distance travelled in 1 revolution is 204.2mm. This means I can measure distances as small as 0.2154mm.
The gearboxes I'm using also allow me to put the encoder disc directly on the output shaft. This would then give me an encoder resolution of just 8 state changes per revolution of the wheel and the minimum distance I could measure would be 25.5mm.
Controlling Speed Without Encoders:
When a robot does not have encoders and wants to drive in a straight line the software simply gives both motors the same PWM (Pulse Width Modulation) and assumes they are both running at the same speed. More advanced programs may multiply the PWM values by a correction factor previously determined from a calibration test but this is still not 100% accurate. With encoders the software specifies the speed the motors should run at and then uses the encoders to ensure the motors maintain that speed.
Using Encoders With Interrupts:
Connect your encoders to the external interrupt pins (usually D2 & D3) on most Arduino controllers. Check here if you're not sure: http://arduino.cc/en/Reference/AttachInterrupt. We use the external interrupts because they are the most efficient and accurate method of responding to the encoder sensors. My hall-effect sensors have an open drain output so I need to enable the internal pullup resistors on these pins. D2 & D3 are inputs by default. When I write a digital 1 to these pins while they are set to input it enables the internal pullup resistors.
Although we could control the speed by counting the number of encoder state changes per second this would mean that the software could be very slow to respond to changes in desired speed. We could count pulses over a shorter time such as 1mS for a faster response but this would require high-resolution encoders and can still loose precision at low speeds.
In this tutorial we will measure the time between state changes in microseconds to determine the speed. This allows the software to respond much quicker as it can measure the wheel speed after 2 state changes. For this method we need to know what is the maximum number of state changes your encoder will generate at full speed. From this we can calculate the time in μS between pulses that want to achieve a set speed.
When an interrupt occurs the processor stops what it is doing and jumps to an ISR (Interrupt Service Routine). The ISR should be as small as possible to minimize any chance of it interfering with other interrupts. Some functions such as Servo control can be affected if the ISR is too big.
In my sample code the ISR only measures the time in μS since it was last called, increments the counter used for measuring distance and then sets a flag that tells the code to call the MotorControl() function where the rest of the calculations are done. It will not matter if the rest of the calculations take a little longer to complete because the time between state changes was accurately measured in the ISR.
In reality the timing accuracy of the micros() function has a maximum resolution of 4μS so the time could be out by as much as 3μS but this will be close enough for our purposes. Momentum limits how quickly the motor can change speed and averages out these small inaccuracies.
Maximum State Changes:
In my example, I measured the time it took for my wheel to complete 1 revolution (no load) with a 6V supply and determined the speed to be approximately 135rpm. My gearbox has a ratio of 118.5:1 so this means my motor speed is 15997.5rpm, which is about right for this type of motor running at 6V.
So round it up to 16000rpm and my motor shaft spins 16000 / 60 = 266.66 revolutions per second. My encoder gives 8 state changes per revolution so my maximum possible number of state changes per second is 266.66 x 8 = 2133.33. As this was with no load on the motor I'm going to round it down to 1900 which is about 89%.
You may need to experiment with this figure a bit. If it is higher than the slowest motor can achieve under load then the robot may still not achieve a straight line at full speed. If you set this value lower then you guarantee both motors can achieve this speed, even under load but you may limit your maximum speed. I suggest starting at around 90% of maximum speed.
This value is defined in my sample code as the constant "maxspeed" but in my sample code I use a variable called "bestspeed" that has been adjusted to allow for changes in battery voltage. If you're using a battery where the voltage drops as it discharges (e.g. Alkaline or LiPo) then you may want to use a voltage monitor to tweak this value as the battery voltage drops. I prefer NiMh batteries because their voltage remains constant for about 90% of their discharge cycle.
Correcting for Battery Voltage:
In my example I am using a DAGU Mini Driver, which is the equivalent to an Arduino NG or older w/ ATmega8. This low cost controller has a dual "H" bridge rated at 2.5A per motor built in and a battery monitor on A7. My sample code checks the battery voltage every 5 seconds and adjust the variable "bestspeed" so that as the battery voltage drops the maximum possible speed is also reduced.
As power is equal to voltage x current and the current is proportional to the voltage this suggest that when the battery voltage is halved, the maximum current draw should also be halve and therefore the power would be ½ x ½ = ¼. This formula is not perfect as it does not allow for gearbox friction and the motor is not a pure resistor but it should be accurate enough for this application.
Now my bestspeed = maxspeed x (batvolt / maxvolt)²
This should ensure that as the battery voltage drops the best speed should remain obtainable allowing the robot to maintain a straight line.
Converting State Changes to Microseconds:
So now we have a value for maximum state changes per second and we have the number of microseconds between state changes. We need to put this information to work. The reason we bothered with the maximum number of state changes per second is it gives us a linear value to calculate our power from. The time between state changes in μS however is a logarithmic scale as you can see in the table below.
Power SC/sec μS/SC 100% = 1900 = 526μS 90% = 1710 = 585μS 80% = 1520 = 658μS 70% = 1330 = 751μS 60% = 1140 = 877μS 50% = 950 = 1053μS 40% = 760 = 1316μS 30% = 570 = 1754μS 20% = 380 = 2631μS 10% = 190 = 5263μS
In my sample code the desired speed is expressed as a value from -100% to +100% and the desired number of state changes is a fraction of the maxspeed value. Negative values indicate reverse direction.
State changes per second = abs(desired speed) / 100 * bestspeed.
Therefore μS between state changes = 1 / (abs(desired speed) / 100 * bestspeed) * 1000000
As the Arduino IDE processes the formula strictly from left to right a more accurate result is given by re-arranging the formula to:
Correcting the Speed:
Now that we know how many μS we should be getting between state changes if the speed was correct we can now compare this to the actual time between state changes.
To ensure smooth motor control my sample code calls the MotorControl() function once every mS. When the motor is stopped the encoders do not generate any information to work from so my code jumpstarts the motor speed a small amount if the encoder has not turned after 20mS. If the motor is not supposed to turn then the control code will quickly reduce the power back to 0 again.
Using the Sample Code:
The sample code is pretty straightforward. You can add your own code for sensors etc. very easily. The main loop has a timer that calls the motor control routine once every mS. You can change this but it will affect how quickly the motors respond to change.
The ISR's and the motor control code are in their own separate tabs to keep the code neat and easy to read.
If you are using different geared motors or different encoders then you will need to change the maxspeed constant. You will also need to change the maxvolt constant to suit your battery or if you do not have a battery monitor, just eliminate the 3 lines of code in the main loop where the battery voltage is read and a new bestspeed value is calculated.
Note: the sample code was written in Arduino 1.04 IDE. I cannot guarantee it will work with other versions of the IDE because the Arduino IDE is not always backward compatible.
Good luck and enjoy!