Diff: main.cpp

Revision:

3:4b0e7e8a06ae

Parent:

1:cea9d83b8636

--- a/main.cpp	Sun Jun 29 21:53:23 2014 +0000
+++ b/main.cpp	Sat Feb 28 15:36:41 2015 +0000
@@ -28,7 +28,7 @@
  
 #include "mbed.h"
 #include "MPU6050.h"
-#include "N5110.h"
+//#include "N5110.h"
 
 // Using NOKIA 5110 monochrome 84 x 48 pixel display
 // pin 9 - Serial clock out (SCLK)
@@ -38,9 +38,27 @@
 // pin 6 - LCD reset (RST)
 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
 
+AnalogIn analog_value0(A0);
+AnalogIn analog_value1(A1);
+AnalogIn analog_value2(A2);
+AnalogIn analog_value3(A3);
+AnalogIn analog_value4(A4);
+AnalogIn analog_value5(A5);
+
+
+
+
 float sum = 0;
 uint32_t sumCount = 0;
 
+int time_us = 0;
+unsigned int yawH,yawL,pitchH,pitchL,rowH,rowL;
+unsigned int axH,axL,ayH,ayL,azH,azL;
+unsigned int gxH,gxL,gyH,gyL,gzH,gzL;
+unsigned int f0,f1,f2;
+unsigned int baroH,baroL;
+
+
    MPU6050 mpu6050;
    
    Timer t;
@@ -48,19 +66,51 @@
    Serial pc(USBTX, USBRX); // tx, rx
 
    //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
-   N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
+   //N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
+   
+void callbackPCSerial() {
+    pc.putc(255);
+    pc.putc(rowH);
+    pc.putc(rowL);
+    pc.putc(pitchH);
+    pc.putc(pitchL);
+    pc.putc(yawH);
+    pc.putc(yawL);
+ 
+    //pc.putc(axH);
+    //pc.putc(axL);
+    //pc.putc(ayH);
+    //pc.putc(ayL);
+    //pc.putc(azH);
+    //pc.putc(azL);
+ 
+    //pc.putc(gxH);
+    //pc.putc(gxL);
+    //pc.putc(gyH);
+    //pc.putc(gyL);
+    //pc.putc(gzH);
+    //pc.putc(gzL);
+
+    //pc.putc(baroH);
+    //pc.putc(baroL);
+
+    //pc.putc(f0);
+    //pc.putc(f1);
+    //pc.putc(f2);
+
+}
         
 int main()
 {
-  pc.baud(9600);  
-
+  pc.baud(115200);  
+    pc.attach(&callbackPCSerial);
   //Set up I2C
   i2c.frequency(400000);  // use fast (400 kHz) I2C   
   
   t.start();        
   
-  lcd.init();
-  lcd.setBrightness(0.05);
+  //lcd.init();
+  //lcd.setBrightness(0.05);
   
     
   // Read the WHO_AM_I register, this is a good test of communication
@@ -71,8 +121,8 @@
   {  
     pc.printf("MPU6050 is online...");
     wait(1);
-    lcd.clear();
-    lcd.printString("MPU6050 OK", 0, 0);
+    //lcd.clear();
+    //lcd.printString("MPU6050 OK", 0, 0);
 
     
     mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
@@ -90,20 +140,20 @@
     mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
     mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
 
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("pass self test", 0, 1);
-    lcd.printString("initializing", 0, 2);  
+    //lcd.clear();
+    //lcd.printString("MPU6050", 0, 0);
+    //lcd.printString("pass self test", 0, 1);
+    //lcd.printString("initializing", 0, 2);  
     wait(2);
        }
     else
     {
     pc.printf("Device did not the pass self-test!\n\r");
  
-       lcd.clear();
-       lcd.printString("MPU6050", 0, 0);
-       lcd.printString("no pass", 0, 1);
-       lcd.printString("self test", 0, 2);      
+       //lcd.clear();
+       //lcd.printString("MPU6050", 0, 0);
+       //lcd.printString("no pass", 0, 1);
+       //lcd.printString("self test", 0, 2);      
       }
     }
     else
