robot.cpp

00001 /* mbed ROBOT Library, for SUTD evobot project, Generation 1
00002  * Copyright (c) 2013, SUTD
00003  * Author: Mark VanderMeulen
00004  * Modified: Mayuran Saravanapavanantham (this code used for STARS)
00005  *
00006  * April 22, 2013
00007  */
00008 
00009 #include "robot.h"
00010 #include "math.h"
00011 //*********************************CONSTRUCTOR*********************************//
00012 //*********************************CONSTRUCTOR*********************************//
00013 HC05 bt(tx_bt,rx_bt,EN_BT);
00014 //QEI wheel (PTA16, PTA17, NC, 24);
00015 QEI right (PTA16, PTA17, NC, 300, QEI::X4_ENCODING);
00016 QEI left (PTA14, PTA13, NC, 300, QEI::X4_ENCODING);
00017 //Serial bt(rx_bt,tx_bt);
00018 //MPU6050 mpu(PTE0, PTE1);
00019 DigitalOut myled(myledd);
00020 DigitalOut key(PTA15);
00021 DigitalOut btSwitch(EN_BT);
00022 //AnalogIn  currentSensor(CURRENTSENSOR_PIN);
00023 DigitalOut buzzer(buzz);
00024 
00025 AnalogIn LDRsensor1(LDR1);
00026 AnalogIn LDRsensor2(LDR2);
00027 //AnalogIn voltageSensor(VOLTAGESENSOR_PIN);
00028 //PwmOut  buzzer(buzz);
00029 PwmOut PWMA(MOT_PWMA_PIN);
00030 PwmOut PWMB(MOT_PWMB_PIN);
00031 DigitalOut AIN1(MOT_AIN1_PIN);
00032 DigitalOut AIN2(MOT_AIN2_PIN);
00033 DigitalOut BIN1(MOT_BIN1_PIN);
00034 DigitalOut BIN2(MOT_BIN2_PIN);
00035 DigitalOut STBY(MOT_STBY_PIN);
00036 
00037 DigitalOut SRX(PTB10);
00038 TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN);
00039 TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN);
00040 Motors motors( &MotorA, &MotorB, MOT_STBY_PIN);
00041 
00042 AnalogIn uL(ulL);
00043 AnalogIn uF(ulF);
00044 AnalogIn uR(ulR);
00045 AnalogIn urR(ulrR);
00046 AnalogIn urL(ulrL);
00047 AnalogIn uB(ulB);
00048 MPU6050 accelgyro;
00049 int16_t ax, ay, az;
00050 int16_t gx, gy, gz;
00051 int rMotor = 1;
00052 int lMotor = 1;
00053 int m_speed = 100;
00054 int speed;
00055 Mutex stdio_mutex;
00056 int freq=0;
00057 Timer t;
00058 int heading=0;
00059 int last_time=0;
00060 int dy =0;
00061 int dx=0;
00062 float left_current_reading=0;
00063 float   right_current_reading= 0;
00064 float   left_change  = 0;
00065 float   right_change =0;
00066 float   left_prev_read=0;
00067 float   right_prev_read=0;
00068 int   delta_y=0;
00069 int   delta_x=0;
00070 float   delta_thetha=0;
00071 float   encoder_yaw =0;
00072 float   G_thetha=0;
00073 int Encoder_x=0;
00074 int Encoder_y=0;
00075 int dtheta=0;
00076 int software_interuupt;
00077 int Rmotor_speed=0;
00078 int Lmotor_speed=0;
00079 char Selected_robot;
00080 bool bt_connected=false;
00081 int r_time ()
00082 {
00083     int mseconds = (int)time(NULL)+(int)t.read_ms();
00084     return mseconds;
00085 }
00086 void initRobot()
00087 {
00088     key  = 0;
00089     //btSwitch = 1;
00090     bt.start();
00091     myled = 0;
00092     wait_ms(500);
00093     bt.baud(BaudRate_bt);
00094     accelgyro.initialize();
00095     MotorA.scale = R_MOTOR_SCALE;
00096     MotorB.scale = L_MOTOR_SCALE;
00097     t.start();
00098     SRX = 1;
00099     wait_us(30);
00100     SRX=0;
00101     wait_ms(300);
00102     SRX = 1;
00103     wait_us(30);
00104     SRX=0;
00105     wait(1);
00106     myled = 1;
00107 }
00108 
00109 //*********************************MOTORS*********************************//
00110 void motor_control(int Lspeed, int Rspeed)
00111 {
00112     //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
00113     if (!Lspeed && !Rspeed) {   //stop//
00114         STBY = 0;
00115     } else
00116         STBY = 1;
00117     //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
00118     if(Lspeed > 100) Lspeed = 100;
00119     else if (Lspeed < -100) Lspeed = -100;
00120     else if (Lspeed < 0 && Lspeed > -15)   Lspeed = -15;    //prevent speed from being less than 15
00121     else if (Lspeed > 0 &&  Lspeed < 15)    Lspeed = 15;
00122     if(Rspeed > 100) Rspeed = 100;
00123     else if (Rspeed < -100) Rspeed = -100;
00124     else if (Rspeed < 0 && Rspeed > -15)   Rspeed = -15;
00125     else if (Rspeed > 0 &&  Rspeed < 15)    Rspeed = 15;
00126     if (!Rspeed) {  //if right motor is stopped
00127         AIN1 = 0;
00128         AIN2 = 0;
00129         PWMA = 0;
00130     } else if (!Lspeed) { //if left motor is stopped
00131         BIN1 = 0;
00132         BIN2 = 0;
00133         PWMB = 0;
00134     }
00135     //RIGHT MOTOR//
00136     if(Rspeed > 0) {     //Right motor fwd
00137         AIN1 = MOTOR_R_DIRECTION;   //set the motor direction
00138         AIN2 = !AIN1;
00139     } else {     //Right motor reverse
00140         AIN2 = MOTOR_R_DIRECTION;
00141         AIN1 = !AIN2;
00142     }
00143     PWMA = abs(Rspeed/100.0);
00144     //LEFT MOTOR//
00145     if(Lspeed >0) {
00146         BIN1 = MOTOR_L_DIRECTION;
00147         BIN2 = !BIN1;
00148     } else {
00149         BIN2 = MOTOR_L_DIRECTION;
00150         BIN1 = !BIN2;
00151     }
00152     PWMB = abs(Lspeed/100.0);
00153 }
00154 
00155 void stop()
00156 {
00157     motor_control(0,0);
00158 }
00159 void set_speed(int Speed)
00160 {
00161     speed = Speed;
00162     motor_control(speed,speed);
00163 }
00164 
00165 double ldrread1()
00166 {
00167     double r = LDRsensor1.read();
00168     return r;
00169 
00170 }
00171 double ldrread2()
00172 {
00173     double r = LDRsensor2.read();
00174     return r;
00175 
00176 }
00177 void Led_on()
00178 {
00179     // pulseIn
00180     myled= 0;
00181 }
00182 void Led_off()
00183 {
00184     // pulseIn
00185     myled= 1;
00186 }
00187 /*double pl_buzzer()
00188 {
00189       for (int i=0;i<1000;i++)
00190     {
00191         int freq = 150+(i*10);
00192         buzzer=1;
00193         wait_us(1000000/freq);
00194         buzzer=0;
00195         wait_us(1000000/freq);
00196         wait_ms(1);
00197     }
00198 
00199 }
00200 */
00201 void pl_buzzer(int freq, int f_time)
00202 {
00203        int elapsed_time=0;
00204        while (elapsed_time < f_time*10) {
00205         buzzer=1;
00206         wait_us(1000000/freq);
00207         buzzer=0;
00208         wait_us(1000000/freq);
00209         elapsed_time++;
00210         }
00211 
00212 }
00213 void Imu_yaw(void const *args)
00214 {
00215     while(true)
00216 
00217     {
00218         accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00219         int m_seconds = r_time();
00220         float dt = ((float)(m_seconds-last_time))/1000;
00221         last_time=m_seconds;
00222         if ((gz)<800&& gz>-800) {
00223             gz=0;
00224         }
00225         stdio_mutex.lock();
00226         heading = heading + (dt*gz)*3/4/100;
00227         if(heading>360)
00228             heading= heading -360;
00229         else if (heading <0)
00230             heading = heading +360;
00231         stdio_mutex.unlock();
00232         Thread:: wait(50);
00233     }
00234 }
00235 void encoder_thread(void const *args)
00236 {
00237     while(true) {
00238         left_current_reading=left.getPulses()*(-1)/9.85;
00239         right_current_reading= right.getPulses()/10;
00240         left_change  = left_current_reading- left_prev_read;
00241         right_change =right_current_reading- right_prev_read;
00242         left_prev_read=left_current_reading;
00243         right_prev_read=right_current_reading;
00244         delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2;
00245         delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2;
00246         delta_thetha=atan((right_change-left_change)/100);
00247         encoder_yaw =encoder_yaw+delta_thetha;
00248         G_thetha=encoder_yaw*180/M_PI;           //global thetha, overall
00249         Encoder_x=Encoder_x+delta_x;
00250         Encoder_y=Encoder_y+delta_y;
00251         stdio_mutex.lock();
00252         dx=delta_x+dx;
00253         dy=delta_y+dy;
00254         dtheta=delta_thetha*180/M_PI;
00255         //bt.lock();
00256         //bt.printf(">>D;%0.2lf;%0.2lf;%0.2lf;%0.2lf;%d;%d;%d;\r\n",
00257         //    left_current_reading,right_current_reading,left_change ,right_change,\
00258         //   dx,dy,\
00259         //  dtheta);
00260         //bt.unlock();
00261         stdio_mutex.unlock();
00262         Thread:: wait(50);
00263     }
00264 }