Diff: realtime.h

Revision:

4:904b737ef08a

Parent:

3:8e8458f45d19

Child:

6:6e89cdc3db92

--- a/realtime.h	Sun May 07 08:37:22 2017 +0000
+++ b/realtime.h	Sun May 14 11:28:29 2017 +0000
@@ -1,12 +1,17 @@
 Ticker rt_ticker;
-float tm_speed;
+//float tm_speed;
+float rt_delta;
+bool realtime_flag;
 
-void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600)
+void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos)
     //myled = !myled;
-    test = 1;
+//    test = 1;
     
-    current_path += (motor_speed[1]+motor_speed[0])/2*0.05;
+    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
+    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
     if(motor_busy == 1) {
@@ -22,7 +27,7 @@
                             if((angle_dir == 1) && (speed>0) || (angle_dir == 0) && (speed<0)){
                                 skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));} 
                             else {skorost(1,speed*(1+half_width/radius));skorost(0,speed*(1-half_width/radius));}} 
-                        else {if(angle_dir == 1){skorost(1,-speed); skorost(0,speed);} else {skorost(1,speed); skorost(0,-speed);}
+                        else {if(angle_dir == 1){skorost(1,-speed); skorost(0,speed);} else {skorost(1,speed); skorost(0,-speed);} //turn on the spot
                             }
                         }
                     } 
@@ -34,10 +39,10 @@
                 }
             }
        }     
-    gyro_process();
-    echo_step(1);
+   echo_step(1);
+   realtime_flag = 1; 
     
-    test = 0;
+//    test = 0;
     //pc.printf("%d \n", proc_counter); proc_counter = 0;
   }