svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
realtime.h@12:721a9ea55e91, 2018-11-03 (annotated)
- Committer:
- dima285
- Date:
- Sat Nov 03 11:43:03 2018 +0000
- Revision:
- 12:721a9ea55e91
- Parent:
- 10:5bdd3dfd5f59
- Child:
- 13:789b451cc27d
rabotaet;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Stas285 | 0:e9488589a8ee | 1 | Ticker rt_ticker; |
Stas285 | 4:904b737ef08a | 2 | bool realtime_flag; |
dima285 | 12:721a9ea55e91 | 3 | /*float balance_prop = 0.8; |
dima285 | 12:721a9ea55e91 | 4 | float balance_diff = 1.2; |
dima285 | 12:721a9ea55e91 | 5 | float x_prop = 40; |
dima285 | 12:721a9ea55e91 | 6 | float x_diff = 25; |
dima285 | 12:721a9ea55e91 | 7 | float azimuth_prop = 0.8; |
dima285 | 12:721a9ea55e91 | 8 | float azimuth_diff = 0.65;*/ |
Stas285 | 0:e9488589a8ee | 9 | |
dima285 | 12:721a9ea55e91 | 10 | void balance() |
dima285 | 12:721a9ea55e91 | 11 | { |
dima285 | 12:721a9ea55e91 | 12 | float a = x_prop * (current.x - target.x) + x_diff * current.speed; |
dima285 | 12:721a9ea55e91 | 13 | if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; |
dima285 | 12:721a9ea55e91 | 14 | float delta_v = (2*balance_prop*ax + balance_diff*5.73*gy - a) * t_step; |
dima285 | 12:721a9ea55e91 | 15 | |
dima285 | 12:721a9ea55e91 | 16 | set_motor_speed(motor_speed[0] - delta_v, motor_speed[1] - delta_v); |
dima285 | 12:721a9ea55e91 | 17 | |
dima285 | 12:721a9ea55e91 | 18 | wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100); |
dima285 | 12:721a9ea55e91 | 19 | } |
dima285 | 12:721a9ea55e91 | 20 | |
dima285 | 9:8f98b1c277a4 | 21 | |
Stas285 | 4:904b737ef08a | 22 | void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos) |
dima285 | 12:721a9ea55e91 | 23 | /* float path_error = target_path-current_path; |
dima285 | 12:721a9ea55e91 | 24 | float target_speed = 0.1*path_error;//1m/s at 10cm |
dima285 | 12:721a9ea55e91 | 25 | float linear_acceleration = 1*(target_speed-current_speed);*/ |
dima285 | 12:721a9ea55e91 | 26 | |
Stas285 | 0:e9488589a8ee | 27 | |
dima285 | 12:721a9ea55e91 | 28 | realtime_flag = 1; |
dima285 | 12:721a9ea55e91 | 29 | } |
dima285 | 12:721a9ea55e91 | 30 | |
dima285 | 12:721a9ea55e91 | 31 | void realtime_init(){ |
dima285 | 12:721a9ea55e91 | 32 | rt_ticker.attach(&realtime, t_step); } |
dima285 | 12:721a9ea55e91 | 33 | |
dima285 | 12:721a9ea55e91 | 34 | |
Stas285 | 0:e9488589a8ee | 35 |