robot.cpp

00001 /* mbed ROBOT Library, for SUTD evobot project, Generation 1
00002  * Copyright (c) 2013, SUTD
00003  * Author: Mark VanderMeulen
00004  *
00005  * Dec 18, 2013
00006  *
00007  * You may want to rewrite this entire library, as this one is only half-finished
00008  *
00009  * This library allows the user to use each module on the Evobot Generation 1
00010  * Functionality:
00011  *      -Bluetooth connection
00012  *      -MPU (accelerometer/gyroscope) connection
00013  *      -Orientation in the XY plane (using Z-axis gyroscope only)
00014  *      -Remote control functionality (via bluetooth)
00015  *
00016  * Future functions:
00017  *      -Switch to turn bluetooth on/off
00018  *      -Switch to turn MPU on/off (in case of connection errors with the MPU: restart and reconnect
00019  *      -Calculation of distance travelled (use integration of accelerometer, maybe use Kalman filter)
00020  *      -Access to the DMP (Digital motion processor) on the MPU to calculate 3D orientation.
00021  *      -RF mesh network so robots can connect to eachother
00022  *      -Camera (requires an add-on to the circuit board at the moment)
00023  */
00024 
00025 
00026 #include "robot.h"
00027 #include "math.h"
00028 //*********************************CONSTRUCTOR*********************************//
00029 Robot::Robot() : bt(tx_bt,rx_bt),
00030     rf24(PTD2, PTD3, PTD1, PTD0, PTD5, PTD4),
00031     mpu(PTE0, PTE1),
00032     myled(LED),
00033     btSwitch(PTE25),
00034     currentSensor(CURRENTSENSOR_PIN),
00035     irSensorL(irL),
00036     irSensorC(irC),
00037     irSensorR(irR),
00038     voltageSensor(VOLTAGESENSOR_PIN),
00039     PWMA(MOT_PWMA_PIN),
00040     PWMB(MOT_PWMB_PIN),
00041     AIN1(MOT_AIN1_PIN),
00042     AIN2(MOT_AIN2_PIN),
00043     BIN1(MOT_BIN1_PIN),
00044     BIN2(MOT_BIN2_PIN),
00045     STBY(MOT_STBY_PIN)
00046 {
00047 
00048     stop();  //Initialize motors as stopped//
00049 
00050     btSwitch = 1;    //turn on bluetooth
00051     myled = 0;   //turn ON status LED (0 = on, 1 = 0ff)
00052     timeNext = 0;
00053     //currentAvg = 0;          /////////////REMOVE currentAvg LATER. TESTING PURPOSES ONLY////////
00054 
00055     target_angle = 0;    //direction we want the robot to be pointed
00056     angle_origin = 0;    //you can use this to modify the angle of origin
00057     origin = 0;          //the (x,y) location of the origin (this should be a point, not a double)
00058     rz = 0;          //The current rotation in the Z-axis
00059     dx = 0;          //The current displacement in the x-axis    (side-side)
00060     dy = 0;          //The current displacement in the y-axis    (forward-back)
00061     dz = 0;          //The current displacement in the z-axis    (up-down)
00062 
00063     AUTO_ORIENT = 1;     //robot will automatically orient iteslf using the gyroscope
00064     REMOTE_CONTROL = 1;  //robot can be controlled over bluetooth
00065 
00066     tt.start();   //start timer
00067     mpu.testConnection();
00068     wait(1);
00069     MPU_OK = 0;
00070 
00071     //initialize MPU
00072     if (mpu.testConnection()) {
00073         mpu.setBW(MPU6050_BW_10);   //default set low pass filter bandwidth to 10HZ
00074         mpu.setGyroRange(MPU6050_GYRO_RANGE_500); //default set the gyro range to 500deg/s (robot turning exceeds 250deg/s)
00075         mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);        //default set accelero range to 2g
00076         MPU_OK=1;
00077         myled = 0;  //turn on LED
00078         calibrate();
00079     } else if(0) { //this section is current disabled. change to (1) if you want to retry the accelerometer connection.
00080         myled = 1;  //turn off LED
00081         for (int i = 0; i<25; i++) {
00082             myled = !myled;
00083             wait_ms(50);
00084             if (mpu.testConnection()) {
00085                 i = 25;
00086                 myled = 0;
00087                 MPU_OK=1;
00088             } else
00089                 myled = 1;
00090             wait_ms(50);
00091         }
00092     }
00093     myled = MPU_OK;  //If LED is off, it is ok. If LED is on, there was a MPU error. Board needs to be restarted.
00094     //In the future, at a transistor to switch on/off the mpu in case there is an MPU error. Right now it needs a manual restart.
00095 
00096     bt.baud(BaudRate_bt);    //Set bluetooth baud rate
00097 
00098     //Initialize RF chip
00099 //char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
00100 // int txDataCnt = 0;
00101 // int rxDataCnt = 0;
00102     rf24.powerUp();
00103     rf24.setTransferSize( TRANSFER_SIZE );
00104 
00105     rf24.setReceiveMode();
00106     rf24.enable();
00107 
00108 
00109 
00110 }
00111 
00112 //*********************************MOTORS*********************************//
00113 void Robot::motor_control(int Lspeed, int Rspeed)
00114 {
00115     //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
00116     if (!Lspeed && !Rspeed)     //stop//
00117         STBY = 0;
00118     else
00119         STBY = 1;
00120 
00121     //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
00122     if(Lspeed > 100) Lspeed = 100;
00123     else if (Lspeed < -100) Lspeed = -100;
00124     else if (Lspeed < 0 && Lspeed > -15)   Lspeed = -15;    //prevent speed from being less than 15
00125     else if (Lspeed > 0 &&  Lspeed < 15)    Lspeed = 15;
00126     if(Rspeed > 100) Rspeed = 100;
00127     else if (Rspeed < -100) Rspeed = -100;
00128     else if (Rspeed < 0 && Rspeed > -15)   Rspeed = -15;
00129     else if (Rspeed > 0 &&  Rspeed < 15)    Rspeed = 15;
00130 
00131     if (!Rspeed) {  //if right motor is stopped
00132         AIN1 = 0;
00133         AIN2 = 0;
00134         PWMA = 0;
00135     } else if (!Lspeed) { //if left motor is stopped
00136         BIN1 = 0;
00137         BIN2 = 0;
00138         PWMB = 0;
00139     }
00140     //RIGHT MOTOR//
00141     if(Rspeed > 0) {     //Right motor fwd
00142         AIN1 = MOTOR_R_DIRECTION;   //set the motor direction
00143         AIN2 = !AIN1;
00144     } else {     //Right motor reverse
00145         AIN2 = MOTOR_R_DIRECTION;
00146         AIN1 = !AIN2;
00147     }
00148     PWMA = abs(Rspeed/100.0);
00149     //LEFT MOTOR//
00150     if(Lspeed >0) {
00151         BIN1 = MOTOR_L_DIRECTION;
00152         BIN2 = !BIN1;
00153     } else {
00154         BIN2 = MOTOR_L_DIRECTION;
00155         BIN1 = !BIN2;
00156     }
00157     PWMB = abs(Lspeed/100.0);
00158 }
00159 
00160 void Robot::stop()
00161 {
00162     motor_control(0,0);
00163 }
00164 void Robot::set_speed(int Speed)
00165 {
00166     speed = Speed;
00167     motor_control(speed,speed);
00168 }
00169 
00170 //************************Setting direction of robot****************************//
00171 void Robot::set_direction(double angle)
00172 {
00173     //Set the direction the robot should be facing from origin(in radians)
00174     target_angle = angle;
00175 }
00176 void Robot::set_direction_deg(double angle)
00177 {
00178     //Set the direction the robot should be facing from origin (in degrees)
00179     target_angle = angle*M_PI/180;
00180 }
00181 void Robot::auto_enable(bool x)
00182 {
00183     AUTO_ORIENT = x;
00184 }
00185 
00186 //************************UPDATE: To be called in main loop***************************//
00187 //This calculates position and angle
00188 //Also orients robot in the correct direction
00189 //if 'print' is 1, print acceleration and gyro data over bluetooth
00190 void Robot::update()
00191 {
00192     double SP;
00193     int rotationSpeed;
00194     //int n=50;
00195 
00196     double Kp = 100; //weighting values for PID controller. Only Proportional component is used.
00197     double Ki = 0;  //Integral component is not used
00198     double Kd = 0;  //derivative component is not used
00199 
00200     if(MPU_OK) { //only do this if MPU is connected
00201         wait(0.1);
00202         mpu.getAcceleroRaw(accdata);
00203         mpu.getGyroRaw(gyrodata);
00204         time = tt.read();
00205 //bt.printf("s;t_%d;1_%lf;2_%lf;3_%d;4_%d;5_%d;6_%d;7_%d;8_%d;9_%lf;10_%lf;11_%lf;s\n\r",time_robot,getCurrent(),getVoltage(),accdata[0],accdata[1],accdata[2],gyrodata[0],gyrodata[1],gyrodata[2],irReadL(),irReadC(),irReadR());
00206         /*if(REMOTE_CONTROL) {    //if remote control over bluetooth is enabled
00207             m_ctrl=remote_ctrl();      //call the romote control function
00208         }*/
00209         if(AUTO_ORIENT) {       //enable PID control of angle
00210             SP = target_angle;  //desired angle (radians)
00211             rz = rz + ((gyrodata[2]-gyroOffset[2])*(time-timePrev)/gyroCorrect);    //rz is the rotation(radians) from start
00212             Irz = Irz + (rz-SP)*(time-timePrev);    //Irz needs to be reset every so often, or it should be ignored
00213             rotationSpeed = (Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect);
00214             /*data[0]= getCurrent();
00215             data[1]= getVoltage();
00216             data[2]= accdata[0];
00217             data[3]= accdata[1];
00218             data[4]= accdata[2];
00219             data[5]= gyrodata[0];
00220             data[6]= gyrodata[1];
00221             data[7]= gyrodata[2];
00222             data[8]= irReadL();
00223             data[9]= irReadC();
00224             data[10]= irReadC();
00225             data[11]= rz;*/
00226 
00227             //TODO: pull "rotationspeed" up to 10 if it is less than 10. This will(should) improve the ability to drive straight
00228 
00229             if(time > timeNext || (speed==0 && rotationSpeed ==0)) {   //prevent the motor control from being set too often
00230                 timeNext = time + MOTOR_INTERVAL/1000.0;   //only set the motor speed every 10ms
00231                 //    if(abs(rz-SP)>(10*M_PI/180))
00232                 //  {
00233                 //  motor_control((speed+rotationSpeed)/20, (speed-rotationSpeed)/20);    //Set motor speeds
00234                 //}
00235                 // else
00236                 //{
00237                 motor_control((speed+rotationSpeed), (speed-rotationSpeed));    //Set motor speeds
00238                 //}
00239             }
00240         }
00241         timePrev = time;
00242     }
00243 }
00244 
00245 void Robot::remote_ctrl(char c,int desired_speed,int desired_angle)
00246 {
00247     //This is a private function
00248     Irz = 0;
00249     switch (c) {
00250         case ctrl_forward:
00251             speed = -1*desired_speed;
00252             break;
00253         case ctrl_backward:
00254             speed = 1*desired_speed;
00255             break;
00256         case ctrl_right:
00257             target_angle += 90*M_PI/180;
00258             break;
00259         case ctrl_left:
00260             target_angle -= 90*M_PI/180;
00261             break;
00262         case ctrl_calibrate:
00263             calibrate();
00264             break;
00265         case ctrl_turn_angle_cw:
00266             target_angle +=desired_angle*M_PI/180;
00267             break;
00268         case ctrl_turn_angle_ccw:
00269             target_angle -=desired_angle*M_PI/180;
00270             break;
00271         default:
00272             speed = 0;
00273             stop();
00274             break;
00275     }
00276 }
00277 //*********************************CALIBRATE*********************************//
00278 void Robot::calibrate()
00279 {
00280     stop();
00281     wait(1.5);
00282     double timeNOW = tt.read();
00283     int count=0;
00284     int i;
00285 
00286     //set the accelerometer and gyro offsets
00287     while(tt.read()<timeNOW+1.5) { //calculate gyro offset
00288         mpu.getGyroRaw(gyrodata);
00289         mpu.getAcceleroRaw(accdata);
00290         for(i=0; i<3; i++) {
00291             gyroOffset[i] += gyrodata[i];
00292             accOffset[i] += accdata[i];
00293         }
00294         count++;
00295     }
00296     for(i=0; i<3; i++) {
00297         gyroOffset[i] = gyroOffset[i]/count; //rxOffset
00298         accOffset[i] = accOffset[i]/count;
00299     }
00300     accOffset[2] = 0;   //we don't want to remove GRAVITY from the z-axis accelerometer.
00301 }
00302 
00303 //*********************************ROBOT-SENSORS*********************************//
00304 
00305 double Robot::getCurrent()
00306 {
00307 
00308     double Vsensor = currentSensor.read();
00309     Vsensor = 1000*((.8998-Vsensor)/.6411);
00310     return Vsensor;
00311 }
00312 double Robot::getVoltage()
00313 {
00314     float voltage = 3.3*(voltageSensor.read()); // convert analog value to voltage at node
00315     voltage *= 1.5; // inverse of voltage divider
00316     return voltage;
00317 }
00318 void Robot::getIMU(float *adata, float *gdata)
00319 {
00320     mpu.getAcceleroRaw(accdata);
00321     mpu.getGyroRaw(gyrodata);
00322     /*imuFilter.updateFilter(gyrodata[0],gyrodata[1],gyrodata[2],accdata[0],accdata[1],accdata[2]);
00323        imuFilter.computeEuler();
00324        gdata[0]=(float)imuFilter.getRoll();
00325        gdata[1]=(float)imuFilter.getPitch();
00326        gdata[2]= (float)imuFilter.getYaw();*/
00327     // adata[0] = (float) (accdata[0]);
00328     //adata[1] = (float) (accdata[1]);
00329     //adata[2] = (float) (accdata[2]);
00330     adata[0] = ((float) (accdata[0]-accOffset[0]))/32655*9.81*2;
00331     adata[1]= ((float) (accdata[1]-accOffset[1]))/32655*9.81*2;
00332     adata[2]= ((float) (accdata[2]-accOffset[2]))/32655*9.81*2;
00333     gdata[0]=(((float)(gyrodata[0]-gyroOffset[0]))/((float)gyroCorrect))*180/(M_PI);
00334     gdata[1]=(((float)(gyrodata[1]-gyroOffset[1]))/((float)gyroCorrect))*180/(M_PI);
00335     gdata[2]=(((float)(gyrodata[2]-gyroOffset[2]))/((float)gyroCorrect))*180/(M_PI);
00336 }
00337 
00338 double Robot::irReadL()
00339 {
00340     int C = 14;
00341     double D = -1.1;
00342     double r = irSensorL.read();
00343     double reading=(pow(r,D)* C )+ 4;
00344     return reading;
00345 
00346 }
00347 
00348 double Robot::irReadC()
00349 {
00350     int C = 14;
00351     double D = -1.1;
00352     double r = irSensorC.read();
00353     double reading=(pow(r,D)* C )+ 4;
00354     return reading;
00355 }
00356 
00357 double Robot::irReadR()
00358 {
00359     int C = 14;
00360     double D = -1.1;
00361     double r = irSensorR.read();
00362     double reading=(pow(r,D)* C )+ 4;
00363     return reading;
00364 }
00365 double Robot::return_rotation()
00366 {
00367     return rz;
00368 }
00369 int Robot::isMPU()
00370 {
00371     return MPU_OK;
00372 }
00373 
00374 //******************RF24 CHIP FUNCTIONS****************************//
00375 void Robot::rf24_power(int x)
00376 {
00377     if(x)   //power up
00378         rf24.powerUp();
00379     else    //power down
00380         rf24.powerDown();
00381 }