svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
Diff: realtime.h
- Revision:
- 13:789b451cc27d
- Parent:
- 12:721a9ea55e91
- Child:
- 14:e12d0fdc3a48
diff -r 721a9ea55e91 -r 789b451cc27d realtime.h --- a/realtime.h Sat Nov 03 11:43:03 2018 +0000 +++ b/realtime.h Sun Nov 11 09:20:35 2018 +0000 @@ -9,13 +9,40 @@ void balance() { - float a = x_prop * (current.x - target.x) + x_diff * current.speed; - if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; - float delta_v = (2*balance_prop*ax + balance_diff*5.73*gy - a) * t_step; + float a; + float eps; + float azimuth_to_target; + float normal_error; + float tan_error; + + float delta_x = (target.x - current.x); + if (delta_x == 0) delta_x = 0.0001; + float delta_y = (target.y - current.y); + float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y); + if (delta_x < 0) {delta_s = -delta_s;} + + azimuth_to_target = atan(delta_y/delta_x); + float delta_phi_0 = azimuth_to_target - current.azimuth; + float delta_phi_1 = (target.azimuth - azimuth_to_target) ;//* (current.speed * t_step / delta_s); + float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * delta_phi_1 + delta_s/(delta_s + coord_accuracy) * delta_phi_0; //ZNAK - set_motor_speed(motor_speed[0] - delta_v, motor_speed[1] - delta_v); + normal_error = delta_s * sin(delta_phi); + tan_error = delta_s * cos(delta_phi); + - wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100); + if (abs(current.speed) > max_speed) a = 0 ; else a = x_prop * tan_error - x_diff * current.speed; + if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel; + float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step; + + if (abs(normal_error) < coord_accuracy) {eps = 0;current.omega = 0;} else eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; //gz * azimuth_diff; + float delta_omega = eps * t_step; + + set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis); + + if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy)) ready = 1; else ready = 0; + + // wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100); + 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); }