| SRS Home | Front Page | Monthly Issue | Index |
|
|
| Author: | Dafydd Walters < dafydd@walters.net > |
| Level: | Novice |
| Revision: | 1.0 |
| Date: | September 30, 2000 |
This article describes how to implement a simple, but robust, optical wheel encoder system on a robot that uses hobby servos for two-wheel differential drive. In my case I am using the TJ-PROTM robot platform from MekatronixTM, and some aspects of this article are specific to that platform. However, it should be easy to adapt the hardware and software for virtually any robot with an HC11 controller board.
This article details the hardware you will need (and where you can buy it), construction methods, and software techniques for implementing navigation by dead reckoning. The software examples are written in Newton Labs' Interactive C ("IC") version 3.2. To make use of the code samples, I am assuming you know how to load C and binary modules in IC, and that you know how to use the start_process( ) function.
Rather than being heavy in theory, this is written more as a "how to" article. If you are interested in the theory, I would refer you to the Rossum Project article in the Links section at the end.
At the heart of this design is a pair of Hamamatsu P5587 photoreflectors (figure 1), one mounted on each side of the robot behind the wheels. Each wheel has a cardboard disc fixed to it with 48 alternating black and white segments.
The tiny 5-legged Hamamatsu photoreflector package contains an infrared ("IR") LED and a matching IR phototransistor, both mounted on the top of the device in such a way that the phototransistor will detect reflected IR light emitted from the LED when a bright surface (such as white card) is positioned within a few millimeters of the device.
Figure 1. Hamamatsu P5587 Photoreflector
As the wheel turns, and the photoreflector "sees" the white segments between the black segments, the phototransistor outputs a digital pulse train. By counting the pulses, and knowing the number of segments and diameter of the wheel, the robot can compute its position based on the distance traveled by the wheels.
Table 1 lists the complete bill of materials for this project.
| Qty | U.O.M. | Part | Source |
| 2 | each | Hamamatsu P5587 photoreflector |
Acroname (part # R64-P5587) |
| 1 | each |  Stripboard |
RadioShack® |
| 2 | each |  750 ohm 1/4 watt resistor |
RadioShack® |
| 2 | each |  6.8 K ohm 1/8 watt resistor |
RadioShack® |
| 2 | each |  0.1 µF capacitor (ceramic) |
RadioShack® |
| 8 | inch |  Ribbon cable (2 runs of 3 way used) |
RadioShack® |
| 1 | each |  Header strip (0.1" pitch) |
Mr RobotTM |
| 1 | roll |  Double sided tape (foam center) |
Office supplies store |
| 8 | each | Nylon washer #6 | Hardware store |
| 2 | sheet | Card stock suitable for feeding through printer | Office supplies store |
Table 1. Parts List
Cut out two small pieces from the stripboard (figure 2). These will form the circuit boards that will be attached to the servos, upon which the Hamamatsu P5587 photoreflectors will be mounted. The smallest size into which it is practical to cut the boards is 1.4 inch x 0.4 inch. This is the size I used, although in retrospect I could have made the boards wider (perhaps 1.0 inch rather than 0.4 inch) to make it easier to populate the components and wire them up.
 |
 |
Figure 2. Circuit board (front & back) - right wheel
Drill two 3mm diameter holes in the boards, 0.15 inch from one side, 0.2 inch from the bottom, and 0.4 inch apart. IMPORTANT: The two boards are not the same! One is the mirror image of the other, so the holes must be drilled on the correct side as determined by which side of the robot the board will be mounted. Figure 2 shows the board for the RIGHT side wheel. See also figure 8 which shows the assembled board mounted on the right servo.
Cut an 8-inch strip (this length is correct for the TJ-PROTM robot; use whatever length is appropriate for your own robot) of black, white and gray wire from the ribbon cable (this is just for consistency with the wiring color scheme used on MekatronixTM robots; use whichever colors make sense in your own robot's wiring scheme). Cut a 3-connection header from the header strip, and solder one end of the wires to the header pins.
Insert the photoreflector into the board first (figure 3) and solder it in place. The pins of the photoreflector have to be bent apart in order to reach the 0.1" pitch holes in the stripboard. Solder the wires at the free end of the 3-way ribbon to the top of the board (refer to figure 8).
Figure 3. Board with photoreflector fitted.
Referring to the schematic in figure 4, build the circuit on the board. If you buy the P5587 from Acroname, it comes with a datasheet that identifies the pin numbers of the part.
Figure 4. Circuit Schematic
ALWAYS carefully double check all your connections visually after you've finished assembling your boards.
Figure 5. Assembled board
Right-click on the picture of the encoder disk in figure 6 and save it to your computer's hard disk. Print out two copies of the image at 300 dots per inch on card (not paper). Use a laser printer if possible (ink jet "splatter" could cause problems). Using a hobby knife on a cutting mat, carefully cut around the circles and also cut out the center of the circles so that the card discs fit snugly over the wheel bushes.
Figure 6. Encoder disc
Using small pieces of double-sided foam-centered sticky tape, attach the card discs to the wheels (figure 7).
 |
 |
Figure 7. Wheels with discs attached
Mount the boards on the servos (figure 8) using nylon washers to separate the back of the boards from the metal bushes in the servo mounting holes (figure 9) to prevent short circuits.
Figure 8. Finished board mounted in place - right wheel
Figure 9. Metal bushes in servo mounting holes
Plug the header for the right wheel encoder into the PA1 connector (JP10) on the MTJPRO11 board, and the header for the left wheel encoder into the PA2 connector (JP11). The black (GND) wires are closest to the outside of the MTJPRO11 board.
Figure 10. Boards connected to MTJPRO11
With power applied to the board, watch the output of the photoreflector with a voltmeter flick between high (5V) to low (0V) as you SLOWLY turn the wheels by hand.
Before we can start writing any meaningful code for navigation, we need to define a coordinate system for the robot's world. We will assume that the robot lives in a two-dimensional world. This means that at any given time its position can be defined in terms of 2-D Cartesian coordinates and the direction it is facing can be defined as an angle measured from one of the axes (see figure 11).
Figure 11. Coordinate System
In keeping with mathematical conventions, we will assume that the angle that defines the orientation of the robot (theta) is measured counterclockwise from the x-axis. This means that in figure 11, for example, theta is positive. If the robot was facing "south", theta would be negative.
We will assume that the position coordinates, x and y, are in meters, and that the orientation angle, theta, is in radians (PI radians = 180 degrees, where PI = 3.14159 approximately).
Examples (assuming north is straight up the y-axis):
Working from the lowest level of functionality upwards, the first thing we will need, in order to implement a successful odometry system is accurate pulse counts from the wheel encoders. By counting the pulses from our wheel encoders, we can deduce the distance each wheel has traveled. However, our simple wheel encoders do not tell us the direction a wheel is turning (more sophisticated quadrature encoders do). We will overcome this limitation by using the fact that we know which way the wheels should be turning by adapting the code we use to drive the servo motors (more on this later).
Listing 1 is an IC assembly language source module that counts the pulses output by the photoreflectors as the white and black segments fly past. The variables right_count and left_count increase in value with each passing segment when the wheels are being driven forwards, and decrease when the wheels are being driven backwards. This works by using the input capture function of PORT A bits 1 and 2 (input capture pins IC2 and IC1 respectively) to generate interrupts every time there is a rising or falling edge on these pins. The interrupt handlers increment or decrement the 16-bit counters depending on the values of the variables left_direction and right_direction (which are set by the motor drive code - more on this later).
Right-click here to download odometer.asm. Right-click here to download odometer.icb (the file you must load into IC).
/* odometer.asm - Copyright (C) 2000, Dafydd Walters */ #include <6811regs.asm> ORG MAIN_START * C variables variable_right_count FDB 0 /* Right wheel odometer */ variable_left_count FDB 0 /* Left wheel odometer */ /* Last directions of wheels, 0=forward, 1=backward */ variable_left_direction FDB 0 /* Left wheel */ variable_right_direction FDB 0 /* Right wheel */ /**************************************************************/ subroutine_initialize_module: #include <ldxibase.asm> * X now has base pointer to interrupt vectors ($FF00 or $BF00) * chain current IC2 vector LDD TIC2INT,X STD IC2_interrupt_exit+1 * install new IC2 handler as new vector LDD #IC2_interrupt_handler STD TIC2INT,X * chain current IC1 vector LDD TIC1INT,X STD IC1_interrupt_exit+1 * install new IC1 handler as new vector LDD #IC1_interrupt_handler STD TIC1INT,X * IC2 and IC1 effective on both rising and falling edges LDX #BASE LDA #%00111100 STAA TCTL2,X * enable IC2 and IC1 interrupts BSET TMSK1,X %00000100 /* IC1 */ BSET TMSK1,X %00000010 /* IC2 */ RTS /**************************************************************/ IC2_interrupt_handler: LDD variable_right_count ADDD #1 TST variable_right_direction+1 BEQ right_forward SUBD #2 right_forward: STD variable_right_count LDX #BASE LDAA #%00000010 STA TFLG1,X IC2_interrupt_exit: JMP $0000 /**************************************************************/ IC1_interrupt_handler: LDD variable_left_count ADDD #1 TST variable_left_direction+1 BEQ left_forward SUBD #2 left_forward: STD variable_left_count LDX #BASE LDAA #%00000100 STA TFLG1,X IC1_interrupt_exit: JMP $0000 |
| Listing 1. |
Listing 2 shows the changes required to the motor drive function so that the interrupt handlers in listing 1 know which way the motors are being driven. The bold text shows the code that has been added to the function.
void my_motor_drive_function(int motor, int power)
{
if (motor == LEFT_MOTOR_NUMBER)
{
if (power < 0)
left_direction = 1; /* reverse */
else if (power > 0)
left_direction = 0; /* forward */
/* Rest of left motor drive code here */
}
if (motor == RIGHT_MOTOR_NUMBER)
{
if (power < 0)
right_direction = 1; /* reverse */
else if (power > 0)
right_direction = 0; /* forward */
/* Rest of right motor drive code here */
}
}
|
| Listing 2. |
Listing 3 contains the code that calculates the position of the robot from the wheel encoder pulse counts. The theory behind the math in this listing may be found in the Rossum Project article by G. W. Lucas referenced at the end of this article in the Links section.
Right-click here to download odometer.c
In your code, (e.g. main( ) function), you need to call initialize_odometry( ), and then start the function odometer_thread( ) in its own thread with start_process(odometer_thread( )); The position of the robot is stored in the global variable current_position, and is continuously updated.
/***********************************************************/
/* odometer.c - Copyright (C) 2000, Dafydd Walters */
/***********************************************************/
/********************/
/* define constants */
/********************/
/* User defined constants */
#define WHEEL_DIAMETER 0.069
#define PULSES_PER_REVOLUTION 48.0
#define AXLE_LENGTH 0.125
/* Fixed constants */
#define PI 3.14159
/*********************/
/* define structures */
/*********************/
struct position
{
float x; /* meter */
float y; /* meter */
float theta; /* radian (counterclockwise from x-axis) */
};
/********************/
/* global variables */
/********************/
struct position current_position;
/********************/
/* define functions */
/********************/
void initialize_odometry()
{
current_position.x = 0.0;
current_position.y = 0.0;
current_position.theta = 0.0;
}
void odometer_thread()
{
float dist_left;
float dist_right;
int left_ticks;
int right_ticks;
float expr1;
float cos_current;
float sin_current;
float right_minus_left;
float MUL_COUNT;
MUL_COUNT = PI * WHEEL_DIAMETER / PULSES_PER_REVOLUTION;
while(1)
{
enable_interrupts(0); /* Ensure we don't lose any odometer counts */
left_ticks = left_count;
right_ticks = right_count;
left_count = 0;
right_count = 0;
enable_interrupts(1);
dist_left = (float)left_ticks * MUL_COUNT;
dist_right = (float)right_ticks * MUL_COUNT;
cos_current = cos(current_position.theta);
sin_current = sin(current_position.theta);
if (left_ticks == right_ticks)
{
/* Moving in a straight line */
current_position.x += dist_left * cos_current;
current_position.y += dist_left * sin_current;
}
else
{
/* Moving in an arc */
expr1 = AXLE_LENGTH * (dist_right + dist_left)
/ 2.0 / (dist_right - dist_left);
right_minus_left = dist_right - dist_left;
current_position.x += expr1 * (sin(right_minus_left /
AXLE_LENGTH + current_position.theta) - sin_current);
current_position.y -= expr1 * (cos(right_minus_left /
AXLE_LENGTH + current_position.theta) - cos_current);
/* Calculate new orientation */
current_position.theta += right_minus_left / AXLE_LENGTH;
/* Keep in the range -PI to +PI */
while(current_position.theta > PI)
current_position.theta -= (2.0*PI);
while(current_position.theta < -PI)
current_position.theta += (2.0*PI);
}
sleep(0.1);
}
}
|
| Listing 3. |
Notice that the function enable_interrupts(0) is called just before "sampling" the encoder counts and setting them back to zero, and enable_interrupts(1) is called just afterwards. This function, implemented as an assembly language module (listing 4), masks the interrupts when the parameter is 0, and re-enables interrupts when the parameter is 1. The purpose of having interrupts disabled briefly while sampling and resetting the wheel counts is to ensure that no counts are lost in the brief moment between sampling and resetting.
WARNING: Don't attempt to use this function in your own code unless you are certain of what you are doing. Several parts of the system are dependent on interrupts, including the IC multitasking executive, timers, output compare functions (that drive the servos), asynchronous communications with the PC, and more! Only disable interrupts for the briefest possible period. If in doubt, don't do it!
Right-click here to download intr.asm. Right-click here to download intr.icb (the file you must load into IC).
/* intr.asm - Copyright (C) 2000, Dafydd Walters */ ORG MAIN_START /*************************************************/ subroutine_enable_interrupts: tstb beq dis_ints cli rts dis_ints: sei rts |
| Listing 4. |
Because our encoder hardware does not allow the robot to know the direction of rotation of the wheels, we are making the assumption that the robot can deduce which way a wheel is rotating from the direction it is being driven.
For this assumption to be correct, we must be careful not to rapidly change the direction of rotation of the wheel at any time. This is because a turning wheel has inertia. If we drive the direction of a wheel from forward to reverse without commanding the wheel to stop in between (and waiting sufficiently long to ensure that the wheel has actually stopped) the wheel may actually turn one or two ticks of the encoder output in the forward direction under its own inertia before it actually changes direction. However, our code would believe that those one or two ticks were in the reverse direction!
The simplest solution to this problem is to command the motor to stop and then pause between each direction change. A wheel that's turning quickly has more inertia that one that's turning slowly, so you can either choose a "worst case" pause duration, or adjust the duration according to how much power was being driven to the wheel.
The approach I took was to develop a layer of motor drive software above the my_motor_drive_function( ) primitive, that ramps the motor speed gradually up and down, so that there are never any sudden changes of motor speed except when a collision is detected (and in this instance, the simple delay mechanism mentioned earlier is employed). Details of this approach are beyond the scope of this article and are left as an exercise for the reader.
I've had very good results with this system on hard floors. The robot can travel for several meters, performing several turns, and still knows its position accurately within a few centimeters. However, on carpeted floors, wheel slippage results in very rapid odometry drift, especially when the robot turns.
At the time of writing this article, I'm working on a system for correcting odometry drift by using sonar readings from a sonar sensor mounted on a pan-and-tilt head. The code is being written in C (ImageCraft's ICC11TM). I hope to document this work in a future article.
Please don't hesitate to email me and let me know if you've found any errors or omissions in this article.
Copyright © 2000, Dafydd Walters
dafydd@walters.net
Permission to copy all or part of this article, and to use or modify the code samples is freely granted, with the condition that copyright messages must be retained.
TJ-PROTM and MekatronixTM
are trademarks of Mekatronix, Inc.
ICC11TM is a trademark of ImageCraft Creations, Inc.
Mr. RobotTM is a trademark of NovaSoft, Inc and Mr. Robot
RadioShack® is a registered trademark of RadioShack, Corp.