Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

realtime.h@1:e2a6e523bf1f, 2017-05-06 (annotated)

Committer:
Stas285
Date:
Sat May 06 18:21:55 2017 +0000
Revision:
1:e2a6e523bf1f
Parent:
0:e9488589a8ee
Child:
3:8e8458f45d19
wifi + echo + rc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stas285 0:e9488589a8ee 1 Ticker rt_ticker;
Stas285 0:e9488589a8ee 2 float tm_speed;
Stas285 0:e9488589a8ee 3
Stas285 0:e9488589a8ee 4 void realtime(){
Stas285 0:e9488589a8ee 5 //myled = !myled;
Stas285 0:e9488589a8ee 6
Stas285 0:e9488589a8ee 7 current_path += (motor_speed[1]+motor_speed[0])/2*0.05;
Stas285 0:e9488589a8ee 8 current_angle += (motor_speed[0]-motor_speed[1])*0.05/16*57.3;//deg
Stas285 0:e9488589a8ee 9
Stas285 1:e2a6e523bf1f 10 //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
Stas285 0:e9488589a8ee 11 if(motor_busy == 1) {
Stas285 0:e9488589a8ee 12 if(stop_flag == 1) {skorost(0,0);skorost(1,0);if(abs(motor_speed[0])<1) motor_busy = 0;}
Stas285 1:e2a6e523bf1f 13 else { //motion
Stas285 1:e2a6e523bf1f 14 if(infinite_flag == 1){skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));}
Stas285 1:e2a6e523bf1f 15 else{
Stas285 1:e2a6e523bf1f 16 if(angle_task == 1) {//radial motion
Stas285 1:e2a6e523bf1f 17 if((angle_dir == 1) && (target_angle <= current_angle)) motor_busy = 0;
Stas285 1:e2a6e523bf1f 18 if((angle_dir == 0) && (target_angle >= current_angle)) motor_busy = 0;
Stas285 1:e2a6e523bf1f 19 if(motor_busy == 1) {
Stas285 1:e2a6e523bf1f 20 if(radius > 1) {
Stas285 1:e2a6e523bf1f 21 if((angle_dir == 1) && (speed>0) || (angle_dir == 0) && (speed<0)){
Stas285 1:e2a6e523bf1f 22 skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));}
Stas285 1:e2a6e523bf1f 23 else {skorost(1,speed*(1+half_width/radius));skorost(0,speed*(1-half_width/radius));}}
Stas285 1:e2a6e523bf1f 24 else {if(angle_dir == 1){skorost(1,-speed); skorost(0,speed);} else {skorost(1,speed); skorost(0,-speed);}
Stas285 1:e2a6e523bf1f 25 }
Stas285 0:e9488589a8ee 26 }
Stas285 1:e2a6e523bf1f 27 }
Stas285 1:e2a6e523bf1f 28 if(angle_task == 0) {//linear motion
Stas285 1:e2a6e523bf1f 29 if((path_dir == 1) && (target_path <= current_path)) motor_busy = 0;
Stas285 1:e2a6e523bf1f 30 if((path_dir == 0) && (target_path >= current_path)) motor_busy = 0;
Stas285 1:e2a6e523bf1f 31 if(motor_busy == 1) {if(path_dir == 1) {skorost(1,speed);skorost(0,speed);} else{skorost(1,-speed);skorost(0,-speed);} }
Stas285 0:e9488589a8ee 32 }
Stas285 0:e9488589a8ee 33 }
Stas285 0:e9488589a8ee 34 }
Stas285 0:e9488589a8ee 35 }
Stas285 1:e2a6e523bf1f 36 gyro_process();
Stas285 1:e2a6e523bf1f 37 echo_step(1);
Stas285 0:e9488589a8ee 38
Stas285 1:e2a6e523bf1f 39 //pc.printf("%d \n", proc_counter); proc_counter = 0;
Stas285 0:e9488589a8ee 40 }
Stas285 0:e9488589a8ee 41
Stas285 0:e9488589a8ee 42 void realtime_init(){
Stas285 0:e9488589a8ee 43 rt_ticker.attach_us(&realtime, 50000);
Stas285 0:e9488589a8ee 44 }