Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motor.h@0:e9488589a8ee, 2017-05-02 (annotated)

Committer:
Stas285
Date:
Tue May 02 16:50:18 2017 +0000
Revision:
0:e9488589a8ee
Child:
4:904b737ef08a
debil

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stas285 0:e9488589a8ee 1 float accel = 5;
Stas285 0:e9488589a8ee 2 /*float speed_right;
Stas285 0:e9488589a8ee 3 float speed_left;*/
Stas285 0:e9488589a8ee 4 float motor_speed[2] = {1,1};
Stas285 0:e9488589a8ee 5
Stas285 0:e9488589a8ee 6 DigitalOut dir_left(PC_15);
Stas285 0:e9488589a8ee 7 DigitalOut dir_right(PA_2);
Stas285 0:e9488589a8ee 8 DigitalOut motor_enable(PA_0);//sleep inverted
Stas285 0:e9488589a8ee 9 PwmOut step_left(PB_5);
Stas285 0:e9488589a8ee 10 PwmOut step_right(PA_1);
Stas285 0:e9488589a8ee 11
Stas285 0:e9488589a8ee 12 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 = -
Stas285 0:e9488589a8ee 13 float nu_l;
Stas285 0:e9488589a8ee 14 if ((-motor_target_speed + motor_speed[right]) > accel) {motor_speed[right] = motor_speed[right] - accel;} // calculate new curent speed
Stas285 0:e9488589a8ee 15 if ((-motor_speed[right] + motor_target_speed) > accel) motor_speed[right] = motor_speed[right] + accel; // calculate new curent speed
Stas285 0:e9488589a8ee 16 if (abs(-motor_target_speed + motor_speed[right]) <= accel) motor_speed[right] = motor_target_speed; // calculate new curent speed
Stas285 0:e9488589a8ee 17 if (right) {if (motor_speed[right] > 0) dir_right = 1; else dir_right = 0;}
Stas285 0:e9488589a8ee 18 else {if (motor_speed[right] > 0) dir_left = 0; else dir_left = 1;}
Stas285 0:e9488589a8ee 19 nu_l = abs(motor_speed[right])*500/3.8; //frequency in herz !!!!(cm)
Stas285 0:e9488589a8ee 20 if (nu_l < 1) nu_l = 1;
Stas285 0:e9488589a8ee 21 if(right) step_right.period_us (1e6/nu_l); else step_left.period_us (1e6/nu_l);
Stas285 0:e9488589a8ee 22 // pc.printf("%u \n", nu_l);
Stas285 0:e9488589a8ee 23 }
Stas285 0:e9488589a8ee 24
Stas285 0:e9488589a8ee 25 void motor_init(){
Stas285 0:e9488589a8ee 26 step_left = 0.5; step_right = 0.5;
Stas285 0:e9488589a8ee 27 motor_enable = 1;
Stas285 0:e9488589a8ee 28 skorost(0,0.1); skorost(1,0.1);
Stas285 0:e9488589a8ee 29 }