svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
Diff: realtime.h
- Revision:
- 17:bd6b6ac89e0e
- Parent:
- 15:960b922433d1
- Child:
- 19:2fe650d29823
--- a/realtime.h Sat Dec 01 14:31:24 2018 +0000 +++ b/realtime.h Sun Feb 10 12:05:16 2019 +0000 @@ -14,7 +14,7 @@ float tan_error = delta_s * cos(delta_phi_0); float a = x_prop * tan_error - x_diff * current.speed; //Motion PID - if ((current.speed > max_speed) && (a > 0)) a = 0 ; if ((current.speed < -max_speed) && (a < 0)) a = 0 ; + if ((current.speed > max_speed) && (a > 0)) a = max_accel * (2 - current.speed/max_speed); if ((current.speed < -max_speed) && (a < 0)) a = -max_accel * (2 + current.speed/max_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; //Balance PID @@ -35,7 +35,7 @@ set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis); //Set motor - if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy/5)) coord_ready = 1; else coord_ready = 0; + if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy/2) && (abs(target.azimuth - current.azimuth) < 0.1)) coord_ready = 1; else coord_ready = 0; } void balance_motion(){