Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

gyro.h@10:5bdd3dfd5f59, 2018-09-05 (annotated)

Committer:
dima285
Date:
Wed Sep 05 18:25:54 2018 +0000
Revision:
10:5bdd3dfd5f59
Parent:
9:8f98b1c277a4
Child:
12:721a9ea55e91
Stable 05.09.2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dima285 10:5bdd3dfd5f59 1 float gx,gy,gz,ax,ay,az; //deg/s, cm/s^2 (x-vert, y-forward, z-left)
Stas285 6:6e89cdc3db92 2 #include "MPU6050.h" //Rewrite !!!
Stas285 1:e2a6e523bf1f 3
Stas285 4:904b737ef08a 4 MPU6050 gyro(PB_9,PB_8);
dima285 10:5bdd3dfd5f59 5
Stas285 4:904b737ef08a 6 float gyro_x, gyro_y, gyro_angle, gyro_v, gyro_s; //cm, deg
Stas285 4:904b737ef08a 7
Stas285 4:904b737ef08a 8 void gyro_init(){
Stas285 4:904b737ef08a 9 gyro.getID(); //dummy read (first transmission is bad)
Stas285 4:904b737ef08a 10 //pc.printf("%hhu ",gyro.getID());
Stas285 4:904b737ef08a 11 gyro.start();
Stas285 4:904b737ef08a 12 }
Stas285 1:e2a6e523bf1f 13
Stas285 4:904b737ef08a 14 void gyro_process(){ // reading - 500 uS // doesn't work in interrupt
Stas285 4:904b737ef08a 15 gyro.read(&gx,&gy,&gz,&ax,&ay,&az);
dima285 9:8f98b1c277a4 16
Stas285 6:6e89cdc3db92 17 //wifi.printf("gx %.1f, gy %.1f, gz %.1f, ax %.2f, ay %.2f, az %.2f\n",gx,gy,gz,ax,ay,az);
dima285 9:8f98b1c277a4 18 //wifi.printf("%.4f %.2f ",ay,gz);
Stas285 1:e2a6e523bf1f 19
Stas285 4:904b737ef08a 20 gyro_v -= 0.05*100*ay; //0.05 -> realtime_step //inertial_navigation
Stas285 4:904b737ef08a 21 gyro_s += 0.05*gyro_v;
Stas285 4:904b737ef08a 22 gyro_angle += 0.05*gx;
Stas285 4:904b737ef08a 23 gyro_x += 0.05*gyro_v*sin(gyro_angle);
Stas285 4:904b737ef08a 24 gyro_y += 0.05*gyro_v*cos(gyro_angle); //sqr(1-x^2)
Stas285 4:904b737ef08a 25
Stas285 6:6e89cdc3db92 26 //wifi.printf("v:%.1f, s:%.1f, deg:%.1f, x:%.1f, y:%.1f\n",gyro_v,gyro_s,gyro_angle,gyro_x,gyro_y);
Stas285 4:904b737ef08a 27 }
Stas285 4:904b737ef08a 28
Stas285 6:6e89cdc3db92 29 void gyro_balance(){ //?????
Stas285 6:6e89cdc3db92 30 //gyro_read_ay();
Stas285 6:6e89cdc3db92 31 }
Stas285 6:6e89cdc3db92 32