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

DocBot, part 2

Gary Livick, glivick@pacbell.net

 

image_1.jpg (35943 bytes)

 

General Review:

In part one, we provided detail about the physical makeup of DocBot and some of the electronics. We also discussed DocBot's main and only task, which is to go from point A to point B and back by planning his own route around known or discovered obstacles.

In this part, we'll review the task, and then discuss the hardware. Next, we'll discuss key parts of the software in detail. Finally, we'll see how it all worked out, and talk about what could have been done differently.

Review of the task:

As stated in part 1, the only thing DocBot is designed and equipped to do is navigate through the office where he lives, through an adjacent office and into a third office to deliver notes to the occupant. Having done that, DocBot is to turn around and come back. The offices are cluttered, there are no straight runs longer than three or four feet, and the office doorways are aligned at right angles to each other. Initially, DocBot will be provided with the information on where he is, and his angular orientation in the room. He is also informed of the location of any fixed objects that are known about along any potential route of travel, and the coordinates of his destination. While enroute, if DocBot detects uncharted obstacles, he is to remember them so as to avoid them in the future, and then replan the route around the obstacle and on to the destination. From time to time, DocBot will approach a known landmark and use it to update his position and orientation to correct for navigation errors.

Hardware:

There have been two changes to the hardware which were needed to make completion of the task possible. One of the changes was major, the other minor.

There are two recognized sources of error in dead reckoning navigation; systematic and non-systematic. Systematic errors are due to mechanical problems with the robot itself, such as wheels not the same size, wheel slip, missing encoder counts, wheel spacing not constant due to motor shaft axial play, etc. Nonsystematic errors are due to the environment in which the robot operates. Examples of this type error include bumps in the floor, uneven or slippery surfaces, and dogs.

It is known that a robot can deal with the first type of error by minimizing mechanical effects through careful consideration during the design phase, and by calibrating the finished robot after extensive performance testing. However, non-systematic errors are more difficult to accommodate, and ultimately spell doom to a robot relying solely on wheel encoder counts for navigation. On DocBot, some of each type of error were encountered, and adaptations had to be made to overcome them.

