Odometry
updated on 31-12-2007

 dsNavCon 

dsODO

The Supervisor drives both MCs through UART1 communication, sending commands and reading information (space, speed, motor current). It estimates the robot position using that information (dead reckoning by odometry) and creates a map of the ambient explored with path covered, obstacles found, and so on. This is done with the help of trigonometry capabilities of dsPIC30F/C30.

Peripherals used on the Supervisor

dsPIC30F3013:

-      UART1 to communicate with the MCs.

-      UART2 for telemetry with the remote PC.

-      I2C to communicate with the main board.

-      OC simple PWM to generate clock for both MCs.

All functions Flow Charts
All ISRs Flow Charts
Main Flow Charts
MPLAB project

dsODO

UART1 – UART2 peripherals are used to communicate with the MCs and for telemetry with a remote PC, respectively. They are used in the same way as in the MCs: similar ISRs, similar functions. The protocol used for the handshake is also the same. The protocol is physical layer independent, and it is used with the I2C bus as well to communicate with the main board.

 

The first layer is controlled by dsPIC peripheral interface. Frame or overrun errors (UART) or collisions (I2C) are detected by hardware, setting the appropriate flag. The second layer is handled by ISR routines. They fill the RX buffer with the bytes received from the interfaces. They also detect buffer overflow and command overrun. UartRx or UartRx2 functions manage the third layer. As already described (see also flow charts) these routines act as a state machine, getting bytes from the buffer and decoding the command string.

The bytes are exchanged between second and third layers (ISR and UartRx function) through a circular buffer. ISR receives a byte, stores it in an array and increments a pointer to the array, if the pointer reaches the end of the array it is restarted to the beginning. The UartRx function has its own pointer to read the same array, incremented (in a circular way too) as soon as the byte is decoded in the current RX status. Main loop calls the UartRx function whenever the "in" pointer differs from "out" pointer.

 

Each command packet is composed by:

0 - Header            @

1 - ID                    0-9      ASCII

2 - Cmd                A-Z    ASCII

3 - CmdLen          N = 1-MAX_RX_BUFF      # of bytes following (checksum included)

4 - Data              ...       

                        ...

N-1 - Data

N - Checksum 0-255   obtained by simply adding up in an 8 bits variable, all bytes composing the message (checksum itself excluded).

 

This layer controls timeout and checksum errors, as well as packet consistency (correct header, correct length). If everything is ok it enables Parser routine (fourth layer) to decode the message and to execute the action required. This routine sets the appropriate error flag if the message code received is not known.

PWM   (output Compare 1) is used to obtain the OSC frequency for the Motor Controllers. OC simple PWM I/O peripheral is set to have a PWM at 50% duty cycle, with a 7.3728 MHz frequency (the same as the Supervisor crystal quartz):

PWM_period=(PRx+1)•4•TOSC•(TMRx_prescale_value)

With Prx=3 and prescale=1 a 7.3728 MHz is obtained again at output. With this output, both MCs can be driven in EC 16xPLL mode. In this way all three DSCs have exactly the same clock and some components are saved on the board.

With data coming from the MCs, the Supervisor performs field mapping. Some theoretical background regarding dead reckoning by odometry can be found in the already mentioned book by Borenstein, Where Am I? - Sensors And Methods For Mobile Robot Positioning, and on the following web pages:

www.seattlerobotics.org/encoder/200010/dead_reckoning_article.html

rossum.sourceforge.net/papers/DiffSteer/DiffSteer.html

Also, some simplified algorithms can be found in that same documentation, so it’s possible to obtain the correct compromise between precision and speed of elaboration, using the mathematic (trigonometric) capability of dsPIC30F series.

Every Xms, after the current position elaboration, a field mapping is performed dividing the unknown field in a 10 x 10cm cells grid. Defining a maximum field dimension of 5 x 5m, we obtain a 50 x 50 = 2500 cells matrix. Each cell is defined with a nibble, with a total memory occupation of 1250 Bytes. Sixteen different values can be assigned to each cell:

n = 00 unknown cell

n = 01 – 10 cell visited n times

n = 11 obstacle found

n = 12 gas target found

n = 13 light target found

n = 14 sound target found

TMR1 generates a 1000 Hz timing clock - the heartbeat of the program. On each TMR1’s interrupt, internal timers are updated, the watchdog is cleared, and a flag is set to enable the function that asks the MCs the traveled space value. Every 10ms “All_Parameters_Ask” function (speed, position, current) is enabled. The same clock is also used, through a pulse on RB5, to synchronize MCs for PID and position elaboration.

The robot can start from any position in the field; these will be the reference coordinates (0,0) in its reference system.

 

To translate robot reference system coordinates to a 50 x 50 matrix indexes pair, their values must be "normalized" in a 0-49 range:

Xnorm = (Xrel + 50) mod 50

Ynorm = (Yrel + 50) mod 50

Index is the remainder of division, in a range 0-49.

A range check must be performed in advance to avoid overflow if the field is greater than 5 x 5m.

 

To create a 50 x 50 nibble matrix, we need to define a struct:

typedef struct

{

     unsigned char Low :4;

     unsigned char High :4;

}_Coordinate;

 

_Coordinate MapXY [25][50];

 

It fills up 1250 Bytes. Freeing heap space (not needed if dynamic memory allocation or file I/O library functions are not used) there still is enough RAM to work with.

 

X axis coordinates available are 25 High and 25 Low; matrix index can be calculated as:

Xindx = Xnorm / 2

If there is a remainder in this division, it uses High nibble; otherwise it uses Low nibble.

There are 50 Y axis coordinates available, and they can be used directly after normalization:

Yindx = Ynorm

 

Usage example:

Xnorm=(__builtin_modsd((PosX+5000),5000))/100;//from mm to index in 0-49 range

Yindx=(__builtin_modsd((PosY+5000),5000))/100;

z=div(Xnorm,2);    // X index in the range 0-24 and remainder 0-1

Xindx=z.quot;

if ((Xnorm != XnormPrev) || (Yindx != YindxPrev)) //only when cell changes

{

     if (z.rem)

    {// increment cell value only if < 10

         if (MapXY[Xindx][Yindx].High < 10) MapXY[Xindx][Yindx].High ++;

    }

    else

    {

         if (MapXY[Xindx][Yindx].Low < 10) MapXY[Xindx][Yindx].Low ++;       

    }

    XnormPrev=Xnorm;

    YindxPrev=Yindx;                                                  

}

Below some links to the high resolution version of the flow charts for dsODO program, as well as the link to the whole MPLAB® IDE project written with MPLAB® C30 compiler, both (of course) by Microchip.

MCLR 1 28 AVDD
2 27 AVSS
Generic chip select 1 RB1 3 26 RB6 Led 2
TX enable 1 RB2 4 25 EMUD2 EMUD2
Generic chip select 2 RB3 5 24 OC1 Clk out for motor controllers
TX enable 2 RB4 6 23 RB9 Led 1
1 ms heartbeat RB5 7 22 U2RX Serial 2 RX
VSS 8 21 U2TX Serial 2 TX
OSC1 9 20 VDD
OSC2 10 19 VSS
Serial 1 TX U1ATX 11 18 PGC/SDA PGC also I2C dat
Serial 1 RX U1ARX 12 17 PGD/SCL PGD also I2C clk
VDD 13 16
14 15 EMUC2

Field mapping is useful to find the best exploring strategy in an unknown field. The robot can direct itself to the less explored portion of the field (lower “n” value), can save time by not stopping twice in an already discovered target, can find the best path to reach a given coordinate, and more.