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:
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‘:
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.
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:
I tried to create the encoder disk with Scott Boskovich’s encoder design program which can be downloaded fromhttp://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:
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.
