Diff: main.cpp

Revision:

5:3a9280cea2ff

Parent:

4:9d706f783b5a

Child:

6:981cb17c7ea3

--- a/main.cpp	Thu Aug 10 14:45:40 2017 +0000
+++ b/main.cpp	Fri Aug 11 07:44:36 2017 +0000
@@ -8,10 +8,9 @@
 unsigned char fifo_count[2]={32,32};
 unsigned char temp[1]={32};
 unsigned char outbuff[4];
-long DEBUG[]={2,2,2,2};
-unsigned long tamper;
+unsigned char inbuff[4];
 unsigned char regadd;
-char registeradress[20];
+char registeradress[5];
 unsigned char registerdata[]={33};
 void printIMUData(void);
 int main() 
@@ -20,7 +19,7 @@
   pc.printf("Hello World\n");
   imu_init();
   stamper_init();
-#if 1
+#if 1//Regular program
   // Call imu.begin() to verify communication and initialize
   if (imu.begin() != INV_SUCCESS){
     while (1){
@@ -29,7 +28,7 @@
       wait_ms(5000);
     }
   }
-  pc.printf("imu.begin() suceeded\n");
+    pc.printf("imu.begin() suceeded\n");
   
     if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
                DMP_FEATURE_GYRO_CAL, // Use gyro calibration
@@ -40,16 +39,17 @@
         pc.printf("imu.dmpBegin() suceeded\n");
     }
 #endif
-#if 0//processes here is not important if imu.dmpBegin is not working -> 0 if dmpBegin fails
+#if 0//this scope is only for debugging purposes of mbed_i2c_read and write functions
     mbed_i2c_read(0x68,0x75,1,whoami);
     pc.printf("whoami=%d\n",whoami[0]);
-    mbed_i2c_read(0x68,0x23,1,outbuff);
-    pc.printf("out buffer=%d\n",outbuff[0]);
-    outbuff[0]|=1;
-    pc.printf("updated out buffer=%d\n",outbuff[0]);
+    mbed_i2c_read(0x68,0x23,2,inbuff);
+    pc.printf("Buffer=%d-%d\n",inbuff[0],inbuff[1]);
+    outbuff[0]=inbuff[0]&0x01;
+    outbuff[1]=inbuff[1]&0x02;
+    pc.printf("updated buffer=%d-%d\n",outbuff[0],outbuff[1]);
     mbed_i2c_write(0x68,0x23,2,outbuff);
-    mbed_i2c_read(0x68,0x23,1,outbuff);
-    pc.printf("new out buffer=%d(if zero, write function still needs to be fixed)\n",outbuff[0]);
+    mbed_i2c_read(0x68,0x23,2,inbuff);
+    pc.printf("new out buffer=%d-%d(if zero, write function still needs to be fixed)\n",inbuff[0],inbuff[1]);
 #endif
 while(1){
     if(pc.readable()){
@@ -58,41 +58,33 @@
         mbed_i2c_read(0x68,(unsigned)regadd,1,registerdata);
         pc.printf("%d is gotten from serial port, data at that register is %d\n",regadd,registerdata[0]);
     }
-    /*unsigned char buff[2]={0x38,0x39};
-    pc.printf("trial-%s-%s\n",buff,(const char*)buff);*/
-  /*imu.updateTemperature();
-  temp[0]=imu.temperature;
-  get_ms(&tamper);
-  pc.printf("%d-%d\n",temp[0],tamper);*/
+    /*if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
+               DMP_FEATURE_GYRO_CAL, // Use gyro calibration
+              20)==INV_ERROR){ // Set DMP FIFO rate to 20 Hz
+        pc.printf("imu.dmpBegin have failed\n");//dmpLoad function under it fails which is caused by memcmp(firmware+ii, cur, this_write) (it is located at row 2871 of inv_mpu.c)
+              }
+    else{
+        pc.printf("imu.dmpBegin() suceeded\n");
+    }*/
   // Check for new data in the FIFO
   if (imu.fifoAvailable()){//fifo is not being available
     led=0;
-    wait_ms(500);
+    wait_ms(50);
     led=1;
-    wait_ms(500);
+    wait_ms(50);
     // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
-    if ( imu.dmpUpdateFifo(DEBUG) == INV_SUCCESS){
+    if ( imu.dmpUpdateFifo() == INV_SUCCESS){
       // computeEulerAngles can be used -- after updating the
       // quaternion values -- to estimate roll, pitch, and yaw
       imu.computeEulerAngles();
-      //printIMUData();
-        pc.printf("dmpUpdateFifo sucesfully occured\n");
-     #if 1
-      pc.printf("%lf",DEBUG[0]);
-      pc.printf("\t");
-      pc.printf("%lf",DEBUG[1]);
-      pc.printf("\t");
-      pc.printf("%lf",DEBUG[2]);
-      pc.printf("\t");
-      pc.printf("%lf\n ",DEBUG[3]);
-     #endif
+      printIMUData();
     }
   }
   else{
     led=1;
-    wait_ms(100);
+    wait_ms(500);
     led=0;
-    wait_ms(100);      
+    wait_ms(500);      
   }
 }
   return 0;
@@ -109,5 +101,5 @@
   float q2 = imu.calcQuat(imu.qy);
   float q3 = imu.calcQuat(imu.qz);
   
-  pc.printf("R:%lf  P:%lf   Y:%lf \n", imu.roll,imu.pitch,imu.yaw);
+  pc.printf("Y:%lf  P:%lf   R:%lf \n", imu.yaw,imu.pitch,imu.roll);
 }