Diff: realtime.h

Revision:

17:bd6b6ac89e0e

Parent:

15:960b922433d1

Child:

19:2fe650d29823

diff -r 6ec1aa32cf1f -r bd6b6ac89e0e realtime.h
--- 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(){