main.cpp

00001 /** This program constantly sends the accelerometer data over the bluetooth to the computer.
00002 *   The robot will drive FWD and REVERSE at maximum speed when you press W and S. //Turn 90 degrees at a time//
00003 *   TODO: implement 90deg rotation 
00004 *   TODO: Get rid of flags. Turn everything into a class
00005 *   TODO: Use a PID controller for turning instead of whatever mess you used today
00006 */
00007 #include "mbed.h"
00008 #include "MPU6050.h"
00009 #include "Terminal.h"
00010 #include "string"
00011  #include "main.h"
00012 
00013 //const int startMotor = 2;   //time (in seconds) when motor starts
00014 //const int stopMotor = 4;    //time (in seconds) when motor stops 
00015  
00016 Terminal pc(PTA2,PTA1);  //bluetooth
00017 
00018 DigitalOut myled(PTE3);     //LED
00019 
00020 MPU6050 mpu(PTE0, PTE1);    //Accelerometer (needs the MPU6050 library)
00021 
00022 //things for the motor controller
00023 PwmOut PWMA(PTA4);
00024 PwmOut PWMB(PTA5);
00025 DigitalOut AIN1(PTA14);
00026 DigitalOut AIN2(PTA13);
00027 DigitalOut BIN1(PTA16);
00028 DigitalOut BIN2(PTA17);
00029 DigitalOut STBY(PTA15);
00030 
00031 //motor functions
00032 void motor_control(bool motor, int speed, bool dir);    //speed should be between 1 and 100
00033 void stop();
00034 void straight(int speed);   //drive robot straight. Speed between -100 and 100. Accelerometer works best at 100
00035 void turn_left(int speedA, int speedB);
00036 void turn_right(int speedA, int speedB);
00037 
00038 double integrate(data *prev, int acc[3], int time);     //integrate. this is not implemented yet
00039 void remote_control(char c, int *turn);                      //Send a cmmand to this function, it will drive the robot
00040 
00041 int accFlag = 0;    //flag to continue acceleration or deceleration (1 for acc, 0 for none, -1 for dec)
00042 int turnFlag = 0;   //flag for direction of turn. If 0, no turn, if 1 turn right, -1 turn left
00043 int accLimit = 16384.0/9.81* 1;  //acceleration limit = 2 m/s2
00044 int maxRot = 0;
00045 
00046 
00047 int main()
00048 {
00049     double t2 = 0;
00050     double tnow;
00051     double SP;
00052     int Kp = 75; //proportional gain (tuning)
00053     int Ki = 0; //integral gain (tuning)
00054     double Kd = 0; //derivative gain (tuning) *Not used = 0*
00055     double Irz = 0;
00056     
00057     double rotationSpeed=0;
00058     double rz=0; //rotation in the z axis
00059     double tprev=0;//time of previous iteration
00060     double pi = 3.14159265359;
00061     int i=0;
00062     int turnTarget = 0; //angle for the robot to drive at (in degrees)
00063 //    data *state;    //this will be passed into the integrate fucntion for recursiveness
00064     //send data to the integrate function, delayed by 10 samples
00065     //
00066     char c;
00067 //    int flag = 0;
00068     int accdata[3];
00069     int avAcc[3];
00070     int gyrodata[3];
00071     Timer t;
00072     pc.baud(115200);    //Make sure this baud rate matches the baud rate of the bluetooth (set baud rate with "BluetoothAT")
00073     wait(0.1);
00074     char test = mpu.testConnection();
00075 
00076 
00077     pc.baud(115200); //this can only be changed after the BLUETOOTH baud rate has been changed
00078     if (mpu.testConnection()){
00079         //pc.printf("MPU connection succeeded\n\r");
00080         myled = 0;
00081         }
00082     else{
00083         pc.printf("MPU connection failed\n\r"); //Todo: If connection fails, retry a couple times. Try resetting MPU (this would need another wire?)
00084         myled = 1;
00085     }
00086     wait(3);
00087 
00088     t.reset();
00089   //  mpu.setBW(MPU6050_BW_10);   //set bandwidth of low pass filter for Accerlerometer AND Gyro.
00090   //  mpu.setBW(MPU6050_BW_42);    
00091  //    mpu.setBW(MPU6050_BW_98);    
00092     mpu.setBW(MPU6050_BW_20);
00093   //  mpu.setGyroRange(MPU6050_GYRO_RANGE_250);   //To convert gyro output to RAD/S, divide by 7505.7
00094     mpu.setGyroRange(MPU6050_GYRO_RANGE_500);   //to convert gyro output to RAD/S, divide by 3752.9
00095     //double gyroCorrect = 3752.9;    //////change this number!!!!Decrease to decrease rotation
00096     double gyroCorrect = 3720;   //prev3754.82prev3752.9  Use this number to convert gyro reading to rad/s
00097     int count = 0;
00098     double rzOff = 0;
00099     while(t.read()<1) //calculate gyro offset
00100     {
00101         mpu.getGyroRaw(gyrodata);
00102         rzOff += gyrodata[2];
00103         count++;
00104     }
00105     rzOff = rzOff/count; //rzOffset
00106     while(1) 
00107     {
00108         //speed = 100;
00109         mpu.getAcceleroRaw(accdata);
00110         mpu.getGyroRaw(gyrodata);
00111         
00112         //ROTATION//------------------------------------//
00113         
00114         //MV = 0; //input (speed setting)
00115         SP = turnTarget*pi/180; //desired angle (radians)
00116         tnow = t.read();
00117         rz = rz + ((gyrodata[2]-rzOff)*(tnow-tprev)/gyroCorrect);    //rz is the rotation(radians) from start
00118         Irz = Irz + (rz-SP)*(tnow-tprev); //Irz needs to be reset every so often, or it should be ignored
00119         tprev = tnow;
00120         rotationSpeed = (int(Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect));
00121         pc.printf("\n\r%f\t",rotationSpeed);
00122         if(maxRot < gyrodata[2])
00123             maxRot = gyrodata[2];
00124             
00125             
00126         
00127         
00128 
00129         
00130         if(abs(rotationSpeed)<1)//(abs(rz-SP)<(0.0175*gyroCorrect))
00131         {
00132             rotationSpeed = 0;
00133             Irz = 0;
00134         }
00135         else if(abs(rotationSpeed)<10)
00136         {
00137             
00138             if (rotationSpeed>0)
00139                 rotationSpeed = 10;
00140             else
00141                 rotationSpeed = -10;
00142         }
00143         if(tnow>t2)
00144         {
00145             turn_right(-rotationSpeed,rotationSpeed);
00146             t2 = tnow+0.01;  
00147         }
00148         
00149         
00150         //END ROTATION//---------------------------------//
00151         if (accFlag > 0)
00152             accFlag --;
00153         else if(accFlag < 0)
00154             accFlag++;
00155      
00156      //  avAcc[0] = 0;
00157      //   avAcc[1] = 0;
00158      //   avAcc[2] = 0;
00159      //   for(i = 0; i<4; i++)
00160      //   {
00161      //   
00162      //       mpu.getAcceleroRaw(accdata);
00163      //       mpu.getGyroRaw(gyrodata);
00164      //       
00165      //       avAcc[0] += accdata[0];
00166     //        avAcc[1] += accdata[1];
00167     //        avAcc[2] += accdata[2];
00168     //    }
00169 
00170         
00171       pc.printf("%f\t%d\t%d\t%d\t", tnow,accdata[0], accdata[1], accdata[2]);
00172       // pc.printf("%d\t%d\t%d\t%d\n\r", gyrodata[0], gyrodata[1], gyrodata[2],STBY.read());
00173 //      pc.printf("%f\n\r",tnow);
00174         
00175         //Old Code for moving the robot//
00176         /*
00177         if (t.read() > startMotor && flag ==0){
00178             myled = 0;
00179             straight(100);
00180             flag = 1;
00181         }
00182         if (t.read() > stopMotor){
00183             stop();
00184             myled = 1;
00185             STBY=0;
00186             flag = 2;
00187         }
00188         if (t.read() > 20)
00189             break;
00190             */
00191         //New code for moving robot//
00192         if(pc.readable() || accFlag){
00193             if(accFlag)
00194                 c = 'i';
00195             else          
00196                 c = pc.getc();
00197             remote_control(c,&turnTarget);
00198             if (c=='t'){        //reset timer if t is pressed
00199                 wait(2);
00200                 t.reset();
00201                 turnTarget = 0;
00202             }
00203         }    
00204     }
00205         
00206 }
00207 void motor_control(bool motor, int speed, bool dir)     //motor: 0=right, 1=left;   speed: 0-100; dir 0=fwd, 1=bkwd                                 
00208 {
00209     if (speed == 0)     //stop//
00210     {
00211         STBY = 0;
00212         if (motor ==0)
00213         {
00214             AIN1 = 0;
00215             AIN2 = 0;
00216             PWMA = 0;
00217         }
00218         else
00219         {
00220             BIN1 = 0;
00221             BIN2 = 0;
00222             PWMB = 0;
00223         }
00224     }
00225     else
00226         STBY = 1;
00227     if (!dir)    //forward//
00228     {
00229         if(motor == 0) //right motor//
00230         {
00231             AIN1 = 1;  
00232             AIN2 = 0;
00233             PWMA = abs(speed/100.0);
00234         }
00235         else            //left motor//
00236         {
00237             BIN1 = 0;  //REVERSE THESE WHEN FIX WIRING//////*****//////
00238             BIN2 = 1; 
00239             PWMB = abs(speed/100.0);
00240         }
00241     }
00242     else
00243     {
00244         if(motor == 0)  //right motor//
00245         {
00246             AIN1 = 0;
00247             AIN2 = 1;
00248             PWMA = abs(speed/100.0);
00249         }
00250         else            //left motor//
00251         {
00252             BIN1 = 1;   //REVERSE THESE WHEN FIXED WIRING!!////*****////////
00253             BIN2 = 0;
00254             PWMB = abs(speed/100.0);
00255         }
00256     }
00257     
00258 }
00259 void stop()
00260 {
00261     motor_control(0,0,0);   //motorA, speed=0, direction = fwd
00262     motor_control(1,0,0);
00263     STBY=0;
00264 }
00265 void straight(int speed)    //if speed is +ve, go straight. -ve reverse
00266 {
00267     if (speed == 0)
00268         stop();
00269     else if (speed > 100)
00270         speed = 100;
00271     else if (speed < -100)
00272         speed = -100;
00273     else if (speed>0)   //fwd//
00274     {
00275         motor_control(0,speed,0);
00276         motor_control(1,speed,0);
00277     }
00278     else
00279     {
00280         motor_control(0,speed,1);
00281         motor_control(1,speed,1);
00282     }
00283 }
00284 double integrate(int *prev, int acc[3], int time)
00285 {
00286     /*Returns the distance travelled*/
00287     return 0;
00288 }
00289 
00290 
00291 /*Makes robot go straight, stop, and turn*/
00292 void remote_control(char c, int *turn)
00293 {
00294 int speed;
00295 int y;
00296     switch (c)
00297     {
00298         case 'w':   //forward - increment speed
00299  //           turn=0;
00300             speed = 100;
00301             straight(speed);
00302             break;
00303         case 's':   //backwards
00304 //            turn=0;
00305             speed = -100;
00306             straight(speed);
00307             break;
00308         case 'd':   //right
00309             /*if(BIN1.read()==0 && AIN1.read()==0)     //if previously turning right, go straight
00310                 stop();
00311             //{
00312             //    turn = 0;
00313             //    turnRate = 0;
00314             //    straight(speed);
00315             //}
00316             else
00317             {
00318                 //turn_left(50,-50);
00319             }*/
00320             *turn+=90;    //rotate right 90 degrees
00321             break;
00322         case 'a':   //left
00323             /*if(BIN1.read() && AIN1.read())   //if previously turning left, go straight;
00324                 stop();
00325             else
00326                 turn_right(-50,50);
00327             */
00328             *turn-=90;    //rotate left 90 degrees
00329             break;
00330         case 'i':   //constant acceleration
00331             //if(BIN1.read() || (AIN!.read() == 0))   //if previously reverse, stop
00332             //    stop();
00333             //else    //go in reverse, at a constant acceleration
00334             speed = (PWMA.read() + PWMB.read())/2*100;
00335             y = mpu.getAcceleroRawY();
00336             if(speed<100 || BIN1.read() || (AIN1.read()==0) && accFlag < 2)
00337             {
00338                 accFlag = 5;
00339                 if(BIN1.read()) stop();
00340                 if(abs(y)<accLimit)
00341                 {
00342                     speed = speed + 1;
00343                     straight(speed);
00344                 }
00345             }
00346             else
00347                 accFlag = 0;
00348             break;
00349         default:    //stop
00350             speed = 0;
00351             stop();
00352     }
00353 }
00354 
00355 
00356 void turn_left(int speedA, int speedB)
00357 {
00358     
00359     if (speedA < 0) //R reverse//
00360     {
00361         motor_control(0,speedA,1);  //Right reverse
00362         motor_control(1,speedB,1);  //Left reverse
00363     }
00364     else if(speedB <= 0)
00365     {
00366         motor_control(1,speedB,1);  //Left reverse//
00367         motor_control(0,speedA,0);  //Right fwd
00368     }
00369     else
00370     {
00371         motor_control(0,speedA,0);  //Right fwd
00372         motor_control(1,speedB,0);  //Left fwd
00373     }  
00374 }
00375 void turn_right(int speedA, int speedB)
00376 {
00377     if (speedA > 0) //R fwd//
00378     {
00379         motor_control(0,speedA,0);  //Right fwd
00380     }
00381     else
00382     {
00383         motor_control(0,speedA,1);  //Right reverrse
00384     }
00385     if(speedB<0)   //L rev
00386     {
00387         motor_control(1,speedB,1);  //Left reverse
00388     }
00389     else
00390          motor_control(1,speedB,0); 
00391     
00392 }