Encoder Front Page
SRS Home | Front Page | Monthly Issue | Index
Google
Search WWW Search seattlerobotics.org

Implementing Dead Reckoning by Odometry on a Robot with R/C Servo Differential Drive


Author: Dafydd Walters < dafydd@walters.net >
Level: Novice
Revision: 1.0
Date: September 30, 2000

1. Abstract

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.

2. Hardware

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.

2.1 Principle of Operation

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.

2.2 Parts List

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

2.3 Construction

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

2.4 Initial hardware testing

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.

3. Software

3.1 Coordinate system

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):

3.2 Pulse counting

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.

3.3 Position calculation

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.

3.4 Driving the robot

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.

4. Results

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.

5. Links


Bugs in this 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.