The focus of this blog post is the motor control system. Let’s start with a photo of the current state of the robot platform:

Robot base with motor driver and arduino controller

The wheels have been mounted and the main electric wiring is in place. The two batteries are wired up in parallel for a 12V power supply resulting in a maximum motor speed of about 60 rpm . The motors could be supplied with 24V but at least for now the power and speed that I get out of 12V is enough.

The Motor Driver

The power to the motors is provided by a Sabertooth 25 dual 25A motor driver (I got mine from The RobotMarketPlace). It is mounted vertically directly behind the batteries. The driver is controlled by a Seeeduino Mega, an Arduino Mega clone from SeeedStudio Depot. In the photo the Seeeduino board is barely visible in the back hidden underneath an attached protoshield. The input mode of the motor driver is configured like this:

  • 1 & 2: R/C input signal
  • 3: no lithium battery cutoff
  • 4: no mixing mode
  • 5: linear
  • 6: micro controller mode (no timeout fail-safe and no auto-calibration)

The corresponding signal is provided by the Arduino controller. Currently (for testing purposes only) the motor speed is controlled by a 100k potentiometer that is hooked up between ground and 3.3V with its wiper connected to an analog input of the controller. The resulting Arduino program is listed below.

#include "WProgram.h"

unsigned long previousMilliseconds = 0;
unsigned long updateInterval = 50; // update interval in milli seconds

// temporary: contolling the motors with one potentiometer
int potiAnalogInPin = 5;
int motor1PwmOutPin = 11;
int motor2PwmOutPin = 12;

void setup()
{
 analogWrite(motor1PwmOutPin, 127);  // motors stopped at middle value; analogWrite values from 0 to 255
 analogWrite(motor2PwmOutPin, 127);  // motors stopped at middle value; analogWrite values from 0 to 255
}

void loop()
{
 unsigned long currentMilliseconds = millis();

 unsigned long milliSecsSinceLastUpdate = currentMilliseconds - previousMilliseconds;
 if(milliSecsSinceLastUpdate > updateInterval)
 {
 // save the last time we updated
 previousMilliseconds = currentMilliseconds;

 int potiValue = analogRead(potiAnalogInPin);  // read the input pin
 analogWrite(motor1PwmOutPin, potiValue / 4);  // analogRead values go from 0 to 1023, analogWrite values from 0 to 255
 analogWrite(motor2PwmOutPin, potiValue / 4);  // analogRead values go from 0 to 1023, analogWrite values from 0 to 255
 }
}

The Wheel Encoders

The speed and direction of each motor is measured by two home-brew quadrature encoders that are mounted on the upper ends of the motors where the electric brake used to be. The electronic circuit for each quadrature encoder is based on two Hamamatsu P5587 IR photo reflectors and is described in David P. Anderson’s article ‘Home-Brew Shaft Encoders for the Pittman GM8712 Gearhead Motor‘:

One half of the quadrature encoder circuit

The space on top of the motor is wide enough to mount the two photo reflectors in one line which simplifies the calibration. The two photos below show the circuit board mounted on the back of the motor first without and then with the encoder disk attached to the motor shaft. The board is glued to a plastic disk that is attached to cut off screws. This setup allows me to easily fine tune the distance between the sensors and the encoder disk.

Quadrature encoder attached to the end of the motor

Encoder disk mounted above the sensors

Encoder disk mounted above the sensors

In order to get a quadrature encoder signal that conveys speed and direction the bright and dark sequences for the two sensors need to be offset from each other. Since the sensors are lined up the offset must be created by the encoder disk itself:

Encoder disk

I tried to create the encoder disk with Scott Boskovich’s encoder design program which can be downloaded from http://www.societyofrobots.com/sensors_encoder.shtml but the output was too coarse. Instead I created a C# console application that builds up the encoder disk as an SVG (scalable vector graphics) file which I then open and print from Inkscape (my robot arm calibration page has more details). The C# GridBuilder source code code requires .Net 2.0 and either VisualStudio 2008 or the open source IDE SharpDevelop.

In order to reliably detect the encoder wheel transitions external interrupts need to be used in the Arduino program. I can highly recommend the Arduino Playground article ‘Reading Rotary Encoders‘ which provided invaluable information for my implementation. The main part of the encoder logic is encapsulated in the QuadratureEncoder class. It heavily borrows from mikkoh’s contribution to the Arduino Playground article. The SendInfo is used for calibration purposes (see below).