The minor change from the base-line configuration was modification of the wheel encoder system. This change was to reduce a systematic error due to wheel count resolution. Each wheel now has a pair of encoders, instead of just one, to count clicks from the targets on the inside surfaces of the wheels. The targets were also redone to maximize the number of stripes per revolution. These two changes in the encoder system magnified the resolution from 128 to 340 counts per revolution. Initially, the second set of encoders was added with the intent of using phase quadrature (see http://asrignc3.snu.ac.kr/encoder.htm for details), a technique that would have provided 680 counts per revolution. However, the encoders on DoBot provide a narrow analog spike when detecting a stripe, and it was not possible to overlap the signals as needed in phase quadrature. Schmidt triggers, which are used to convert analog signals to digital, were added to make the analog signals more tidy. However the signals were still too narrow to overlap for phase quadrature. With some effort, the encoders and targets could have been modified to get the broad square waves needed, however the 340 counts seem to work so the effort was not undertaken.

The second change, and the more serious of the two, was the installation of a Vector 2X compass (see http://www.precisionnav.com/vector2xmain2.html ). For a long time, DocBot had a cumulative heading error that would creep in with longer straight runs. After careful testing, it was determined that the heading error was not due to missing encoder counts, which would have been a systematic error, or to a difference in wheel diameter (they were cut from the same piece of knurled bar stock) which also would have been a systematic error. After a couple of months of head scratching, the cause of heading deviation was finally identified to be a non-systematic error, explained in the next paragraph, and hence beyond practical solution by the robot itself.

The floor in the office environment in which DocBot spends most of his waking hours is a concrete slab with indoor-outdoor carpet layed over the top. It floor is smooth, and without perceptible irregularities. However, an interesting thing happens when DocBot transits the room. Running parallel to a North/South wall in either direction, DocBot tends to build up a heading error toward the wall while counting clicks to maintain a straight line. Going either North or South, the robot still turns West toward the wall. That would seem not possible, given the robot's configuration. However, when heading perpendicular to the same wall, either towards it or away from it, DocBot tracks straight! It was enough to end a robot hobby, until the light finally came on. Any skier knows what was happening: The floor surface had to be sloped toward the wall, causing wheel slip downhill. There was no horizontal gravity-induced turning force component tending to make the robot turn when heading up or downhill (perpendicular to the wall), so the robot tracked straight. Measuring with a small level could not pick up the floor tilt, but the fact that it was there was inescapable. This is a non-systematic error, and hence has no explicit programming-based solution. Therefore, an electronic compass was needed to make periodic heading corrections to overcome the unavoidable error that creeps in due to floor tilt.

The Vector 2X compass added to DocBot is electronic. However, it works in exactly the same manner as a conventional compass by giving the user heading information based on the Earth's local magnetic field. Most people know that there is a difference between true North and magnetic North. The true North pole is located at the exit of the axis of the Earth's rotation in the Northern hemisphere. The magnetic North pole, on the other hand, is the location where the Earth's magnetic field exits in the Northern hemisphere, which is not the same location as the rotation axis at all. The effect of this for compass users is the need for correction of the magnetic heading provided by a compass to determine true North. The correction factor is called "magnetic deviation." To further complicate matters, the magnetic deviation is not constant, but varies depending on the physical location of the user on the face of the earth. Magnetic deviations are relatively constant, changing only slightly over time, and are published in charts for hikers, pilots, and others who depend on navigation by compass.

One of the cautions to be observed when using a compass to obtain heading information is to stay well away from large structures, power lines, cars, and other constructions that can disturb the local magnetic field in the area. This includes belt buckles as well, to which a few novice hikers can no doubt attest. Magnetic deviation corrections are published assuming that the compass used for taking measurements is in an area undisturbed by magnetic anomalies. Hence, a clever roboticist would have reservations about using such a device inside a building full of wiring, computers, filing cabinets and so forth. His concern about the accuracy of a compass in such a setting would be well founded.

Inside the office world of DocBot, disturbance of the magnetic field is seen to vary as much as 30 degrees or so from place to place. On the face of it, it would seem that a compass in such an environment would be useless. However, with a little thought it can be understood that the magnetic heading observed in one location in the office is different from that observed in a different location in the same way that differences exist in that measurement taken in much more widely spaced areas on the face of the Earth. The differences are accounted for out in the world at large by using a chart of magnetic deviations to make the correction. DocBot's magnetic world is made up of deviations that can also be charted.

DocBot goes from the center of one square in his navigation grid to the center of the next. If, at the center of each square that makes up his world, a measurement is taken that can be compared to the known heading at that location, then a chart can be made up for DocBot which is exactly a local table of magnetic deviations for his limited navigational space. The actual determination of all the chart entries is tedious, but only need be done once. It is even possible to have a robot make up it's own table, traveling in a straight line and recording readings as it goes.

Passing information about measured local deviation to DocBot has allowed the effective and very accurate use of the Vector 2X compass.

The addition of the compass required the expansion of DocBot's HC11 brain with an I/O upgrade. Added were 32 input/output ports, software configurable in groups of eight to either input or output. The board was obtained from Ray's Robotic Racers at http://www.teleport.com/~raybutts/. Note that the company is owned by the author. We fly what we build.

Software:

Rather than discuss the entire program en masse, which in a word is ponderous, the major tools will be reviewed in detail and the rest can be studied at the leisure of the reader.

Probably all robot programs contain collections of subroutines that are used as a tool set. Sometimes these take the form of library routines, such as are found in C and C++ compiler packages. In IC, the language used on DocBot, a lot of subroutine tools are contained in the IC pcode, and some in library files that go with the IC target processor, such as the Rug Warrior board or the Handy Board.

The Super-TCOMP is very similar to the Rug Warrior board, including having the motor drivers addressed identically. The Super-T will run IC just as it is, using the Rug Warrior pcode and library. This gains access to just about every math, timing or port access call one would need. Only one board modification is required to get the Super-T to run in 'special test' mode so it will drive an LCD under IC, consisting of cutting a trace. For all intents and purposes, the Super-T is a bare Rug Warrior board, and is programmed in IC as if it were. IC version 3.2 is used.

The library file for the Rug Warrior was modified to take out unnecessary functions, and add some others specific to the Super-T. The file was renamed as lib_nav.c and is listed below.

 

/* lib_nav.c created from lib_rw11.c for DocNav project 11/25/99 by GL
/*
    VERSION HISTORY FOR lib_rw11.c
    V2.81Jun 11 1994jlj
    Created library for Rug Warrior from an earlier 
    library by jsr and fgm
*/
int abs(int arg)
 {if (arg<0)
   return -arg;
  else
   return arg;}
/*******************/
/* TIME PRIMITIVES */
/*******************/
/***********************************/
/* location of various time stuff: */
/* 0x14: time in milliseconds      */
/***********************************/
void reset_system_time()
{
  pokeword(0x14, 0);
  pokeword(0x12, 0);
}
    
/*  returns valid time from 0 to 32,767 minutes (approx)  */
float seconds()
{
  return ((float) mseconds()) / 1000.;
}
void sleep(float seconds)
{ 
    msleep((long)(seconds * 1000.));
}
void msleep(long msec)
{
    long end_time= mseconds() + msec;
    while (1) {
/* if the following test doesn't execute at least once a second,
   msleep may not halt */
long done= mseconds()-end_time;
if (done >= 0L && done <= 1000L) break;
    }
}
long timer_create_seconds(float timeout)
{
    return mseconds() + (long) (timeout * 1000.);
}
int timer_done(long timer)
{
    return timer < mseconds();
}
int analog(int port)
{   poke(0x1039, 0b10000000);
    poke(0x1030, port);
    return peek(0x1031);
}
/*****************************/
/*** Multi-Tasking Support ***/
/*****************************/
/*  gives process that calls it 256 ticks (over 1/4 sec)
    more to run before being swapped out
    call repeatedly to hog processor indefinitely  */
void hog_processor()
{
    poke(0x0a, 0);
}
int input_f(int port)/* Return 1 bit from portf PF1 or PF2 (or PF0) */
{ return 1 & (peek(0x7400) >> (port & 3));
}
int digital(int port) /* Return 1 bit from PA1 or PA2 (or PA0) */
{ return 1 & (peek(0x1000) >> (port & 3));
}
int round(float arg) /* thanks to George Musser for this one */
  {int integer;
   integer = (int)arg;
   arg -= (float)integer;
   if(arg >= 0.5)
      return integer+1;
   else if(arg <= -0.5)
      return integer-1;
   return integer;}  
/***********************************************************/
/*
/* Motor Control Primitives       
/*
/*    init_motors()- Must be called to enable motors
/*    motor(index, speed)- Control velocity of motor 0 (left) or 1 (right)
/*    drive(trans_vel, rot_vel)- Control robot translation and rotation
/*    stop()- Stop both motors
/*    note motor control revised to match Rug Warrior Pro version (int velocity) 
/* Setup two PWM channels for motor control */
/*                 Left        Right            */
int TOCx[2]     = {0x1018,     0x101A};/* Index for timer register */
int dir_mask[2] = {0b00010000, 0b00100000};/* Port D direction bits */
int pwm_mask[2] = {0b01000000, 0b00100000};/* Port A PWM bits */
int init_motors()
{ bit_set(0x1009,0b110000);/* Set PD4,5 as outputs for motor direction */
  poke(0x100C,0b01100000);/* OC1M Output compare 1 affects PA5,6 */
  bit_set(  0x1020,0b10100000);/* TCTL1 OC3 turns off PA5, OC2 PA6 */
  bit_clear(0x1020,0b01010000);/* Use set and clear to avoid other bits */
  pokeword(0x1016,0);/* When TCNT = 0, OC1 fires */
}
/* Make sure init_motors is called after a reset */
int init_motors_dummy = init_motors();
void stop()/* Stop both drive motors */
{ bit_clear(0x100D,pwm_mask[0]);/* Let OC1 turn off motors rather */
  bit_clear(0x100D,pwm_mask[1]);/* than turn them on */
}
/* Vel is in the range [-100, +100], index = 0 => Left, = 1 => Right */
void motor(int index, int vel)
{ int avel = vel;
  if (vel == 0) /* Check first for zero vel */
    { bit_clear(0x100D,pwm_mask[index]); /*  OC1 turns off the motor */
      return; } /* And, you're done */
  bit_set(0x100D,pwm_mask[index]); /* Otherwise let OC1 control the motors */
  if (vel > 0)
    { bit_set(0x1008, dir_mask[index]);} /* Forward rotation */
  else
    { avel = (- vel);
      bit_clear(0x1008, dir_mask[index]);} /* Backward rotation */
  if (avel > 99)
    pokeword(TOCx[index], 0);  /* 0 means OC1 wins, motor on */
  else
    pokeword(TOCx[index], (655 * avel));  /* Compute TOCx value */
}
/* Use drive to control motion of the robot.  A positive rot_vel makes the robot
   turn left. */
void drive(int trans_vel, int rot_vel)
{ motor(0,trans_vel - rot_vel);
  motor(1,trans_vel + rot_vel);
}
 /* WARNING:  printf's will wedge once the print buffer becomes full
    if system printing is disabled  */
void system_print_on() {bit_set(0x39, 0b00000001);}
void system_print_off() {bit_clear(0x39, 0b00000001);}
 
/********************** END *******************/

As can be seen from the above, a number of functions necessary for the operation of DocBot are missing. There are no wheel encoder functions for example. Encoder drivers are present in the original Rug Warrior library, but they wouldn't work with the type of encoder sensors originally used on DocBot. Also missing are servo drivers, which are not supplied with the Rug Warrior library.

Fortunately, encoder drivers are available from the Handy Board web site, one for each of the eight analog ports on the HC11. See http://lcs.www.media.mit.edu/groups/el/projects/handy-board/software/libs.html for the details. These work well, but do have a drawback. First, as they are witten, a driver must be loaded for each encoder, and DocBot uses four of them. They run in the 'background' on the IC system interrupt, and therefore tie up the processor for some period of time each time they run. The drivers also have a velocity capture feature that is not used on DocBot, wasting the time taken to generate the velocity data. Finally, the encoder drivers cannot be shut off, as there is no feature to do it in the drivers. The encoders aren't needed when the robot is stationary, but the drivers run anyway, again wasting valuable processor time. The effect of all this time wasting on DocBot is to double the amount of time needed for him to plan his route. Before modification, the encoder drivers running in the background caused the route-planning optimization routine to take 7 minutes to complete! After modification of the encoder drivers, incorporating them into one program and adding a means to shut the drivers off, the planning time was reduced to 3.5 minutes. The modified driver is listed below. See the Internet pointer above for an explanation on how the original works, which will allow understanding of DocBot's modified encoder code. It is written in a relocatable form of assembly language that is specific to and used with IC.

fencdr.asm

 

 compile using as11-ic
* icb file: "fencdr.asm"
* placed in system interrupt
* samples at 1000 Hz
* Gary Livick 14 May 2000
*   patterned after work by Fred Martin 22 Apr 1996
* 6811 registers
BASE EQU $1000
ADCTL EQU $1030 		; A/D Control/status Register
ADR1 EQU $1031 			; A/D Result Register 1
ADR2 EQU $1032			; A/D Result Register 2
ADR3 EQU $1033 			; A/D Result Register 3
ADR4 EQU $1034 			; A/D Result Register 4
TOC4INT EQU $E2 		; Timer Output Compare 4
ORG MAIN_START
* low and high thresholds for counting pulses
variable_encoder_low_threshold: FDB 80
variable_encoder_high_threshold: FDB 125
* tick counts
variable_encoder0_counts: FDB 0
variable_encoder1_counts: FDB 0
variable_encoder2_counts: FDB 0
variable_encoder3_counts: FDB 0
* defer flag from main program to bypass this module when not needed
variable_enable_encoders: FDB 0 
* internal variables
encoder0_state: FCB 0
encoder1_state: FCB 0
encoder2_state: FCB 0
encoder3_state: FCB 0
* install module into 1 kHz IC system interrupt on TOC4
subroutine_initialize_module:
LDX #$BF00 		; pointer to interrupt base
* get current interrupt vector and save as next vector
 
LDD TOC4INT,X 		; SystemInt on TOC4
STD interrupt_code_exit+1
* install ourself as new vector
LDD #interrupt_code_start
STD TOC4INT,X
* reset encoder variables
LDD #0
STAA encoder0_state
STAA encoder1_state
STAA encoder2_state
STAA encoder3_state
         STD variable_enable_encoders
STD variable_encoder0_counts
STD variable_encoder1_counts
STD variable_encoder2_counts
STD variable_encoder3_counts
RTS
* encoder interrupt code:
interrupt_code_start:
        LDD variable_enable_encoders
        TSTB
        BNE continue
        JMP interrupt_code_exit  ;skip everything if encoders not enabled
continue:
LDX #BASE
* get analog reading
LDAA #$10 		; set ADCTL to read first four ports
STAA ADCTL,X
BRCLR ADCTL,X $80 *  	; wait here until conversion flag is set
* process register ADR1 for state of encoder 0
LDAA ADR1,X
         TST encoder0_state 			; see if last state was 0
BNE test_falling0 		; if not, branch to test_falling
CMPA variable_encoder_high_threshold+1
BLO encoder_1 	; if the reading  is less, branch to next encoder
* otherwise, there was a state change
got_click0:
LDY variable_encoder0_counts
INY
STY variable_encoder0_counts
LDAA encoder0_state
EORA #$FF
STAA encoder0_state
BRA encoder_1 			; now go do the next one
test_falling0:
CMPA variable_encoder_low_threshold+1
BLO got_click0
* process register ADR2 for state of encoder 1
encoder_1:
LDAA ADR2,X
      	TST encoder1_state			; see if last state was 1
BNE test_falling1		; if not, branch to test_falling
CMPA variable_encoder_high_threshold+1
BLO encoder_2	; if the reading is less, branch to next encoder
* otherwise, there was a state change
got_click1:
LDY variable_encoder1_counts
INY
STY variable_encoder1_counts
LDAA encoder1_state
EORA #$FF
STAA encoder1_state
BRA encoder_2				; now go do the next one
test_falling1:
CMPA variable_encoder_low_threshold+1
BLO got_click1 
process register ADR3 for state of encoder 2
encoder_2:
LDAA ADR3,X
     	TST encoder2_state			; see if last state was 0
BNE test_falling2		; if not, branch to test_falling
CMPA variable_encoder_high_threshold+1
BLO encoder_3	; if the reading is less, branch to next encoder
* otherwise, there was a state change
got_click2:
LDY variable_encoder2_counts
INY
STY variable_encoder2_counts
LDAA encoder2_state
EORA #$FF
STAA encoder2_state
BRA encoder_3			; now go do the next one
test_falling2:
CMPA variable_encoder_low_threshold+1
BLO got_click2
* process register ADR4 for state of encoder 3
encoder_3:
LDAA ADR4,X
     	TST encoder3_state; see if last state was 0
BNE test_falling3		; if not, branch to test_falling
CMPA variable_encoder_high_threshold+1
BLO interrupt_code_exit	; if the reading is less, branch exit
* otherwise, there was a state change
got_click3:
LDY variable_encoder3_counts
INY
STY variable_encoder3_counts
LDAA encoder3_state
EORA #$FF
STAA encoder3_state
BRA interrupt_code_exit		; done, go to the exit
test_falling3:
CMPA variable_encoder_low_threshold+1
BLO got_click3
interrupt_code_exit:
JMP$0000 			; this value poked in by init routine

 

With the above library routine, the encoders can be enabled by setting the IC global variable 'enable_encoders' (created by fencdr.asm) equal to 1, and disabled by setting it equal to 0. This is done with a simple assignment statement (enable_encoders=1; in IC). The count of each encoder can be read by simply calling it's name, for instance a = encoder2_counts;.

Next it was necessary to locate servo drivers. Two are needed on DocBot, one for the tilt servo and one for the pan servo. Those don't exist in the Rug Warrior libraries, but again the Handy Board library has some. Once again, though, there was a problem that needed resolution. The Handy Board implements its motor driver function in a manner not at all like the Rug Warrior. For that reason, Fred Martin, the father of the Handy Board, had no reason to avoid the motor driver ports used by the Rug Warrior when he wrote his servo drivers. Specifically, he uses port A, pins 5 and 7 for his servo drivers and both are a problem for DocBot. Pin 7 can be worked around, but pin 5 is a disaster. So, it was necessary to translate the driver using pin 5 to use pin 3 instead. See http://lcs.www.media.mit.edu/groups/el/projects/handy-board/software/servo.html for Fred's standard drivers and information on how they are used, and http://lcs.www.media.mit.edu/groups/el/projects/handy-board/software/contrib/glivick/servo_a3.txt for the translated driver that uses PA3 instead of PA5.

IC comes with most of the floating point math functions needed for DocBot's wanderings, everything in fact except for the inverse trig function 'arcsine.' A lot of time was spent on this one, focusing on a couple of forms of series expansions. The Taylor series worked just fine, up to the point where the argument was close to the area where the function would return 90 degrees. At that point, the errors get significant. The McLauren series was worse. The number of terms to carry for accuracy at the extremes was just too great. The problem was presented to both the Handy Board and the Seattle Robotics Society list servers, and someone on the HB list server popped up with a suggestion for a series that I had not heard of. I tried the suggested mystery series, which is still a mystery because I lost the email that delivered it, and it works great! Here it is, written as a function in IC.

arcsin.c

 

/******************************
*           file: arcsine.c 
* returns angles in degrees   *
*                             *
*  note: 0<x<1.               *
*                             *
******************************/           
float Pie;
float arcsin(float x) 
{
 float a0 = 1.5707963050;
 float a1 = -.2145988016;
 float a2 = .0889789874;
 float a3 = -.0501743046;
 float a4 = .0308918810;
 float a5 = -.0170881256;
 float a6 = .0066700901;
 float a7 = -.0012624911;
 float solution;
 Pie=3.14159265359;
 solution = (Pie/2. - (sqrt(1.0 - x))*(a0 + a1*x + a2*(x^2.) + a3*(x^3.) + a4*(x^4.) + a5*(x^5.) + a6*(x^6.) + a7*(x^7.)));
  return solution* 57.2957795;}
/*******************END*******************/

An important function for any robot using the Sharp GP2D02 or GP2D12 IR ranging sensor is a means to convert the inverse, non-linear return of the sensor into a useable form. Unfortunately, the output characteristics of the these sensors does not fit any classic mathematical curve. Therefore, a polynomial expression was developed to describe the curve.

The technique used to derive a describing polynomial equation, given a locus of points that finititely describe a curve, is to write an equation of the form y = a0 + a1*x + a2*x^2 + a3*x^3 + ... a(n)*x^(n), with n being the total number of points measured, for the x and y coordinate at each point. The more points measured, the more accurate the final polynomial equation. Then the coefficients are determined in a massive simultaneous equation of n equations in n unknowns. The set-up is simple enough, but solving simultaneaous equations by hand is tedious at best, and very likely to get screwed up.

Searching around on the Internet for a site that might help with large simultaneous equations produced this site: http://www.intergalact.com/java/polyfit/polyfit.html. The site has a down-loadable application that determines the coefficients for you in a series of expressions of the above form. However, the application does not like large values of x, so the powers of ten were fiddled with in the measured values, allowing the determination of reasonable coefficients. The notation in the program that follows stating: /*get to fit in polynomial*/ is an artifact of that fiddling.

Another feature of the GP2D02 or GP2D12 sensors (DocBot uses the 12 version) that is dealt with in the range.c program below is the potential inaccuracy of the sensor. An average of 10 ranges was found to produce a repeatable and accurate final range. However, some of the ranges gathered might be wildly erroneous, skewing the final average. To eliminate the excess variation in the average by taking just any reported value to use in the average, an average of 10 ranges called 'test_range' was first determined. Test_range was an average of ten raw ranges that were not above or below the boundary numbers of 101 and 18, (note that this is raw sensor data, taken right from the sensor). Then test_range was used as a qualification range for the next ten readings, which were not allowed to vary more than a few percent above or below the test_range value. These final 'qualified' ranges were then included to calculate the final average. This turned out to be highly accurate. Here is the technique:

range.c

//********************************
      file: range.c
This code was modified to average 10 readings for both test and final results.  Identical runs using a total of 40 readings show no improvement in accuracy.
**************************************/
   
float range_avg()
  {int temp_range;
   int wedge_check;
   int q;
   int total_range;
   float sensor;
   float result;
   int test_range; /*use to throw out bad values*/
   int offset;
  
   { test_range=0;
      wedge_check=0;
      total_range=0;
      for (q=1;q<11;q++)
      {sleep(.03);
      temp_range=analog(4);
      if(temp_range > 101 || temp_range < 18)
        {q--;
         wedge_check++;
         if (wedge_check>=10)
         {/*printf("range violation...\n");*/
         return 0.;
         break;}}
      else 
        test_range += temp_range;}
      test_range=test_range/10; /*use this number to check and discard ranges */
     
      wedge_check=0;
      offset=1;
      if (test_range>53)
        offset=3;
      if (test_range>73)
        offset=4;
    
      for (q=1;q<11;q++)
      {sleep(.03);
      temp_range=analog(4);
      if(temp_range > test_range + offset|| temp_range < test_range - offset)
        {q--;
        wedge_check++;
         if (wedge_check>=10)
          {total_range=0;
           return 0.;
           break;}}
         else 
          total_range += temp_range;}
  
          sensor = (float)(total_range)/10.;
          sensor = sensor/10.; /*get to fit in polynomial*/
         if (sensor>1.9)
{result=251.074-305.18338*sensor+175.0571222*(sensor^2.)-55.761907597*(sensor^3.0)+10.41118763938*(sensor^4.0)-1.1318716320922*(sensor^5.0)+.066254012440301*(sensor^6.0)-.00161179616687254*(sensor^7.0);
     
      return result;}
}
}
/***********************END**************************/

Why all this need for range accuracy? If all that is needed is just the ability to detect and avoid objects, then the standard accuracy of the GP2D12 would be fine. However, there is a need to update both position and orientation with respect to fixed flat surfaces (landmarks). The method chosen to do this is by taking several precise measurements with the robot parked next to a landmark, and then using right and oblique angle trigonometry to calculate distance from the landmark, and the angle to it.

Those desiring to review (or learn something about) trigonometry are referred to an excellent Internet site: http://aleph0.clarku.edu/~djoyce/java/trig/functions.html for right angle trig and http://aleph0.clarku.edu/~djoyce/java/trig/oblique.html for oblique angle trig. The explanation that follows assumes the reader has the basic math skills.

fig1.gif (21245 bytes)

Refer to figure 1 above. DocBot approaches a landmark and stops in the position shown at some angle to the landmark (theta deviation) and some distance perpendicular from the landmark (wall offset). DocBot is able to determine side b by ranging at an angle of 50 degrees off his longitudinal axis, and side c by ranging directly abeam. Angle C is known, as it is just the difference between the two ranging angles, 90 degrees - 50 degrees = 40 degrees. The two unknowns, theta deviation and the wall offset, are to be determined. Later in another subroutine, DocBot will use the two values to align himself parallel to the landmark, and to move to the correct distance from it.

Having found the lengths of sides b and c by ranging, and knowing angle A, side a can be found by use of the law of cosines. That gives us: a=sqrt((b^2 + c^2 2*b*c*cosA))). Now equipped with the length of side a, we can find angle C using the law of sines: C=arcsin(c*sin(A)/a)). From that, angle B is found with the knowledge that the sum of the angles in any triangle are 180 degrees.

