Diff: main.cpp

Revision:

1:a8a5c74bdfa6

Parent:

0:4ff212e163fe

Child:

2:c463326536ae

--- a/main.cpp	Wed Nov 13 00:55:05 2019 +0000
+++ b/main.cpp	Wed Nov 13 02:55:57 2019 +0000
@@ -13,9 +13,14 @@
 
 Serial pc(USBTX, USBRX);
 Serial Ardu(p9,p10);
+void inline MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX); 
+float inline filteredAngle(float *pitch, float *gyrX);
 int main()
 {
     uint8_t charArr[6];
+    int16_t accy;
+    int16_t accz;
+    int16_t gyr;
     float angleX =0;
     float samplerate = 9600.0;
     float finalAx=0;
@@ -25,6 +30,24 @@
     int k =1; // this is for getting the initial angle the first time around so we can start at zero
     int j=0;
     while (true) {
+        MPU6050Values(&Ardu,&accy,&accz,&gyr, &samplerate, &k, &offset,&pitch, &angleX);
+        finalAx = filteredAngle(&pitch, &angleX);
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        if(j=1000){
+        pc.printf("calc angle: %f.2\n\r",finalAx);
+        j=0;
+        }else
+        j++;
+        /*
         for(int i =0; i<6;i++)
         charArr[i]=Ardu.getc();
         
@@ -47,5 +70,27 @@
         j=0;
         }else
         j++;
+        */
     }
 }
+
+inline void MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX){
+    uint8_t charArr[6];
+    for(int i =0; i<6;i++)
+        charArr[i]=Ardu->getc();
+    
+    (*accy)= charArr[0]<<8|charArr[1];
+    (*accz)= charArr[2]<<8|charArr[3];
+    (*gyr)= charArr[4]<<8|charArr[5];
+    (*pitch) = atan((*accy)/(*accz));
+    if(*k){
+        (*offset) = (*gyr);//gets the offset or starting point the first time around
+        *k=0;
+        }
+        
+    (*angleX) += ((*gyr) - (*offset))/(*samplerate);
+    }
+
+inline float filteredAngle(float *pitch, float *angleX){
+    return (*pitch)*(0.02)+(*angleX)*(0.98);
+    }