svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
realtime.h@3:8e8458f45d19, 2017-05-07 (annotated)
- Committer:
- Stas285
- Date:
- Sun May 07 08:37:22 2017 +0000
- Revision:
- 3:8e8458f45d19
- Parent:
- 1:e2a6e523bf1f
- Child:
- 4:904b737ef08a
wifi+echo scan+rc+921600
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Stas285 | 0:e9488589a8ee | 1 | Ticker rt_ticker; |
Stas285 | 0:e9488589a8ee | 2 | float tm_speed; |
Stas285 | 0:e9488589a8ee | 3 | |
Stas285 | 3:8e8458f45d19 | 4 | void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) |
Stas285 | 0:e9488589a8ee | 5 | //myled = !myled; |
Stas285 | 3:8e8458f45d19 | 6 | test = 1; |
Stas285 | 0:e9488589a8ee | 7 | |
Stas285 | 0:e9488589a8ee | 8 | current_path += (motor_speed[1]+motor_speed[0])/2*0.05; |
Stas285 | 0:e9488589a8ee | 9 | current_angle += (motor_speed[0]-motor_speed[1])*0.05/16*57.3;//deg |
Stas285 | 0:e9488589a8ee | 10 | |
Stas285 | 1:e2a6e523bf1f | 11 | //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 | 12 | if(motor_busy == 1) { |
Stas285 | 0:e9488589a8ee | 13 | if(stop_flag == 1) {skorost(0,0);skorost(1,0);if(abs(motor_speed[0])<1) motor_busy = 0;} |
Stas285 | 1:e2a6e523bf1f | 14 | else { //motion |
Stas285 | 1:e2a6e523bf1f | 15 | if(infinite_flag == 1){skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));} |
Stas285 | 1:e2a6e523bf1f | 16 | else{ |
Stas285 | 1:e2a6e523bf1f | 17 | if(angle_task == 1) {//radial motion |
Stas285 | 1:e2a6e523bf1f | 18 | if((angle_dir == 1) && (target_angle <= current_angle)) motor_busy = 0; |
Stas285 | 1:e2a6e523bf1f | 19 | if((angle_dir == 0) && (target_angle >= current_angle)) motor_busy = 0; |
Stas285 | 1:e2a6e523bf1f | 20 | if(motor_busy == 1) { |
Stas285 | 1:e2a6e523bf1f | 21 | if(radius > 1) { |
Stas285 | 1:e2a6e523bf1f | 22 | if((angle_dir == 1) && (speed>0) || (angle_dir == 0) && (speed<0)){ |
Stas285 | 1:e2a6e523bf1f | 23 | skorost(1,speed*(1-half_width/radius));skorost(0,speed*(1+half_width/radius));} |
Stas285 | 1:e2a6e523bf1f | 24 | else {skorost(1,speed*(1+half_width/radius));skorost(0,speed*(1-half_width/radius));}} |
Stas285 | 1:e2a6e523bf1f | 25 | else {if(angle_dir == 1){skorost(1,-speed); skorost(0,speed);} else {skorost(1,speed); skorost(0,-speed);} |
Stas285 | 1:e2a6e523bf1f | 26 | } |
Stas285 | 0:e9488589a8ee | 27 | } |
Stas285 | 1:e2a6e523bf1f | 28 | } |
Stas285 | 1:e2a6e523bf1f | 29 | if(angle_task == 0) {//linear motion |
Stas285 | 1:e2a6e523bf1f | 30 | if((path_dir == 1) && (target_path <= current_path)) motor_busy = 0; |
Stas285 | 1:e2a6e523bf1f | 31 | if((path_dir == 0) && (target_path >= current_path)) motor_busy = 0; |
Stas285 | 1:e2a6e523bf1f | 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 | 1:e2a6e523bf1f | 37 | gyro_process(); |
Stas285 | 1:e2a6e523bf1f | 38 | echo_step(1); |
Stas285 | 0:e9488589a8ee | 39 | |
Stas285 | 3:8e8458f45d19 | 40 | test = 0; |
Stas285 | 1:e2a6e523bf1f | 41 | //pc.printf("%d \n", proc_counter); proc_counter = 0; |
Stas285 | 0:e9488589a8ee | 42 | } |
Stas285 | 0:e9488589a8ee | 43 | |
Stas285 | 0:e9488589a8ee | 44 | void realtime_init(){ |
Stas285 | 0:e9488589a8ee | 45 | rt_ticker.attach_us(&realtime, 50000); |
Stas285 | 0:e9488589a8ee | 46 | } |