Having all this data allows the simple determination of the wall offset, which is found with right angle trig: wall_offset=c*sin(B).

In figure 1, DocBot is shown pointed slightly toward the landmark, and angle C is seen to be less than 90 degrees. If DocBot was more steeply inclined toward the landmark, then the basic shape of the geometry could change to the point that angle C became greater than 90 degrees, as shown in figure 2. The math to determine C is the same as above, however a problem becomes apparent here. The sine of an angle is also the sine of its complement -- the sine of 75 degrees is the same as the sine of 105 degrees for instance. When you use the law of sines to find angle C, you get two answers, one for an angle less than 90 degrees and one greater than 90 degrees. The correct one is generally determined by visual inspection (by humans, anyway) as the solution is mathematically indeterminate.

fig2.gif (16378 bytes)

DocBot can't 'visually' inspect anything the way humans do it, so to decide which of the two solutions for C is the correct one, DocBot takes a third range reading as shown in Figure 3. From this it is seen that if the third reading (side d) is greater than the range of side b, then angle C is greater than 90 degrees, and if the third reading is less than side b, the angle is less than 90 degrees.

fig3.gif (15673 bytes)

 

The program below that does all this tap dances around between radians and degrees. I prefer degrees, but as a robot with an HC11 brain, DocBot prefers radians. It has been necessary to strike a compromise with him. The accuracy of the angle determined by the combination of this routine, and the tools described above that form a part of it, seems to be on average +/- one degree, or two at the most. The accuracy is highly dependent on the landmark used as a reference being 1' x 12' sanded lumber painted gloss white, another small thing I worked out with DocBot. Here is the program:

