Diff: motor.h

Revision:

10:5bdd3dfd5f59

Parent:

9:8f98b1c277a4

Child:

12:721a9ea55e91

--- a/motor.h	Sat Sep 16 13:24:25 2017 +0000
+++ b/motor.h	Wed Sep 05 18:25:54 2018 +0000
@@ -17,62 +17,73 @@
 //float delta_v_filtered;
 //float gy_f;
 //float ax_f;
- 
+
 DigitalOut  dir_left(PC_15);
 DigitalOut  dir_right(PA_2);
 DigitalOut  motor_enable(PA_0);//sleep inverted
 PwmOut step_left(PB_4);
 PwmOut step_right(PA_1);
 
-void skorost_1(float target_speed, float angular_acceleration = 0){
+void skorost_1(float linear_acceleration = 0, float angular_acceleration = 0)
+{
 //    ax_f = 0*ax_f+1*ax;
+    float tma;                                                          
+    if (linear_acceleration < 5) tma = linear_acceleration;
+    else tma = 5;
+    if (linear_acceleration > -5) tma = linear_acceleration;
+    else tma = -5;
     float sin_phi = (ax)/g;
-    float a = (g*sin_phi)/(sqrt(1-sin_phi*sin_phi));     
+    float a = (g*sin_phi)/(sqrt(1-sin_phi*sin_phi));
 //    float epsilon = gy - gy_old;
 //    gy_f = 0*gy_f + 1*gy;
-    
-    float delta_v = 2*k_2*a*t_step + k_1*1e-3*gy + accel; //- 0*1e-3*epsilon - 0*motor_speed[1];
-    
+
+    float delta_v = 2*k_2*a*t_step + k_1*1e-3*gy /*+ accel*/ - tma * t_step; //- 0*1e-3*epsilon - 0*motor_speed[1];
+
 //    delta_v_filtered = 0*delta_v_filtered + 1*delta_v;
     //if (delta_v > 0.01) delta_v = 0.01 ;
     //if (delta_v < -0.01) delta_v = -0.01 ;
-    motor_speed[0] += delta_v; //+ rotation_rate*half_axis; 
-    motor_speed[1] += delta_v; //- rotation_rate*half_axis;     
-    
+    motor_speed[0] += delta_v; //+ rotation_rate*half_axis;
+    motor_speed[1] += delta_v; //- rotation_rate*half_axis;
+
     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] > 0) dir_right = 0; else dir_right = 1;
-    if (motor_speed[0] > 0) dir_left = 1; else dir_left = 0;
+
+
+    if (motor_speed[1] > 0) dir_right = 0;
+    else dir_right = 1;
+    if (motor_speed[0] > 0) dir_left = 1;
+    else dir_left = 0;
     float nu_l = abs(motor_speed[0]/(2*pi*r_wheel))*ppr; //frequency in herz
     float nu_r = abs(motor_speed[1]/(2*pi*r_wheel))*ppr; //frequency in herz
     if (nu_l < 1) nu_l = 1;
     if (nu_r < 1) nu_r = 1;
-    step_right.period_us (1e6/nu_r);step_right = 0.5;
-    step_left.period_us (1e6/nu_l);step_left = 0.5;
-    wifi.printf("%.2f %.2f %.2f %.2f;",gy,a,ax,delta_v);
+    step_right.period_us (1e6/nu_r);
+    step_right = 0.5;
+    step_left.period_us (1e6/nu_l);
+    step_left = 0.5;
+    //wifi.printf("%.2f %.2f %.2f %.2f;",linear_acceleration,a,ax,delta_v);
     //pc.printf("s:%.2f,a:%.2f,ax:%.2f,delta_v:%.2f,nu:%.2f\n",motor_speed[0],a,ax,delta_v,nu_l);
-    }
-    
-void skorost(int right, float motor_target_speed){ //(0-left,1-right), cm/s [-400;400] 
-//nu = 500 Hz => 500/3200 = 0.15625 rps (d = 7.8cm) => V_real = Pi*d*0.15625 = 0.038 m/s = 3.8 cm/s 
+}
+
+void skorost(int right, float motor_target_speed)  //(0-left,1-right), cm/s [-400;400]
+{
+//nu = 500 Hz => 500/3200 = 0.15625 rps (d = 7.8cm) => V_real = Pi*d*0.15625 = 0.038 m/s = 3.8 cm/s
 //float nu_l;
-    //if (abs(-motor_target_speed + motor_speed[right]) <= accel)  motor_speed[right] = motor_target_speed;  // calculate new curent speed  
+    //if (abs(-motor_target_speed + motor_speed[right]) <= accel)  motor_speed[right] = motor_target_speed;  // calculate new curent speed
     //if ((motor_target_speed + motor_speed[right]) > accel)  motor_speed[right] = motor_speed[right] - accel; // calculate new curent speed at big step
     //if ((-motor_speed[right] + motor_target_speed) > accel)  motor_speed[right] = motor_speed[right] + accel; // calculate new curent speed at big step
-    
+
     vert = 1*(motor_target_speed - motor_speed[right]) - 0.6;
     if (vert > 3) vert = 3;
     if (vert < -3) vert = -3;
-    
+
     //if (right == 1) wifi.printf("a:%.2f,t:%.2f,s:%.2f,v:%.2f",ax,motor_target_speed,motor_speed[right],vert);
-        
+
     //float diff = ax - ax_old;                                                       //balance feedback
-    //motor_speed[right] -= k_dif*gy + 0*diff + k_prop*ax ; 
-    //gz_old = gz;                                               
+    //motor_speed[right] -= k_dif*gy + 0*diff + k_prop*ax ;
+    //gz_old = gz;
     /*if (motor_speed[right] > 100) motor_speed[right] = 100;
     if (motor_speed[right] < -100) motor_speed[right] = -100;
     if (right == 1) wifi.printf("s:%.2f",motor_speed[right]);
@@ -81,17 +92,20 @@
     nu_l = abs(motor_speed[right])*(500.0/3.8); //frequency in herz !!!!(cm)
     if (nu_l < 1) nu_l = 1;
     if(right) {step_right.period_us (1e6/nu_l);step_right = 0.5;} else {step_left.period_us (1e6/nu_l);step_left = 0.5;}
-   // pc.printf("%u \n", nu_l);
+    // pc.printf("%u \n", nu_l);
     //wifi.printf("ay:%.2f, ",ay);*/
-    }
+}
 
-void set_motors(float speed_left, float speed_right){ //sets target speed for both motors, but the result will take into account accel and ay
+void set_motors(float speed_left, float speed_right)  //sets target speed for both motors, but the result will take into account accel and ay
+{
     skorost(0,speed_left);
     skorost(1,speed_right);
-    }
-    
-void motor_init(){
-    step_left = 0.5;   step_right = 0.5;
+}
+
+void motor_init()
+{
+    step_left = 0.5;
+    step_right = 0.5;
     motor_enable = 1;
     //skorost(0,0.01); skorost(1,0.01);
-    }
\ No newline at end of file
+}
\ No newline at end of file