//This is a non-blocking function. It is called once on every iteration of the main loop if "white_line_flag" is ON //It reads the whiteline sensor values and determines what it should do next to stay on the white line void whiteline_follow_continue() { read_sensors(); flag=0; print_sensor_data(); if(Center_white_line<W_THRESHOLD_STOP && Left_white_line<W_THRESHOLD_STOP && Right_white_line<W_THRESHOLD_STOP ){ if (whiteline_stop_intersection_flag) { whiteline_follow_end(); send_char(SUCCESS); } /*else { forward(); velocity(100,100); stop_on_timer4_overflow = 1; start_timer4(); while (stop_on_timer4_overflow != 0) {;} } return;*/ } if( Front_IR_Sensor<0xF0) { stop(); buzzer_on(); } //Sensor config : 010 else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD) { forward(); velocity(150,150); black_flag = 0; buzzer_off(); } //Sensor config : 110 else if(Left_white_line < W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD) { forward(); velocity(120,150); black_flag = 0; buzzer_off(); } //Sensor config : 100 else if(Left_white_line < W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line > W_THRESHOLD) { PORTA = 0x05; velocity(50,130); last_on = LEFT_SENSOR; black_flag = 0; buzzer_off(); } //Sensor config : 011 else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line < W_THRESHOLD) { forward(); velocity(150,120); black_flag = 0; buzzer_off(); } //Sensor config : 001 else if(Left_white_line > W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line < W_THRESHOLD) { PORTA = 0x0A; velocity(130,50); last_on = RIGHT_SENSOR; black_flag = 0; buzzer_off(); } //Sensor config : 000 else { buzzer_off(); if(black_flag >= CONT_BLACK) { if(last_on == LEFT_SENSOR) motion_set(0x05); else if(last_on == RIGHT_SENSOR) motion_set(0x0A); velocity(100,100); while(1){ print_sensor_data(); read_sensors(); if(Center_white_line < W_THRESHOLD) break; } } black_flag = (black_flag < CONT_BLACK)?black_flag+1:CONT_BLACK; forward(); velocity(0,0); } }
//The main invoker routine. It takes as argument the next command to execute and does what is necessary //Self-explanatory code! void my_invoker (unsigned char command) { if(command == BUZZER_ON){ buzzer_on(); return; } else if(command == BUZZER_OFF){ buzzer_off(); return; } else if(command == MOVE_FORWARD) { forward(); //forward return; } else if(command == MOVE_BACKWARD) { back(); //back return; } else if(command == MOVE_LEFT) { left(); //left return; } else if(command == MOVE_RIGHT) { right(); //right return; } else if(command == STOP) { stop(); //stop return; } else if(command == SET_VELOCITY) { int numargs; unsigned char * ch = recieve_args(&numargs); //assert(numargs == 1); int velleft = (int)*(ch); int velright = (int)*(ch+1); velocity(velleft,velright); return; } else if(command == MOVE_BY) { int numargs; unsigned char * ch = recieve_args(&numargs); int pos_a = (int)*(ch); int pos_b = (int)*(ch+1); //int pos = 10; //while (pos_b--) pos *= 10; //pos *= pos_a; //forward_mm(pos); pos_a += (pos_b << 8); forward(); velocity(120,120); while (pos_a--) { //delay on 5 ms stop_on_timer4_overflow = 1; start_timer4(); while (stop_on_timer4_overflow != 0) {;} } stop(); send_char(SUCCESS); leftInt = 0; rightInt = 0; return; } else if(command == MOVE_BACK_BY) { int numargs; unsigned char * ch = recieve_args(&numargs); int pos_a = (int)*(ch); int pos_b = (int)*(ch+1); //int pos = 10; //while (pos_b--) pos *= 10; //pos *= pos_a; //forward_mm(pos); pos_a += (pos_b << 8); back(); velocity(120,120); while (pos_a--) { //delay on 5 ms stop_on_timer4_overflow = 1; start_timer4(); while (stop_on_timer4_overflow != 0) {;} } stop(); send_char(SUCCESS); leftInt = 0; rightInt = 0; return; } else if(command == TURN_LEFT_BY) { int numargs; unsigned char * ch = recieve_args(&numargs); already_stopped = 0; int pos_a = (int)*(ch); int pos_b = (int)*(ch+1); pos_a += (pos_b << 8); _delay_ms(500); left(); velocity(200,200); while (pos_a--) { //delay on 5 ms stop_on_timer4_overflow = 1; start_timer4(); while (stop_on_timer4_overflow != 0) {;} } stop(); send_char(SUCCESS); leftInt = 0; rightInt = 0; already_modified_stopped = 0; return; } else if(command == TURN_RIGHT_BY) { int numargs; unsigned char * ch = recieve_args(&numargs); //assert(numargs == 2); int pos_a = (int)*(ch); int pos_b = (int)*(ch+1); pos_a += (pos_b << 8); _delay_ms(500); right(); velocity(200,200); while (pos_a--) { //delay on 5 ms stop_on_timer4_overflow = 1; start_timer4(); while (stop_on_timer4_overflow != 0) {;} } stop(); send_char(SUCCESS); leftInt = 0; rightInt = 0; already_modified_stopped = 0; return; } else if(command == LCD_SET_STRING) { int numargs; unsigned char * ch = recieve_args(&numargs); int i =0; lcd_clear(); for(;i<numargs;i++) { lcd_wr_char(*(ch+i)); } return; } else if (command == SET_PORT){ int numargs; unsigned char * ch = recieve_args(&numargs); ; if (numargs != 2){ } int portnum = (int) *(ch); unsigned char value = (unsigned char) *(ch+1); setPort(portnum,value); } else if(command == GET_SENSOR_VALUE) { int numargs; unsigned char * ch = recieve_args(&numargs); ; if (numargs != 1){ } int sensornum = (int) *(ch); //setPort(portnum,value); getSensorValue(sensornum); } else if(command == GET_PORT) { int numargs; unsigned char * ch = recieve_args(&numargs); ; if (numargs != 1){ } int portnum = (int) *(ch); getPort(portnum); } else if (command == WHITELINE_FOLLOW_START) { whiteline_follow_start(); } else if(command == PRINT_STATE){ buzzer_on(); lcd_num(state); _delay_ms(1000); buzzer_off(); } else if (command == WHITELINE_FOLLOW_END) { whiteline_follow_end(); } else if (command == WHITELINE_STOP_INTERSECTION) { whiteline_stop_intersection_flag = 1; } else if(command == ACC_START) { acc_flag = 1; } else if(command == ACC_STOP) { acc_flag = 0; acc_modified_flag = 0; buzzer_off(); } else if(command == ACC_MODIFIED){ acc_modified_flag = 1; already_modified_stopped = 0; } else if(command == ACC_CHECK){ if (acc_modified_flag == 1 && already_modified_stopped == 1){ send_char((char)1); } else { char value = PORTA; if (value == 0) send_char((char)2); else send_char((char)0); } } else if (command == ENABLE_LEFT_WHEEL_INTERRUPT) { leftInt = 0; left_position_encoder_interrupt_init(); } else if (command == ENABLE_RIGHT_WHEEL_INTERRUPT) { rightInt = 0; right_position_encoder_interrupt_init(); } else if (command == GET_LEFT_WHEEL_INTERRUPT_COUNT) { send_int (leftInt); leftInt = 0; } else if (command == GET_RIGHT_WHEEL_INTERRUPT_COUNT) { send_int (rightInt); rightInt = 0; } else if (command == SET_TIMER) { int numargs; unsigned char * ch = recieve_args(&numargs); ; if (numargs != 1){ } int time = (int) *(ch); timer4_init2(time); } else if (command == DISCONNECT) { disconnect(); } else { //Error!!! Unrecognized Command buzzer_on(); _delay_ms(1000); buzzer_off(); } }