newranging.c

 

/********************************************
*           file: NEWranging.c                                     *
********************************************/
float deg_to_rad;
int angle_error;
float distance_error;
int side; /* 1 for left, -1 for right */
void error_check(int which_way)
  {side=which_way; /* 1 for left, -1 for right */
   angle_error=theta_deviation();
   distance_error=wall_distance();}
   
int theta_deviation()  /* Note: + is away from the wall */
   {int deviation;
     b=c=d=0.;
     deviation = wall_angle_B() - 90;
     return deviation;}
int B;  /* Wall angle B */
int wall_angle_B()
 {return B=140 - wall_angle_C();} 
float wall_distance()
  {float B_rad = (float)(B)/deg_to_rad; /* convert angle B to radians*/
   float wall_offset;  /* distance to measured wall */
   wall_offset = c * sin(B_rad);
   return wall_offset;}
  int C;
  float a;
  float b ;
  float c;
  float d; /* used to determine if C > 90_ */
  float  A; /* A = included angle between readings */
int wall_angle_C()
 {int q;
  int temp_range;
  pan_deg(50*side); /*look to the side 50_ */
   while (!(int)(b))
     {b=range_avg();}
 
  pan_deg(60*side); /*look to the side 60_, used to find C > 90_ */
     while (!(int)(d))
     {d=range_avg();}
  pan_deg(90*side);
      while (!(int)(c))
     {c=range_avg();}  
  pan_deg(0);
 
  a=sqrt((b^2.) + (c^2.) - (2.*b*c*cos(A)));
  C=(int)(arcsin(c*sin(A)/a));
  if (d > b)  /* then C is more than 90_ */
    {C=180-C;}
  return C;}
void pan_deg(int deg)
  {float pan_temp;
   pan_temp=3035. + ((float)(deg)*18.30); 
   pan=(int)(pan_temp);
   servo3_on();
   pan=(int)(pan_temp);
   sleep(.8);
   pan=(int)(pan_temp);
   servo3_off();
   }
/********************END**********************/

A main feature of DocBot's overall operation is his ability to align himself at a given distance and angle to a landmark he is parked next to. The function call for the subroutine to do it is "correct_heading(int dir, int angle, int distance);." The side that the landmark is on (dir) is passed in the call, as is the angle for the robot to establish relative to the landmark and the distance away from it. After the function is called, DocBot first aligns himself to the landmark at the desired angle to an accuracy of +/- 3 degrees. Then he will turn directly toward or away from the landmark as required to establish the right spacing from it, and move whatever distance is required to correct his spacing. He then turns back and finishes alignment to within +/- 1_. Of all the things DocBot does, this is the only thing that gets the attention of non-roboticists. It is fun to watch, as he gives the appearance of concentrating on what he is doing. Here is the routine for it.

turn_correct.c

 

/******************************************
*             file: turn_correct.c                             *
******************************************/
void correct_heading(int dir, int angle, int distance)
 {while(1)
   {error_check(dir);
    if(abs(angle_error-angle)>3)
      {turn(-dir*((angle_error-angle)/2));}
    else break;}
  if((int)(distance_error)<distance)
    {turn(dir*90);
     go(distance-(int)(distance_error));
     turn(dir*-90);}
  if((int)(distance_error)>distance)
    {turn(dir*-90);
     go((int)(distance_error)-distance);
     turn(dir*90);}
   while(1)
   {error_check(dir);
    if(abs(angle_error-angle)>1)
      {turn(-dir*((angle_error-angle)/2));}
    else break;}
}
/******************END******************/

The Vector 2X compass is a cool device, but it makes you work to get information out of it. IC, a fairly slow interpreted operating system, is used in the following compass driver routine. In fact, IC is not fast enough to get the last degree of accuracy from the compass. However, giving the variances of compass navigation inside a building in any event, using assembly language to caputure the last bit of accuracy from the compass did not seem to warrant the extra work. Although the code that follows appears to retrieve all the data accurately, the last bit is actually random, giving up one degree to entropy.

compass.c

 

/*******  compass driver  ***********/
#define porth 0x7C00
#define set_h bit_set(0x7e00,0b00000100)
#define pc_low bit_clear(porth,0b00100000)
#define pc_high bit_set(porth,0b00100000)
#define sclk_low bit_clear(porth,0b01000000)
#define sclk_high bit_set(porth,0b01000000)
#define ss_low bit_clear(porth,0b00001000)
#define ss_high bit_set(porth,0b00001000)
#define reset_low bit_clear(porth,0b00010000)
#define reset_high bit_set(porth,0b00010000)
#define read_sdo input_j(1)
#define check_eoc input_j(0)
void reset_compass()
 {set_h;
  poke(porth,0);
  ss_high;
  pc_high;
  sclk_high;
  reset_high;
  reset_low;
  msleep(15L);
  reset_high;
  msleep(5L);}
int read()
  {int i;
   int j;
   int heading;
   
   heading=0;
   sclk_high;
   ss_high;
   msleep(15L);
   pc_low;
   msleep(15L);
   pc_high; 
   
   while(!check_eoc)
    {;}
   
   msleep(15L);
   ss_low;
   msleep(15L);
   for(i=1;i<8;i++)
    {sclk_low;
     msleep(5L);
     sclk_high;
     msleep(5L);}
   
   sclk_low;
   msleep(5L);
   sclk_high;
   msleep(5L);
   if(read_sdo==1)
     {heading=256;}
   
   msleep(15L);
   for(j=7;j>0;j--)
    {sclk_low;
     msleep(10L);
     sclk_high;
     msleep(10L);
     if(read_sdo != 0)
      {heading +=(int)((2.^(float)(j))+1.);}}
   sclk_low;
   msleep(5L);
  
   pc_high;
    sclk_high;
   
   return heading;}

/*************************END*************************/     

 

The main program contains the navigation scheme, the heading and distance translators, and the motor driver routines. The main() function drives the whole thing as the control executive. The grid-based navigation scheme is interesting, and relatively simple, and will be discussed shortly. The control executive housed in main() is obvious, requiring no explanation. The heading and distance translators are used to take data from the navigation scheme and translate it into translation and rotation information to pass to the motor drivers. Again, the methods used to make the translations are transparent, so space will not be taken to go over them here.

Dealing with the motors was really ugly. The motors are geared-head DC with a large gear reduction and lots of torque. Lots of torque and a large gear reduction translates into lots of inertia. A further complication is that the motors are not exactly matched. One motor has a part machined off-center somewhere, causing it to bind in part of its rotation, although on average it seems to be slightly faster than the other motor. These two factors, inertia and motor mismatch, makes motor control in a dead reckoning robot a bit of a challenge.

