int main(void) //Main program starts from here { double acc_Angle; int gyro_Angle; int filt_Angle; unsigned int pwm_value; init_adxl(); //Initialise accelerometer init_gyro(); //Initialise gyroscope init_devices1(); uart0_init(); //Initailize UART1 for serial communiaction start_timer4(); //Timer for timing calculations SetTunings(8.1,8,5); lcd_print(1,1,kp*10,4); lcd_print(1,6,ki*10,4); lcd_print(1,11,kd*10,4); while(1) { acc_Angle=0.1*acc_angle(); //Accelerometer angle gyro_Angle=gyro_Rate(); //Anugular rate from Gyroscope filt_Angle=comp_filter(acc_Angle,gyro_Angle); //Filtered angle after passing through Complementary filter Input=filt_Angle; //Input for error calculation of PID Compute(); //Calling PID if (Output>0) //Mapping PID output to velocity of motors { pwm_value = (Output+THRESHOLD); if(pwm_value>=255) { pwm_value=255; } set_PWM_value(pwm_value); forward(); } else if(Output<0) { pwm_value = (-Output)+THRESHOLD; if(pwm_value>=255) { pwm_value=255; } set_PWM_value(pwm_value); back(); } else if(Input==0) { stop(); } //_delay_ms(20); UDR0=0xFF; _delay_ms(1); UDR0=(uint8_t)(filt_Angle+100); _delay_ms(1); uint8_t op=(Output/2)+127; UDR0=op; } }
//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(); } }