/* vac.c vacuum robot control program. This program is compiled using the Linux shareware C2C compiler available at: http://www.geocities.com/SiliconValley/Network/3656/ The output .asm code is assembled with the Linux freeware assembler gpasm avaiable at: http://reality.sgi.com/jamesb/gpasm/ MPASM can also be used. **Note: I had to make some small modifications to gpasm so it would work with the 16C74A chip. For reference here is the makefile for this project -------------------------------------------------------- #vac.c make file all: vac.hex # Assemble the program from the asm source vac.hex: vac.asm gpasm -p 16c74 vac.asm # # Generate the asm source file from the C source . vac.asm: vac.c c2c -vs0x20 -Ip16c74.inc -ovac.asm vac.c # # # Clean up the directory clean: rm -f vac.hex core vac.asm vac.lst # pgm: tm4 -c 16c74 -z 3ffa vac.hex ----------------------------------------------------------- */ #define FOSC 8000000 #pragma CLOCK_FREQ 8000000 #include "16c74.h" //#define TMR0DIV 256-(FOSC/(16000*16)) //Compiler will not do this math! //Divider constant for 16,000 interrupts per second //Timer 0 is running at 500K counts/sec //Calculated value is 224.75 but I had to set it at 227 to get correct timing. #define TMR0DIV 227 //4000 interrupts per sec. //#define TMR0DIV 136 #define BAUD 2400 //#define BAUDDIV (FOSC/(BAUD*64)) - 1 #define BAUDDIV 51 /* these values written into ADCCON0 set the A-D channel mux address and converison clock to 1/32 osc. This makes a 32uS converson time*/ #define AD_LMFB 0x99 /* channel 3 */ #define AD_RMFB 0x91 /* channel 2 */ #define AD_SPEED 0x81 /* channel 0 */ #define AD_TRACK 0x89 /* channel 1 */ #define AD_LINEFIND 0xa1 /* ch 4 */ #define AD_DRIVE_CURRENT 0xa9 /* ch 5 */ #define AD_DRIVE_BATTERY 0xb1 /* ch 6 */ #define AD_VACUUM_BATTERY 0xb9 /* ch 7 */ #define STALL_CURRENT 19 /* 370 MA ( X/256 * 5 ) /* Analog inputs */ char speed; char track; char rmfb; char lmfb; char mode; char left_detect; char right_detect; char vacuum_battery; char drive_battery; char drive_current; char right_speed,right_prm; char left_speed,left_prm; char bot_mode; char bump_sw; char IR_sens; #define MODE_STOP 0 #define MODE_VACUUM 1 #define MODE_CENTER 2 #define MODE_ROTATE_L 3 #define MODE_ROTATE_R 4 #define MODE_TRACKLINE_RAMP_UP 5 #define MODE_TRACKLINE 6 #define MODE_GOHOME 7 #define MODE_FREE_ROTATE_R 0x24 #define MODE_BACK_OUT 0x25 #define MODE_ROTATE_BACK_OUT 0x26 #define MODE_BACKUP_AND_TURN_AROUND 27 #define MODE_BACK_OUT_RIGHT 0x29 #define MODE_BACK_OUT_LEFT 0x2a #define MODE_FIND_CLEAR_PATH_L 0x2b #define MODE_FIND_CLEAR_PATH_R 0x2c #define MODE_FIND_CLEAR_PATH 0x2d #define MODE_GO_STRAIGHT 0x2e #define MODE_FWD 0x2f #define MODE_CHAIR_A 0x30 #define MODE_CHAIR_B 0x31 #define MODE_CHAIR_C 0x32 #define MODE_SPIN 0xfe #define MODE_TEST 0xff #define FWD1 0xec #define FWD2 0xde #define FWD3 0xd8 #define FWD4 0xd0 #define REV1 20 #define REV2 34 #define REV3 40 #define REV4 48 char prescale; char timer; char prm_ctl; /* bits in prm_ctl */ #define R_fwd 0 #define L_fwd 1 char workB; #define FwdLeft 7 /*Left Motor pulse forward*/ #define RevLeft 6 /*Left Motor pulse reverse*/ #define FwdRight 5 /*Right Motor pulse forward */ #define RevRight 4 /*Right MOtor pulse reverse */ //Port_B Bit masks #define BS_L 1 /*Left bump sensor */ #define BS_T 4 /* Top bump switch */ #define BS_R 8 /*Right bump sensor */ //PORTC bit masks #define Vac_ON 1 /*Output, high true, Vac motor ON */ //Port_D Sensor bit masks #define OBJ_L 1 /*Left IR sensor (low true) */ #define OBJ_C 2 /* center IR sensor (low true) */ #define OBJ_R 4 /*Right IR sensor (low true) */ #define BI_L 8 /*Left Beam Interrupted (high true) */ #define BI_R 0x10 /*Right Beam Interrupted (high true) */ #define Beacon_R 0x20 /*Right Beacon (low true) */ #define Beacon_L 0x40 /*Left Beacon (low true) */ #define UndefinedBit 0x80 #define P_width 22 /*Motor pulse width in interrupt ticks*/ char r_speed; char l_speed; char speed_to_pulse_rate(char speed); char crcH,crcL; char tx_ptr,rx_ptr,txstate,rxstate; char timer2,limit_timer; char mode_bits; char runTimer_H; /* measures run time in seconds */ char runTimer_L; char rnd; /*random number*/ char ctr; /* a counter */ char temp; char magic; char tx_buf[34]@0xa8; /* Tx serial port buffer */ char rx_buf[34]@0xca; /* Rx serial port buffer */ #define COUNT 1 /* offset to count field in tx/rx buffers */ #define ADDRESS 0 /* offset to address field in tx/rx buffers */ #define DATA 2 /* offset to start of data in tx/rx buffers */ #define DIR 0 /* Direction indicator bit 1=right 0=left */ char test_hi@0xfe; char detect_line(); //------------------------------------------------------------------ /* Interrupts from Timer 0 occur at about 16,000 per second. */ void interrupt() { char R_pulse,L_pulse; static char divide10; static char divide100; if ((INTCON & (1 << T0IF))) { //Make sure it's a TMR0 interrupt! clear_bit(INTCON,T0IF); //Clear timer 0 interrupt flag TMR0 = TMR0DIV + TMR0; //restart timer 0 if(prescale-- == 0){ prescale = 160; if(timer != 0) timer--; //10 ms pertick timer if(timer2 != 0) timer2--; if(divide10++ >= 10){ if(limit_timer != 0) limit_timer--; //100ms per tick timer divide10 = 0; } if(divide100++ >= 100){ // 1 second per tick timer runTimer_L++; if(runTimer_L == 0) runTimer_H++; divide100 = 0; //After 4 minutes start looking for beacon if((runTimer_L == 240) && runTimer_H == 0){ mode = MODE_GOHOME; } //After 6 minutes stop everything if((runTimer_L >= 0x68) && (runTimer_H >= 1)){ mode = MODE_STOP; } } } PORTB = workB; //output the prm motor control bits to port B workB = 0; /* adjust right pulse rate modulator */ if (right_prm < P_width) { if (prm_ctl & (1 << R_fwd)) set_bit(workB,FwdRight); else set_bit(workB,RevRight); } if (right_prm-- == 0) { right_prm = speed_to_pulse_rate(right_speed) + P_width; if (right_speed > 127) clear_bit(prm_ctl,R_fwd); else set_bit(prm_ctl,R_fwd); } /*adjust left pulse rate modulator */ if (left_prm < P_width) { if (prm_ctl & (1 << L_fwd)) set_bit(workB,FwdLeft); else set_bit(workB,RevLeft); } if (left_prm-- == 0) { left_prm = speed_to_pulse_rate(left_speed) + P_width; if (left_speed > 127) clear_bit(prm_ctl,L_fwd); else set_bit(prm_ctl,L_fwd); } } if(bot_mode == 0) workB = 0; //Stop the bot. //Make a new random number if (rnd & 0x80) { //rnd msb is 1 asm{ movlw 1ch ; xorwf _rnd,F; bsf STATUS,C; rlf _rnd; //8 bit left shift } } else { //rnd msb is 0 asm{ bcf STATUS,C; rlf _rnd; //8 bit left shift } } } //---------------------------------------------------------------------- /*send character if tx ready and return true else return false if tx not ready */ char tx_byte(char c){ if (PIR1 & (1 << TXIF)) { TXREG = c; return 1; } else return 0; } //----------------------------------------------------------------------- /* compute 16 bit CRC on 1 data byte. crcH and crcL are global variables which accumulate the 16 bit CRC. They must be cleared before starting to compute 16 bit CRC on a string. The final output value will be in crcH and crcL. */ void crc(char data){ char i; short crc16; for (i=0;i<8;i++) { //Do all 8 bits in the data byte if (data & 0x80) crcH ^= 0x80; //if data bit is 1 then flip crc msb data = data << 1; // get next data bit if (crcH & 0x80) { //crc msb is 1 asm{ bcf STATUS,C; rlf _crcL; //16 bit left shift rlf _crcH; movlw 80h; xorwf _crcH,F; //xor with 0x8005 movlw 5; xorwf _crcL,F; } } else { //crc msb is 0 asm{ bcf STATUS,C; rlf _crcL; //16 bit left shift rlf _crcH; } } } } //----------------------------------------------------------------------- void set_tx_buffer_crc(){ int j = tx_buf[COUNT + 0] + 2; //get data size int i; crcH = 0; //Clear crc16 crcL = 0; for (i=0;i 127) { //See if speed is negative ! return speed & 0x7f; } else { return speed ^ 0x7f; } } //---------------------------------------------------------------------- char ad_convert(char channel) { char x; ADCON0 = channel; for (x=8;x>0;x--); //wait at least 20 us for data to stabilize set_bit(ADCON0,GO_DONE); //start A-D conversion while (ADCON0 & (1 << NOT_DONE)); //wait for conversion to complete return ADRES+128; //Return signed result } //--------------------------------------------------------------------- /* Saturating addition. Result is clipped at +127 or -128 instead of overflowing. Note the parameters a and b and signed char values (-128..+127) but the compiler thinks they are unsigned values (0..255) so I had to do some strange looking things below. */ /* char addsat(char a, char b) { char r; if (a > 127) goto a_minus; //See if a is minus if (b > 127) goto a_b_diff; //a is plus, check b for minusness r = a + b; if (r > 127) r = 127; //clip at +127 return r; a_minus: if (b < 0x80) goto a_b_diff; r = a + b; if (r < 0x80) r = 0x80; //clip at -128 return r; a_b_diff: return a + b; //No clipping needed } */ //--------------------------------------------------------------------- /* return the absolute value of x */ /* char abs(char x){ if(x > 127){ x ^= 0xff; x++; } return x; } */ //----------------------------------------------------------------------- /* return the negative of x */ /* char neg(char x){ x ^= 0xff; x++; return x; } */ //---------------------------------------------------------------------- /* return x if its 0 or less else return 0 */ /* char clip_pos(char x){ if(x < 128) x = 0; return x; } */ //---------------------------------------------------------------------- /* return x if it's 0 or higher else return 0 */ /* char clip_neg(char x){ if(x > 127) x = 0; return x; } */ //--------------------------------------------------------------------- /* char motors_stopped(){ if((abs(rmfb) < 8) && (abs(lmfb < 8))) return 1; else return 0; } */ //---------------------------------------------------------------------- char integrate(char speed,char mfb,char integ) { speed = speed + 128; //convert to unsigned mfb = mfb + 128; if (speed > mfb) { integ++; if (integ == 0x80) integ--; //Clip at +127 return integ; } else { integ--; if (integ == 0x7f) integ++; //Clip at -128 return integ; } } //--------------------------------------------------------------------- /* void track_line(char speed){ r_speed = addsat(speed,track); l_speed = addsat(speed, 0 - track); } */ //---------------------------------------------------------------------- //Return true if Beacon can be seen and bot in in MODE_GOHOME char beacon_ahead(){ if(mode != MODE_GOHOME) return 0; if(IR_sens & (Beacon_L | Beacon_R)) return 1; else return 0; } //----------------------------------------------------------------------- char bump_switch(){ if(bump_sw & BS_R){ //Right bump sensor input if((rnd & 3) == 0) set_bit(mode_bits,DIR); // 1 out of 4 probability go right instead else clear_bit(mode_bits,DIR); // 3/4 probabilty left turn after backing up bot_mode = MODE_BACK_OUT; timer = 64 + (rnd & 63); //This determines how far we back up return 1; } if(bump_sw & BS_L){ //Left bump sensor input if((rnd & 3) == 0) clear_bit(mode_bits,DIR); // 1/4 probability left turn else set_bit(mode_bits,DIR); //3/4 probability right turn after backing up bot_mode = MODE_BACK_OUT; timer = 64 + (rnd & 63); //This determines how far we back up return 2; } if((bump_sw & BS_T) //Top bump Switch || (IR_sens & (BI_L | BI_R))){// or Beam interruption if(rnd & 1){ //Random left or right turn after backing up bot_mode = MODE_BACK_OUT_LEFT; }else{ bot_mode = MODE_BACK_OUT_RIGHT; } timer = 255; //determines backup distance return 3; } return 0; } //----------------------------------------------------------------------- void sensor_action_home(char IR_mask){ char IR; IR = IR_sens & IR_mask; if(IR_sens & (Beacon_L | Beacon_R)){ if(bump_sw & (BS_R | BS_L)){ //Front bump switches bot_mode = MODE_CHAIR_A; timer = 96 + (rnd & 63); //This determines how far we back up return; } if((bump_sw & BS_T) //Top bump Switch || (IR & (BI_L | BI_R))){// or Beam interruption bot_mode = MODE_CHAIR_A; timer = 255; //back up farther return; } }else{ if(bump_switch()) return; //exit now if a switch was hit } if(IR & OBJ_L ){ //Left IR sensor set_bit(mode_bits,DIR); //Spin right bot_mode = MODE_FIND_CLEAR_PATH; timer = 20; //Allow motors time to reverse return; } if(IR & OBJ_R){ //Right IR sensor clear_bit(mode_bits,DIR); //Spin left bot_mode = MODE_FIND_CLEAR_PATH; timer = 20; //Allow motors time to reverse return; } if(!(IR_sens & (Beacon_L | Beacon_R))){ if(IR & OBJ_C){ //Center IR sensor if(rnd & 1) set_bit(mode_bits,DIR); //Random spin Direction else clear_bit(mode_bits,DIR); bot_mode = MODE_FIND_CLEAR_PATH; timer = 20; return; } } } //---------------------------------------------------------------------- /* read sensors and change mode accordingly */ void sensor_action(char IR_mask){ char IR; IR = IR_sens & IR_mask; if(bump_switch()) return; if(IR & OBJ_L ){ //Left IR sensor set_bit(mode_bits,DIR); //Spin right bot_mode = MODE_FIND_CLEAR_PATH; timer = 20; //Allow motors time to reverse return; } if(IR & OBJ_R){ //Right IR sensor clear_bit(mode_bits,DIR); //Spin left bot_mode = MODE_FIND_CLEAR_PATH; timer = 20; //Allow motors time to reverse return; } if(IR & OBJ_C){ //Center IR sensor if(rnd & 1) set_bit(mode_bits,DIR); //Random spin Direction else clear_bit(mode_bits,DIR); bot_mode = MODE_FIND_CLEAR_PATH; timer = 20; return; } } //----------------------------------------------------------------------- /* 360 degrees in 7 seconds. */ void set_spin_dir(){ if(mode_bits & (1 << DIR)){ r_speed = REV3; //Spin right l_speed = FWD1; }else{ l_speed = REV3; //Spin Left r_speed = FWD1; } } //----------------------------------------------------------------------- /* void push_mode(){ mode_stack[msp] = bot_mode; msp++; } void pop_mode(){ msp--; bot_mode = mode_stack[msp]; } */ //--------------------------------------------------------------------- //Return TRUE if motor current exceeds stall value char stall_check(){ if((drive_current >= STALL_CURRENT) && (timer2 == 0)){ timer2 = 60; return 1; } return 0; } //---------------------------------------------------------------------- //Reverse spin direction bit if stall is detected //returns true if stall detected. char counter_stall(){ if(stall_check()) { mode_bits = mode_bits ^ (1 << DIR); return 1; } return 0; } //---------------------------------------------------------------------- char steer_to_beacon(){ char IR; IR = IR_sens & (Beacon_L | Beacon_R); if(mode != MODE_GOHOME) return 0; if(Beacon_L & IR) { //Turn left r_speed = FWD3; l_speed = FWD1; } if(Beacon_R & IR){ //Turn right r_speed = FWD1; l_speed = FWD3; } if((Beacon_L & IR) && (Beacon_R & IR_sens)){ r_speed = speed; // Head for beacon l_speed = speed; if(IR_sens & OBJ_C) //Found homing beacon wall bot_mode = MODE_STOP; } if((Beacon_L & IR) || (Beacon_R & IR)){ timer = 100; //Don't time-out if we see beacon ctr = (rnd & 3) + 9; if(stall_check()){ //Check for stall bot_mode = MODE_BACK_OUT; timer = 255; } } return IR; } //---------------------------------------------------------------------- main() { char i; char minus_track; char l_integrate; char r_integrate; short crc16; test_hi = 0; INTCON = 0; STATUS = 0; PORTA = 0; PORTB = 0; PORTC = 0; T1CON = 0; TMR0 = 0; workB = 0; mode_bits = 0; tx_ptr = 0; rx_ptr = 0; RCSTA = 0x90; //SPEN + CREN (configure serial port) set_bit(STATUS,RP0); //Bank1 set_tris_b(0x0d); //Port B bits 0,2,3 are sensor inputs set_tris_c(0xc0); //Port C all outputs except 6:7 are UART pins TRISA = 0xff; //port A all analog inputs TRISD = 0xff; //port D all inputs TRISE = 7; //Port E inputs (analog) OPTION_REG = 0x01; //Prescaler 1:4 assigned to Timer 0, pullups on portB ADCON1 = 0; //Set all port A to analog with VREF=VCC TXSTA = 0xa2; //Set tx usart status/ctrl reg= CSRC + TXEN + TRMT SPBRG = BAUDDIV; //Set baudrate clear_bit(STATUS,RP0); //Bank0 clear_bit(INTCON,T0IF); //Clear Timer 0 interrupt flag TMR0 = TMR0DIV; //Start Timer 0 set_bit(INTCON,T0IE); //Enable Timer 0 interrupts set_bit(INTCON,GIE); //Enable global interrupts left_speed = 0; right_speed = 0; timer = 20; //200 ms while(timer > 0); //Wait for everything to stabilize if(magic != 0xd5){ timer = 100; timer2 = 3; ctr = 5; limit_timer = 0; runTimer_L = 0; runTimer_H = 0; rnd = 0xcc; magic = 0xd5; if(~PORTB & BS_T){ mode = MODE_GOHOME; //Hold top bump switch on at power up to while(~PORTB & BS_T){};//force into GOHOME mode for debug. }else mode = MODE_VACUUM; } PORTC = PORTC | Vac_ON ; //Vacuum motor on make_info_pkt(); send_tx_buffer(); //Send init data bot_mode = MODE_GO_STRAIGHT; clear_bit(mode_bits,DIR); //Left DIR is default while (1) { set_bit(PORTC,2); //Set portc bit 0 for timing testing bump_sw = ~PORTB & (BS_L | BS_R | BS_T); //Get bump switches IR_sens = ~PORTD ^ (BI_L | BI_R); //IR Sensors IR_sens = IR_sens & ~UndefinedBit; speed = ad_convert(AD_SPEED); //get speed value from speed pot track = ad_convert(AD_TRACK); //get balance from L-R balance pot /* unsigned A to D conversion values */ vacuum_battery = ad_convert(AD_VACUUM_BATTERY) + 128; //6V Vacuum Battery voltage, unsigned drive_battery = ad_convert(AD_DRIVE_BATTERY) + 128; //24V Drive motor battery voltage drive_current = ad_convert(AD_DRIVE_CURRENT) + 128; // Drive motor current 1v/amp /* if(battery < 136){ battery = ad_convert(AD_BATTERY); //Check it again to be sure if(battery < 136){ bot_mode = 0; //Stop motors if Battery < 8 volts !!! PORTB = 0; } } */ clear_bit(PORTC,2); //clear timing test bit switch(bot_mode){ case MODE_VACUUM: //Roam around and vacuum the room bot_mode = mode; //refresh the mode in case clock changed it set_spin_dir(); //Spin counter_stall(); //reverse spin if stall detected sensor_action(0xff); if(timer == 0){ if(ctr > 0) { ctr--; timer = 100; } } if((timer == 0) && (ctr == 0)){ // Time-out after spinning timer = 100; // and go straight ctr = (rnd & 7) + 3; bot_mode = MODE_GO_STRAIGHT; } break; case MODE_GOHOME: //Attempt to find the homing beacon bot_mode = mode; //refresh the mode in case clock changed it set_spin_dir(); //Spin if no beacon counter_stall(); //reverse spin if stall detected if(IR_sens & (Beacon_L | Beacon_R)){ timer = 0; ctr = 0; } /* temp = steer_to_beacon(); if(temp == 0) sensor_action_home(0xff); if(temp == Beacon_L) sensor_action_home(OBJ_R | BI_L | BI_R); if(temp == Beacon_R) sensor_action_home(OBJ_L | BI_L | BI_R); if(temp == (Beacon_L | Beacon_R)) sensor_action_home( BI_L | BI_R); */ if(timer == 0){ if(ctr > 0) { ctr--; timer = 100; } } if((timer == 0) && (ctr == 0)){ // Time-out after spinning more than 360 timer = 100; // and not finding the beacon ctr = (rnd & 3) + 3; bot_mode = MODE_GO_STRAIGHT; } break; case MODE_GO_STRAIGHT: r_speed = speed; //Else go straight for a while l_speed = speed; temp = steer_to_beacon(); //Rets zero if not "MODE_GOHOME" //else returns beacon bits if(mode == MODE_GOHOME){ if(temp == 0) sensor_action_home(0xff); if(temp == Beacon_L) sensor_action_home(OBJ_R | BI_L | BI_R); if(temp == Beacon_R) sensor_action_home(OBJ_L | BI_L | BI_R); if(temp == (Beacon_L | Beacon_R)) sensor_action_home( BI_L | BI_R); }else sensor_action(0xff); if(stall_check()){ //Check for stall bot_mode = MODE_BACK_OUT; timer = (rnd & 63) + 192; } if(timer == 0){ if(ctr > 0) { ctr--; timer = 100; } } if((timer == 0) && (ctr == 0)){ if(mode == MODE_GOHOME) ctr = (rnd & 3) + 6; //MODE_GOHOME spin value else ctr = (rnd & 3) + 2; //MODE_VACUUM spin value timer = 100; bot_mode = mode; //Return to main mode } break; case MODE_FIND_CLEAR_PATH: set_spin_dir(); if(beacon_ahead()){ if(IR_sens & (OBJ_L | OBJ_R)) timer = (rnd & 3) + 1; //10 - 40 ms }else{ if(IR_sens & (OBJ_L | OBJ_C | OBJ_R)) timer = (rnd & 63) + 15; //150 to 780ms } if(counter_stall()) { timer = (rnd & 0x07) + 1; } if(timer == 0){ bot_mode = MODE_GO_STRAIGHT; timer = 100; ctr = (rnd & 3) + 3; //random 4 sec to 7 sec time to go straight } break; case MODE_FREE_ROTATE_R: r_speed = REV2; l_speed = FWD2; if(timer == 0){ bot_mode = mode; timer = 50; } break; case MODE_BACK_OUT_LEFT: set_bit(mode_bits,DIR); bot_mode = MODE_BACK_OUT; break; case MODE_BACK_OUT_RIGHT: clear_bit(mode_bits,DIR); bot_mode = MODE_BACK_OUT; break; case MODE_BACK_OUT: r_speed = REV2; l_speed = REV2; if(stall_check()){ timer = 30; bot_mode = MODE_FWD; } if(timer == 0){ bot_mode = MODE_ROTATE_BACK_OUT; timer = 16 + (rnd & 0x1f); //random angle rotate } break; case MODE_FWD: r_speed = FWD1; l_speed = FWD1; if(timer == 0){ bot_mode = MODE_ROTATE_BACK_OUT; timer = 16 + (rnd & 0x1f); //random angle rotate } break; case MODE_ROTATE_BACK_OUT: set_spin_dir(); //Rotate left or right depending on DIR bit if(stall_check()) timer = 0; if(timer == 0){ bot_mode = MODE_FIND_CLEAR_PATH; timer = 6; } break; case MODE_CHAIR_A: r_speed = REV2; l_speed = REV2; if(stall_check()){ timer = 30; bot_mode = MODE_FWD; } if(timer == 0){ bot_mode = MODE_CHAIR_B; timer = 80 + (rnd & 63); //Time for 40 - 75 deg rotation } break; case MODE_CHAIR_B: r_speed = FWD2; //Turn left 60 deg l_speed = REV2; if(stall_check()){ timer = 250; bot_mode = MODE_FIND_CLEAR_PATH; set_bit(mode_bits,DIR); //Turn right and find clear path } if(IR_sens & OBJ_L){ timer = 10; bot_mode = MODE_FIND_CLEAR_PATH; set_bit(mode_bits,DIR); //Turn right and find clear path } if(timer == 0){ bot_mode = MODE_CHAIR_C; timer = 100; ctr = 4; //go about 2.5 ft. } break; case MODE_CHAIR_C: r_speed = FWD2; //Go straight ahead l_speed = FWD2; if(stall_check()){ timer = 250; bot_mode = MODE_FIND_CLEAR_PATH; set_bit(mode_bits,DIR); //Turn right and find clear path } bump_switch(); if(timer == 0){ if(ctr > 0) { ctr--; timer = 100; } } if((timer == 0) && (ctr == 0)){ bot_mode = MODE_GOHOME; timer = 100; ctr = (rnd & 3) + 6; //MODE_GOHOME spin value set_bit(mode_bits,DIR); //Turn right while seeking beacon } break; case MODE_TEST: r_speed = speed; l_speed = speed; break; case MODE_SPIN: r_speed = FWD2; l_speed = REV2; break; case MODE_STOP: l_speed = 0; r_speed = 0; PORTC = PORTC & ~Vac_ON ; //Vacuum motor off break; }//End switch for (i=0;i<3;i++) { //Adjust motor speeds based on feedback values rmfb = ad_convert(AD_RMFB); //get right motor feedback lmfb = ad_convert(AD_LMFB); //get left motor feedback r_integrate = integrate(r_speed,rmfb,r_integrate); l_integrate = integrate(l_speed,lmfb,l_integrate); right_speed = r_integrate; left_speed = l_integrate; } if (txstate == 0) { make_info_pkt(); //Get new data for transmission } send_tx_buffer(); //Send data on RF link if TX ready } } /* End of source */