svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
realtime.h@13:789b451cc27d, 2018-11-11 (annotated)
- Committer:
- dima285
- Date:
- Sun Nov 11 09:20:35 2018 +0000
- Revision:
- 13:789b451cc27d
- Parent:
- 12:721a9ea55e91
- Child:
- 14:e12d0fdc3a48
1
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 | 13:789b451cc27d | 12 | float a; |
dima285 | 13:789b451cc27d | 13 | float eps; |
dima285 | 13:789b451cc27d | 14 | float azimuth_to_target; |
dima285 | 13:789b451cc27d | 15 | float normal_error; |
dima285 | 13:789b451cc27d | 16 | float tan_error; |
dima285 | 13:789b451cc27d | 17 | |
dima285 | 13:789b451cc27d | 18 | float delta_x = (target.x - current.x); |
dima285 | 13:789b451cc27d | 19 | if (delta_x == 0) delta_x = 0.0001; |
dima285 | 13:789b451cc27d | 20 | float delta_y = (target.y - current.y); |
dima285 | 13:789b451cc27d | 21 | float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y); |
dima285 | 13:789b451cc27d | 22 | if (delta_x < 0) {delta_s = -delta_s;} |
dima285 | 13:789b451cc27d | 23 | |
dima285 | 13:789b451cc27d | 24 | azimuth_to_target = atan(delta_y/delta_x); |
dima285 | 13:789b451cc27d | 25 | float delta_phi_0 = azimuth_to_target - current.azimuth; |
dima285 | 13:789b451cc27d | 26 | float delta_phi_1 = (target.azimuth - azimuth_to_target) ;//* (current.speed * t_step / delta_s); |
dima285 | 13:789b451cc27d | 27 | float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * delta_phi_1 + delta_s/(delta_s + coord_accuracy) * delta_phi_0; //ZNAK |
dima285 | 12:721a9ea55e91 | 28 | |
dima285 | 13:789b451cc27d | 29 | normal_error = delta_s * sin(delta_phi); |
dima285 | 13:789b451cc27d | 30 | tan_error = delta_s * cos(delta_phi); |
dima285 | 13:789b451cc27d | 31 | |
dima285 | 12:721a9ea55e91 | 32 | |
dima285 | 13:789b451cc27d | 33 | if (abs(current.speed) > max_speed) a = 0 ; else a = x_prop * tan_error - x_diff * current.speed; |
dima285 | 13:789b451cc27d | 34 | if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; |
dima285 | 13:789b451cc27d | 35 | float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step; |
dima285 | 13:789b451cc27d | 36 | |
dima285 | 13:789b451cc27d | 37 | if (abs(normal_error) < coord_accuracy) {eps = 0;current.omega = 0;} else eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; //gz * azimuth_diff; |
dima285 | 13:789b451cc27d | 38 | float delta_omega = eps * t_step; |
dima285 | 13:789b451cc27d | 39 | |
dima285 | 13:789b451cc27d | 40 | set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis); |
dima285 | 13:789b451cc27d | 41 | |
dima285 | 13:789b451cc27d | 42 | if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy)) ready = 1; else ready = 0; |
dima285 | 13:789b451cc27d | 43 | |
dima285 | 13:789b451cc27d | 44 | // wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100); |
dima285 | 13:789b451cc27d | 45 | wifi.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f;",current.x,target.x,delta_x,delta_s,current.speed,a,delta_v,ax,gy); |
dima285 | 12:721a9ea55e91 | 46 | } |
dima285 | 12:721a9ea55e91 | 47 | |
dima285 | 9:8f98b1c277a4 | 48 | |
Stas285 | 4:904b737ef08a | 49 | void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos) |
dima285 | 12:721a9ea55e91 | 50 | /* float path_error = target_path-current_path; |
dima285 | 12:721a9ea55e91 | 51 | float target_speed = 0.1*path_error;//1m/s at 10cm |
dima285 | 12:721a9ea55e91 | 52 | float linear_acceleration = 1*(target_speed-current_speed);*/ |
dima285 | 12:721a9ea55e91 | 53 | |
Stas285 | 0:e9488589a8ee | 54 | |
dima285 | 12:721a9ea55e91 | 55 | realtime_flag = 1; |
dima285 | 12:721a9ea55e91 | 56 | } |
dima285 | 12:721a9ea55e91 | 57 | |
dima285 | 12:721a9ea55e91 | 58 | void realtime_init(){ |
dima285 | 12:721a9ea55e91 | 59 | rt_ticker.attach(&realtime, t_step); } |
dima285 | 12:721a9ea55e91 | 60 | |
dima285 | 12:721a9ea55e91 | 61 | |
Stas285 | 0:e9488589a8ee | 62 |