File content as of revision 5:a95a6243c118:

/* mbed ROBOT Library, for SUTD evobot project, Generation 1
 * Copyright (c) 2013, SUTD
 * Author: Mark VanderMeulen
 * Modified: Mayuran Saravanapavanantham (this code used for STARS)
 *
 * April 22, 2013
 */

#include "robot.h"
#include "math.h"
//*********************************CONSTRUCTOR*********************************//
//*********************************CONSTRUCTOR*********************************//
HC05 bt(tx_bt,rx_bt,EN_BT);
//QEI wheel (PTA16, PTA17, NC, 24);
QEI right (PTA16, PTA17, NC, 300, QEI::X4_ENCODING);
QEI left (PTA14, PTA13, NC, 300, QEI::X4_ENCODING);
//Serial bt(rx_bt,tx_bt);
//MPU6050 mpu(PTE0, PTE1);
DigitalOut myled(myledd);
DigitalOut key(PTA15);
DigitalOut btSwitch(EN_BT);
//AnalogIn  currentSensor(CURRENTSENSOR_PIN);
DigitalOut buzzer(buzz);

AnalogIn LDRsensor1(LDR1);
AnalogIn LDRsensor2(LDR2);
//AnalogIn voltageSensor(VOLTAGESENSOR_PIN);
//PwmOut  buzzer(buzz);
PwmOut PWMA(MOT_PWMA_PIN);
PwmOut PWMB(MOT_PWMB_PIN);
DigitalOut AIN1(MOT_AIN1_PIN);
DigitalOut AIN2(MOT_AIN2_PIN);
DigitalOut BIN1(MOT_BIN1_PIN);
DigitalOut BIN2(MOT_BIN2_PIN);
DigitalOut STBY(MOT_STBY_PIN);

DigitalOut SRX(PTB10);
TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN);
TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN);
Motors motors( &MotorA, &MotorB, MOT_STBY_PIN);

AnalogIn uL(ulL);
AnalogIn uF(ulF);
AnalogIn uR(ulR);
AnalogIn urR(ulrR);
AnalogIn urL(ulrL);
AnalogIn uB(ulB);
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int rMotor = 1;
int lMotor = 1;
int m_speed = 100;
int speed;
Mutex stdio_mutex;
int freq=0;
Timer t;
int heading=0;
int last_time=0;
int dy =0;
int dx=0;
float left_current_reading=0;
float   right_current_reading= 0;
float   left_change  = 0;
float   right_change =0;
float   left_prev_read=0;
float   right_prev_read=0;
int   delta_y=0;
int   delta_x=0;
float   delta_thetha=0;
float   encoder_yaw =0;
float   G_thetha=0;
int Encoder_x=0;
int Encoder_y=0;
int dtheta=0;
int software_interuupt;
int Rmotor_speed=0;
int Lmotor_speed=0;
char Selected_robot;
bool bt_connected=false;
int r_time ()
{
    int mseconds = (int)time(NULL)+(int)t.read_ms();
    return mseconds;
}
void initRobot()
{
    key  = 0;
    //btSwitch = 1;
    bt.start();
    myled = 0;
    wait_ms(500);
    bt.baud(BaudRate_bt);
    accelgyro.initialize();
    MotorA.scale = R_MOTOR_SCALE;
    MotorB.scale = L_MOTOR_SCALE;
    t.start();
    SRX = 1;
    wait_us(30);
    SRX=0;
    wait_ms(300);
    SRX = 1;
    wait_us(30);
    SRX=0;
    wait(1);
    myled = 1;
}

