Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

gyro.h@19:2fe650d29823, 2019-02-24 (annotated)

Committer:
dima285
Date:
Sun Feb 24 11:02:56 2019 +0000
Revision:
19:2fe650d29823
Parent:
12:721a9ea55e91
Child:
22:14e85f2068c7
3 PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dima285 19:2fe650d29823 1 float gx,gy,gz,ax,ay,az; //deg/s, m/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 gyro.start();
Stas285 4:904b737ef08a 11 }
Stas285 1:e2a6e523bf1f 12
dima285 12:721a9ea55e91 13 /*void gyro_process(){ // reading - 500 uS // doesn't work in interrupt
Stas285 4:904b737ef08a 14 gyro.read(&gx,&gy,&gz,&ax,&ay,&az);
dima285 9:8f98b1c277a4 15
Stas285 6:6e89cdc3db92 16 //wifi.printf("gx %.1f, gy %.1f, gz %.1f, ax %.2f, ay %.2f, az %.2f\n",gx,gy,gz,ax,ay,az);
dima285 9:8f98b1c277a4 17 //wifi.printf("%.4f %.2f ",ay,gz);
dima285 12:721a9ea55e91 18 }*/
Stas285 1:e2a6e523bf1f 19
dima285 12:721a9ea55e91 20 void inertial_navigation(){
dima285 12:721a9ea55e91 21 gyro_v -= t_step*ax; //inertial_navigation
dima285 12:721a9ea55e91 22 gyro_s += t_step*gyro_v;
dima285 12:721a9ea55e91 23 gyro_angle += t_step*gx; //gx?
dima285 12:721a9ea55e91 24 gyro_x += t_step*gyro_v*sin(gyro_angle);
dima285 12:721a9ea55e91 25 gyro_y += t_step*gyro_v*cos(gyro_angle); //sqr(1-x^2)
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