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