Diff: realtime.h

Revision:

0:e9488589a8ee

Child:

1:e2a6e523bf1f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/realtime.h	Tue May 02 16:50:18 2017 +0000
@@ -0,0 +1,44 @@
+Ticker rt_ticker;
+
+
+
+float tm_speed;
+
+void realtime(){
+    //myled = !myled;
+    
+    current_path += (motor_speed[1]+motor_speed[0])/2*0.05;
+    current_angle += (motor_speed[0]-motor_speed[1])*0.05/16*57.3;//deg
+    
+    //pc.printf("%3.1f - %3.1f ; %3.1f - %3.1f ; %d\n",target_path,current_path,target_angle,current_angle,motor_busy);
+    if(motor_busy == 1) {
+        if(stop_flag == 1) {skorost(0,0);skorost(1,0);if(abs(motor_speed[0])<1) motor_busy = 0;}
+        else {
+            if(angle_task == 1) {//radial motion
+                if((angle_dir == 1) && (target_angle <= current_angle)) motor_busy = 0;
+                if((angle_dir == 0) && (target_angle >= current_angle)) motor_busy = 0;
+                if(motor_busy == 1) {
+                    if(radius > 1) {
+                        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);}
+                        }
+                    }
+                } 
+            if(angle_task == 0) {//linear motion
+                if((path_dir == 1) && (target_path <= current_path)) motor_busy = 0;
+                if((path_dir == 0) && (target_path >= current_path)) motor_busy = 0;
+                if(motor_busy == 1) {if(path_dir == 1) {skorost(1,speed);skorost(0,speed);} else{skorost(1,-speed);skorost(0,-speed);} }
+                }
+            }
+       }     
+    
+    echo_step();
+    
+    pc.printf("%d \n", proc_counter); proc_counter = 0;
+  }
+  
+void realtime_init(){
+    rt_ticker.attach_us(&realtime, 50000);
+    }
\ No newline at end of file