Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

realtime.h@0:e9488589a8ee, 2017-05-02 (annotated)

Committer:
Stas285
Date:
Tue May 02 16:50:18 2017 +0000
Revision:
0:e9488589a8ee
Child:
1:e2a6e523bf1f
debil

Who changed what in which revision?

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