File content as of revision 3:d1197b5ea68a:

// 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"
#include "bt_shell.h"
int Current_Right_pulse=0;
int Current_Left_pulse=0;
int Linput=0;
int Rinput=0;
int Change_Right_pulse=0;
int Change_Left_pulse=0;
int Previous_Left_pulse=0;
int Previos_Right_pulse=0;
int time_int = 50;
int time_factor=1000/time_int ;
int Error_Right=0;
int Error_Left=0;
float L_Kp=0.1;
float R_Kp=0.1;
int previous_Linput= 0 ;
int previous_Rinput= 0 ;
RtosTimer *Motor_controller_Timer;

void Motorcontroller(void const *args)
{

    Current_Right_pulse= right.getPulses()/5;
    Current_Left_pulse=left.getPulses()*(-1)/5;

    Change_Right_pulse=Current_Right_pulse- Previos_Right_pulse;
    Change_Left_pulse=Current_Left_pulse- Previous_Left_pulse;

    Previous_Left_pulse=Current_Left_pulse;
    Previos_Right_pulse=Current_Right_pulse;

    Error_Right=(Rmotor_speed- (Change_Right_pulse*time_factor) );
    Error_Left=(Lmotor_speed- (Change_Left_pulse*time_factor));
    Linput=(Linput+Error_Left*L_Kp);
    Rinput=(Rinput+Error_Right*R_Kp);
    if(Linput>100)
        Linput=100;
    else if (Linput<-100)
        Linput= -100;
    else if (-11<Linput && Linput<11)
        Linput= 0;
    if(Rinput>100)
        Rinput=100;
    else if(Rinput<-100)
        Rinput= -100;
    else if (-11<Rinput && Rinput<11)
        Rinput= 0;
    if (Linput== previous_Linput && Rinput ==previous_Rinput) {
    } else {
        previous_Linput= Linput ;
        previous_Rinput= Rinput ;
        motor_control(-Rinput,-Linput);
    }
    /*
    bt.lock();
    bt.printf(">>D;%d;%d;%d;%d;%d;%d;%d;%d;\r\n",
    Rmotor_speed,Lmotor_speed,Error_Right ,Error_Left,\
    Rinput,Linput,Change_Right_pulse,Change_Left_pulse);
    bt.unlock();
    */
}


char c;
char buffer[10];
void bt_shell_thr(void const *args)
{
    while(true) {
        bt_shell_run();
        Thread::wait(50);
    }
}

int main()
{
    initRobot();
    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("eBot#2");
                }
            }
        }
        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);
    Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL);
    Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL);
    Motor_controller_Timer->start(time_int);
    while(1) {
        if(ldrread1()>0.6) {
        }
        if(ldrread2()>0.6) {
            Led_on();
        } else {
            Led_off();
        }
        ultrasonic_run();
        Thread::yield();
    }
}