Diff: motor.h

Revision:

7:9e4a997ad23a

Parent:

6:6e89cdc3db92

--- a/motor.h	Sun May 21 12:04:00 2017 +0000
+++ b/motor.h	Sun May 21 17:15:32 2017 +0000
@@ -4,23 +4,32 @@
 DigitalOut  dir_left(PC_15);
 DigitalOut  dir_right(PA_2);
 DigitalOut  motor_enable(PA_0);//sleep inverted
-PwmOut step_left(PB_5);
+PwmOut step_left(PB_4);
 PwmOut step_right(PA_1);
 
 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;
+float bal;
     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
-    motor_speed[right] -= 5*ay;                                                 //balance feedback
+    
+    if (ax>0) bal = 0.9*ax+0.8*ax*ax; else bal=0.9*ax-0.8*ax*ax;
+    if (bal>15) bal =15;
+    if (bal<-15) bal =-15;
+    //bal = ax*1;
+    
+    if (right) motor_speed[right] -= bal; else motor_speed[right] -= bal;                                                 //balance feedback
     if (right) {if (motor_speed[right] > 0) dir_right = 1; else dir_right = 0;}
     else {if (motor_speed[right] > 0) dir_left = 0; else dir_left = 1;}
     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); else step_left.period_us (1e6/nu_l);
+    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);
-    //wifi.printf("ay:%.2f, ",ay);
+    pc.printf("motor:%d, %.2f, %.2f, %.2f ",right,ax,motor_speed[right],motor_target_speed);
+    if (right) pc.printf("\n");
+   
     }
 
 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
@@ -31,5 +40,5 @@
 void motor_init(){
     step_left = 0.5;   step_right = 0.5;
     motor_enable = 1;
-    skorost(0,0.01); skorost(1,0.01);
+    skorost(0,0.1); skorost(1,0.1);
     }
\ No newline at end of file