Common motor control algorithms are out the window when the wheels must be tightly slaved at all times. The first attempt to get them under control was to slave one wheel to the other. Controlling power to one motor based on the result of comparing the encoder counts causes the robot to drift away from the master wheel while still maintaining heading. The effect is rather like a boat heading directly across a river as the current carries it away from the place on the opposite bank where line-of-site parallel to the keel would have it land. The drift as it is occurring is imperceptible, but the robot winds up at the wrong place pointed in the right direction. To keep this from happening, it was necessary to cross slave each wheel to the other. An error of one encoder count (out of 340 counts per revolution) between wheels will result in a power cut to the faster wheel. However, because of the motor mismatch, the maximum power to each wheel must be biased toward the slow motor, or else the drift occurs. Because one motor (the fast one) binds in part of its rotation, the bias number was determined by experimentation. It has not been fun.

To add to the enjoyment, the motors cannot be stopped accurately due to inertia drift. Complicating even that, the motors do not stop the same, as the binding motor will stop first if power is cut to both motors simultaneously. From the point where power is cut, DocBot will drift another whole inch and turn four or five degrees to the left if allowed to stop on its own. So, to get the robot to stop while translating or rotating, it is necessary to slow down as the desired position is neared, and then stop while still a few clicks away. DocBot then jogs himself into final position by firing quick pulses to each motor while maintaining exact synchronicity between them until the actual position is equal to the commanded position.

The final complicating factor is that the wheels will as often as not overshoot the desired encoder count by one or two clicks on one or both wheels. Because DocBot can stand no errors at all that he can do anything about (systematic errors), the overshoot of each wheel is passed forward to the next translation or rotation so that they can be corrected for at the beginnning of the next motion.

The lesson learned here is that geared head motors were the wrong choice for this project. DocBot is trying to use them as stepper motors, which probably would have been the correct choice. It was a hard-learned lesson, for sure.

Now, the real meat of this project: the grid-based navigation routine. It is the neatest work I ever did, but like most other things, I stole the whole thing. Back in 1996, 'The Robotics Practitioner,' a now defunct robotics magazine, carried an article by Tucker Balch entitled 'Grid-based Navigation for Mobile Robots.' (The article starts on page seven. On the facing page are two new product announcements, one for the Handy Board, and one for the BOTBoard 2 that some of us have heard about.) Even though the magazine that the article originally appeared in is defunct, the article itself is available on-line as a pdf file at http://www.cs.cmu.edu/~trb/software/trp.pdf. The software described in the article can be found at http://www.teambots.org/software.html under "GridNav 1.0.'

Note that the software authored by Tucker Balch, that I translated from C to IC and then hacked up for my purposes, is protected by copyright. The terms of the copyright as summarized at the beginning of the original work by Dr. Balch, reads as follows:

'gridnav.c

Version 1.0 A grid-based navigation algorithm for mobile robots.

Copyright (C) 1995 Tucker Balch

This program is free software; you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by

the Free Software Foundation; either version 2 of the License, or

(at your option) any later version.

This program is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

GNU General Public License for more details.

You should have received a copy of the GNU General Public License

along with this program; if not, write to the Free Software

Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

Full text of the GNU General Public License was packaged with this

software in the file GPL.'

The full text of the Public License is contained in the zip file that also contains the original software. Since my work is derived from work covered under the license, my program now falls under the same license. Hence you may obtain my work from me for free (if I have to mail it, I can charge you for my distribution costs), and you may use it and/or modify it in any way you see fit. However, your work containing any of my code in any form, modified or otherwise, would then fall under the terms of the license. So if you use it, be sure to read and comply with the terms of the license.

At the tail end of this article will be found the whole monster main code, combining my adaptation of the grid based navigation system, the drive interpreter, the main control executive, and a lot of questionable program structure. It is put at the end, as otherwise few would ever get to the next paragraph.

So, how well does it work?

He does exactly what I set out for him to do. We'll let DocBot tell it from his perspective. DocBot?

Yes, thanks programmer G. OK, first I sit by your desk where you have very carefully aligned me on some scuffed up masking tape, and wait for you to push the reset button on my head. I take this to mean 'go!' I think slowly, and I know everybody is impatient with me, but I just ignore it while I spend 3.5 minutes crunching a huge matrix. The matrix is filled with things like where I am, which way I'm pointing, where I'm supposed to check in for position updates, where the desks, chairs, and walls are, etc. Oh, and where I'm supposed to go is in there too. So, I crunch away rather slowly, having to optimize the matrix so I know the best way to go to the places I'm supposed to go. You know, the matrix has dimensions of 10 x 37, and I have to go through it a lot of times to get it perfect.

It's easy enough to tell which way to go, because all I have to do is look in the cost grid and find the lowest numbered of the six closest squares. If its on my forward right hand diagonal, then I turn right 45 degrees, if its off my left beam, I turn left 90 degrees, etc. Sometimes its straight ahead, and I don't need to turn at all. Once I have made my turn, or determined that no turn is needed, I scan ahead and make sure that the square I'm going to is not occupied by something some human set there. John, your partner, is real bad about leaving things laying around, and I don't want to run into anything. If I happen to find something, then I make a note of it in a place in my memory where I will never forget, even if I'm turned off. I can learn the locations of 13 uncharted objects, in case John is having a bad day. Then, taking the new obstacle into consideration, I have to replan the whole route, which once again takes me 3.5 minutes. It seems ridiculous, I know, to take so long in this planning thing. But I have to get it right. Remember what happened when I ran into the dog while she was sleeping? We wouldn't want that again.

After I've made sure the coast is clear, I go ahead and drive over to the next square. I have to sneak up on the center of the thing, because I'm overpowered to the extreme. What were you thinking of, anyway? I kind of jog myself into final position to make sure I get it just right. If I slip up and overshoot a little, I'm real quiet about it, and then make a correction later so nobody notices.