@@ -111,10 +161,10 @@
     pc.printf("Could not connect to MPU6050: \n\r");
     pc.printf("%#x \n",  whoami);
  
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("no connection", 0, 1);
-    lcd.printString("0x", 0, 2);  lcd.setXYAddress(20, 2); lcd.printChar(whoami);
+    //lcd.clear();
+    //lcd.printString("MPU6050", 0, 0);
+    //lcd.printString("no connection", 0, 1);
+    //lcd.printString("0x", 0, 2);  lcd.setXYAddress(20, 2); lcd.printChar(whoami);
  
     while(1) ; // Loop forever if communication doesn't happen
   }
@@ -131,7 +181,16 @@
     // Now we'll calculate the accleration value into actual g's
     ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
     ay = (float)accelCount[1]*aRes - accelBias[1];   
-    az = (float)accelCount[2]*aRes - accelBias[2];  
+    az = (float)accelCount[2]*aRes - accelBias[2]; 
+    ax = (ax+5)*100;
+    ay = (ay+5)*100;
+    az = (az+5)*100;
+    axH = ((int)ax)/256 ;
+    axL = ((int)ax)%256;
+    ayH = ((int)ay)/256 ;
+    ayL = ((int)ay)%256;
+    azH = ((int)az)/256 ;
+    azL = ((int)az)%256; 
    
     mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
     mpu6050.getGres();
@@ -140,6 +199,15 @@
     gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
     gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
     gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
+    gx = (gx+500)*10;
+    gy = (gy+500)*10;
+    gz = (gz+500)*10;
+    gxH = ((int)gx)/256 ;
+    gxL = ((int)gx)%256;
+    gyH = ((int)gy)/256 ;
+    gyL = ((int)gy)%256;
+    gzH = ((int)gz)/256 ;
+    gzL = ((int)gz)%256;
 
     tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
     temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
@@ -162,29 +230,29 @@
 
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - count;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
+    if (delt_t > 50) { // update LCD once per half-second independent of read rate
 
-    pc.printf("ax = %f", 1000*ax); 
-    pc.printf(" ay = %f", 1000*ay); 
-    pc.printf(" az = %f  mg\n\r", 1000*az); 
+    //pc.printf("ax = %f", 1000*ax); 
+    //pc.printf(" ay = %f", 1000*ay); 
+    //pc.printf(" az = %f  mg\n\r", 1000*az); 
 
-    pc.printf("gx = %f", gx); 
-    pc.printf(" gy = %f", gy); 
-    pc.printf(" gz = %f  deg/s\n\r", gz); 
+    //pc.printf("gx = %f", gx); 
+    //pc.printf(" gy = %f", gy); 
+    //pc.printf(" gz = %f  deg/s\n\r", gz); 
     
-    pc.printf(" temperature = %f  C\n\r", temperature); 
+    //pc.printf(" temperature = %f  C\n\r", temperature); 
     
-    pc.printf("q0 = %f\n\r", q[0]);
-    pc.printf("q1 = %f\n\r", q[1]);
-    pc.printf("q2 = %f\n\r", q[2]);
-    pc.printf("q3 = %f\n\r", q[3]);      
+    //pc.printf("q0 = %f\n\r", q[0]);
+    //pc.printf("q1 = %f\n\r", q[1]);
+    //pc.printf("q2 = %f\n\r", q[2]);
+    //pc.printf("q3 = %f\n\r", q[3]);      
     
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("x   y   z", 0, 1);
-    lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
-    lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
-    lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
+    //lcd.clear();
+    //lcd.printString("MPU6050", 0, 0);
+    //lcd.printString("x   y   z", 0, 1);
+    //lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
+    //lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
+    //lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
     
     
   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
@@ -212,7 +280,7 @@
 //    pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
 
      pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-     pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
  
     myled= !myled;
     count = t.read_ms();