Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motor.h@20:e73f49ba5001, 2019-03-23 (annotated)

Committer:
dima285
Date:
Sat Mar 23 16:38:44 2019 +0000
Revision:
20:e73f49ba5001
Parent:
15:960b922433d1 
prost

Who changed what in which revision?

UserRevisionLine numberNew 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