Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motor.h@9:8f98b1c277a4, 2017-09-16 (annotated)

Committer:
dima285
Date:
Sat Sep 16 13:24:25 2017 +0000
Revision:
9:8f98b1c277a4
Parent:
8:891e4f54e9e2
Child:
10:5bdd3dfd5f59
shoto rabotaet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dima285 9:8f98b1c277a4 1 float const g= 9.81;//SI
dima285 9:8f98b1c277a4 2 float const r_wheel = 0.038;//SI
dima285 9:8f98b1c277a4 3 float const center_mas = 0.022;//SI (ot osi)
dima285 9:8f98b1c277a4 4 float const mass = 1.3;//SI
dima285 9:8f98b1c277a4 5 float const m_inert = 5e-3;//SI
dima285 9:8f98b1c277a4 6 float const half_axis = 0.08;//SI
dima285 9:8f98b1c277a4 7 float const pi = 3.1415926535;
dima285 9:8f98b1c277a4 8 float const ppr = 3200;//pulses per revolution
Stas285 0:e9488589a8ee 9
dima285 9:8f98b1c277a4 10 float accel = 0.01; //cm/c/50mS - 5 = 1 cm/S^2
dima285 9:8f98b1c277a4 11 float motor_speed[2] = {0,0}; //left, right (real exact current speed of each motor for the next raltime slot) SI
dima285 9:8f98b1c277a4 12 float gy_old;
dima285 9:8f98b1c277a4 13 float k_1 = 0.5;
dima285 9:8f98b1c277a4 14 float k_2 = 0.5;
dima285 9:8f98b1c277a4 15 float vert;//ugol
dima285 9:8f98b1c277a4 16 float max_speed = 2;
dima285 9:8f98b1c277a4 17 //float delta_v_filtered;
dima285 9:8f98b1c277a4 18 //float gy_f;
dima285 9:8f98b1c277a4 19 //float ax_f;
dima285 9:8f98b1c277a4 20
Stas285 0:e9488589a8ee 21 DigitalOut dir_left(PC_15);
Stas285 0:e9488589a8ee 22 DigitalOut dir_right(PA_2);
Stas285 0:e9488589a8ee 23 DigitalOut motor_enable(PA_0);//sleep inverted
dima285 8:891e4f54e9e2 24 PwmOut step_left(PB_4);
Stas285 0:e9488589a8ee 25 PwmOut step_right(PA_1);
Stas285 0:e9488589a8ee 26
dima285 9:8f98b1c277a4 27 void skorost_1(float target_speed, float angular_acceleration = 0){
dima285 9:8f98b1c277a4 28 // ax_f = 0*ax_f+1*ax;
dima285 9:8f98b1c277a4 29 float sin_phi = (ax)/g;
dima285 9:8f98b1c277a4 30 float a = (g*sin_phi)/(sqrt(1-sin_phi*sin_phi));
dima285 9:8f98b1c277a4 31 // float epsilon = gy - gy_old;
dima285 9:8f98b1c277a4 32 // gy_f = 0*gy_f + 1*gy;
dima285 9:8f98b1c277a4 33
dima285 9:8f98b1c277a4 34 float delta_v = 2*k_2*a*t_step + k_1*1e-3*gy + accel; //- 0*1e-3*epsilon - 0*motor_speed[1];
dima285 9:8f98b1c277a4 35
dima285 9:8f98b1c277a4 36 // delta_v_filtered = 0*delta_v_filtered + 1*delta_v;
dima285 9:8f98b1c277a4 37 //if (delta_v > 0.01) delta_v = 0.01 ;
dima285 9:8f98b1c277a4 38 //if (delta_v < -0.01) delta_v = -0.01 ;
dima285 9:8f98b1c277a4 39 motor_speed[0] += delta_v; //+ rotation_rate*half_axis;
dima285 9:8f98b1c277a4 40 motor_speed[1] += delta_v; //- rotation_rate*half_axis;
dima285 9:8f98b1c277a4 41
dima285 9:8f98b1c277a4 42 if (motor_speed[0] > max_speed) motor_speed[0] = max_speed;
dima285 9:8f98b1c277a4 43 if (motor_speed[0] < -max_speed) motor_speed[0] = -max_speed;
dima285 9:8f98b1c277a4 44 if (motor_speed[1] > max_speed) motor_speed[1] = max_speed;
dima285 9:8f98b1c277a4 45 if (motor_speed[1] < -max_speed) motor_speed[1] = -max_speed;
dima285 9:8f98b1c277a4 46
dima285 9:8f98b1c277a4 47
dima285 9:8f98b1c277a4 48 if (motor_speed[1] > 0) dir_right = 0; else dir_right = 1;
dima285 9:8f98b1c277a4 49 if (motor_speed[0] > 0) dir_left = 1; else dir_left = 0;
dima285 9:8f98b1c277a4 50 float nu_l = abs(motor_speed[0]/(2*pi*r_wheel))*ppr; //frequency in herz
dima285 9:8f98b1c277a4 51 float nu_r = abs(motor_speed[1]/(2*pi*r_wheel))*ppr; //frequency in herz
dima285 9:8f98b1c277a4 52 if (nu_l < 1) nu_l = 1;
dima285 9:8f98b1c277a4 53 if (nu_r < 1) nu_r = 1;
dima285 9:8f98b1c277a4 54 step_right.period_us (1e6/nu_r);step_right = 0.5;
dima285 9:8f98b1c277a4 55 step_left.period_us (1e6/nu_l);step_left = 0.5;
dima285 9:8f98b1c277a4 56 wifi.printf("%.2f %.2f %.2f %.2f;",gy,a,ax,delta_v);
dima285 9:8f98b1c277a4 57 //pc.printf("s:%.2f,a:%.2f,ax:%.2f,delta_v:%.2f,nu:%.2f\n",motor_speed[0],a,ax,delta_v,nu_l);
dima285 9:8f98b1c277a4 58 }
dima285 9:8f98b1c277a4 59
Stas285 6:6e89cdc3db92 60 void skorost(int right, float motor_target_speed){ //(0-left,1-right), cm/s [-400;400]
Stas285 6:6e89cdc3db92 61 //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
dima285 9:8f98b1c277a4 62 //float nu_l;
dima285 9:8f98b1c277a4 63 //if (abs(-motor_target_speed + motor_speed[right]) <= accel) motor_speed[right] = motor_target_speed; // calculate new curent speed
dima285 9:8f98b1c277a4 64 //if ((motor_target_speed + motor_speed[right]) > accel) motor_speed[right] = motor_speed[right] - accel; // calculate new curent speed at big step
dima285 9:8f98b1c277a4 65 //if ((-motor_speed[right] + motor_target_speed) > accel) motor_speed[right] = motor_speed[right] + accel; // calculate new curent speed at big step
dima285 9:8f98b1c277a4 66
dima285 9:8f98b1c277a4 67 vert = 1*(motor_target_speed - motor_speed[right]) - 0.6;
dima285 9:8f98b1c277a4 68 if (vert > 3) vert = 3;
dima285 9:8f98b1c277a4 69 if (vert < -3) vert = -3;
dima285 9:8f98b1c277a4 70
dima285 9:8f98b1c277a4 71 //if (right == 1) wifi.printf("a:%.2f,t:%.2f,s:%.2f,v:%.2f",ax,motor_target_speed,motor_speed[right],vert);
dima285 9:8f98b1c277a4 72
dima285 9:8f98b1c277a4 73 //float diff = ax - ax_old; //balance feedback
dima285 9:8f98b1c277a4 74 //motor_speed[right] -= k_dif*gy + 0*diff + k_prop*ax ;
dima285 9:8f98b1c277a4 75 //gz_old = gz;
dima285 9:8f98b1c277a4 76 /*if (motor_speed[right] > 100) motor_speed[right] = 100;
dima285 9:8f98b1c277a4 77 if (motor_speed[right] < -100) motor_speed[right] = -100;
dima285 9:8f98b1c277a4 78 if (right == 1) wifi.printf("s:%.2f",motor_speed[right]);
Stas285 0:e9488589a8ee 79 if (right) {if (motor_speed[right] > 0) dir_right = 1; else dir_right = 0;}
Stas285 0:e9488589a8ee 80 else {if (motor_speed[right] > 0) dir_left = 0; else dir_left = 1;}
Stas285 6:6e89cdc3db92 81 nu_l = abs(motor_speed[right])*(500.0/3.8); //frequency in herz !!!!(cm)
Stas285 0:e9488589a8ee 82 if (nu_l < 1) nu_l = 1;
dima285 9:8f98b1c277a4 83 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 84 // pc.printf("%u \n", nu_l);
dima285 9:8f98b1c277a4 85 //wifi.printf("ay:%.2f, ",ay);*/
Stas285 6:6e89cdc3db92 86 }
Stas285 6:6e89cdc3db92 87
Stas285 6:6e89cdc3db92 88 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 89 skorost(0,speed_left);
Stas285 6:6e89cdc3db92 90 skorost(1,speed_right);
Stas285 0:e9488589a8ee 91 }
Stas285 0:e9488589a8ee 92
Stas285 0:e9488589a8ee 93 void motor_init(){
Stas285 0:e9488589a8ee 94 step_left = 0.5; step_right = 0.5;
Stas285 0:e9488589a8ee 95 motor_enable = 1;
dima285 9:8f98b1c277a4 96 //skorost(0,0.01); skorost(1,0.01);
Stas285 0:e9488589a8ee 97 }