update: This document has been subsumed by two newer web articles: http://www.geology.smu.edu/dpa-www/robo/Encoder/imu_odo/ and http://geology.heroy.smu.edu/dpa-www/robo/subsumption/ dpa ---------------------------------------------------------------------------- howdy Great robo-contest Saturday! Many 'bots and a good time had by all. I have some suggestions for the next one (like any 14 year-old that can complete the course deserves an award whether he places or not!) but we can speak of these things anon... My kids were quite energized by the whole event, what more can we ask? Congratulations are due all around, to everyone who built a robot or contributed, and especially to those who organized and brought off this event! Roger Arrick and a number of other folks had quite a few questions after the contest about the dead-reckoning algorithm that I'm currently running on my robot, which I was not able to answer very satisfactorily, so I thought I'd try it again here. You guys let me know if this is altogether too much verbiage for the news group. An excellent reference is the home page of Johann Borenstein at the University of Michigan, http://www-personal.engin.umich.edu/~johannb, look for: "Where am I?" comprehensive report on mobile robot positioning systems (the new and expanded 1996 edition was just posted! 282 pages, ~200 illustrations, ~400 references). This report is available for free and can be downloaded via Anonymous FTP as an Adobe Acrobat file named "pos96rpt.pdf." Borenstein has also devised a very useful benchmark for testing mobile robot dead-reckoning accuracy called the "UMBmark" (which would make a good contest) that is referenced on the same page. I think Clay has a CD from these people that includes some movie clips of their algorithms in action. ======================================================================== Onward. The basic proposition is quite simple. For a two wheel differential drive robot (i.e. almost all of the robots at the contest last weekend), the total distance the robot has traveled is distance = (left_encoder + right_encoder) / 2.0; and the heading of the robot, it's rotation about it's center in radians, is theta = (left_encoder - right_encoder) / WHEEL_BASE; where WHEEL_BASE is the distance between the two wheels. With these two quantities, a bit of trig can give you the robot's position in X and Y, as follows: X_position = distance * sin(theta); Y_position = distance * cos(theta); You can also multiply theta * (180.0/PI) to get the heading in degrees. ======================================================================== The SR04 dead-reckoning code consists of three parts: the shaft-encoder counts on each wheel, position calculations, and target acquisition calculations. What follows is a discussion of each of these some detail. I. Shaft-Encoder Counts. SR04 has shaft encoders on the main drive motors which are used for motor speed control as well as position odometry. I'll reference here the "sr04.pdf" document that is, or soon will be, available from the DPRG Web page (is this correct, Larry? Jim? ) for a more exact discussion of the mechanical design. These two optical shaft encoders generate 12 interrupts per rotation of the motors, which works out to about 80 ticks per inch with the present wheels and gear reduction. This number was determined empirically by setting the robot down in a long hallway tiled with standard 1 foot square tiles, printing the accumulated encoder ticks (32 bit "longs") on the robot's LCD display, and halting the robot manually at the 100th tile. Probably 10 tiles would do, or just a tape measure for those without access to long institutional hallways. I'm presently using a Motorola HC6811 microprocessor and coding in ImageCraft's "icc11" C compiler and assembler. The interrupt code for the left optical encoder looks like this: ; variable storage declaration. ; 32 bit distance accumulator, defined as two 16 bit integers. ; icc11 does not support 32 bit longs, so we fake it as a C typedef, "INT32" ; here implemented in assembly. .area bss _left_distance: ; INT32 left_distance_hi: .word 0 ; hi 16 bits (HC6811 is Big Endian) left_distance_lo: .word 0 ; lo 16 bits .area text left_interrupt: ; TIC3INT vector ldd #_left_distance ; inc or dec based on sign ldx _signL ; Sign of motor direction, 1 or -1 bmi left_dec jsr _uint32_inc ; left_distance++, icc11 library call bra left_exit left_dec: jsr _uint32_dec ; left_distance--, icc11 library call left_exit: ldx #BASE ; HC6811 register base address bclr TFLG1,x,#0b11111110 ; reset TIC3 interrupt flag rti ; return from interrupt With an identical routine running on the right encoder, we have two 32 bit values in memory, left_distance and right_distance, which collect encoder ticks accumulated from the last time the robot was reset. At 80 ticks per inch, signed 16 bit values will overflow at 32767/80 = 409.5 inches, not enough for our needs, hence the 32 bit counters. ======================================================================== II. Position Calculations. The SR04 position calculations run 20 times per second in the main "sensor" loop. These calculations use the left_distance and right_distance values maintained by the shaft encoders to determine how much the robot has moved since the last calculation, and accumulate the X_position, Y_position, and theta variables. As an aside, these calculations are done in floating point, and the icc11 library is missing a long-to-float conversion utility (itsa always something!) So I wrote one, which is appended to the end of this correspondence. Further, the icc11 sin() and cos() functions expect arguments in degrees rather than radians, for reasons that are not clear to me (this is not the ANSI standard), so we'll have to fix that on the fly. Here goes: /* ----------------------------------------------------------------------- */ #include /* icc11 struct definitions for longs */ #include /* trig function header */ #include "../include/srat.h" /* my own hardware definitions */ /* ----------------------------------------------------------------------- */ /* SR04 defines */ #define LEFT_CLICKS_PER_INCH 79.52 /* turns out that the wheels are */ #define RIGHT_CLICKS_PER_INCH 79.48 /* not exactly the same size! */ #define WHEEL_BASE 9.73 /* distance between wheels, inches */ #define TWOPI 6.2831853070 /* nice to have float precision */ #define RADS 57.2958 /* radians to degrees conversion */ /* ----------------------------------------------------------------------- */ /* odometers() maintains these global accumulator variables: */ float theta; /* bot heading */ float X_pos; /* bot X position in inches */ float Y_pos; /* bot Y position in inches */ float total_inches; /* total inches traveled */ /* ----------------------------------------------------------------------- */ /* using these local variables */ float left_inches; float right_inches; float inches; INT32 L_ticks, R_ticks; /* icc11 typedefs defined in */ INT32 last_left, last_right; INT32 lsamp, rsamp; /* ----------------------------------------------------------------------- */ /* odometers() is a task run by the round-robin multi-tasking scheduler 20 times per second. It uses the left_distance and right_distance variables as inputs to maintain a set of global position and heading variables: X_pos,Y_pos, and theta. */ void odometers() { while (1) { /* sample the left and right encoder counts as close together */ /* in time as possible */ lsamp = left_distance; rsamp = right_distance; /* determine how many ticks since our last sampling? */ L_ticks = int32_sub(lsamp,last_left); R_ticks = int32_sub(rsamp,last_right); /* and update last sampling for next time */ last_left = lsamp; last_right = rsamp; /* convert the longs to floats */ long_to_float(&L_ticks,&left_inches); long_to_float(&R_ticks,&right_inches); /* and convert ticks to inches */ left_inches /= LEFT_CLICKS_PER_INCH; right_inches /= RIGHT_CLICKS_PER_INCH; /* calculate distance we have traveled since last sampling */ inches = (left_inches + right_inches) / 2.0; /* and accumulate total inches we have traveled */ total_inches += inches; /* accumulate total rotation around our center */ theta += (left_inches - right_inches) / WHEEL_BASE; /* and clip the rotation to plus or minus 360 degrees */ theta -= (float)((int)(theta/TWOPI))*TWOPI; /* now calculate and accumulate our position in inches */ Y_pos += (inches * (cos(theta*RADS))); X_pos += (inches * (sin(theta*RADS))); /* and suspend ourself in the multi-tasking queue for 50 ms */ /* giving other tasks a chance to run */ msleep(50); } } ======================================================================== III. Target Acquisition The X_pos, Y_pos, and theta variables maintained by the odometer() task give the robot a running account of it's location in Cartesian space. In order to navigate toward a pre-defined point in that same space, we need to know, 1) how far away is the target point, and 2) what is the bearing or angle from our current heading. By knowing the angle, we can generate a simple steer-left/steer-right command, and by knowing the distance we can adjust our speed as we approach the target. The determination of these values is the responsibility of the "locate_target()" task, which also runs at the 20 Hz sensor loop rate. It takes as its input an X_target and Y_target value, and returns the target_distance and heading_error. The latter is the difference between the current robot heading and the desired robot heading needed to close on the target. /* ----------------------------------------------------------------------- */ /* locate_target() uses these global variables */ float X_target; /* X lateral target position */ float Y_target; /* Y vertical target position */ float target_bearing; /* bearing in radians from current position */ float target_distance; /* distance in inches from position */ float heading_error; /* heading error in degrees */ /* ----------------------------------------------------------------------- */ /* calculate distance and bearing to target. inputs are: X_target, X_pos, and Y_target, Y_pos outputs are: target_distance, heading_error */ void locate_target() { float x,y; while(1) { x = X_target - X_pos; y = Y_target - Y_pos; target_distance = sqrt((x*x)+(y*y)); /* no divide-by-zero allowed! */ if (x > 0.00001) target_bearing = 90.0 - atan(y/x); else if (x < -0.00001) target_bearing = -90.0 - atan(y/x); heading_error = target_bearing - (theta*RADS); if (heading_error > 180.0) heading_error -= 360.0; else if (heading_error < -180.0) heading_error += 360.0; msleep(50); } } /* NOTE: The ImageCraft icc11 atan() function used above returns degrees rather than radians. This is not the ANSI standard. For ANSI standard atan(), use: if (x > 0.00001) target_bearing = 90.0 - (atan(y/x)*RADS); else if (x < -0.00001) target_bearing = -90.0-(atan(y/x)*RADS); */ /* ----------------------------------------------------------------------- */ The navigation routine which uses the "target_distance" and "heading_error" values as inputs is just another layer in the SR04's subsumption-style architecture. In it's simplest implementation, it turns right when the heading_error is positive and turns left when the heading_error is negative, stopping and declaring victory when the target_distance falls below one inch, or whatever level of accuracy you want. The present algorithm is only a bit more complicated, turning more sharply for larger heading_errors, slowing as it approaches the target, and increasing the allowed target ellipse accuracy as time and bumper presses accumulate. The priority of this layer is below the sonar path-finding, IR collision avoidance, and bumper collision recovery layers, so while it runs continuously, it only controls the robot when everybody else is happy. This is why I can "dance" with the robot, the IR and sonar trying their best to avoid my clumsy feet, and the dead-reckoning taking over from wherever and whenever they leave off. Hope this is helpful and not too verbose! cheers dpa Attachment: Long to Float conversion utility for ImageCraft's icc11 C compiler for the Motorola HC6811 microprocessor. ; ------------------------------------------------------------------------ ; long -> float conversion ; ------------------------------------------------------------------------ .area bss _long_hi: .blkb 2 _long_lo: .blkb 2 .area text ; ------------------------------------------------------------------------ ; unsigned long to float ; expects: long in long_hi, long_lo ; returns: float conversion in fp_exp, fp_mantissa, fp_sign pos_long_to_fp: ; entry for positive long ldx #0x009d ; positive sign and init exponent bra ulong_to_fp neg_long_to_fp: ; entry point for negative long ldx #0xff9d ; negative sign and init exponent ulong_to_fp: ldd _long_lo ; return zero if lo and hi = 0 bne ultofp_1 ldd _long_hi bne ultofp_1 jmp __fp_return_zero ultofp_1: dex ; decrement exponent ldd _long_lo lsld ; mantissa * 2, lo word std _long_lo ldd _long_hi bcc ultofp_2 addd #1 ultofp_2: lsld ; mantissa * 2 + carry, hi word std _long_hi bpl ultofp_1 ; keep going until normalized ultofp_3: std __fpacc1_mantissa ; hi word is hi 16 bits of mantissa ldd _long_lo staa __fpacc1_mantissa+2 ; hi byte of lo word is lo 8 bits of mantissa xgdx ; get sign and exponent bytes staa __fpacc1_mantissa_sign ; save sign byte stab __fpacc1_exp ; save exponent byte clrb ; no errors rts ; ------------------------------------------------------------------------ ; signed long to float ; expects: pointer to long in D ; returns: float in fp_exp, fp_mantissa, fp_sign _signed_long_to_fp: xgdx ldd 2,x std _long_lo ldd 0,x std _long_hi tsta bpl pos_long_to_fp com _long_hi com _long_hi+1 ldd _long_lo comb coma addd #1 std _long_lo ldd _long_hi bcc ltofp_1 addd #1 ltofp_1: std _long_hi bra neg_long_to_fp ; ------------------------------------------------------------------------ ; long_to_fp convert positive or negative long to fp ; ; expects: pointer to long in D ; pointer to float on stack ; ; returns: float .globl _long_to_float _long_to_float: jsr _signed_long_to_fp tsx ldx 2,x jmp __from_fpacc1 ; ------------------------------------------------------------------------ ; eof math.s