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