bot.c

00001 /* 
00002    Copyright (C) 2001 Free Software Foundation, Inc.
00003    Written by Duane Gustavus (duane@denton.com)
00004 
00005 This file is free software; you can redistribute it and/or modify it
00006 under the terms of the GNU General Public License as published by the
00007 Free Software Foundation; either version 2, or (at your option) any
00008 later version.
00009 
00010 In addition to the permissions in the GNU General Public License, the
00011 Free Software Foundation gives you unlimited permission to link the
00012 compiled version of this file with other programs, and to distribute
00013 those programs without any restriction coming from the use of this
00014 file.  (The General Public License restrictions do apply in other
00015 respects; for example, they cover modification of the file, and
00016 distribution when not linked into another program.)
00017 
00018 This file is distributed in the hope that it will be useful, but
00019 WITHOUT ANY WARRANTY; without even the implied warranty of
00020 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00021 General Public License for more details.
00022 
00023 You should have received a copy of the GNU General Public License
00024 along with this program; see the file COPYING.  If not, write to
00025 the Free Software Foundation, 59 Temple Place - Suite 330,
00026 Boston, MA 02111-1307, USA.  */
00027 
00028 /* bot.c is a program to control a robot using services provided by libbot.
00029  * The first section initializes the set of services desired, then an endless
00030  * loop is entered.  Each pass through the loop sets the State variable based
00031  * on the contents of the Detect variable, then invokes a behavior based
00032  * on that information.
00033  */
00034 
00035 #include <sys/locks.h>
00036 #include <sys/ports.h>
00037 #include <bot/bot.h>
00038 
00039 void status_report(void);
00040 void bumper(void);
00041 void frob_speed(void);
00042 void spinner(int);
00043 void photo(char);
00044 void ir_detect(void);
00045 int randmask(int);
00046 
00047 /* global indicating the current state of the bot */
00048 volatile int State;
00049 /* event counters for Detect and non-Detect */
00050 int Dcount, Ncount;
00051 /* buffer for constructing output string */
00052 static char buf[80];
00053 
00054 int
00055 main()
00056 {
00057   static char signon[] = "\nhi guys:\n";
00058 
00059   /* disable interrupts to initialize system */
00060   lock();
00061   /* run 68hc11 in expanded bus mode */
00062   set_bus_expanded();
00063   /* initialize buffered serial I/O */
00064   init_buffered_io();
00065   /* initialize system clock */
00066   init_sysclock();
00067   /* initialize analog conversions */
00068   init_analog();
00069   /* initialize encoder services */
00070   init_encoders();
00071   /* initialize MIT 6.270 ir detect subsystem */
00072   init_ir();
00073   /* initialize sensor sampling */
00074   init_poll_sensors();
00075   /* initialize pwm service */
00076   init_pwm();
00077   /* enable interrupts */
00078   unlock();
00079 
00080   /* print signon message to serial port */
00081   serial_print(signon);
00082   /* initial bot state */
00083   State = ACTIVE; start_speed_ctl();
00084   /* wait for analog to come online */
00085   while (Lspeed < 3) { Lspeed = run_speed; Rspeed = Lspeed; }
00086   /* enter bot state machine */
00087   while (1) {  /* no exit */
00088     /* set State based on Detect */
00089     if (Detect) {
00090       /* keep history of both detect and non-detect */
00091       if (Dcount > 0) Dcount += 1; else Dcount = 1;
00092       if (Ncount > 0) Ncount = 0;
00093       /* bumper event */
00094       if (Detect & 0xF000) { State |= BUMPER; }
00095       else { State &= ~BUMPER; }
00096       /* IR event */
00097       if (Detect & 0xC0) { State |= IR; }
00098       else { State &= ~IR; }
00099     } else {
00100       /* keep history of both detect and non-detect */
00101       if (Ncount > 0) Ncount += 1; else Ncount = 1;
00102       if (Dcount > 0) Dcount -= 1; else Dcount = 0;
00103       /* clear sensor subsumption modes */
00104       State &= (~BUMPER & ~IR);
00105     }
00106     /* invoke behavior for State */
00107     /* print status info to the serial port */
00108     if (State & STATUS) { status_report(); }
00109     /* bumper detect! wriggle free, then spin a bit */
00110     if (State & BUMPER) { bumper(); continue; }
00111     /* ir obstacle detectors */
00112     if (State & IR) { ir_detect(); continue; }
00113     /* photo vore/phobe behavior */
00114     if (DIP1) {
00115       State |= PHOTO;
00116       if (av_velo > ((run_speed /2) + (run_speed /8))) {
00117         photo(-1); continue;
00118       } 
00119     } else {
00120       State &= ~PHOTO;
00121     }
00122     /* default behavior is full speed ahead */
00123     if (State & ACTIVE) { 
00124       if (Lspeed < run_speed) { Lspeed += 1; }
00125       if (Rspeed < run_speed) { Rspeed += 1; }
00126     } else {
00127       if (Lspeed) { Lspeed >>1; }
00128       if (Rspeed) { Rspeed >>1; }
00129     }
00130   }
00131 }
00132 
00133 /* print status info to serial out */
00134 void
00135 status_report()
00136 {
00137   /* add usful report variables here */
00138   sprintf(buf,"\n%x: S-%x D-%x", (unsigned short) sysclock, State, Detect);
00139   /* this function will drop chars if the output buffer is full */
00140   serial_print(buf);
00141 }
00142 
00143 /* return timer counter with top bits masked off */
00144 inline int
00145 randmask(int mask)
00146 {
00147   return((int) get_timer_counter() & mask);
00148 }
00149 
00150 /* bumper behavior */
00151 void
00152 bumper()
00153 {
00154   static int last, jutz, spin;
00155 
00156   /* turn speed controller off and disable pwm interrupt */
00157   stop_speed_ctl(); pwm_off();
00158   /* use encoder counter as failsafe */
00159   Lticks = Rticks = 512 + randmask(0xFF); jutz = -1;
00160   /* while bumper sensors are active */
00161   while (Detect & 0xC000) {
00162     last = Detect & 0xC000;
00163     /* if both front bumper switches are active */
00164     if ((Detect & 0XC000) == 0xC000) {
00165       /* backup */
00166       if (jutz) { digital_shadowbits |=
00167                (LEFT_MTR | RIGHT_MTR | LEFT_REVERSE | RIGHT_REVERSE); }
00168       else { /* go forward */
00169         digital_shadowbits |= (LEFT_MTR | RIGHT_MTR);
00170         digital_shadowbits &= (~LEFT_REVERSE & ~RIGHT_REVERSE);
00171       }
00172     } else {
00173       /* front left bumper */
00174       if (last & 0x8000) {
00175         /* jutz between one or both motors reacting */
00176         if (jutz) { digital_shadowbits |= (LEFT_MTR | LEFT_REVERSE); }
00177         else { digital_shadowbits |=
00178                  (LEFT_MTR | RIGHT_MTR | LEFT_REVERSE | RIGHT_REVERSE); }
00179       } else { digital_shadowbits &= ~LEFT_MTR; }
00180       /* front right bumper */
00181       if (last & 0x4000) {
00182         if (jutz) { digital_shadowbits |= (RIGHT_MTR | RIGHT_REVERSE); }
00183         else { digital_shadowbits |=
00184                  (LEFT_MTR | RIGHT_MTR | LEFT_REVERSE | RIGHT_REVERSE); }
00185       } else { digital_shadowbits &= ~RIGHT_MTR; }
00186     }
00187     /* update hardware register */
00188     DIGITAL_PORT = digital_shadowbits;
00189     /* hungup failsafe */
00190     if ((Lticks == 0) | (Rticks == 0)) {
00191       jutz = ~jutz;
00192       if (jutz) { break; }
00193       else { Lticks = Rticks = 768 + randmask(0xFF); }
00194     }
00195   }
00196   /* continue breakfree action a bit */
00197   Lticks = Rticks = 512;
00198   while (Lticks & Rticks) { if (Bcount > 10) { break; } }
00199   /* turn speed controller on and enable pwm interrupts */
00200   Lspeed = Rspeed = run_speed;
00201   start_speed_ctl(); pwm_on();
00202   /* ensure both motors are turned on */
00203   digital_shadowbits |= (LEFT_MTR | RIGHT_MTR);
00204   DIGITAL_PORT = digital_shadowbits;
00205   /* spin away from last detect if possible */
00206   spin = randmask(0x3ff);
00207   if (last & 0x4000) { spin = -spin; }
00208   spinner(spin);
00209 }
00210 
00211 /* spin using speed controller */ 
00212 void
00213 spinner(int ticks)
00214 {
00215   /* set direction of spin */
00216   if (ticks < 0) {
00217     if (Lspeed > 0) { Lspeed = -Lspeed; }
00218     if (Rspeed < 0) { Rspeed = -Rspeed; }
00219     ticks = -ticks;
00220   } else {
00221     if (Lspeed < 0) { Lspeed = -Lspeed; }
00222     if (Rspeed > 0) { Rspeed = -Rspeed; }
00223   }
00224   /* load encoder counter */
00225   Lticks = ticks;
00226   /* wait for ticks encoder counts */
00227   while (Lticks > 0) {
00228     if (Bcount > 10) { break; }
00229   }
00230 }
00231 
00232 /* respond to photo sensors */
00233 void
00234 photo(char flag)
00235 {
00236   static int pdiff, lphoto, rphoto;
00237 
00238   /* equalize sensors a bit -- TWEAK! */
00239   lphoto = PHOTOL;
00240   rphoto = PHOTOR + (PHOTOR >>1);
00241   /* calculate the absolute difference between sensors */
00242   if ((pdiff = lphoto - rphoto) < 0) { pdiff = -pdiff; }
00243   /* if difference is great enough, respond to light based on flag */
00244   if (pdiff > ((lphoto + rphoto) /8)) {
00245     /* slow down one wheel based on difference between sensors */
00246     if (lphoto > rphoto) {
00247       /* photovore */
00248       if (flag) { if (Rspeed > 1) { Rspeed -= 1; } }
00249       /* photophobe */
00250       else { if (Lspeed > 1) { Lspeed -= 1; } }
00251     } else {
00252       /* photovore */
00253       if (flag) { if (Lspeed > 1) { Lspeed -= 1; } }
00254       /* photophobe */
00255       else { if (Rspeed > 1) { Rspeed -= 1; } }
00256     }
00257   /* if difference is not enough, try to increase speed */
00258   } else {
00259     if (Lspeed < run_speed) { Lspeed += 1; }
00260     if (Rspeed < run_speed) { Rspeed += 1; }
00261   }
00262 }
00263 
00264 /* veer away from active IR sensors */
00265 void
00266 ir_detect()
00267 {
00268   static int idiff;
00269 
00270   /* if backing up */
00271   if ((Lspeed < 0) && (Rspeed < 0)) {
00272     idiff = Lspeed - Rspeed;
00273     if (idiff >= 0) { Lspeed += idiff; Rspeed -= idiff; }
00274     else { Rspeed += idiff; Lspeed -= idiff; }
00275   } else { /* going forward */
00276     /* if both sensors are active, flip a coin */
00277     if ((Detect & 0x0C) == 0x0C) {
00278       if (randmask(0x01)) { Rspeed -= 1; Lspeed += 1; }
00279       else { Lspeed -= 1; Rspeed -= 1; }
00280     } else {
00281       if (Detect & 0x08) { Rspeed -= 1; Lspeed += 1; }
00282       if (Detect & 0x04) { Lspeed -= 1; Rspeed += 1; }
00283     }
00284   }
00285 }