Diff: motor.h

Revision:

6:6e89cdc3db92

Parent:

4:904b737ef08a

Child:

7:9e4a997ad23a

Child:

8:891e4f54e9e2

--- a/motor.h	Sun May 14 11:30:26 2017 +0000
+++ b/motor.h	Sun May 21 12:04:00 2017 +0000
@@ -1,7 +1,5 @@
-float accel = 5;
-/*float speed_right;
-float speed_left;*/
-float motor_speed[2] = {1,1};
+float accel = 5; //cm/c/50mS - 5 = 1 cm/S^2
+float motor_speed[2] = {1,1}; //left, right (real exact current speed of each motor for the next raltime slot)
 
 DigitalOut  dir_left(PC_15);
 DigitalOut  dir_right(PA_2);
@@ -9,18 +7,25 @@
 PwmOut step_left(PB_5);
 PwmOut step_right(PA_1);
 
-void skorost(int right, float motor_target_speed){ //(0-left,1-right), cm/s [-400;400] //V=1 => 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 //motor_speed = -
+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 ((-motor_target_speed + motor_speed[right]) > accel)  {motor_speed[right] = motor_speed[right] - accel;} // calculate new curent speed
-    if ((-motor_speed[right] + motor_target_speed) > accel)  motor_speed[right] = motor_speed[right] + accel; // calculate new curent speed
     if (abs(-motor_target_speed + motor_speed[right]) <= accel)  motor_speed[right] = motor_target_speed;  // calculate new curent speed  
-    motor_speed[right] -= 2*ay; 
+    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 (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/3.8; //frequency in herz !!!!(cm)
+    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);
    // 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
+    skorost(0,speed_left);
+    skorost(1,speed_right);
     }
     
 void motor_init(){