svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
motion.h@12:721a9ea55e91, 2018-11-03 (annotated)
- Committer:
- dima285
- Date:
- Sat Nov 03 11:43:03 2018 +0000
- Revision:
- 12:721a9ea55e91
- Parent:
- 10:5bdd3dfd5f59
- Child:
- 15:960b922433d1
rabotaet;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Stas285 | 6:6e89cdc3db92 | 1 | //3 main parameters of the movement: |
Stas285 | 6:6e89cdc3db92 | 2 | //float accel (defined in motor.h) |
dima285 | 9:8f98b1c277a4 | 3 | float speed = 1;//cm/s |
Stas285 | 6:6e89cdc3db92 | 4 | float radius = 10; |
Stas285 | 6:6e89cdc3db92 | 5 | |
dima285 | 10:5bdd3dfd5f59 | 6 | float current_speed; |
dima285 | 10:5bdd3dfd5f59 | 7 | //float half_width = 8.5; |
Stas285 | 4:904b737ef08a | 8 | float current_x, current_y; |
dima285 | 10:5bdd3dfd5f59 | 9 | float current_path, target_path; |
Stas285 | 4:904b737ef08a | 10 | float current_angle, target_angle;//deg |
Stas285 | 0:e9488589a8ee | 11 | float old_path = 0; |
Stas285 | 0:e9488589a8ee | 12 | float old_angle = 0; |
Stas285 | 0:e9488589a8ee | 13 | bool volatile motor_busy; |
Stas285 | 4:904b737ef08a | 14 | bool path_dir, angle_dir; |
Stas285 | 0:e9488589a8ee | 15 | bool angle_task;//1 - angle, 0 - path |
Stas285 | 0:e9488589a8ee | 16 | bool stop_flag = 0; |
Stas285 | 1:e2a6e523bf1f | 17 | bool infinite_flag = 0; |
Stas285 | 0:e9488589a8ee | 18 | |
Stas285 | 0:e9488589a8ee | 19 | //Timer trap_timer; |
Stas285 | 0:e9488589a8ee | 20 | |
Stas285 | 0:e9488589a8ee | 21 | void motion_init(){ |
Stas285 | 0:e9488589a8ee | 22 | //trap_timer.start(); |
Stas285 | 0:e9488589a8ee | 23 | } |
Stas285 | 0:e9488589a8ee | 24 | |
dima285 | 12:721a9ea55e91 | 25 | /*void go(float distance_m, bool brake = 1){ |
dima285 | 10:5bdd3dfd5f59 | 26 | //if(cm != 0) { |
dima285 | 10:5bdd3dfd5f59 | 27 | //stop_flag = 0; infinite_flag = 0; |
dima285 | 10:5bdd3dfd5f59 | 28 | //motor_enable = 1; |
dima285 | 10:5bdd3dfd5f59 | 29 | target_path = current_path + distance_m; |
dima285 | 10:5bdd3dfd5f59 | 30 | wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,distance_m,current_speed); |
dima285 | 10:5bdd3dfd5f59 | 31 | while(abs(target_path - current_path) > 0.01){gyro_process();wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);} |
dima285 | 10:5bdd3dfd5f59 | 32 | //if(cm > 0) path_dir = 1; else path_dir = 0; |
dima285 | 10:5bdd3dfd5f59 | 33 | //angle_task = 0; |
dima285 | 10:5bdd3dfd5f59 | 34 | //motor_busy = 1; |
dima285 | 10:5bdd3dfd5f59 | 35 | //while(motor_busy == 1){proc_counter++;} |
dima285 | 10:5bdd3dfd5f59 | 36 | //} |
Stas285 | 0:e9488589a8ee | 37 | } |
dima285 | 8:891e4f54e9e2 | 38 | |
dima285 | 8:891e4f54e9e2 | 39 | void go_no_wait(int cm, bool brake = 1){ |
dima285 | 8:891e4f54e9e2 | 40 | if(cm != 0) { |
dima285 | 8:891e4f54e9e2 | 41 | stop_flag = 0; infinite_flag = 0; |
dima285 | 8:891e4f54e9e2 | 42 | motor_enable = 1; |
dima285 | 8:891e4f54e9e2 | 43 | target_path =old_path + cm; old_path = target_path; |
dima285 | 8:891e4f54e9e2 | 44 | if(cm > 0) path_dir = 1; else path_dir = 0; |
dima285 | 8:891e4f54e9e2 | 45 | angle_task = 0; |
dima285 | 8:891e4f54e9e2 | 46 | motor_busy = 1; |
dima285 | 8:891e4f54e9e2 | 47 | //while(motor_busy == 1){proc_counter++;} |
dima285 | 8:891e4f54e9e2 | 48 | } |
dima285 | 8:891e4f54e9e2 | 49 | } |
Stas285 | 0:e9488589a8ee | 50 | |
Stas285 | 0:e9488589a8ee | 51 | void turn(int deg, bool brake = 1){ |
Stas285 | 0:e9488589a8ee | 52 | if(deg != 0) { |
Stas285 | 1:e2a6e523bf1f | 53 | stop_flag = 0; infinite_flag = 0; |
Stas285 | 0:e9488589a8ee | 54 | motor_enable = 1; |
Stas285 | 0:e9488589a8ee | 55 | target_angle = old_angle + deg; old_angle = target_angle; |
Stas285 | 0:e9488589a8ee | 56 | if(deg > 0) angle_dir = 1; else angle_dir = 0; |
Stas285 | 0:e9488589a8ee | 57 | angle_task = 1; |
Stas285 | 0:e9488589a8ee | 58 | motor_busy = 1; |
Stas285 | 0:e9488589a8ee | 59 | while(motor_busy == 1){proc_counter++;} |
Stas285 | 0:e9488589a8ee | 60 | old_path = current_path; |
Stas285 | 0:e9488589a8ee | 61 | } |
Stas285 | 0:e9488589a8ee | 62 | } |
Stas285 | 0:e9488589a8ee | 63 | |
Stas285 | 0:e9488589a8ee | 64 | void stop(){ |
Stas285 | 0:e9488589a8ee | 65 | motor_enable = 1; |
Stas285 | 0:e9488589a8ee | 66 | stop_flag = 1; |
Stas285 | 0:e9488589a8ee | 67 | motor_busy = 1; |
Stas285 | 0:e9488589a8ee | 68 | while(motor_busy == 1){} |
Stas285 | 6:6e89cdc3db92 | 69 | target_path = current_path; |
Stas285 | 4:904b737ef08a | 70 | //motor_enable = 0; |
Stas285 | 0:e9488589a8ee | 71 | } |
Stas285 | 0:e9488589a8ee | 72 | |
Stas285 | 0:e9488589a8ee | 73 | void dance(float dance_speed,float dance_accel){ |
Stas285 | 0:e9488589a8ee | 74 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 75 | accel=dance_accel; |
Stas285 | 0:e9488589a8ee | 76 | radius=20; |
Stas285 | 0:e9488589a8ee | 77 | |
Stas285 | 0:e9488589a8ee | 78 | go(25); |
Stas285 | 0:e9488589a8ee | 79 | go(-50); |
Stas285 | 0:e9488589a8ee | 80 | go(25); |
Stas285 | 0:e9488589a8ee | 81 | turn(90); |
Stas285 | 0:e9488589a8ee | 82 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 83 | turn(-180); |
Stas285 | 0:e9488589a8ee | 84 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 85 | turn(90); |
Stas285 | 0:e9488589a8ee | 86 | turn(-360); |
Stas285 | 0:e9488589a8ee | 87 | turn(360); |
Stas285 | 0:e9488589a8ee | 88 | |
Stas285 | 0:e9488589a8ee | 89 | go(20); |
Stas285 | 0:e9488589a8ee | 90 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 91 | turn(-90); |
Stas285 | 0:e9488589a8ee | 92 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 93 | turn(-90); |
Stas285 | 0:e9488589a8ee | 94 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 95 | turn(-90); |
Stas285 | 0:e9488589a8ee | 96 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 97 | turn(-90); |
Stas285 | 0:e9488589a8ee | 98 | go(-20); |
Stas285 | 0:e9488589a8ee | 99 | |
Stas285 | 0:e9488589a8ee | 100 | radius =0; |
Stas285 | 0:e9488589a8ee | 101 | turn(720); |
Stas285 | 0:e9488589a8ee | 102 | turn(-720); |
dima285 | 10:5bdd3dfd5f59 | 103 | } |
dima285 | 12:721a9ea55e91 | 104 | */ |
dima285 | 10:5bdd3dfd5f59 | 105 |