So I do this until I get around the desk and out the door into Monique's office. My first destination is right in front of the copier, and I stop there next to a nice looking board. Sometimes I get confused if surface contrasts are different than what I'm calibrated for, but fortunately I think John left a shelf there when he was cleaning out his office a few months ago (his idea of cleaning being to move stuff out of his office and into Monique's). Anyway, that's fortuitous, because I can get perfectly realigned with where I'm supposed to be from a white surface like that board. I like this part, so I sort of fool around for awhile at it. And I'm finicky, too. If I were to get lost, it would not go well around here. Sometimes I carry the notes about what's for lunch.

After I realign, then I replan. Yep, 3.5 more minutes. But it could be worse. It used to take 7 minutes before we redid some things.

Eventually, I leave the copier and make my way into John's office. Sometimes John is on the phone, so I take the opportunity to realign myself on one of his other shelves, and go ahead and replan my way back. That usually gives him enough time to put any notes on my back that need delivering. After I replan (3.5 more minutes!) I head on back, not necessarily taking the exact same route. It can be just as efficient to go several different ways, and I like a change in scenery. I stop once at the copier just to check up, and then reappear back next to your desk where I started.

No big deal, really. But as long as I have a chance to comment, I do have a couple of complaints. First, I don't hit anything, but what if someone steps on me? Shouldn't I have a beeper that sounds while I'm moving or something? Or at least a death ray to protect myself?

Second, why don't you have me scan where I'm going while I'm enroute? That way I wouldn't have to sit and scan while not moving, which seems to waste a lot of time.

Third, why do I have to go from square to square as if I was a checker on a checker board? I have all this fancy planning software, so why not just take a direct route? I don't get it.

Forth, why do I have to reevaluate my entire matrix each time I stop for a new obstacle or at a realignment station? Most of it is behind me, or on the other side of my next destination, but I have to optimize the whole darn thing. Would you check into that?

Fifth, when I'm searching for new obstacles, I check to see if the square ahead of me is occupied. I really don't care if the whole square is occupied, I just want to know if there is enough room to get through. Sometimes, I pick up doorways or something that are not actually in the way, but I have to record them as map violations anyway, and then REPLAN AGAIN! That should be easy enough to fix.

Sixth, I could use a lot more wheel resolution. Can you look into either putting more sectors on the wheels or using some other kind of encoder so we can do phase quadrature?

Seventh, the program in my memory has got a lot of unnecessary globals, and some sloppy work. Anybody who sees it like this is going to know you don't write programs for a living.

Finally, can you talk to John? This place is a pig pen.

Lessons learned:

Every time we roboticists build a new creature, it's a prototype by definition. The whole thing falls under the heading of research and development. In the process of building these little machines, we do things that in retrospect we wouldn't do again, or would do differently. In the case of DocBot, there were a couple of major things that should have been done some other way.

The biggest mistake was in thinking that geared motors would work in this application. The extent of this mistake was in thinking there would be no problem at all. It was a major error. As it stands, DocBot does his task well, but the motors are barely tamed.

Another poor design choice was the use of just a single encoder per wheel. That could never have worked. Given that this was foreseeable, another error was made in the type of encoder pickups used. They are what are described in the Handy Board literature where the software for them comes, and they are analog. With analog encoders, phase quadrature is prohibitively difficult to implement.

A good lesson was learned in pushing the limits of IC. With so much of the user memory taken up with the pcode interpreter, it was found that gobal variables could not be assigned a value during declaration -- an obvious bug in IC. Something like int global_variable=10; would create the variable, but it would be assigned the value of 0. To assign a value, a separate statement was required. This was not a major problem, except for the havoc it wreaked suddenly when the program crossed some critical size. With small programs, it works as it should, but as they get large it all of a sudden stops working.

IC was a bad choice for a programming language because of the size of the interpreter that lives in memory (16K), and the execution speed. It works so well for smaller programs that the limitations imposed by a program as large as what is in DocBot was not foreseeable. I have the ICC11 compiler from ImageCraft, which would have been a good choice in lieu of the IC compiler. ICC11 compiles into HC11 native code, and there is no interpreter living in RAM. I've even learned how to use ICC11, no small task for a newbie. But having learned what little I know about IC on my own, a subset of the real C, I struggle with the full-bore product. However, in retrospect I should have learned ANSI C and used the ImageCraft compiler. My apologies to Randy Sargent if he should read this. He has a great tool in IC, and I love the thing. It's irreplaceable for what it is designed for, but I pushed its limits.

As a huge inprovement, were I to do this again, I'd switch processors completely and use a 68332 board in place of the HC11. I have the board from Mark Castelluchio, the MRM, and the comparison between it and the best-equipped HC11 board are staggering: 512K of RAM compared to 32K, 512K of EEPROM compared to 2K at most, clock speed ot 25 MHz compared to 2 MHz, 32 bit processor compared to 8 bit, 16 IC/OC/PWM pins individually programmable compared to 8 dedicated and less versatile pins on the HC11, and so on. It is not a board for people new to processors (in my opinion), but I've cut my teeth now, and next time I'd use that.

Conclusion:

I've learned a lot of things with this project that I could have learned no other way. Having done a navigating robot has provided insights into the navigation environment, motor control and sensor integration that will prove of value in every project I do from now on.

For the immediate future, I plan to continue on with DocBot to resolve some of the issues he outlined for us above. Optimizing his planning stage should be the easiest, and modifying his encoder strategy to implement phase quadrature will be the most difficult due to mechanical constraints. Getting John to clean up his office is out of the question, and I'll not try that. Finally, as a tool to learn ANSI C, the whole program will be converted to C and run as native code using the ICC11 compiler.

After DocBot, I think I'm about ready for something that is really fast. All my robots are slow, and I'm jealous of Randy Sargent's fast little cars that chase orange hats. But rather than a mobile robot, this time I'm thinking more along the lines of a fully automatic ping-pong ball gun that has autonomous real-time target acquisition, tracking and attack capability. I'm tired of everyone asking of my creations, "What does it do?" I want one that has a readily recognizable function to answer those folks: monitor the room for the phrase "what does it do?," and kill.

And now, the main code:

 

nav.c

/********* file nav.c *************/
/* note: largest possible grid with existing memory = 15 x 40 */
#define GRID_SIZE_X 10  
#define GRID_SIZE_Y 37  
#define EMPTY 0
#define FULL 1
#define BIG_COST 500.
#define MAX_VELOCITY 0.1
#define SQRT2 1.4142136
#define Pi 3.14159
/***********************
Globals 
***********************/
char occupancy[GRID_SIZE_X][GRID_SIZE_Y];
float cost[GRID_SIZE_X][GRID_SIZE_Y];
int robot_x, robot_y;
char goal_x, goal_y;
float x_part, y_part;
float x1;
float x0;
float y1;
float y0;
float theta_0;
float theta_1;
/* the following are for robot discovered obstacles */
persistent char new_obstacle=0;
persistent char A1=0;
persistent char B1=0;
persistent char C1=0;
persistent char D=0;
persistent char E=0;
persistent char F=0;
persistent char G=0;
persistent char H=0;
persistent char I=0;
persistent char J=0;
persistent char K=0;
persistent char L=0;
persistent char M=0;
persistent char N=0;
persistent char O=0;
persistent char P=0;
persistent char Q=0;
persistent char R1=0;
persistent char S=0;
persistent char T=0;
persistent char U=0;
persistent char V=0;
persistent char W=0;
persistent char X=0;
persistent char Y=0;
persistent char Z=0;
void save_new_obstacle(int obstacle_x, int obstacle_y)
  {if(new_obstacle==1)
    {A1=obstacle_x;
     B1=obstacle_y;}
  else if (new_obstacle==2)
    {C1=obstacle_x;
     D=obstacle_y;}
  else if (new_obstacle==3)
    {E=obstacle_x;
     F=obstacle_y;}
  else if (new_obstacle==4)
    {G=obstacle_x;
     H=obstacle_y;}
  else if (new_obstacle==5)
    {I=obstacle_x;
     J=obstacle_y;}
  else if (new_obstacle==6)
    {K=obstacle_x;
     L=obstacle_y;}
  else if (new_obstacle==7)
    {M=obstacle_x;
     N=obstacle_y;}
  else if (new_obstacle==8)
    {O=obstacle_x;
     P=obstacle_y;}
  else if (new_obstacle==9)
    {Q=obstacle_x;
     R1=obstacle_y;}
  else if (new_obstacle==10)
    {S=obstacle_x;
     T=obstacle_y;}
  else if (new_obstacle==11)
    {U=obstacle_x;
     V=obstacle_y;}
  else if (new_obstacle==12)
    {W=obstacle_x;
     X=obstacle_y;}
  else if (new_obstacle==13)
    {Y=obstacle_x;
     Z=obstacle_y;}
  
}
/*******************************************************
       set up everything 
*******************************************************/
void set_up()
 {init();
   load_map();
    replan();}
 
/*****************************************************
      load the map into memory (partly provided to the robot by humans)
******************************************************/
void load_map()
{
/* robot-found obstacles........... */
        occupancy[A1][B1]=FULL;
        occupancy[C1][D]=FULL;
        occupancy[E][F]=FULL;
        occupancy[G][H]=FULL;
        occupancy[I][J]=FULL;
        occupancy[K][L]=FULL;        
        occupancy[M][N]=FULL;
        occupancy[O][P]=FULL;
        occupancy[Q][R1]=FULL;
        occupancy[S][T]=FULL;
        occupancy[U][V]=FULL;
        occupancy[W][X]=FULL;
        occupancy[Y][Z]=FULL;
        
/* existing obstacles......................*/
        occupancy[1][28]=FULL;
        occupancy[2][28]=FULL; 	
        occupancy[3][28]=FULL;
        occupancy[4][28]=FULL;
        occupancy[3][29]=FULL;
        occupancy[3][30]=FULL; 	
        occupancy[3][31]=FULL;
        occupancy[3][32]=FULL;
        occupancy[4][33]=FULL;
        occupancy[1][1]=FULL;
        occupancy[1][2]=FULL; 	
        occupancy[1][3]=FULL;
        occupancy[1][8]=FULL;
        occupancy[1][9]=FULL;
        occupancy[3][13]=FULL;
        occupancy[3][14]=FULL; 	
        occupancy[3][15]=FULL;
        occupancy[3][16]=FULL;
        occupancy[4][16]=FULL;
        occupancy[5][16]=FULL; 	
        occupancy[6][16]=FULL;
        occupancy[7][16]=FULL;
        occupancy[2][19]=FULL;
        occupancy[4][10]=FULL;
        occupancy[5][10]=FULL; 	
        occupancy[6][10]=FULL;
        occupancy[7][10]=FULL;
        occupancy[6][8]=FULL;
        occupancy[7][8]=FULL; 	
        occupancy[6][9]=FULL;
        occupancy[7][9]=FULL;
        occupancy[1][21]=FULL;
        occupancy[1][20]=FULL;
        occupancy[2][22]=FULL;
        occupancy[1][22]=FULL;
        occupancy[2][21]=FULL;
        occupancy[2][20]=FULL;
        occupancy[4][11]=FULL;
        occupancy[4][12]=FULL; 	
        occupancy[4][13]=FULL;
        occupancy[5][23]=FULL;
        occupancy[6][23]=FULL; 	
        occupancy[7][23]=FULL;
        occupancy[3][26]=FULL;
        occupancy[3][25]=FULL;
        occupancy[4][25]=FULL;  
        occupancy[5][25]=FULL;
        occupancy[6][25]=FULL; 	
        occupancy[7][25]=FULL;
        occupancy[3][24]=FULL;
        occupancy[5][24]=FULL;
}

/*************************
      init()
Initialize the occupancy and cost grids
*****************************/
void init()
 {int i,j;
  for (i=0; i < GRID_SIZE_X; i++)
    {for (j=0; j< GRID_SIZE_Y; j++)
      {occupancy[i][j] = EMPTY;
       cost[i][j] = BIG_COST;
      
      }
    }
}
/********************************
	cell_cost()
Calculate the cost of traveling from a cell to 
the goal.  Returns 0 if no change, 1 otherwise.
**********************************/
int cell_cost(int x, int y)
{
int i, j, low_x, low_y;
float temp_cost, low_cost = BIG_COST;
if (occupancy[x][y] == FULL)
	{cost[x][y] = BIG_COST;
         return 0;}
/* check to see if its the goal*/
if ((goal_x == x) && (goal_y == y)) 
	{cost[x][y] = 0.;
         return 0;}
low_x = x;
low_y = y;
low_cost = cost[x][y];	
for (i=(x-1);i<=(x+1); i++)
	{for (j=(y-1);j<=(y+1); j++)
		{if ((i==x) || (j==y)) 
                   temp_cost=cost[i][j] + 1.;
                 else
                   temp_cost=cost[i][j] + SQRT2;
		 if (temp_cost<low_cost)
 		   low_cost=temp_cost;
		   low_x = i;
		   low_y = j;
                }
	}
	if (cost[x][y] != low_cost) 
            
                 {cost[x][y] = low_cost;
		 return 1;}
        else
                 return 0;}
/**************************************
	replan()
compute the cost grid based on the map represented in the occupancy grid.
****************************************/
void replan()
{int i, j, count=1;
  enable_encoders=0; /*shut off the encoders in the system interrupt so this will go faster */
  while(count != 0)
   	{count=0;
         for (i=1; i < GRID_SIZE_X-1; i++)
		{
                          for (j=1; j < GRID_SIZE_Y-1; j++)
                          count = count + cell_cost(i,j);
                        }
              }
   enable_encoders = 1;
}
/*********************************************
	check_plan()
Look at the cost grid to decide which way to go.  Returns a heading
in radians between 0 and 2*Pi. East (+x) is 0, north is (+y) is Pi/2.
**********************************************/
void check_plan(int x, int y)
{int i,j;
 float low_x=0., low_y=1., low_cost;
   x_part=0.;
   y_part=0.;
/* Find the x and y parts of the gradient */
for(i=(x-1); i<=(x+1); i++)
 {for(j=(y-1); j<=(y+1); j++)
   {if (i<x) x_part += cost[i][j];
    if (i>x) x_part -= cost[i][j];
    if (j<y) y_part += cost[i][j];
    if (j>y) y_part -= cost[i][j];
   }
 }
        low_cost = cost[x][y];
	x_part = 0.;
	y_part = 1.;
	for(i=(x-1); i<=(x+1); i++)
           {for(j=(y-1); j<=(y+1); j++) 
               if(cost[i][j]<low_cost)
		{low_cost = cost[i][j];
		 low_x=(float)(i);
                 low_y=(float)(j);
                         }
              }
	/* Use the direction towards the lowest cost cell
           as the heading.  */
	x_part = low_x - (float)(x);
            y_part = low_y - (float)(y);
         }
       


/***********************************************************
       run the course
**********************************************************/
void do_course()
	
         {set_up();
            while(1)
             {
                 if(round(x0/12.)==goal_x && round(y0/12.)==goal_y)
                  break;
                  if(y0 == 12.* 11. || y0 == 12.*14. || y0 == 12.*17.|| y0 == 12.*20. || y0 == 12.*23.|| y0 == 12.*26. || y0 == 12.*29. || y0 == 12.*31.)
                  align_to_compass();  /* do this every three feet */
	       		
                  check_plan(round(x0/12.),round(y0/12.));
                  if  (x_part < -1.)
            	{x1 = x0-12.;
                         x_part=-1.;}
                  else if (x_part > 1.)
            	{x1 = x0+12.;
                         x_part=1.;} 
                 else x1 = (x0 + (x_part*12.));
                 if  (y_part < -1.)
                      {y1 = y0-12.;
                         y_part=-1.;}
	    else if (y_part > 1.)
                      {y1 = y0+12.; 
                        y_part=1.;}
                 else y1 = (y0 + (y_part*12.));
	
                 nav();} /* take the solution and run with it */
		 
                 sleep(.5);
               }
                 
/*******************************************************
        engine room
********************************************************/
/* motor 0 is on the right, Encoder2 & 1 is on the right*/
/*motor 0 is the fast one*/
#define tilt servo_a7_pulse
#define pan servo_a3_pulse 
#define left_motor 1
#define right_motor 0
#define range analog(4)
void servo3_on()
 {servo_a3_init(1);
 }
void servo3_off()
 {servo_a3_init(0);}
void servo7_on()
 {servo_a7_init(1);
  }
void servo7_off()
 {servo_a7_init(0);
  pokeword(0x1016,0); /*do this to renable motor speed*/
}
/*********************************
nav routines
************************************/
 
  int diagonal;
 float delta_x;
 float delta_y;
void nav()
 
{int obstacle_x, obstacle_y;
 float R_temp;
 float x2,y2;
 int R;
 float theta;
 
delta_x=x1-x0;
delta_y=y1-y0;
R_temp=sqrt(delta_x*delta_x + delta_y*delta_y);
R=round(R_temp);
         if (delta_x == 0. && delta_y > 0.)
                {theta = Pi / 2.;}
    
         else  if (delta_x == 0. && delta_y < 0.)
                {theta = -Pi / 2.;}
         else if (delta_x > 0. && delta_y >= 0.)
 		{theta = atan(delta_y/delta_x);}
         else if (delta_x < 0. && delta_y >= 0.)
 		{theta = Pi + atan(delta_y/delta_x);}
         else if (delta_x < 0. && delta_y < 0.)
 		{theta = Pi + atan(delta_y/delta_x);} 
         else 
 		{theta = 2.*Pi + atan(delta_y/delta_x);}
   
         theta_1 = theta - theta_0;
    
         theta_1 = theta_1 * 57.29578; /*convert to degrees*/
/*is the robot now aligned on a diagonal? */
          if ( (int)delta_x == 0 || (int)delta_y == 0)
             diagonal = 0;
          else 
             diagonal = 1;
          
         turn (round(theta_1)*-1);} 
         if (diagonal == 1)
            { scan_diagonal();} /*look around with IR ranger to see if anything is out there that shouldn't be */
         else
            { scan_aligned();} /*ditto*/
         theta_0=theta; /*already at new angle, so this is done no matter what */
         if(hit_zone == 0) /*meaning there are no new obstacles*/
            {go(R); /* so go ahead and go */
             x0=x1;
             y0=y1;
             sleep(.5);}
         else if(hit_zone == 1) /* something is out there!  works for both scan_diagonal and scan_aligned */
            {occupancy[obstacle_x=(int)(x1/12.)][obstacle_y=(int)(y1/12.)]=FULL;
             save_new_obstacle(obstacle_x, obstacle_y);
            set_up();
             }
             else
                {go(R);
                 x0=x1;
                 y0=y1;}}
              
