svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
realtime.h
- Committer:
- Stas285
- Date:
- 2017-05-06
- Revision:
- 1:e2a6e523bf1f
- Parent:
- 0:e9488589a8ee
- Child:
- 3:8e8458f45d19
File content as of revision 1:e2a6e523bf1f:
Ticker rt_ticker; float tm_speed; void realtime(){ //myled = !myled; current_path += (motor_speed[1]+motor_speed[0])/2*0.05; current_angle += (motor_speed[0]-motor_speed[1])*0.05/16*57.3;//deg //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 if(motor_busy == 1) { if(stop_flag == 1) {skorost(0,0);skorost(1,0);if(abs(motor_speed[0])<1) motor_busy = 0;} else { //motion if(infinite_flag == 1){skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));} else{ if(angle_task == 1) {//radial motion if((angle_dir == 1) && (target_angle <= current_angle)) motor_busy = 0; if((angle_dir == 0) && (target_angle >= current_angle)) motor_busy = 0; if(motor_busy == 1) { if(radius > 1) { if((angle_dir == 1) && (speed>0) || (angle_dir == 0) && (speed<0)){ skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));} else {skorost(1,speed*(1+half_width/radius));skorost(0,speed*(1-half_width/radius));}} else {if(angle_dir == 1){skorost(1,-speed); skorost(0,speed);} else {skorost(1,speed); skorost(0,-speed);} } } } if(angle_task == 0) {//linear motion if((path_dir == 1) && (target_path <= current_path)) motor_busy = 0; if((path_dir == 0) && (target_path >= current_path)) motor_busy = 0; if(motor_busy == 1) {if(path_dir == 1) {skorost(1,speed);skorost(0,speed);} else{skorost(1,-speed);skorost(0,-speed);} } } } } } gyro_process(); echo_step(1); //pc.printf("%d \n", proc_counter); proc_counter = 0; } void realtime_init(){ rt_ticker.attach_us(&realtime, 50000); }