/*
 QuadratureEncoder.h - Quadrature encoder library.
 Dr. Rainer Hessmer, April, 2010.
 Released into the public domain.

 Inspired by the code from http://www.arduino.cc/playground/Main/RotaryEncoders
 Incorporates code from mikkoh and others that contributed to the referenced article.
*/

#ifndef QuadratureEncoder_h
#define QuadratureEncoder_h

#include "WProgram.h"

class QuadratureEncoder
{
 /*
 Wraps the encoder setup and the update functions in a class

 !!! NOTE : User must call the functions OnAChanged and OnBChanged from an
 interrupt function him/herself!

 // ------------------------------------------------------------------------------------------------
 // Example usage :
 // ------------------------------------------------------------------------------------------------
 #include "QuadratureEncoder.h"

 QuadratureEncoder encoder(2, 3);

 void setup()
 {
 attachInterrupt(0, HandleInterruptA, CHANGE);
 attachInterrupt(1, HandleInterruptB, CHANGE);
 }

 void loop()
 {
 // do some stuff here - the joy of interrupts is that they take care of themselves
 }

 void HandleInterruptA()
 {
 encoder.OnAChanged();
 }

 void HandleInterruptB()
 {
 encoder.OnBChanged();
 }

 // ------------------------------------------------------------------------------------------------
 // Example usage end
 // ------------------------------------------------------------------------------------------------
 */

public:
 // pinA and pinB must be one of the external interupt pins
 QuadratureEncoder(int pinA, int pinB)
 {
 _PinA = pinA;
 _PinB = pinB;

 pinMode(_PinA, INPUT);      // sets pin A as input
 pinMode(_PinB, INPUT);      // sets pin B as input

 _ASet = digitalRead(_PinA);   // read the input pin
 _BSet = digitalRead(_PinB);   // read the input pin

 _Position = 0;
 }

 long int getPosition () { return _Position; };
 void setPosition (const long int p) { _Position = p; };

 // Interrupt on A changing state
 void OnAChanged()
 {
 // Test transition
 _ASet = digitalRead(_PinA) == HIGH;
 // and adjust counter + if A leads B
 _Position += (_ASet != _BSet) ? +1 : -1;
 }

 // Interrupt on B changing state
 void OnBChanged()
 {
 // Test transition
 _BSet = digitalRead(_PinB) == HIGH;
 // and adjust counter + if B follows A
 _Position += (_ASet == _BSet) ? +1 : -1;
 }

 void SendInfo()
 {
 bool a = digitalRead(_PinA);
 bool b = digitalRead(_PinB);
 unsigned long milliSecs = millis();

 Serial.print(milliSecs);
 Serial.print("\t");
 Serial.print(a);
 Serial.print("\t");
 Serial.print(b);
 Serial.println();
 }

private:
 int _PinA, _PinB;
 bool _ASet, _BSet;
 long _Position;
};

#endif

The QuadratureEncoder.h file needs to be copied into a folder called QuadratureEncoder under the libraries folder that is used by the Arduino environment so that it becomes accessible to Arduino programs.

The complete source code of the Arduino program combines the motor control code from above with the encoder logic:

#include "WProgram.h"
#include <HardwareSerial.h>
#include <QuadratureEncoder.h>

unsigned long previousMilliseconds = 0;
unsigned long updateInterval = 50; // update interval in milli seconds

// temporary: contolling the motors with one potentiometer
int potiAnalogInPin = 5;
int motor1PwmOutPin = 11;
int motor2PwmOutPin = 12;

// The quadrature encoder for motor 1 uses external interupts 0 and 1 which are associated with pins 2 and 3
QuadratureEncoder encoderMotor1(2, 3);
// The quadrature encoder for motor 1 uses external interupts 5 and 4 which are associated with pins 18 and 19
QuadratureEncoder encoderMotor2(18, 19);

void setup()
{
 analogReference(EXTERNAL); // we set the analog reference for the analog to digital converters to 3.3V
 Serial.begin(115200);

 analogWrite(motor1PwmOutPin, 127);  // motors stopped at middle value; analogWrite values from 0 to 255
 analogWrite(motor2PwmOutPin, 127);  // motors stopped at middle value; analogWrite values from 0 to 255

 attachInterrupt(0, HandleMotor1InterruptA, CHANGE); // Pin 2
 attachInterrupt(1, HandleMotor1InterruptB, CHANGE); // Pin 3

 attachInterrupt(5, HandleMotor2InterruptA, CHANGE); // Pin 18
 attachInterrupt(4, HandleMotor2InterruptB, CHANGE); // Pin 19
}

