Diff: motor.h

Revision:

13:789b451cc27d

Parent:

12:721a9ea55e91

Child:

14:e12d0fdc3a48

--- a/motor.h	Sat Nov 03 11:43:03 2018 +0000
+++ b/motor.h	Sun Nov 11 09:20:35 2018 +0000
@@ -12,10 +12,11 @@
 
 void geometric_navigation(){  
 
-    current.speed = (motor_speed[1]+motor_speed[0])/2.0;                                                            //MOVE TO SKOROST!!!!!
-    current.azimuth += (motor_speed[0]-motor_speed[1]) * t_step/(half_axis * 2) ;//rad
-    current.x += current.speed * t_step; //* sin(current.azimuth);
-    current.y += current.speed * t_step; //* cos(current.azimuth);
+    current.speed = (motor_speed[1] + motor_speed[0])/2.0; 
+    current.omega = (motor_speed[0] - motor_speed[1])/(2 * half_axis);  //rad/s          //MOVE TO SKOROST!!!!!
+    current.azimuth += current.omega * t_step ;//rad
+    current.x += current.speed * t_step * cos(current.azimuth);
+    current.y += current.speed * t_step * sin(current.azimuth);
     current.path += current.speed * t_step;
     //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
     }
@@ -24,10 +25,10 @@
     motor_speed[0] = speed_left;
     motor_speed[1] = speed_right;    
 
-    if (motor_speed[0] > max_speed) motor_speed[0] = max_speed;
+  /*  if (motor_speed[0] > max_speed) motor_speed[0] = max_speed;
     if (motor_speed[0] < -max_speed) motor_speed[0] = -max_speed;
     if (motor_speed[1] > max_speed) motor_speed[1] = max_speed;
-    if (motor_speed[1] < -max_speed) motor_speed[1] = -max_speed;
+    if (motor_speed[1] < -max_speed) motor_speed[1] = -max_speed;*/
 
     if (motor_speed[1] > 0) dir_right = 1;
     else dir_right = 0;