This Program Was Produced by The

/*********************************************

This program was produced by the

CodeWizardAVR V1.23.8d Standard

Automatic Program Generator

e-mail:

Project :

Version :

Date : 2/23/2005

Author : Chuck Barnett

Company : PurdueUniversity

Comments:

Chip type : ATmega32

Program type : Application

Clock frequency : 4.000000 MHz

Memory model : Small

External SRAM size : 0

Data Stack size : 512

*********************************************/

#include <mega32.h>

// Alphanumeric LCD Module functions

#asm

.equ __lcd_port=0x1B

#endasm

#include <lcd.h>

// Standard Input/Output functions

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <delay.h>

#define DIAGNOSE "SELF DIAGNOSE"

#define MFORWARD "MOVING FORWARD"

#define MREVERSE "REVERSING"

#define MLEFT "TURNING LEFT"

#define MRIGHT "TURNING RIGHT"

#define MIDLE "IDLE SERVOS"

#define VERSION "1.0 Beta"

#define LEFTEYE PINC.3

#define RIGHTEYE PINC.4

#define FOODSENSOR PINC.5

#define BUZZER PORTB.3

#define PWMPERIOD 18 //18ms

#define PWMPERIOD2 19 //18.5

#define LEFTLOOP 19

#define RIGHTLOOP 19

// Declare your global variables here

int i,j,temp,pulse, il;

int food_chk = 1;

int int_one = 0;

int int_two = 0;

int int_three = 0;

int int_four = 0;

int int_seq = 0;

char lcd_buffer[33];

char start_byte, stop_byte, done_byte, inputchar_one, inputchar_two, prev_one, prev_two;

//prototype functions

int getdistm(char *);

int getopcode(char *);

void dec2bin(int , char *);

int move_forward(int );

void move_reverse(int );

void rotate_right();

void rotate_left();

void idle_servo(int );

void self_diagnose();

void beep_me(int );

void seans_dance( );

void ircheck();

int move_obstacle(int );

int send_to_table(int );

int return_from_table(int );

void main(void)