void loop()
{
 unsigned long currentMilliseconds = millis();

 unsigned long milliSecsSinceLastUpdate = currentMilliseconds - previousMilliseconds;
 if(milliSecsSinceLastUpdate > updateInterval)
 {
 // save the last time we updated
 previousMilliseconds = currentMilliseconds;

 int potiValue = analogRead(potiAnalogInPin);  // read the input pin
 analogWrite(motor1PwmOutPin, potiValue / 4);  // analogRead values go from 0 to 1023, analogWrite values from 0 to 255
 analogWrite(motor2PwmOutPin, potiValue / 4);  // analogRead values go from 0 to 1023, analogWrite values from 0 to 255
 }
}

// Interrupt service routines for motor 1 quadrature encoder
void HandleMotor1InterruptA()
{
 encoderMotor1.OnAChanged();
 //encoderMotor1.SendInfo();
}

void HandleMotor1InterruptB()
{
 encoderMotor1.OnBChanged();
 //encoderMotor1.SendInfo();
}

// Interrupt service routines for motor 2 quadrature encoder
void HandleMotor2InterruptA()
{
 encoderMotor2.OnAChanged();
 //encoderMotor2.SendInfo();
}

void HandleMotor2InterruptB()
{
 encoderMotor2.OnBChanged();
 //encoderMotor2.SendInfo();
}

Note: Yesterday I upgraded to Arduino software version 0018. Since the upgrade I can no longer send data over the serial port and at the same time use the external interrupts and the analogWrite functions. I will update this post once I have a solution.

Before the upgrade to 0018 I used the currently commented out lines ‘encoderMotor<1|2>.SendInfo()’ to send the measured transitions to the PC for plotting. As an example here is the plot of the encoder output for Motor1 over a time period of 100 milliseconds:

Quadrature Encoder Signal

The blue and red curves are the output of the two P5587 photo reflectors attached to Motor1. For the measurement I drove the motor as slowly as possible. Due to the diminished power the motor speed varied slightly as can be seen in the varying pulse widths. If the sensors would be perfectly aligned the two signals (blue and red) would be exactly phase shifted by 90°.  However, as long as the two signals are sufficiently offset, four transitions can be accurately measured for each black & white segment of the encoder ring. The encoder disk has 40 black & white pairs in each ring resulting in 160 transitions per full rotation.

The shaft that the encoder disk is attached to makes 32 full rotations for each rotation of the wheel. Thus I get 32 x 160 = 5120 transitions for each turn of the wheel giving a resolution of about 0.07° or 0.2mm for the 350mm (13.8″) diameter wheels. This is more than enough and in the future I will probably only use one interrupt per wheel for a resolution of 0.4mm.

So, the ‘drive train’ is pretty much in place now. Next I will focus on hooking up the accelerometer and the gyroscope.

8 Responses to “Building a Self-Balancing Robot: Motor Driver and Wheel Encoder”

  1. [...] input in the Arduino program I initially used the same code that I described in my blog post ‘Building a Self-Balancing Robot: Motor Driver and Wheel Encoder’. Everything worked fine when I turned the motors manually. The interrupts kicked in and the [...]

  2. Hi there nice optical encoders , where you ever able to find some off the shelf encoders that fit those motors ?
    Edward.

  3. There may be noticeably a bundle to know about this.
    I assume you made sure good factors in features also.

    Also visit my web blog – cell phone plans

  4. My brother rewcommended I might like this web site.
    He wass totally right. This post truly made my day.
    You can not imagine just how much time I had spent
    forr this information! Thanks!

  5. Hello there! This is my first visit to your blog!
    We are a team of volunteers and starting a new initiative in a community in the
    same niche. Your blog provided us beneficial information to work
    on. You have done a extraordinary job!

  6. These shampoos have been designed to work on oily skins and the
    zinc is very good at fighting acne disorder.

    Feel free to visit my web blog :: pimples and acne

  7. I am not positive where you’re getting your information, but great topic.
    I must spend a while studying more or working out more.
    Thanks for fantastic information I used to
    be in search of this information for my mission.

  8. You are so awesome! I do not think I’ve truly read through something like this before.
    So great to discover another person with some unique thoughts on this subject matter.
    Really.. thanks for starting this up. This website is something that’s needed on the internet,
    someone with a bit of originality!

Leave a Reply

(required)

(required)

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>

© 2014 Dr. Rainer Hessmer Suffusion theme by Sayontan Sinha