File content as of revision 6:5ab1735265a9:

// 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"
#include "bt_shell_f.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 Last_Error_Left=0;
int Last_Error_Right=0;
int previous_Linput= 0 ;
int previous_Rinput= 0 ;
RtosTimer *Motor_controller_Timer;

void Motorcontroller(void const *args)
{

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

    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;
    if(Rinput>100)
        Rinput=100;
    else if(Rinput<-100)
        Rinput= -100;
    if(Error_Right==0) {
        Last_Error_Right++;
    } else {
        Last_Error_Right=0;
    }
    if(Error_Left==0) {
        Last_Error_Left++;
    } else {
        Last_Error_Left=0;
    }
    if(Last_Error_Right==100) {
        Rinput=0;
    }
    if(Last_Error_Left==100) {
        Linput=0;
    }
    *motors.Left = Linput;          // HARSH change to -Linput for black and Linput for blue
    *motors.Right = -Rinput;        //// HARSH change to Rinput for black and -Rinput for blue
}
char c;
char buffer[10];
void bt_shell_thr(void const *args)
{
    while(true) {
        if(Selected_robot=='A') {
            bt_shell_run();
        } else if(Selected_robot=='F') {
            bt_shell_f_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);
        }
    }
    bt_connected=true;
    bt.gets(buffer, 2);
    if(buffer[0]=='A') {
        Selected_robot='A';
        //imperial_march();

    } else if(buffer[0]=='F') {
        Selected_robot='F';
        Led_on();
    }
    Thread bt_shell_th(bt_shell_thr, NULL, osPriorityNormal, 2048, NULL);
    Thread IMU_th(Imu_yaw, NULL, osPriorityNormal, 2048, NULL);
    Thread Encoder_th(encoder_thread, NULL, osPriorityNormal, 2048, NULL);
    Motor_controller_Timer = new RtosTimer(&Motorcontroller, osTimerPeriodic, NULL);
    Motor_controller_Timer->start(time_int);
    while(1) {
        if(Selected_robot=='A') {
            if(ldrread2()>0.6) {
                Led_on();
            } else {
                Led_off();
            }
        }
        ultrasonic_run();
        Thread::yield();
    }
}