svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
motor.h@10:5bdd3dfd5f59, 2018-09-05 (annotated)
- Committer:
- dima285
- Date:
- Wed Sep 05 18:25:54 2018 +0000
- Revision:
- 10:5bdd3dfd5f59
- Parent:
- 9:8f98b1c277a4
- Child:
- 12:721a9ea55e91
Stable 05.09.2018
Who changed what in which revision?
User | Revision | Line number | New 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 | 10:5bdd3dfd5f59 | 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 | 10:5bdd3dfd5f59 | 27 | void skorost_1(float linear_acceleration = 0, float angular_acceleration = 0) |
dima285 | 10:5bdd3dfd5f59 | 28 | { |
dima285 | 9:8f98b1c277a4 | 29 | // ax_f = 0*ax_f+1*ax; |
dima285 | 10:5bdd3dfd5f59 | 30 | float tma; |
dima285 | 10:5bdd3dfd5f59 | 31 | if (linear_acceleration < 5) tma = linear_acceleration; |
dima285 | 10:5bdd3dfd5f59 | 32 | else tma = 5; |
dima285 | 10:5bdd3dfd5f59 | 33 | if (linear_acceleration > -5) tma = linear_acceleration; |
dima285 | 10:5bdd3dfd5f59 | 34 | else tma = -5; |
dima285 | 9:8f98b1c277a4 | 35 | float sin_phi = (ax)/g; |
dima285 | 10:5bdd3dfd5f59 | 36 | float a = (g*sin_phi)/(sqrt(1-sin_phi*sin_phi)); |
dima285 | 9:8f98b1c277a4 | 37 | // float epsilon = gy - gy_old; |
dima285 | 9:8f98b1c277a4 | 38 | // gy_f = 0*gy_f + 1*gy; |
dima285 | 10:5bdd3dfd5f59 | 39 | |
dima285 | 10:5bdd3dfd5f59 | 40 | float delta_v = 2*k_2*a*t_step + k_1*1e-3*gy /*+ accel*/ - tma * t_step; //- 0*1e-3*epsilon - 0*motor_speed[1]; |
dima285 | 10:5bdd3dfd5f59 | 41 | |
dima285 | 9:8f98b1c277a4 | 42 | // delta_v_filtered = 0*delta_v_filtered + 1*delta_v; |
dima285 | 9:8f98b1c277a4 | 43 | //if (delta_v > 0.01) delta_v = 0.01 ; |
dima285 | 9:8f98b1c277a4 | 44 | //if (delta_v < -0.01) delta_v = -0.01 ; |
dima285 | 10:5bdd3dfd5f59 | 45 | motor_speed[0] += delta_v; //+ rotation_rate*half_axis; |
dima285 | 10:5bdd3dfd5f59 | 46 | motor_speed[1] += delta_v; //- rotation_rate*half_axis; |
dima285 | 10:5bdd3dfd5f59 | 47 | |
dima285 | 9:8f98b1c277a4 | 48 | if (motor_speed[0] > max_speed) motor_speed[0] = max_speed; |
dima285 | 9:8f98b1c277a4 | 49 | if (motor_speed[0] < -max_speed) motor_speed[0] = -max_speed; |
dima285 | 9:8f98b1c277a4 | 50 | if (motor_speed[1] > max_speed) motor_speed[1] = max_speed; |
dima285 | 9:8f98b1c277a4 | 51 | if (motor_speed[1] < -max_speed) motor_speed[1] = -max_speed; |
dima285 | 10:5bdd3dfd5f59 | 52 | |
dima285 | 10:5bdd3dfd5f59 | 53 | |
dima285 | 10:5bdd3dfd5f59 | 54 | if (motor_speed[1] > 0) dir_right = 0; |
dima285 | 10:5bdd3dfd5f59 | 55 | else dir_right = 1; |
dima285 | 10:5bdd3dfd5f59 | 56 | if (motor_speed[0] > 0) dir_left = 1; |
dima285 | 10:5bdd3dfd5f59 | 57 | else dir_left = 0; |
dima285 | 9:8f98b1c277a4 | 58 | float nu_l = abs(motor_speed[0]/(2*pi*r_wheel))*ppr; //frequency in herz |
dima285 | 9:8f98b1c277a4 | 59 | float nu_r = abs(motor_speed[1]/(2*pi*r_wheel))*ppr; //frequency in herz |
dima285 | 9:8f98b1c277a4 | 60 | if (nu_l < 1) nu_l = 1; |
dima285 | 9:8f98b1c277a4 | 61 | if (nu_r < 1) nu_r = 1; |
dima285 | 10:5bdd3dfd5f59 | 62 | step_right.period_us (1e6/nu_r); |
dima285 | 10:5bdd3dfd5f59 | 63 | step_right = 0.5; |
dima285 | 10:5bdd3dfd5f59 | 64 | step_left.period_us (1e6/nu_l); |
dima285 | 10:5bdd3dfd5f59 | 65 | step_left = 0.5; |
dima285 | 10:5bdd3dfd5f59 | 66 | //wifi.printf("%.2f %.2f %.2f %.2f;",linear_acceleration,a,ax,delta_v); |
dima285 | 9:8f98b1c277a4 | 67 | //pc.printf("s:%.2f,a:%.2f,ax:%.2f,delta_v:%.2f,nu:%.2f\n",motor_speed[0],a,ax,delta_v,nu_l); |
dima285 | 10:5bdd3dfd5f59 | 68 | } |
dima285 | 10:5bdd3dfd5f59 | 69 | |
dima285 | 10:5bdd3dfd5f59 | 70 | void skorost(int right, float motor_target_speed) //(0-left,1-right), cm/s [-400;400] |
dima285 | 10:5bdd3dfd5f59 | 71 | { |
dima285 | 10:5bdd3dfd5f59 | 72 | //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 | 73 | //float nu_l; |
dima285 | 10:5bdd3dfd5f59 | 74 | //if (abs(-motor_target_speed + motor_speed[right]) <= accel) motor_speed[right] = motor_target_speed; // calculate new curent speed |
dima285 | 9:8f98b1c277a4 | 75 | //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 | 76 | //if ((-motor_speed[right] + motor_target_speed) > accel) motor_speed[right] = motor_speed[right] + accel; // calculate new curent speed at big step |
dima285 | 10:5bdd3dfd5f59 | 77 | |
dima285 | 9:8f98b1c277a4 | 78 | vert = 1*(motor_target_speed - motor_speed[right]) - 0.6; |
dima285 | 9:8f98b1c277a4 | 79 | if (vert > 3) vert = 3; |
dima285 | 9:8f98b1c277a4 | 80 | if (vert < -3) vert = -3; |
dima285 | 10:5bdd3dfd5f59 | 81 | |
dima285 | 9:8f98b1c277a4 | 82 | //if (right == 1) wifi.printf("a:%.2f,t:%.2f,s:%.2f,v:%.2f",ax,motor_target_speed,motor_speed[right],vert); |
dima285 | 10:5bdd3dfd5f59 | 83 | |
dima285 | 9:8f98b1c277a4 | 84 | //float diff = ax - ax_old; //balance feedback |
dima285 | 10:5bdd3dfd5f59 | 85 | //motor_speed[right] -= k_dif*gy + 0*diff + k_prop*ax ; |
dima285 | 10:5bdd3dfd5f59 | 86 | //gz_old = gz; |
dima285 | 9:8f98b1c277a4 | 87 | /*if (motor_speed[right] > 100) motor_speed[right] = 100; |
dima285 | 9:8f98b1c277a4 | 88 | if (motor_speed[right] < -100) motor_speed[right] = -100; |
dima285 | 9:8f98b1c277a4 | 89 | if (right == 1) wifi.printf("s:%.2f",motor_speed[right]); |
Stas285 | 0:e9488589a8ee | 90 | if (right) {if (motor_speed[right] > 0) dir_right = 1; else dir_right = 0;} |
Stas285 | 0:e9488589a8ee | 91 | else {if (motor_speed[right] > 0) dir_left = 0; else dir_left = 1;} |
Stas285 | 6:6e89cdc3db92 | 92 | nu_l = abs(motor_speed[right])*(500.0/3.8); //frequency in herz !!!!(cm) |
Stas285 | 0:e9488589a8ee | 93 | if (nu_l < 1) nu_l = 1; |
dima285 | 9:8f98b1c277a4 | 94 | 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;} |
dima285 | 10:5bdd3dfd5f59 | 95 | // pc.printf("%u \n", nu_l); |
dima285 | 9:8f98b1c277a4 | 96 | //wifi.printf("ay:%.2f, ",ay);*/ |
dima285 | 10:5bdd3dfd5f59 | 97 | } |
Stas285 | 6:6e89cdc3db92 | 98 | |
dima285 | 10:5bdd3dfd5f59 | 99 | 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 |
dima285 | 10:5bdd3dfd5f59 | 100 | { |
Stas285 | 6:6e89cdc3db92 | 101 | skorost(0,speed_left); |
Stas285 | 6:6e89cdc3db92 | 102 | skorost(1,speed_right); |
dima285 | 10:5bdd3dfd5f59 | 103 | } |
dima285 | 10:5bdd3dfd5f59 | 104 | |
dima285 | 10:5bdd3dfd5f59 | 105 | void motor_init() |
dima285 | 10:5bdd3dfd5f59 | 106 | { |
dima285 | 10:5bdd3dfd5f59 | 107 | step_left = 0.5; |
dima285 | 10:5bdd3dfd5f59 | 108 | step_right = 0.5; |
Stas285 | 0:e9488589a8ee | 109 | motor_enable = 1; |
dima285 | 9:8f98b1c277a4 | 110 | //skorost(0,0.01); skorost(1,0.01); |
dima285 | 10:5bdd3dfd5f59 | 111 | } |