svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
motor.h@23:bc05a104be10, 2019-08-06 (annotated)
- Committer:
- dima285
- Date:
- Tue Aug 06 14:13:55 2019 +0000
- Branch:
- svoe
- Revision:
- 23:bc05a104be10
- Parent:
- 20:e73f49ba5001
06.08.19
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dima285 | 9:8f98b1c277a4 | 1 | float motor_speed[2] = {0,0}; //left, right (real exact current speed of each motor for the next raltime slot) SI |
dima285 | 12:721a9ea55e91 | 2 | //float gy_old; |
dima285 | 12:721a9ea55e91 | 3 | //float vert;//ugol |
dima285 | 20:e73f49ba5001 | 4 | |
dima285 | 20:e73f49ba5001 | 5 | /*float max_speed = 1; |
dima285 | 12:721a9ea55e91 | 6 | float max_accel = 5; |
dima285 | 20:e73f49ba5001 | 7 | float max_eps = 20;*/ |
dima285 | 10:5bdd3dfd5f59 | 8 | |
Stas285 | 0:e9488589a8ee | 9 | DigitalOut dir_left(PC_15); |
Stas285 | 15:960b922433d1 | 10 | DigitalOut dir_right(PA_4); |
Stas285 | 0:e9488589a8ee | 11 | DigitalOut motor_enable(PA_0);//sleep inverted |
dima285 | 8:891e4f54e9e2 | 12 | PwmOut step_left(PB_4); |
Stas285 | 0:e9488589a8ee | 13 | PwmOut step_right(PA_1); |
Stas285 | 0:e9488589a8ee | 14 | |
dima285 | 12:721a9ea55e91 | 15 | void geometric_navigation(){ |
dima285 | 10:5bdd3dfd5f59 | 16 | |
dima285 | 13:789b451cc27d | 17 | current.speed = (motor_speed[1] + motor_speed[0])/2.0; |
dima285 | 13:789b451cc27d | 18 | current.omega = (motor_speed[0] - motor_speed[1])/(2 * half_axis); //rad/s //MOVE TO SKOROST!!!!! |
dima285 | 13:789b451cc27d | 19 | current.azimuth += current.omega * t_step ;//rad |
dima285 | 14:e12d0fdc3a48 | 20 | if (current.azimuth > pi) current.azimuth -= 2*pi; |
dima285 | 14:e12d0fdc3a48 | 21 | if (current.azimuth < -pi) current.azimuth += 2*pi; |
dima285 | 13:789b451cc27d | 22 | current.x += current.speed * t_step * cos(current.azimuth); |
dima285 | 13:789b451cc27d | 23 | current.y += current.speed * t_step * sin(current.azimuth); |
dima285 | 12:721a9ea55e91 | 24 | current.path += current.speed * t_step; |
dima285 | 12:721a9ea55e91 | 25 | //pc.printf("%3.1f - %3.1f ; %3.1f - %3.1f ; %d\n",target_path,current_path,target_angle,current_angle,motor_busy); //be careful in interrupt |
dima285 | 12:721a9ea55e91 | 26 | } |
dima285 | 10:5bdd3dfd5f59 | 27 | |
dima285 | 12:721a9ea55e91 | 28 | void set_motor_speed(float speed_left,float speed_right){ |
dima285 | 12:721a9ea55e91 | 29 | motor_speed[0] = speed_left; |
dima285 | 12:721a9ea55e91 | 30 | motor_speed[1] = speed_right; |
dima285 | 10:5bdd3dfd5f59 | 31 | |
dima285 | 20:e73f49ba5001 | 32 | if (motor_speed[0] > max.speed*2) motor_speed[0] = max.speed*2; |
dima285 | 20:e73f49ba5001 | 33 | if (motor_speed[0] < -max.speed*2) motor_speed[0] = -max.speed*2; |
dima285 | 20:e73f49ba5001 | 34 | if (motor_speed[1] > max.speed*2) motor_speed[1] = max.speed*2; |
dima285 | 20:e73f49ba5001 | 35 | if (motor_speed[1] < -max.speed*2) motor_speed[1] = -max.speed*2; |
dima285 | 10:5bdd3dfd5f59 | 36 | |
dima285 | 12:721a9ea55e91 | 37 | if (motor_speed[1] > 0) dir_right = 1; |
dima285 | 12:721a9ea55e91 | 38 | else dir_right = 0; |
dima285 | 12:721a9ea55e91 | 39 | if (motor_speed[0] > 0) dir_left = 0; |
dima285 | 12:721a9ea55e91 | 40 | else dir_left = 1; |
dima285 | 9:8f98b1c277a4 | 41 | float nu_l = abs(motor_speed[0]/(2*pi*r_wheel))*ppr; //frequency in herz |
dima285 | 9:8f98b1c277a4 | 42 | float nu_r = abs(motor_speed[1]/(2*pi*r_wheel))*ppr; //frequency in herz |
dima285 | 9:8f98b1c277a4 | 43 | if (nu_l < 1) nu_l = 1; |
dima285 | 9:8f98b1c277a4 | 44 | if (nu_r < 1) nu_r = 1; |
dima285 | 10:5bdd3dfd5f59 | 45 | step_right.period_us (1e6/nu_r); |
dima285 | 10:5bdd3dfd5f59 | 46 | step_right = 0.5; |
dima285 | 10:5bdd3dfd5f59 | 47 | step_left.period_us (1e6/nu_l); |
dima285 | 12:721a9ea55e91 | 48 | step_left = 0.5; |
dima285 | 12:721a9ea55e91 | 49 | geometric_navigation(); |
dima285 | 10:5bdd3dfd5f59 | 50 | } |
dima285 | 10:5bdd3dfd5f59 | 51 | |
dima285 | 10:5bdd3dfd5f59 | 52 | void motor_init() |
dima285 | 10:5bdd3dfd5f59 | 53 | { |
dima285 | 10:5bdd3dfd5f59 | 54 | step_left = 0.5; |
dima285 | 10:5bdd3dfd5f59 | 55 | step_right = 0.5; |
Stas285 | 0:e9488589a8ee | 56 | motor_enable = 1; |
dima285 | 12:721a9ea55e91 | 57 | } |
dima285 | 12:721a9ea55e91 | 58 | |
dima285 | 12:721a9ea55e91 | 59 |