Diff: realtime.h

Revision:

6:6e89cdc3db92

Parent:

4:904b737ef08a

Child:

7:9e4a997ad23a

Child:

8:891e4f54e9e2

--- a/realtime.h	Sun May 14 11:30:26 2017 +0000
+++ b/realtime.h	Sun May 21 12:04:00 2017 +0000
@@ -1,21 +1,21 @@
 Ticker rt_ticker;
-//float tm_speed;
 float rt_delta;
 bool realtime_flag;
 
 void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos)
-    //myled = !myled;
+    myled = !myled;
 //    test = 1;
-    
-    rt_delta = (motor_speed[1]+motor_speed[0])/2*0.05; //geometric navigation
-    current_angle += (motor_speed[0]-motor_speed[1])*0.05/16*57.3;//deg
+
+//geometric navigation    
+    rt_delta = (motor_speed[1]+motor_speed[0])/2.0*0.05; 
+    current_angle += (motor_speed[0]-motor_speed[1])*0.05/16.0*57.3;//deg
     current_path += rt_delta;
     current_x += rt_delta*sin(current_angle/57.3);
     current_y += rt_delta*cos(current_angle/57.3);
+//pc.printf("%3.1f - %3.1f ; %3.1f - %3.1f ; %d\n",target_path,current_path,target_angle,current_angle,motor_busy); //be careful in interrupt
     
-    //pc.printf("%3.1f - %3.1f ; %3.1f - %3.1f ; %d\n",target_path,current_path,target_angle,current_angle,motor_busy); //be careful in interrupt
     if(motor_busy == 1) {
-        if(stop_flag == 1) {skorost(0,0);skorost(1,0);if(abs(motor_speed[0])<1) motor_busy = 0;}
+        if(stop_flag == 1) {skorost(0,0);skorost(1,0);if(abs(motor_speed[0])<1) motor_busy = 0;} //Stop with accel
         else { //motion
             if(infinite_flag == 1){skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));}
             else{