Revision 14:e12d0fdc3a48, committed 2018-11-18

Comitter:

dima285

Date:

Sun Nov 18 13:33:28 2018 +0000

Parent:

13:789b451cc27d

Child:

15:960b922433d1

Commit message:

12

Changed in this revision

--- a/MPU6050.lib	Sun Nov 11 09:20:35 2018 +0000
+++ b/MPU6050.lib	Sun Nov 18 13:33:28 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/dima285/code/MPU6050/#f806657f0009
+https://os.mbed.com/users/dima285/code/MPU6050/#b4f5edf825be

--- a/common.h	Sun Nov 11 09:20:35 2018 +0000
+++ b/common.h	Sun Nov 18 13:33:28 2018 +0000
@@ -19,7 +19,7 @@
 struct coord{
 float x; //m //vpered
 float y; //m //vpravo
-float azimuth; // rad
+float azimuth; // rad [-pi;+pi]
 float speed; //m/s
 float path; //m
 float omega; //rad/s    
@@ -30,13 +30,13 @@
 
 float balance_prop = 160;
 float balance_diff = 7;
-float x_prop = 18;
-float x_diff = 20;
-float azimuth_prop = 10;
-float azimuth_diff = 5;
-float azimuth_corr = 0;
+float x_prop = 40;//18;
+float x_diff = 35;//20;
+float azimuth_prop = 15;
+float azimuth_diff = 8;
+//float azimuth_corr = 0;
 
-float coord_accuracy = 0.05;
+float coord_accuracy = 0.1;
 bool ready;
 
 float trajectory [9][2] = {{0,0},{0.2,0.2},{0.2,0.2},{0,0},{0.2,0},{-0.2,0},{0,0},{0,0.2},{0,0}};

--- a/motor.h	Sun Nov 11 09:20:35 2018 +0000
+++ b/motor.h	Sun Nov 18 13:33:28 2018 +0000
@@ -15,6 +15,8 @@
     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
+    if (current.azimuth > pi) current.azimuth -= 2*pi;
+    if (current.azimuth < -pi) current.azimuth += 2*pi;
     current.x += current.speed * t_step * cos(current.azimuth);
     current.y += current.speed * t_step * sin(current.azimuth);
     current.path += current.speed * t_step;

--- a/realtime.h	Sun Nov 11 09:20:35 2018 +0000
+++ b/realtime.h	Sun Nov 18 13:33:28 2018 +0000
@@ -22,9 +22,10 @@
     if (delta_x < 0) {delta_s = -delta_s;}
     
     azimuth_to_target = atan(delta_y/delta_x);
-    float delta_phi_0 = azimuth_to_target - current.azimuth;
-    float delta_phi_1 = (target.azimuth - azimuth_to_target) ;//* (current.speed * t_step / delta_s);
-    float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * delta_phi_1 + delta_s/(delta_s + coord_accuracy) * delta_phi_0;  //ZNAK 
+    if (delta_x < 0){if (delta_y > 0) azimuth_to_target += pi; else azimuth_to_target -= pi;}
+    float delta_phi_0 = azimuth_to_target - current.azimuth;                                              //target error
+    float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s);       //sever error
+    float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * 0.3 * delta_phi_1 + delta_s/(delta_s + coord_accuracy) * delta_phi_0;  //ZNAK 
     
     normal_error  = delta_s * sin(delta_phi);
     tan_error = delta_s * cos(delta_phi);
@@ -34,15 +35,18 @@
     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; 
        
-    if (abs(normal_error) < coord_accuracy) {eps = 0;current.omega = 0;} else eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; //gz * azimuth_diff;
+    //if (abs(normal_error) < coord_accuracy) {eps = 0;current.omega = 0;} else 
+    eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; //gz * azimuth_diff;
     float delta_omega = eps * t_step;
     
+    wifi.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f;\r\n",current.x * 100,current.y * 100,current.azimuth,target.x * 100,azimuth_to_target,eps,delta_omega,delta_phi_0,delta_phi_1,delta_phi); 
+    
     set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis);
     
     if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy))  ready = 1; else ready = 0;
     
    // wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100);
-   wifi.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f;",current.x,target.x,delta_x,delta_s,current.speed,a,delta_v,ax,gy);
+   
 }