Diff: gyro.h

Revision:

4:904b737ef08a

Parent:

1:e2a6e523bf1f

Child:

6:6e89cdc3db92

--- a/gyro.h	Sun May 07 08:37:22 2017 +0000
+++ b/gyro.h	Sun May 14 11:28:29 2017 +0000
@@ -1,12 +1,27 @@
 #include "MPU6050.h"
 
-//MPU6050 gyro(PB_9,PB_8);
-float gx,gy,gz,ax,ay,az;
+MPU6050 gyro(PB_9,PB_8);
+float gx,gy,gz,ax,ay,az; //deg/s, cm/s^2  (x-vert, y-forward, z-left)
+float gyro_x, gyro_y, gyro_angle, gyro_v, gyro_s; //cm, deg
+
+void gyro_init(){
+    gyro.getID(); //dummy read (first transmission is bad)
+    //pc.printf("%hhu ",gyro.getID());
+    gyro.start();
+    }
 
-void gyro_process(){
-//    gyro.read(&gx,&gy,&gz,&ax,&ay,&az);
-//    pc.printf("gx,gy,gz,ax,ay,az %.1f,%.1f,%.1f,%.2f,%.2f,%.2f\r\n",gx,gy,gz,ax,ay,az);
+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);
 
+    gyro_v -= 0.05*100*ay; //0.05 -> realtime_step //inertial_navigation
+    gyro_s += 0.05*gyro_v;
+    gyro_angle += 0.05*gx;
+    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
-// Inertial navigation
-}
\ No newline at end of file
+}
+