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 }