svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
motor.h
- Committer:
- dima285
- Date:
- 2019-08-06
- Branch:
- svoe
- Revision:
- 23:bc05a104be10
- Parent:
- 20:e73f49ba5001
File content as of revision 23:bc05a104be10:
float motor_speed[2] = {0,0}; //left, right (real exact current speed of each motor for the next raltime slot) SI //float gy_old; //float vert;//ugol /*float max_speed = 1; float max_accel = 5; float max_eps = 20;*/ DigitalOut dir_left(PC_15); DigitalOut dir_right(PA_4); DigitalOut motor_enable(PA_0);//sleep inverted PwmOut step_left(PB_4); PwmOut step_right(PA_1); void geometric_navigation(){ current.speed = (motor_speed[1] + motor_speed[0])/2.0; current.omega = (motor_speed[0] - motor_speed[1])/(2 * half_axis); //rad/s //MOVE TO SKOROST!!!!! current.azimuth += current.omega * t_step ;//rad if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi; current.x += current.speed * t_step * cos(current.azimuth); current.y += current.speed * t_step * sin(current.azimuth); current.path += current.speed * t_step; //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 } void set_motor_speed(float speed_left,float speed_right){ motor_speed[0] = speed_left; motor_speed[1] = speed_right; if (motor_speed[0] > max.speed*2) motor_speed[0] = max.speed*2; if (motor_speed[0] < -max.speed*2) motor_speed[0] = -max.speed*2; if (motor_speed[1] > max.speed*2) motor_speed[1] = max.speed*2; if (motor_speed[1] < -max.speed*2) motor_speed[1] = -max.speed*2; if (motor_speed[1] > 0) dir_right = 1; else dir_right = 0; if (motor_speed[0] > 0) dir_left = 0; else dir_left = 1; float nu_l = abs(motor_speed[0]/(2*pi*r_wheel))*ppr; //frequency in herz float nu_r = abs(motor_speed[1]/(2*pi*r_wheel))*ppr; //frequency in herz if (nu_l < 1) nu_l = 1; if (nu_r < 1) nu_r = 1; step_right.period_us (1e6/nu_r); step_right = 0.5; step_left.period_us (1e6/nu_l); step_left = 0.5; geometric_navigation(); } void motor_init() { step_left = 0.5; step_right = 0.5; motor_enable = 1; }