Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motor.h@7:9e4a997ad23a, 2017-05-21 (annotated)

Committer:
dima285
Date:
Sun May 21 17:15:32 2017 +0000
Revision:
7:9e4a997ad23a
Parent:
6:6e89cdc3db92 
stoit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stas285 6:6e89cdc3db92 1 float accel = 5; //cm/c/50mS - 5 = 1 cm/S^2
Stas285 6:6e89cdc3db92 2 float motor_speed[2] = {1,1}; //left, right (real exact current speed of each motor for the next raltime slot)
Stas285 0:e9488589a8ee 3
Stas285 0:e9488589a8ee 4 DigitalOut dir_left(PC_15);
Stas285 0:e9488589a8ee 5 DigitalOut dir_right(PA_2);
Stas285 0:e9488589a8ee 6 DigitalOut motor_enable(PA_0);//sleep inverted
dima285 7:9e4a997ad23a 7 PwmOut step_left(PB_4);
Stas285 0:e9488589a8ee 8 PwmOut step_right(PA_1);
Stas285 0:e9488589a8ee 9
Stas285 6:6e89cdc3db92 10 void skorost(int right, float motor_target_speed){ //(0-left,1-right), cm/s [-400;400]
Stas285 6:6e89cdc3db92 11 //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
Stas285 0:e9488589a8ee 12 float nu_l;
dima285 7:9e4a997ad23a 13 float bal;
Stas285 0:e9488589a8ee 14 if (abs(-motor_target_speed + motor_speed[right]) <= accel) motor_speed[right] = motor_target_speed; // calculate new curent speed
Stas285 6:6e89cdc3db92 15 if ((-motor_target_speed + motor_speed[right]) > accel) motor_speed[right] = motor_speed[right] - accel; // calculate new curent speed at big step
Stas285 6:6e89cdc3db92 16 if ((-motor_speed[right] + motor_target_speed) > accel) motor_speed[right] = motor_speed[right] + accel; // calculate new curent speed at big step
dima285 7:9e4a997ad23a 17
dima285 7:9e4a997ad23a 18 if (ax>0) bal = 0.9*ax+0.8*ax*ax; else bal=0.9*ax-0.8*ax*ax;
dima285 7:9e4a997ad23a 19 if (bal>15) bal =15;
dima285 7:9e4a997ad23a 20 if (bal<-15) bal =-15;
dima285 7:9e4a997ad23a 21 //bal = ax*1;
dima285 7:9e4a997ad23a 22
dima285 7:9e4a997ad23a 23 if (right) motor_speed[right] -= bal; else motor_speed[right] -= bal; //balance feedback
Stas285 0:e9488589a8ee 24 if (right) {if (motor_speed[right] > 0) dir_right = 1; else dir_right = 0;}
Stas285 0:e9488589a8ee 25 else {if (motor_speed[right] > 0) dir_left = 0; else dir_left = 1;}
Stas285 6:6e89cdc3db92 26 nu_l = abs(motor_speed[right])*(500.0/3.8); //frequency in herz !!!!(cm)
Stas285 0:e9488589a8ee 27 if (nu_l < 1) nu_l = 1;
dima285 7:9e4a997ad23a 28 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;}
Stas285 0:e9488589a8ee 29 // pc.printf("%u \n", nu_l);
dima285 7:9e4a997ad23a 30 pc.printf("motor:%d, %.2f, %.2f, %.2f ",right,ax,motor_speed[right],motor_target_speed);
dima285 7:9e4a997ad23a 31 if (right) pc.printf("\n");
dima285 7:9e4a997ad23a 32
Stas285 6:6e89cdc3db92 33 }
Stas285 6:6e89cdc3db92 34
Stas285 6:6e89cdc3db92 35 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
Stas285 6:6e89cdc3db92 36 skorost(0,speed_left);
Stas285 6:6e89cdc3db92 37 skorost(1,speed_right);
Stas285 0:e9488589a8ee 38 }
Stas285 0:e9488589a8ee 39
Stas285 0:e9488589a8ee 40 void motor_init(){
Stas285 0:e9488589a8ee 41 step_left = 0.5; step_right = 0.5;
Stas285 0:e9488589a8ee 42 motor_enable = 1;
dima285 7:9e4a997ad23a 43 skorost(0,0.1); skorost(1,0.1);
Stas285 0:e9488589a8ee 44 }