File content as of revision 1:1da89c13dfa1:

// FORMAT_CODE_START
#include "QEI.h"
#include "mbed.h"
#include "robot.h"
#include "rtos.h"
#include "MPU6050.h"
#include "I2Cdev.h"
#include "tone.h"
#include "ultrasonic.h"
char c;
char buffer[10];
int bit_size=20;
int thetha1=300;
int thetha=0;
int stall=0;
int bump=1;
int UL_rR = 0;
int UL_R = 0;
int UL_F = 0;
int UL_L = 0;
int UL_rL = 0;
int UL_B = 0;
int  dx1=100;
int dy1=200;

int linear_velocity_value ;
int linear_velocity_direction;
int rotational_velocity_value ;
int rotational_velocity_direction;
int Lspeed=1;
int Rspeed=1;

int main()
{
    initRobot();
    //motor_control((100),(-100));
    while(buffer[3] != 'O') {
        while(buffer[3] != 'E') {
            if(bt.readable()) {
                bt.gets(buffer, 5);
                if(buffer[3] == 'E') {
                    bt.printf(">>1B");
                } else if(buffer[3] == '?') {
                    bt.printf("K");
                }
            }
        }
        if(bt.readable()) {
            bt.gets(buffer, 5);
        }
    }
     
    //imperial_march();
    Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL);
    Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL);
    buzzer=0;
    //motor_control(lMotor*m_speed,rMotor*m_speed*0.8);
    //motor_control(lMotor*m_speed,0);
    while(1) {
        //bt.lock();
        //bt.printf("LDR1_%lf;LDR2_%lf;Pulses is: %i  Revolutions is: %i Pulses is: %i  ultrasonic: , \r\n",ldrread1(),ldrread2(), left.getPulses(), left.getRevolutions(),right.getPulses());//, uL.read()*5*102.54*2.54);
        //bt.printf("s;LDR1_%lf;LDR2_%lf;Pulses is:  %i ; %i:s\n\r",ldrread1(),ldrread2(),wheel.getPulses(), wheel.getRevolutions());
        // motor_control(lMotor*m_speed,rMotor*m_speed);
        //bt.printf("a/g:\t %d:\t %d:\t %d:\t %d:\t %d:\t %d:\t\r\n",ax ,ay,az,gx,gy,gz);
        //bt.unlock();
        if(bt.readable()) {
            bt.gets(buffer, 5);
            if(buffer[3] == 'S') {
                SRX=1;
                wait_ms(2);
                UL_rR=sensorvalue(urR);
                SRX=0;
                wait_ms(2);
                
                SRX=1;
                wait_ms(2);
                UL_R=sensorvalue(uR);
                SRX = 0;
                wait_ms(2);
                
                SRX=1;
                wait_ms(2);
                UL_F=sensorvalue(uF);
                SRX=0;
                wait_ms(2);
                
                SRX=1;
                wait_ms(2);
                UL_L=sensorvalue(uL);
                SRX=0;
                wait_ms(2);
                
                SRX=1;
                wait_ms(2);
                UL_rL=sensorvalue(urL);
                SRX=0;
                wait_ms(2);
                
                SRX=1;
                wait_ms(2);
                UL_B=sensorvalue(uB);
                SRX=0;
                
                bt.lock();
                stdio_mutex.lock();
                heading=heading*11.375;
                bt.printf(">>D%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
                          dx/256,dx%256,dy/256,dy%256,\
                          (heading)/256,(heading)%256,\
                          stall, bump,\
                          UL_rR/256,UL_rR%256,\
                          UL_R/256,UL_R%256, \
                          UL_F/256,UL_F%256,\
                          UL_L/256,UL_L%256,\
                          UL_rL/256,UL_rL%256,\
                          UL_B/256,UL_B%256);
                dx=0;
                dy=0;
                stdio_mutex.unlock();
                bt.unlock();
            } else if (buffer[3] == 'R') {
                Thread::wait(20);
                if(bt.readable()) {
                    bt.gets(buffer, 10);
                    linear_velocity_value = buffer[4]<<8|buffer[3];
                    linear_velocity_direction= buffer[5];
                    rotational_velocity_value = buffer[7]<<8|buffer[6];
                    rotational_velocity_direction= buffer[8];
                    if( linear_velocity_direction==0x01) {
                        Lspeed=lMotor;
                        Rspeed=rMotor;
                    } else if( linear_velocity_direction==0x10) {
                        Lspeed=-lMotor;
                        Rspeed=-rMotor;
                    }
                    if( rotational_velocity_direction==0x01 && linear_velocity_value !=0) {
                        
                       
                        motor_control((Lspeed*linear_velocity_value/35)-10,(Rspeed*linear_velocity_value/35) + 10);
                        
                    } else if( rotational_velocity_direction==0x10 && linear_velocity_value !=0) {
                      motor_control((Lspeed*linear_velocity_value/35)+10,(Rspeed*linear_velocity_value/30) -10);
                    }
                    else
                      motor_control((Lspeed*linear_velocity_value/30),(Rspeed*linear_velocity_value/30));
                    
                }
            }
            //memset(buffer, 0, sizeof buffer);
        }

        if(ldrread1()>0.6) {
        }
        if(ldrread2()>0.6) {
            Led_on();
        } else {
            Led_off();
        }
        Thread::wait(20);
    }
}