svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
motion.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:
- 4:904b737ef08a
wifi + echo + rc
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Stas285 | 0:e9488589a8ee | 1 | //float current_speed;//cm/s |
Stas285 | 0:e9488589a8ee | 2 | float half_width = 8.5; |
Stas285 | 0:e9488589a8ee | 3 | float radius = 10; |
Stas285 | 0:e9488589a8ee | 4 | float speed = 100;//cm/s |
Stas285 | 0:e9488589a8ee | 5 | float current_path;//cm |
Stas285 | 0:e9488589a8ee | 6 | float target_path;//cm |
Stas285 | 0:e9488589a8ee | 7 | float current_angle;//deg |
Stas285 | 0:e9488589a8ee | 8 | float target_angle;//deg |
Stas285 | 0:e9488589a8ee | 9 | float old_path = 0; |
Stas285 | 0:e9488589a8ee | 10 | float old_angle = 0; |
Stas285 | 0:e9488589a8ee | 11 | bool volatile motor_busy; |
Stas285 | 0:e9488589a8ee | 12 | bool path_dir; |
Stas285 | 0:e9488589a8ee | 13 | bool angle_dir; |
Stas285 | 0:e9488589a8ee | 14 | bool angle_task;//1 - angle, 0 - path |
Stas285 | 0:e9488589a8ee | 15 | bool stop_flag = 0; |
Stas285 | 1:e2a6e523bf1f | 16 | bool infinite_flag = 0; |
Stas285 | 0:e9488589a8ee | 17 | |
Stas285 | 0:e9488589a8ee | 18 | //Timer trap_timer; |
Stas285 | 0:e9488589a8ee | 19 | |
Stas285 | 0:e9488589a8ee | 20 | void motion_init(){ |
Stas285 | 0:e9488589a8ee | 21 | //trap_timer.start(); |
Stas285 | 0:e9488589a8ee | 22 | } |
Stas285 | 0:e9488589a8ee | 23 | |
Stas285 | 0:e9488589a8ee | 24 | void go(int cm, bool brake = 1){ |
Stas285 | 0:e9488589a8ee | 25 | if(cm != 0) { |
Stas285 | 1:e2a6e523bf1f | 26 | stop_flag = 0; infinite_flag = 0; |
Stas285 | 0:e9488589a8ee | 27 | motor_enable = 1; |
Stas285 | 0:e9488589a8ee | 28 | target_path =old_path + cm; old_path = target_path; |
Stas285 | 0:e9488589a8ee | 29 | if(cm > 0) path_dir = 1; else path_dir = 0; |
Stas285 | 0:e9488589a8ee | 30 | angle_task = 0; |
Stas285 | 0:e9488589a8ee | 31 | motor_busy = 1; |
Stas285 | 0:e9488589a8ee | 32 | while(motor_busy == 1){proc_counter++;} |
Stas285 | 0:e9488589a8ee | 33 | } |
Stas285 | 0:e9488589a8ee | 34 | } |
Stas285 | 0:e9488589a8ee | 35 | |
Stas285 | 0:e9488589a8ee | 36 | void turn(int deg, bool brake = 1){ |
Stas285 | 0:e9488589a8ee | 37 | if(deg != 0) { |
Stas285 | 1:e2a6e523bf1f | 38 | stop_flag = 0; infinite_flag = 0; |
Stas285 | 0:e9488589a8ee | 39 | motor_enable = 1; |
Stas285 | 0:e9488589a8ee | 40 | target_angle = old_angle + deg; old_angle = target_angle; |
Stas285 | 0:e9488589a8ee | 41 | if(deg > 0) angle_dir = 1; else angle_dir = 0; |
Stas285 | 0:e9488589a8ee | 42 | angle_task = 1; |
Stas285 | 0:e9488589a8ee | 43 | motor_busy = 1; |
Stas285 | 0:e9488589a8ee | 44 | while(motor_busy == 1){proc_counter++;} |
Stas285 | 0:e9488589a8ee | 45 | old_path = current_path; |
Stas285 | 0:e9488589a8ee | 46 | } |
Stas285 | 0:e9488589a8ee | 47 | } |
Stas285 | 0:e9488589a8ee | 48 | |
Stas285 | 0:e9488589a8ee | 49 | void stop(){ |
Stas285 | 0:e9488589a8ee | 50 | motor_enable = 1; |
Stas285 | 0:e9488589a8ee | 51 | stop_flag = 1; |
Stas285 | 0:e9488589a8ee | 52 | motor_busy = 1; |
Stas285 | 0:e9488589a8ee | 53 | while(motor_busy == 1){} |
Stas285 | 0:e9488589a8ee | 54 | motor_enable = 0; |
Stas285 | 0:e9488589a8ee | 55 | } |
Stas285 | 0:e9488589a8ee | 56 | |
Stas285 | 0:e9488589a8ee | 57 | void dance(float dance_speed,float dance_accel){ |
Stas285 | 0:e9488589a8ee | 58 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 59 | accel=dance_accel; |
Stas285 | 0:e9488589a8ee | 60 | radius=20; |
Stas285 | 0:e9488589a8ee | 61 | |
Stas285 | 0:e9488589a8ee | 62 | go(25); |
Stas285 | 0:e9488589a8ee | 63 | go(-50); |
Stas285 | 0:e9488589a8ee | 64 | go(25); |
Stas285 | 0:e9488589a8ee | 65 | turn(90); |
Stas285 | 0:e9488589a8ee | 66 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 67 | turn(-180); |
Stas285 | 0:e9488589a8ee | 68 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 69 | turn(90); |
Stas285 | 0:e9488589a8ee | 70 | turn(-360); |
Stas285 | 0:e9488589a8ee | 71 | turn(360); |
Stas285 | 0:e9488589a8ee | 72 | |
Stas285 | 0:e9488589a8ee | 73 | go(20); |
Stas285 | 0:e9488589a8ee | 74 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 75 | turn(-90); |
Stas285 | 0:e9488589a8ee | 76 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 77 | turn(-90); |
Stas285 | 0:e9488589a8ee | 78 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 79 | turn(-90); |
Stas285 | 0:e9488589a8ee | 80 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 81 | turn(-90); |
Stas285 | 0:e9488589a8ee | 82 | go(-20); |
Stas285 | 0:e9488589a8ee | 83 | |
Stas285 | 0:e9488589a8ee | 84 | radius =0; |
Stas285 | 0:e9488589a8ee | 85 | turn(720); |
Stas285 | 0:e9488589a8ee | 86 | turn(-720); |
Stas285 | 0:e9488589a8ee | 87 | } |