Diff: gyro.h

Revision:

6:6e89cdc3db92

Parent:

4:904b737ef08a

Child:

7:9e4a997ad23a

Child:

8:891e4f54e9e2

--- a/gyro.h	Sun May 14 11:30:26 2017 +0000
+++ b/gyro.h	Sun May 21 12:04:00 2017 +0000
@@ -1,4 +1,4 @@
-#include "MPU6050.h"
+#include "MPU6050.h" //Rewrite !!!
 
 MPU6050 gyro(PB_9,PB_8);
 float gx,gy,gz,ax,ay,az; //deg/s, cm/s^2  (x-vert, y-forward, z-left)
@@ -12,7 +12,8 @@
 
 void gyro_process(){ // reading - 500 uS  // doesn't work in interrupt
     gyro.read(&gx,&gy,&gz,&ax,&ay,&az);
-    wifi.printf("gx %.1f, gy %.1f, gz %.1f, ax %.2f, ay %.2f, az %.2f\n",gx,gy,gz,ax,ay,az);
+    //wifi.printf("gx %.1f, gy %.1f, gz %.1f, ax %.2f, ay %.2f, az %.2f\n",gx,gy,gz,ax,ay,az);
+    //wifi.printf("ay:%.2f, ",ay);
 
     gyro_v -= 0.05*100*ay; //0.05 -> realtime_step //inertial_navigation
     gyro_s += 0.05*gyro_v;
@@ -20,8 +21,10 @@
     gyro_x += 0.05*gyro_v*sin(gyro_angle);
     gyro_y += 0.05*gyro_v*cos(gyro_angle); //sqr(1-x^2)
     
-    wifi.printf("v:%.1f, s:%.1f, deg:%.1f, x:%.1f, y:%.1f\n",gyro_v,gyro_s,gyro_angle,gyro_x,gyro_y);
-    
-// Balance feedback
+    //wifi.printf("v:%.1f, s:%.1f, deg:%.1f, x:%.1f, y:%.1f\n",gyro_v,gyro_s,gyro_angle,gyro_x,gyro_y);
 }
 
+void gyro_balance(){ //?????
+    //gyro_read_ay();
+    }
+