main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #include "SDFileSystem.h"
00004 #include "Mouse.h"
00005 
00006 SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // MOSI, MISO, SCK, CS
00007 
00008 Serial pc(USBTX, USBRX);
00009 
00010 FILE *Data;   // Creates name of file
00011 char buffer[5000000];  // Buffer size (# of characters) for SD card
00012 
00013 MPU6050 mpu6050;
00014 MOUSE mouse;
00015 Timer t;
00016 
00017 DigitalIn button(PTB9);
00018 
00019 signed long mx, my;
00020 float d,dx,dy;
00021 double x,xx,y,yy;
00022 double psi,lpsi=0;   // psi - current orientation, lpsi - last orientation, theta - current angle around circle
00023 double gyroZ,lgyroZ;
00024 double dt, timer;
00025 
00026 bool closed = 1;    // Flag for sd card, to open and close with switch
00027 
00028 
00029 //================================================================================
00030 //=============               Sensor Initialization                  =============
00031 //================================================================================
00032 
00033 void gyro_init(){
00034     i2c.frequency(400000);
00035     
00036     uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
00037     pc.printf("I AM 0x%x\n\r", whoami);
00038     pc.printf("I SHOULD BE 0x68\n\r");
00039 
00040     if (whoami == 0x68){
00041         pc.printf("MPU6050 is online...");
00042         wait(1);
00043 
00044         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
00045         if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
00046             mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00047             mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00048             mpu6050.initMPU6050();
00049             pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00050 
00051             pc.printf("\nMPU6050 passed self test... Initializing");
00052             wait(2); //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00053         } else pc.printf("\nDevice did not the pass self-test!\n\r");
00054     } else {
00055         pc.printf("Could not connect to MPU6050: \n\r");
00056         pc.printf("%#x \n",  whoami);
00057         while(1) ; // Loop forever if communication doesn't happen
00058     }
00059 }
00060 
00061 
00062 //================================================================================
00063 //=============                     Get Inputs                       =============
00064 //================================================================================
00065 
00066 void get_Rates(){
00067     if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00068         mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc gyro values
00069         mpu6050.getGres();
00070         gyroZ = gyroCount[2]*gRes; // - gyroBias[2];
00071     }
00072 }
00073 
00074 
00075 //================================================================================
00076 //=============                 Main Program Start                   =============
00077 //================================================================================
00078 
00079 int main(){
00080     pc.baud(115200);
00081     
00082     Data = fopen("/sd/Data.txt", "w");
00083     closed = 0;
00084     
00085     gyro_init();
00086     mouse.init();
00087     
00088     t.start();
00089     timer = t.read_us();
00090 
00091     while(1) {
00092         if(button) {
00093             
00094             if (closed){
00095                 Data = fopen("/sd/Data.txt", "w");
00096                 closed = 0;
00097             }
00098             
00099             DigitalOut myled(LED1);
00100                 
00101             get_Rates();  // Gyro rates & Mouse position
00102 
00103             dt = (t.read_us() - timer) / 1000000.00;
00104             timer = t.read_us();
00105 
00106 
00107 //================================================================================
00108 //=============                   Char Conversion                    =============
00109 //================================================================================
00110 
00111             psi += ((lgyroZ+gyroZ)/2)*dt;  // Current orientation/pose
00112             if (psi>360) psi -= 360; // resets current orientation to keep between 0 & 360
00113             if (psi<-360) psi += 360; // resets current orientation to keep between 0 & -360
00114             lgyroZ = gyroZ;
00115                         
00116             mx = mouse.getDx();
00117             my = mouse.getDy();
00118             dx = (mx/231.25);  // Converts char into float in inches
00119             dy = (my/231.25);  // Experimentally obtained 231.25 (1850/8in)
00120                         
00121             double a1 = psi;
00122             a1 *= (PI/180);
00123             double a2 = lpsi;
00124             a2 *= (PI/180);
00125             double a3 = a1-a2;
00126                         
00127             double a = dx*tan(a3);
00128             double b = dx/cos(a3);
00129             double c = (dy+a)*cos(a2)-(b*sin(a1));
00130             double d = sqrt((dx*dx)+(dy*dy));
00131             double e = sqrt((d*d)-(c*c));
00132             double f = (dy+a)*cos(a3);
00133             double g = c*cos(a1);
00134             double h = (f-g)/(sin(a1));
00135             
00136             yy -= c;
00137             xx += h;
00138             
00139             x = xx + 3;
00140             y = yy - 9.5;
00141             
00142             lpsi = psi;
00143             
00144             if (Data == NULL) pc.printf("Unable to write the file \n");  // This chunk for writing to file on SD card
00145             else fprintf(Data, "%f %f %f %f\r\n",psi,x,y,timer);  // can read float or double not int (only checked these three types)
00146                         
00147         }else{
00148             if(!closed){                                                                                                           
00149                 fclose(Data);
00150                 closed = 1;
00151             }
00152         }
00153     }
00154 }