/*********************************
 translation directive go()
 
**********************************/
float clicks=0.0;
int left_offset=0;
int right_offset=0;
void go(int inches)
   {int speed=60;
     clicks=20.16*(float)(inches)*1.34375; /* fiddled with this during development, could be one constant */
     encoder0_counts=encoder1_counts=encoder2_counts=encoder3_counts=0; /*zero the encoders*/
     left_encoder= left_offset + left_turn_offset; /* the offsets are corrections from previous overshoots */
     right_encoder= right_offset + right_turn_offset;
    
     motor(right_motor,86);
     while (right_encoder+22<round(clicks))
     { if(right_encoder+28 >= round(clicks))
         speed=10;  /*deacellerate*/
     
       {if (left_encoder<right_encoder)
         {motor(right_motor,10); }
        else
          motor(right_motor,speed);}
       {if(left_encoder>right_encoder)
        {motor(left_motor,10);}
        else
        motor(left_motor,speed);}}
        motor(right_motor,0);
        sleep(.3);
        motor(left_motor,0);
        sleep(1.0);
      
 while (right_encoder<round(clicks) || left_encoder<round(clicks))
     {
     if (left_encoder<round(clicks) && left_encoder<=right_encoder)
         {jog_left(.04,1);}
     if (right_encoder<round(clicks) && right_encoder<=left_encoder)
         {jog_right(.04,1);}
     sleep(.25);}
     
     sleep(1.); 
     left_offset= left_encoder-round(clicks); /*these are overshoot numbers to use next time*/
     right_offset= right_encoder-round(clicks);
     right_turn_offset=left_turn_offset=0; /*only use once after a turn */       
       }
/*****************************************
    Rotation directive turn()
******************************************/
      int left_turn_offset=0;
      int right_turn_offset=0;
void turn(int degrees)
      {
        int adder;  /* correct a turn deficiency at 45 degrees */
        int speed = 45;
        float turn_cal=1.0;
        float right_turn_cal=45./43.26; /*41.5   43.26*/
        float left_turn_cal=45./43.26;
        int dir;
      
        if (degrees==45 || degrees==-45)
          adder=-2; 
          else if (degrees==180 || degrees==-180)
          adder=-5; 
          else adder=0;
        if (degrees > 180)
             degrees= (degrees-360); /*don't go the long way*/
        if (degrees < -180)
             degrees= (degrees+360); /* ditto */
	
        {if(degrees < 0)
        	{dir=-1;
              turn_cal=left_turn_cal;
              degrees=-degrees;
                 }
   	else
	{dir=1;
	  turn_cal=right_turn_cal;}}
              clicks = 2. *.65946 * (float)(degrees) * turn_cal*1.34375 ;
              encoder0_counts=encoder1_counts=encoder2_counts=encoder3_counts=0; /*zero the encoders*/
            if (dir==1) /*clockwise turn */
           {left_encoder=(left_turn_offset+left_offset-adder);
	 right_encoder=-(right_turn_offset+right_offset+adder);}
            else
            {left_encoder= -(left_turn_offset+left_offset+adder);
	  right_encoder= (right_turn_offset+right_offset-adder);}
        motor(left_motor,dir*speed);
        motor(right_motor,-dir*speed);
        while (right_encoder+12<round(clicks))
       {if(right_encoder+23 >= round(clicks))
         speed=10;  /*deacellerate*/
  
       {if(right_encoder<left_encoder)
        motor(left_motor,dir*7);
        else
        motor(left_motor,dir*speed);}
       {if(right_encoder>left_encoder)
        motor(right_motor,-dir*7);
        else
        motor(right_motor,-dir*speed);}}
     
       stop();
       sleep(.5);
      
       while (right_encoder<round(clicks) || left_encoder<round(clicks))
     {
        if (right_encoder<round(clicks) && right_encoder<=left_encoder)
        {jog_right(.04,-dir);}
     if (left_encoder<round(clicks) && left_encoder <= right_encoder)
         {jog_left(.04,dir);}
          sleep(.25);}
          sleep(1.0);  /*let wheels get stopped*/
          left_turn_offset = dir*(left_encoder-round(clicks)); /* overshoot corrections */
          right_turn_offset = -dir*(right_encoder-round(clicks));
          left_offset=right_offset=0; /*already used these from last translation, kill 'em */
       }
	