//*********************************MOTORS*********************************//
void motor_control(int Lspeed, int Rspeed)
{
    //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
    if (!Lspeed && !Rspeed) {   //stop//
        STBY = 0;
    } else
        STBY = 1;
    //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
    if(Lspeed > 100) Lspeed = 100;
    else if (Lspeed < -100) Lspeed = -100;
    else if (Lspeed < 0 && Lspeed > -15)   Lspeed = -15;    //prevent speed from being less than 15
    else if (Lspeed > 0 &&  Lspeed < 15)    Lspeed = 15;
    if(Rspeed > 100) Rspeed = 100;
    else if (Rspeed < -100) Rspeed = -100;
    else if (Rspeed < 0 && Rspeed > -15)   Rspeed = -15;
    else if (Rspeed > 0 &&  Rspeed < 15)    Rspeed = 15;
    if (!Rspeed) {  //if right motor is stopped
        AIN1 = 0;
        AIN2 = 0;
        PWMA = 0;
    } else if (!Lspeed) { //if left motor is stopped
        BIN1 = 0;
        BIN2 = 0;
        PWMB = 0;
    }
    //RIGHT MOTOR//
    if(Rspeed > 0) {     //Right motor fwd
        AIN1 = MOTOR_R_DIRECTION;   //set the motor direction
        AIN2 = !AIN1;
    } else {     //Right motor reverse
        AIN2 = MOTOR_R_DIRECTION;
        AIN1 = !AIN2;
    }
    PWMA = abs(Rspeed/100.0);
    //LEFT MOTOR//
    if(Lspeed >0) {
        BIN1 = MOTOR_L_DIRECTION;
        BIN2 = !BIN1;
    } else {
        BIN2 = MOTOR_L_DIRECTION;
        BIN1 = !BIN2;
    }
    PWMB = abs(Lspeed/100.0);
}

void stop()
{
    motor_control(0,0);
}
void set_speed(int Speed)
{
    speed = Speed;
    motor_control(speed,speed);
}

double ldrread1()
{
    double r = LDRsensor1.read();
    return r;

}
double ldrread2()
{
    double r = LDRsensor2.read();
    return r;

}
void Led_on()
{
    // pulseIn
    myled= 0;
}
void Led_off()
{
    // pulseIn
    myled= 1;
}
/*double pl_buzzer()
{
      for (int i=0;i<1000;i++)
    {
        int freq = 150+(i*10);
        buzzer=1;
        wait_us(1000000/freq);
        buzzer=0;
        wait_us(1000000/freq);
        wait_ms(1);
    }

}
*/
void pl_buzzer(int freq, int f_time)
{
       int elapsed_time=0;
       while (elapsed_time < f_time*10) {
        buzzer=1;
        wait_us(1000000/freq);
        buzzer=0;
        wait_us(1000000/freq);
        elapsed_time++;
        }

}
void Imu_yaw(void const *args)
{
    while(true)

    {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        int m_seconds = r_time();
        float dt = ((float)(m_seconds-last_time))/1000;
        last_time=m_seconds;
        if ((gz)<800&& gz>-800) {
            gz=0;
        }
        stdio_mutex.lock();
        heading = heading + (dt*gz)*3/4/100;
        if(heading>360)
            heading= heading -360;
        else if (heading <0)
            heading = heading +360;
        stdio_mutex.unlock();
        Thread:: wait(50);
    }
}
void encoder_thread(void const *args)
{
    while(true) {
        left_current_reading=left.getPulses()*(-1)/9.85;
        right_current_reading= right.getPulses()/10;
        left_change  = left_current_reading- left_prev_read;
        right_change =right_current_reading- right_prev_read;
        left_prev_read=left_current_reading;
        right_prev_read=right_current_reading;
        delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2;
        delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2;
        delta_thetha=atan((right_change-left_change)/100);
        encoder_yaw =encoder_yaw+delta_thetha;
        G_thetha=encoder_yaw*180/M_PI;           //global thetha, overall
        Encoder_x=Encoder_x+delta_x;
        Encoder_y=Encoder_y+delta_y;
        stdio_mutex.lock();
        dx=delta_x+dx;
        dy=delta_y+dy;
        dtheta=delta_thetha*180/M_PI;
        //bt.lock();
        //bt.printf(">>D;%0.2lf;%0.2lf;%0.2lf;%0.2lf;%d;%d;%d;\r\n",
        //    left_current_reading,right_current_reading,left_change ,right_change,\
        //   dx,dy,\
        //  dtheta);
        //bt.unlock();
        stdio_mutex.unlock();
        Thread:: wait(50);
    }
}