//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();
}
}