{

// Declare your local variables here

int opcode, dist_m, dist_cm, dist_err, trvl_dist_cm;

char s, t;

char bin[9];

char bin2[9];

char opcode_bin[5];

char distm_bin[5];

char distcm_bin;

// Input/Output Ports initialization

// Port A initialization

// Func0=Out Func1=Out Func2=Out Func3=Out Func4=Out Func5=Out Func6=Out Func7=Out

// State0=0 State1=0 State2=0 State3=0 State4=0 State5=0 State6=0 State7=0

PORTA=0x00;

DDRA=0xFF;

// Port B initialization

// Func0=Out Func1=Out Func2=Out Func3=Out Func4=In Func5=In Func6=In Func7=In

// State0=0 State1=0 State2=0 State3=0 State4=T State5=T State6=T State7=T

PORTB=0x00;

DDRB=0x0F;

// Port C initialization

// Func0=Out Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=Out

// State0=0 State1=T State2=T State3=T State4=T State5=T State6=T State7=0

PORTC=0x00;

DDRC=0x81;

// Port C initialization

// Func0=Out Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=Out

// State0=0 State1=T State2=T State3=T State4=P State5=P State6=T State7=0

//PORTC=0x30;

//DDRC=0x81;

// Port D initialization

// Func0=In Func1=Out Func2=In Func3=In Func4=Out Func5=Out Func6=In Func7=In

// State0=T State1=0 State2=T State3=T State4=0 State5=0 State6=T State7=T

PORTD=0x00;

DDRD=0x32;

// Timer/Counter 0 initialization

// Clock source: System Clock

// Clock value: Timer 0 Stopped

// Mode: Normal top=FFh

// OC0 output: Disconnected

TCCR0=0x00;

TCNT0=0x00;

OCR0=0x00;

// Timer/Counter 1 initialization

// Clock source: System Clock

// Clock value: Timer 1 Stopped

// Mode: Normal top=FFFFh

// OC1A output: Discon.

// OC1B output: Discon.

// Noise Canceler: Off

// Input Capture on Falling Edge

TCCR1A=0x00;

TCCR1B=0x00;

TCNT1H=0x00;

TCNT1L=0x00;

OCR1AH=0x00;

OCR1AL=0x00;

OCR1BH=0x00;

OCR1BL=0x00;

// Timer/Counter 2 initialization

// Clock source: System Clock

// Clock value: Timer 2 Stopped

// Mode: Normal top=FFh

// OC2 output: Disconnected

ASSR=0x00;

TCCR2=0x00;

TCNT2=0x00;

OCR2=0x00;

// External Interrupt(s) initialization

// INT0: Off

// INT1: Off

// INT2: Off

MCUCR=0x00;

MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization

TIMSK=0x00;

// For 3.68Mhz

// USART initialization

// Communication Parameters: 8 Data, 1 Stop, No Parity

// USART Receiver: On

// USART Transmitter: On

// USART Mode: Asynchronous

// USART Baud rate: 300

//UCSRA=0x00;

//UCSRB=0x18;

//UCSRC=0x86;

//UBRRH=0x02;

//UBRRL=0xFE;

// For 4Mhz

// USART initialization

// Communication Parameters: 8 Data, 1 Stop, No Parity

// USART Receiver: On

// USART Transmitter: On

// USART Mode: Asynchronous

// USART Baud rate: 300

UCSRA=0x00;

UCSRB=0x18;

UCSRC=0x86;

UBRRH=0x03;

UBRRL=0x40;

// Analog Comparator initialization

// Analog Comparator: Off

// Analog Comparator Input Capture by Timer/Counter 1: Off

// Analog Comparator Output: Off

ACSR=0x80;

SFIOR=0x00;

// LCD module initialization

lcd_init(16);

temp = 5;

pulse=12; //number of pulse

// (one cycle)

// |-----| |

// | | | Drive Servo Left

// | |------|

// total time for each cycle= 2300us + 18.3ms ~=20ms (based around this value

// high low for testing)

// (adjust value)

il = 0;

prev_one = (char) il;

prev_two = (char) il;

il = 48;

done_byte = (char) il;

il = 170;

start_byte = (char) il; // initialize preamble byte

il = 85;

stop_byte = (char) il; // initialize conclusion byte

//Introduce itself :)

lcd_clear();

lcd_putsf("I,Robotic ");

lcd_gotoxy(6,1);

lcd_putsf("Waitress");

delay_ms(2000);

// Self diagnosis test

self_diagnose();

lcd_clear();

lcd_putsf("I,Robotic ");

lcd_gotoxy(6,1);

lcd_putsf("Waitress");

while(1){

// Place your code here

while (1)

{

/* receive the pre-abmble character */

s=getchar();

il = (int) s;

if (il == 170)

{

inputchar_one = getchar();

t = getchar();

il = (int) t;

/* echo data back if correct */

if (il == 85)

{

int_one = (int)prev_one;

int_three = (int)inputchar_one;

/* If its the same instruction as the previous don't do anything. */

if ( (int_one == int_three) & (int_seq == 0))

{

il = 0;

s = (char) il;

t = (char) il;

putchar(start_byte);

putchar(done_byte);

putchar(stop_byte);

}

else

{

prev_one = inputchar_one;

putchar(start_byte);

putchar(inputchar_one);

putchar(stop_byte);

int_seq = 1;

break;

}

else

{

int_one = (int)prev_one;

if ((int_one == 0))

{

il = 0;

s = (char) il;

t = (char) il;

putchar(start_byte);

putchar(s);

putchar(stop_byte);

}

else

{

il = 0;

s = (char) il;

t = (char) il;

putchar(start_byte);

putchar(done_byte);

putchar(stop_byte);

}

lcd_clear();

i = (int) inputchar_one;

dist_cm = (int) inputchar_one;

dec2bin(i, bin);

dec2bin(dist_cm, bin2);

for(il=0; il < 4; il++)

{

opcode_bin[il] = bin[il];

}

opcode_bin[il] = 0; // null terminate;

for(il=4; il < 8; il++)

{

distm_bin[il-4] = bin[il];

}

distm_bin[il-4] = 0; // null terminate;

//lcd_putsf(" opcd:");

for(il=0; il < 4; il++)

{

//lcd_putchar(opcode_bin[il]);

}

//lcd_putsf(" m:");

for(il=0; il < 4; il++)

{

//lcd_putchar(distm_bin[il]);

}

//lcd_putsf(" cm:");

for(il=0; il < 8; il++)

{

//lcd_putchar(bin2[il]);

}

dist_m = getdistm(distm_bin);

opcode = getopcode(opcode_bin);

if (dist_cm == 240)

{

lcd_clear();

lcd_putsf("Instr: Tab 1");

delay_ms(1000);

send_to_table(1);

int_seq = 0;

}

else if (dist_cm == 15)

{

lcd_clear();

lcd_putsf("Instr: Tab 2");

delay_ms(1000);

send_to_table(2);

int_seq = 0;

}

else if (dist_cm == 146)

{

lcd_clear();

lcd_putsf("Instr: Tab 3");

delay_ms(1000);

send_to_table(3);

int_seq = 0;

}

else if (dist_cm == 218)

{

lcd_clear();

lcd_putsf("Instr: Tab 4");

delay_ms(1000);

send_to_table(4);

int_seq = 0;

}

};

}

/* This function enables the robot to move in the reverse direction */

/* the number of centimeters specified as its argument. */

void move_reverse(int cm)

{

//lcd_clear();

//lcd_putsf(MREVERSE);

for (i=0;i<(cm*2);++i)

{

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1000);

PORTD.4=0;

delay_us(1000);

PORTD.5=0;

PORTD.4=0;

delay_ms(PWMPERIOD);

}

idle_servo(1);

delay_ms(1000);

}

/* This function enables the robot to move forward the number */

/* of centimeters specified as its argument. */

int move_forward(int cm)

{

char s, t, k, l;

int distance_moved =0;

int j, dist_cm, obs , dist_err;

//lcd_clear();

//lcd_putsf(MFORWARD);

obs =0;

// Forward Function

for (i=0;i<(cm*2);++i)

{

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1250);

PORTD.5=0;

delay_us(750); //was 1000

PORTD.5=0;

PORTD.4=0;

delay_ms(PWMPERIOD2);

distance_moved++;

//implement eye test here

if (food_chk == 1)

{

if (FOODSENSOR == 0)

{

lcd_clear();

lcd_putsf("Food missing from tray");

for (j = 0; j < 20; j++)

{

il = 255;

s = (char) il;

putchar(start_byte);

putchar(s);

putchar(stop_byte);

delay_ms(1000);

}

break;

//dist_cm = (distance_moved/2) % 100;

//dist_err = (4*16) + ((distance_moved/2)/100);

}

if(RIGHTEYE == 1 || LEFTEYE == 1)

{

lcd_clear();

lcd_putsf("OBSTACLE avoid now");

beep_me(2);

idle_servo(1);

delay_ms(1000);

obs = 1;

if ((move_obstacle((cm-(distance_moved/2)))) == 1)

{

for (j = 0; j < 20; j++)

{

il = 153;

s = (char) il;

putchar(start_byte);

putchar(s);

putchar(stop_byte);

delay_ms(1000);

}

break;

}

//***********************

}

idle_servo(1);

delay_ms(1000);

//distance_moved = distance_moved/2; //loop factor to distance travelled

return(obs); // 1 if obstacle 0 if no

}

/* This function makes the robot rotate to the right */

/* by an agle of 90 degrees */

void rotate_right()

{

// Rotate Right Function

//lcd_clear();

//lcd_putsf(MRIGHT);

for (i=0; i<RIGHTLOOP ;++i)

{

lcd_gotoxy(0,1);

//lcd_putsf("Angle: ");

//sprintf(lcd_buffer, "Angle: %2d", (i*7.5));

//itoa(i*9,lcd_buffer);

//lcd_puts(lcd_buffer);

//lcd_putsf("deg ");

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1000);

//PORTD.5=0;

delay_us(1000);

PORTD.5=0;

PORTD.4=0;

delay_ms(PWMPERIOD);

}

idle_servo(1);

delay_ms(2000);

}

/* This function makes the robot rotate to the right */

/* by an agle of 90 degrees */

void rotate_left()

{

// Rotate Left Function

//lcd_clear();

//lcd_putsf(MLEFT);

for (i=0; i<LEFTLOOP ;++i)

{

lcd_gotoxy(0,1);

//lcd_putsf("Angle: ");

//sprintf(lcd_buffer, "Angle: %2d", (i*7.5));

//itoa(i*9,lcd_buffer);

//lcd_puts(lcd_buffer);

//lcd_putsf("deg ");

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1000);

PORTD.4=0;

PORTD.5=0;

delay_us(1000);

PORTD.5=0;

PORTD.4=0;

delay_ms(PWMPERIOD);

}

idle_servo(1);

delay_ms(2000);

}

/* This function test whether the servos are precisely calibrated */

/* at idle i.e zero rotation at 1.5ms pulse width. */

void idle_servo(int mult) //every 1 is 1 second

{

//lcd_clear();

//lcd_putsf(MIDLE);

for (i=0;i<(50*mult);++i)

{

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1500);

PORTD.5=0;

PORTD.4=0;

delay_ms(18.5);

}

/* This function rusn through several diagnostic test:

1. Idling of servos

2. Tests the buzzer

3. Test the 3 IR sensors(2 obstacle and food) */

void self_diagnose()

{

lcd_clear();

lcd_putsf(DIAGNOSE);

delay_ms(1000);

lcd_clear();

lcd_gotoxy(0,0);

lcd_putsf("Diag: Servo Idl"); //test if servos idl

lcd_gotoxy(0,1);

lcd_putsf("1500"); //test if servos idle

for (i=0;i<(400);++i)

{

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1500);

PORTD.5=0;

PORTD.4=0;

delay_ms(18.5);

}

lcd_gotoxy(0,0);

lcd_clear();

lcd_putsf("Diag: Buzz"); //test buzzer

beep_me(2);

lcd_gotoxy(0,0);

lcd_clear();

lcd_putsf("Diag IR"); //test IR

for (i=0; i<150; i++)

{

if (RIGHTEYE == 1)

{

lcd_gotoxy(0,1);

lcd_putsf("R: 1");

}

else

{

lcd_gotoxy(0,1);

lcd_putsf("R: 0");

}

if (LEFTEYE == 1)

{

lcd_gotoxy(5,1);

lcd_putsf("L: 1");

}

else

{

lcd_gotoxy(5,1);

lcd_putsf("L: 0");

}

if (FOODSENSOR == 1)

{

lcd_gotoxy(10,1);

lcd_putsf("F: 1");

}

else

{

lcd_gotoxy(10,1);

lcd_putsf("F: 0");

}

delay_ms(100);

}

/* This function sounds the buzzer for the number of seconds

/* specified as its argument */

void beep_me(int seconds)

{

for (i=0; i<seconds;i++)

{

BUZZER = 1;

delay_ms(100);

BUZZER = 0;

delay_ms(400);

BUZZER = 1;

delay_ms(100);

BUZZER = 0;

delay_ms(400);

}

/* This function was used at the very initial stages of the project */

/* for testing purposes. Its no longer in use. */

void seans_dance()

{

delay_ms(2000);

//idle_servo(10);

// Reverse Function

move_forward(200);

move_reverse(20);

move_forward(20);

rotate_right();

delay_ms(2000);

rotate_left();

delay_ms(2000);

rotate_left();

delay_ms(2000);

lcd_clear();

lcd_putsf(MRIGHT);

rotate_right();

delay_ms(2000);

lcd_clear();

lcd_putsf("DANCE ROUTINE OK");

lcd_gotoxy(0,1);

lcd_putsf("SEAN ROCKS");

}

/* This function is a test for the IR sensors */

void ircheck()

{

while(1)

{

lcd_clear();

if (RIGHTEYE == 1 || LEFTEYE == 1)

{

lcd_gotoxy(0,0);

lcd_putsf("OBJECT DETECTED "); //test if servos idle

}

else

{

lcd_gotoxy(0,0);

lcd_putsf("OBJECT NOT DETECTED"); //test if servos idle

}

delay_ms(10);

}

/* This function is no longer used with the current communication protocl */

/* implementation. I was used in the initial communication protocl which */

/* required sending micro-instructions to the robot */

/* This function determines opcodes from binary bits. */

int getopcode(char *binary)

{

char zero[] = "0000";

char one[] = "0001";

char two[] = "0010";

char three[] = "0011";

char four[] = "0100";

char five[] = "0101";

char six[] = "0110";

char seven[] = "0111";

char eight[] = "1000";

char nine[] = "1001";

char ten[] = "1010";

char eleven[] = "1011";

char twelve[] = "1100";

char thirteen[] = "1101";

char fourteen[] = "1110";

char fifteen[] = "1111";

if (strcmp(binary, zero) == 0)

{

return 0;

}

else if (strcmp(binary, one) == 0)

{

return 1;

}

else if (strcmp(binary, two) == 0)

{

return 2;

}

else if (strcmp(binary, three) == 0)

{

return 3;

}

else if (strcmp(binary, four) == 0)

{

return 4;

}

else if (strcmp(binary, five) == 0)

{

return 5;

}

else if (strcmp(binary, six) == 0)

{

return 6;

}

else if (strcmp(binary, seven) == 0)

{

return 7;

}

else if (strcmp(binary, eight) == 0)

{

return 8;

}

else if (strcmp(binary, nine) == 0)

{

return 9;

}

else if (strcmp(binary, ten) == 0)

{

return 10;

}

else if (strcmp(binary, eleven) == 0)

{

return 11;

}

else if (strcmp(binary, twelve) == 0)

{

return 12;

}

else if (strcmp(binary, thirteen) == 0)

{

return 13;

}

else if (strcmp(binary, fourteen) == 0)

{

return 14;

}

else if (strcmp(binary, fifteen) == 0)

{

return 15;

}

else

{

return -1;

}

/* This function is no longer used with the current communication protocl */

/* implementation. I was used in the initial communication protocl which */

/* required sending micro-instructions to the robot */

/* Determine distance in meters from binary bits */

int getdistm(char *binary)

{

char zero[] = "0000";

char one[] = "0001";

char two[] = "0010";

char three[] = "0011";

char four[] = "0100";

char five[] = "0101";

char six[] = "0110";

char seven[] = "0111";

char eight[] = "1000";

char nine[] = "1001";

char ten[] = "1010";

char eleven[] = "1011";

char twelve[] = "1100";

char thirteen[] = "1101";

char fourteen[] = "1110";

char fifteen[] = "1111";

if (strcmp(binary, zero) == 0)

{

return 0;

}

else if (strcmp(binary, one) == 0)

{

return 1;

}

else if (strcmp(binary, two) == 0)

{

return 2;

}

else if (strcmp(binary, three) == 0)

{

return 3;

}

else if (strcmp(binary, four) == 0)

{

return 4;

}

else if (strcmp(binary, five) == 0)

{

return 5;

}

else if (strcmp(binary, six) == 0)

{

return 6;

}

else if (strcmp(binary, seven) == 0)

{

return 7;

}

else if (strcmp(binary, eight) == 0)

{

return 8;

}

else if (strcmp(binary, nine) == 0)

{

return 9;

}

else if (strcmp(binary, ten) == 0)

{

return 10;

}

else if (strcmp(binary, eleven) == 0)

{

return 11;

}

else if (strcmp(binary, twelve) == 0)

{

return 12;

}

else if (strcmp(binary, thirteen) == 0)

{

return 13;

}

else if (strcmp(binary, fourteen) == 0)

{

return 14;

}

else if (strcmp(binary, fifteen) == 0)

{

return 15;

}

else

{

return -1;

}

/* This function is no longer used with the current communication protocl */

/* implementation. I was used in the initial communication protocl which */

/* required sending micro-instructions to the robot */

/* This function accepts a decimal integer and returns a binary coded string */

void dec2bin(int decimal, char *binary)

{

int k = 0, n = 0;

int neg_flag = 0;

int remain;

char temp[9];

{

remain = decimal % 2;

decimal = decimal / 2; // whittle down decimal

temp[k++] = remain + 0x30; // makes characters 0 or 1

} while (decimal > 0);

while(k <8)

temp[k++] = '0';

k = 8;

while (k >= 0)

{

binary[n++] = temp[--k]; // reverse spelling

}

binary[n-1] = 0; // end with NULL

}

/* This function is used to circumnavigate an obstacle in the robots */

/* path of motion. */

int move_obstacle(int cm)

{

char s, t, k, l;

int distance_moved =0;

int dist_tmp, dist_err;

rotate_right();

// Forward Function

for (i=0;i<(20*2);++i)

{

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1250);

PORTD.5=0;

delay_us(750);

PORTD.5=0;

PORTD.4=0;

delay_ms(PWMPERIOD2);

distance_moved++;

//implement eye test here

if(RIGHTEYE == 1 || LEFTEYE == 1)

{

lcd_clear();

lcd_putsf("OBSTACLE again!!!!");

idle_servo(1);

delay_ms(1000);

move_reverse(distance_moved/2);

rotate_left();

beep_me(5);

return 1;

}

//***********************

}

idle_servo(1);

delay_ms(1000);

rotate_left();

distance_moved = 0;

// Forward Function

for (i=0;i<(cm*2);++i)

{

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1250);

PORTD.5=0;

delay_us(750);

PORTD.5=0;

PORTD.4=0;

delay_ms(PWMPERIOD2);

distance_moved++;

//implement eye test here

if(RIGHTEYE == 1 || LEFTEYE == 1)

{

lcd_clear();

lcd_putsf("OBSTACLE again!!!!");

idle_servo(1);

delay_ms(1000);

move_reverse(distance_moved/2);

rotate_right();

move_reverse(20);

rotate_left();

beep_me(5);

return 1;

}

idle_servo(1);

delay_ms(1000);

distance_moved =0;

rotate_left();

for (i=0;i<(20*2);++i)

{

PORTD.4=1; //left

PORTD.5=1; //right

delay_us(1250);

PORTD.5=0;

delay_us(750);

PORTD.5=0;

PORTD.4=0;

delay_ms(PWMPERIOD2);

distance_moved++;

//implement eye test here

if(RIGHTEYE == 1 || LEFTEYE == 1)

{

lcd_clear();

lcd_putsf("OBSTACLE again!!!!");

idle_servo(1);

delay_ms(1000);

move_reverse(distance_moved/2);

rotate_right();

move_reverse(cm);

rotate_right();

move_reverse(20);

rotate_left();

beep_me(5);

return 1;

}

idle_servo(1);

delay_ms(1000);

return 0;

}

/* This function uses preset maps to navigate to the tables specified from */

/* the control center. */

int send_to_table(int table)

{

if (table == 1)

{ //Map to table 1

lcd_clear();

lcd_putsf("Deliver:");

lcd_gotoxy(0,1);

lcd_putsf("Table 1");

move_forward(60);

rotate_left();

move_forward(30);

lcd_clear();

lcd_putsf("Remove Food");

beep_me(2);

delay_ms(3000);

if (FOODSENSOR == 0)

{

//return

food_chk = 0;

return_from_table(1);

food_chk = 1;

}

else

{

beep_me(10);

food_chk = 0;

return_from_table(1);

food_chk = 1;

}

else if (table == 2)

{ //Map to table 2

lcd_clear();

lcd_putsf("Deliver:");

lcd_gotoxy(0,1);

lcd_putsf("Table 2");

if (move_forward(120) == 0)

rotate_left();

move_forward(30);

lcd_clear();

lcd_putsf("Remove Food");

beep_me(2);

delay_ms(3000);

if (FOODSENSOR == 0)

{

//return

food_chk = 0;

return_from_table(2);

food_chk = 1;

}

else

{

beep_me(10);

food_chk = 0;

return_from_table(2);

food_chk = 1;

}

else if (table == 3)

{ //Map to table 3

lcd_clear();

lcd_putsf("Deliver:");

lcd_gotoxy(0,1);

lcd_putsf("Table 3");

move_forward(60);

rotate_right();

move_forward(30);

lcd_clear();

lcd_putsf("Remove Food");

beep_me(2);

delay_ms(3000);

if (FOODSENSOR == 0)

{

//return

food_chk = 0;

return_from_table(3);

food_chk = 1;

}

else

{

beep_me(10);

food_chk = 0;

return_from_table(3);

food_chk = 1;

}

else if (table == 4)

{ //Map to table 2

lcd_clear();

lcd_putsf("Deliver:");

lcd_gotoxy(0,1);

lcd_putsf("Table 4");

if (move_forward(120) == 0)

rotate_right();

move_forward(30);

lcd_clear();

lcd_putsf("Remove Food");

beep_me(2);

delay_ms(3000);

if (FOODSENSOR == 0)

{

//return

food_chk = 0;

return_from_table(4);

food_chk = 1;

}

else

{

beep_me(10);

food_chk = 0;

return_from_table(4);

food_chk = 1;

}

/* This function uses preset maps to navigate from the tables to a specified */

/* central location. */

int return_from_table(int table)

{

lcd_clear();

//Map back from table 1

if (table == 1)

{

rotate_left();

move_forward(30);

rotate_right();

move_forward(60);

rotate_left();

}

//Map back from table 2

else if (table == 2)

{

rotate_left();

move_forward(30);

rotate_right();

move_forward(120);

rotate_left();

}

//Map back from table 3

else if (table == 3)

{

rotate_right();

move_forward(30);

rotate_left();

move_forward(60);

rotate_left();

}

//Map back from table 4

else if (table == 4)

{

rotate_right();

move_forward(30);

rotate_left();

move_forward(120);

rotate_left();

}