/*************************************
   IR scan functions
**************************************/ 
int hit_zone=0;
void scan_aligned() /* aligned with x or y axis, not on a diagonal track */
   {int deg=40;
    int k;
    float look[10];
    int hit=0;
    hit_zone=0;
    pan_deg(deg);
    sleep(.4); /*allow extra sweep time */
   
    for(k=1;k<10;k++)
     {look[k]=range_avg();
        if (look[k] > 0.)
         {hit=1;}
      deg=deg-10;
      pan_deg(deg);}
      
      pan_deg(0); /*center servo*/
    
    while(hit) /* only look at an area covered by the square directly ahead */
     {if((look[1]>0. && look[1]<=9.31) || (look[9]>0. && look[9]<=9.31) || (look[2]>0. && look[2]<=12.0) || (look[8]>0. && look[8]<=12.0) || (look[3]>0. && look[3]<=17.54) || (look[7]>0. && look[7]<=17.54) || (look[4]>0. && look[4]<=17.54) || (look[6]>0. && look[6]<=17.54) || (look[5]>0. && look[5]<=17.54))
         {hit_zone=1;
          new_obstacle++;}
      
         hit=0;}}

void scan_diagonal() /* look at an area of a square directly diagonal to the square we are on */
   {int deg=40;
    int k;
    float look[10];
    int hit=0;
    
    hit_zone=0;
    pan_deg(deg);
     sleep(.4); /*allow extra sweep time */
    for(k=1;k<10;k++)
     {look[k]=range_avg();
        if (look[k] > 0.)
         {hit=1;}
      deg=deg-10;
      pan_deg(deg);
      }
    pan_deg(0); /*center servo*/
 
    while(hit)
     {if((look[1]>0. && look[1]<=14.61) || (look[9]>0. && look[9]<=14.61) || (look[2]>0. && look[2]<=15.07) || (look[8]>0. && look[8]<=15.07) || (look[3]>0. && look[3]<=16.05) || (look[7]>0. && look[7]<=16.05) || (look[4]>0. && look[4]<=17.77) || (look[6]>0. && look[6]<=17.77) || (look[5]>0. && look[5]<=20.58))
         {hit_zone=1;
       new_obstacle++;}
       hit=0;}}
/***********************************************
           JOG motors to finish move command
***********************************************/
        
void jog_right(float on,int dir)
  {motor (right_motor,dir*30);
   sleep (on);
   motor (right_motor,0);
   }
void jog_left(float on, int dir)
  {motor (left_motor,dir*30);
   sleep (on);
   motor (left_motor,0);
   }

/***********************************************
           ALIGN_TO_COMPASS
***********************************************/
void align_to_compass()
      {float correct_theta_0;
        int local_deviation;
       int mag_heading;
        if (theta_0 < 0.)
        correct_theta_0= theta_0 + (2. * Pi);
        else correct_theta_0=theta_0;
	if(correct_theta_0 + .1 < Pi/4. )  /*.1 added to cure rounding error */
		mag_heading=76;
   	else if(correct_theta_0 + .1 <  Pi/2.) 
		mag_heading=31;  /* these constants represent headings spaced 45 degrees around the compass */
	else if(correct_theta_0 + .1 < 3.*Pi/4.)   
		mag_heading=344;
   	else if(correct_theta_0 + .1 <  Pi)
		mag_heading=304;
   	else if(correct_theta_0 + .1 < 5.*Pi/4.)
		mag_heading=260;
   	else if(correct_theta_0 + .1 < 3.*Pi/2.)
		mag_heading=212;
   	else if(correct_theta_0 + .1 < 7.*Pi/4.)
		mag_heading=159; 
   	else mag_heading=120;
/* make the corrections for mag deviation based on location during compass read */
	if (y0==12.*11.)
      		{if (x0==12.*3.)
                   local_deviation=-28; 
		else if (x0==12.*2.)
                   local_deviation=-24;  
	            else if (x0==12.*1.)
                   local_deviation=-24;}
	else if (y0==12.*14.)
      		{if (x0==12.*2.)
                   local_deviation=-19;
		else if (x0==12.*1.)
                   local_deviation=-29;}
	else if (y0==12.*17.)
      		{if (x0==12.*4.)
                   local_deviation=-12;
		else if (x0==12.*3.)
                   local_deviation=-15; 
		else if (x0==12.*2.)
                   local_deviation=-20; 
		else if (x0==12.*1.)
                   local_deviation=-22;}
	else if (y0==12.*20.)
      		{if (x0==12.*5.)
                   local_deviation=-11;
		else if (x0==12.*4.)
                   local_deviation=-11;
		else if (x0==12.*3.)
                   local_deviation=-10;} 
	else if (y0==12.*23.)
      		{if (x0==12.*2.)
                   local_deviation=-2;
		else if (x0==12.*3.)
                   local_deviation=-2;
		else if (x0==12.*4.)
                   local_deviation=-2;}
	else if (y0==12.*26.)
      		{if (x0==12.*1.)
                   local_deviation=3;
		else if (x0==12.*2.)
                   local_deviation=3;}
	else if (y0==12.*29.)
      		{if (x0==12.*4.)
                   local_deviation=-2;
		else if (x0==12.*5.)
                   local_deviation=1;
		else if (x0==12.*6.)
                   local_deviation=-2;
		else if (x0==12.*7.)
                   local_deviation=-2;}
	else if (y0==12.*31.)
      		{if (x0==12.*4.)
                   local_deviation=-2;
		else if (x0==12.*5.)
                   local_deviation=-3;
		else if (x0==12.*6.)
                   local_deviation=-2;
		else if (x0==12.*7.)
                   local_deviation=-2;}
        while(1)
		{
                   if (read() < (mag_heading - 1 + local_deviation))
                    {jog_left(.04,1);}
                     
                   else if (read() > (mag_heading + 1 + local_deviation))
                   {jog_right(.04,1);}
                   else break;}
        left_offset=right_offset=left_turn_offset=right_turn_offset=0;  /* corrections for any overshoots */
           }
/**************************************
      servo control 
**************************************/
void pan_deg(int deg)
  {float pan_temp;
   pan_temp=3035. + ((float)(deg)*18.30);  /* constants determined by experiment */
   pan=(int)(pan_temp);
   servo3_on();
   pan=(int)(pan_temp);
   sleep(1.0);
   pan=(int)(pan_temp);
   sleep (.4); 
   servo3_off();
   }
/***********************************
   master control executive-- the main() function
***********************************/
int q;
void main()
{
    tilt=3600;
    sleep(.1);
    servo7_on();
    sleep(.8);
    servo7_off();
    pan_deg(0);
    servo3_on();
    sleep(1.);
    encoder_low_threshold=75;
    encoder_high_threshold=150;
    reset_compass();
/* set initial nav variables  */
    x0=5.;/*initial position is x = 5*/
    y0=9.;/*initial position is y = 9*/
    goal_x=3; /*to first waypoint*/
    goal_y=20; /*to first waypoint*/
    theta_0=3.*Pi/2.; /*initial orientation */
    x1=0.;
    y1=0.;
    theta_1=0.;
    robot_x=(int)x0;
    robot_y=(int)y0;
    x0=x0* 12.; /*change to inches*/
    y0=y0* 12.; /*change to inches*/
/* set ranging variables */
   deg_to_rad = 57.2957795131;
   A=40./deg_to_rad;
    do_course();
/* correct the heading and distance from the landmark using IR */
    correct_heading(1,0,18,1);  /*side to look (+is right),angle s/b,normal distance s/b */ 
    printf("At waypoint 1.\n");
/* set couse to next waypoint */
    goal_x=5;
    goal_y=31;
    do_course();
    correct_heading(1,0,12,1);
    printf("At first goal.\n");
    goal_x=3;
    goal_y=22;
    
   do_course();
    correct_heading(-1,0,18,0); 
    printf("At second waypoint.\n");
    goal_x=3;
    goal_y=9;
   do_course();
   printf("You miss me?\n");
}