svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
motor.h@14:e12d0fdc3a48, 2018-11-18 (annotated)
- Committer:
- dima285
- Date:
- Sun Nov 18 13:33:28 2018 +0000
- Revision:
- 14:e12d0fdc3a48
- Parent:
- 13:789b451cc27d
- Child:
- 15:960b922433d1
12
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 | 9:8f98b1c277a4 | 4 | float max_speed = 2; |
dima285 | 12:721a9ea55e91 | 5 | float max_accel = 5; |
dima285 | 10:5bdd3dfd5f59 | 6 | |
Stas285 | 0:e9488589a8ee | 7 | DigitalOut dir_left(PC_15); |
Stas285 | 0:e9488589a8ee | 8 | DigitalOut dir_right(PA_2); |
Stas285 | 0:e9488589a8ee | 9 | DigitalOut motor_enable(PA_0);//sleep inverted |
dima285 | 8:891e4f54e9e2 | 10 | PwmOut step_left(PB_4); |
Stas285 | 0:e9488589a8ee | 11 | PwmOut step_right(PA_1); |
Stas285 | 0:e9488589a8ee | 12 | |
dima285 | 12:721a9ea55e91 | 13 | void geometric_navigation(){ |
dima285 | 10:5bdd3dfd5f59 | 14 | |
dima285 | 13:789b451cc27d | 15 | current.speed = (motor_speed[1] + motor_speed[0])/2.0; |
dima285 | 13:789b451cc27d | 16 | current.omega = (motor_speed[0] - motor_speed[1])/(2 * half_axis); //rad/s //MOVE TO SKOROST!!!!! |
dima285 | 13:789b451cc27d | 17 | current.azimuth += current.omega * t_step ;//rad |
dima285 | 14:e12d0fdc3a48 | 18 | if (current.azimuth > pi) current.azimuth -= 2*pi; |
dima285 | 14:e12d0fdc3a48 | 19 | if (current.azimuth < -pi) current.azimuth += 2*pi; |
dima285 | 13:789b451cc27d | 20 | current.x += current.speed * t_step * cos(current.azimuth); |
dima285 | 13:789b451cc27d | 21 | current.y += current.speed * t_step * sin(current.azimuth); |
dima285 | 12:721a9ea55e91 | 22 | current.path += current.speed * t_step; |
dima285 | 12:721a9ea55e91 | 23 | //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 | 24 | } |
dima285 | 10:5bdd3dfd5f59 | 25 | |
dima285 | 12:721a9ea55e91 | 26 | void set_motor_speed(float speed_left,float speed_right){ |
dima285 | 12:721a9ea55e91 | 27 | motor_speed[0] = speed_left; |
dima285 | 12:721a9ea55e91 | 28 | motor_speed[1] = speed_right; |
dima285 | 10:5bdd3dfd5f59 | 29 | |
dima285 | 13:789b451cc27d | 30 | /* if (motor_speed[0] > max_speed) motor_speed[0] = max_speed; |
dima285 | 9:8f98b1c277a4 | 31 | if (motor_speed[0] < -max_speed) motor_speed[0] = -max_speed; |
dima285 | 9:8f98b1c277a4 | 32 | if (motor_speed[1] > max_speed) motor_speed[1] = max_speed; |
dima285 | 13:789b451cc27d | 33 | if (motor_speed[1] < -max_speed) motor_speed[1] = -max_speed;*/ |
dima285 | 10:5bdd3dfd5f59 | 34 | |
dima285 | 12:721a9ea55e91 | 35 | if (motor_speed[1] > 0) dir_right = 1; |
dima285 | 12:721a9ea55e91 | 36 | else dir_right = 0; |
dima285 | 12:721a9ea55e91 | 37 | if (motor_speed[0] > 0) dir_left = 0; |
dima285 | 12:721a9ea55e91 | 38 | else dir_left = 1; |
dima285 | 9:8f98b1c277a4 | 39 | float nu_l = abs(motor_speed[0]/(2*pi*r_wheel))*ppr; //frequency in herz |
dima285 | 9:8f98b1c277a4 | 40 | float nu_r = abs(motor_speed[1]/(2*pi*r_wheel))*ppr; //frequency in herz |
dima285 | 9:8f98b1c277a4 | 41 | if (nu_l < 1) nu_l = 1; |
dima285 | 9:8f98b1c277a4 | 42 | if (nu_r < 1) nu_r = 1; |
dima285 | 10:5bdd3dfd5f59 | 43 | step_right.period_us (1e6/nu_r); |
dima285 | 10:5bdd3dfd5f59 | 44 | step_right = 0.5; |
dima285 | 10:5bdd3dfd5f59 | 45 | step_left.period_us (1e6/nu_l); |
dima285 | 12:721a9ea55e91 | 46 | step_left = 0.5; |
dima285 | 12:721a9ea55e91 | 47 | geometric_navigation(); |
dima285 | 10:5bdd3dfd5f59 | 48 | } |
dima285 | 10:5bdd3dfd5f59 | 49 | |
dima285 | 10:5bdd3dfd5f59 | 50 | void motor_init() |
dima285 | 10:5bdd3dfd5f59 | 51 | { |
dima285 | 10:5bdd3dfd5f59 | 52 | step_left = 0.5; |
dima285 | 10:5bdd3dfd5f59 | 53 | step_right = 0.5; |
Stas285 | 0:e9488589a8ee | 54 | motor_enable = 1; |
dima285 | 12:721a9ea55e91 | 55 | } |
dima285 | 12:721a9ea55e91 | 56 | |
dima285 | 12:721a9ea55e91 | 57 |