svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
motion.h@20:e73f49ba5001, 2019-03-23 (annotated)
- Committer:
- dima285
- Date:
- Sat Mar 23 16:38:44 2019 +0000
- Revision:
- 20:e73f49ba5001
- Parent:
- 19:2fe650d29823
- Child:
- 22:14e85f2068c7
prost
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 | 15:960b922433d1 | 4 | float radius = 100; |
dima285 | 17:bd6b6ac89e0e | 5 | bool tupic = 0; |
dima285 | 17:bd6b6ac89e0e | 6 | //int corrected_obstacle [100]; |
Stas285 | 1:e2a6e523bf1f | 7 | bool infinite_flag = 0; |
Stas285 | 0:e9488589a8ee | 8 | |
dima285 | 20:e73f49ba5001 | 9 | /*void go(float distance_m, bool brake = 1){ |
Stas285 | 15:960b922433d1 | 10 | target.path = current.path + distance_m; |
Stas285 | 15:960b922433d1 | 11 | // wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,distance_m,current_speed); |
Stas285 | 15:960b922433d1 | 12 | // while(abs(target.path - current.path) > 0.01){ |
Stas285 | 15:960b922433d1 | 13 | // gyro_process(); |
Stas285 | 15:960b922433d1 | 14 | // wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);} |
dima285 | 20:e73f49ba5001 | 15 | }*/ |
dima285 | 17:bd6b6ac89e0e | 16 | |
dima285 | 17:bd6b6ac89e0e | 17 | void correct_obstacle(){ |
dima285 | 19:2fe650d29823 | 18 | for(int point = 0;point <= 12;point ++){ |
dima285 | 19:2fe650d29823 | 19 | corrected_obstacle[point] = obstacle[point]; |
dima285 | 19:2fe650d29823 | 20 | } |
dima285 | 19:2fe650d29823 | 21 | for(int point = 0;point <= 12;point ++){ |
dima285 | 19:2fe650d29823 | 22 | for(int test_point = 0;test_point <= 12;test_point ++){ |
dima285 | 17:bd6b6ac89e0e | 23 | switch(abs(test_point - point)){ |
dima285 | 19:2fe650d29823 | 24 | case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = obstacle[test_point]; break; |
dima285 | 19:2fe650d29823 | 25 | case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.85 * obstacle[test_point];break; |
dima285 | 19:2fe650d29823 | 26 | case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.7 * obstacle[test_point];break; |
dima285 | 19:2fe650d29823 | 27 | case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.5 * obstacle[test_point];break; |
dima285 | 17:bd6b6ac89e0e | 28 | } |
dima285 | 17:bd6b6ac89e0e | 29 | } |
dima285 | 17:bd6b6ac89e0e | 30 | } |
dima285 | 17:bd6b6ac89e0e | 31 | /*for(int i = 0;i <= 12;i++){ |
dima285 | 17:bd6b6ac89e0e | 32 | obstacle[i] = corrected_obstacle[i]; |
dima285 | 17:bd6b6ac89e0e | 33 | }*/ |
dima285 | 17:bd6b6ac89e0e | 34 | } |
dima285 | 17:bd6b6ac89e0e | 35 | |
dima285 | 17:bd6b6ac89e0e | 36 | |
dima285 | 17:bd6b6ac89e0e | 37 | void analyze_obstacle(){ |
dima285 | 19:2fe650d29823 | 38 | min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6]; |
dima285 | 19:2fe650d29823 | 39 | for(int point = 2;point <= 10;point ++){ |
dima285 | 19:2fe650d29823 | 40 | if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;} |
dima285 | 19:2fe650d29823 | 41 | if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;} |
dima285 | 19:2fe650d29823 | 42 | } |
dima285 | 19:2fe650d29823 | 43 | if (min_dist > 10) { |
dima285 | 20:e73f49ba5001 | 44 | float tm = max_dist - 30; if (tm > 0.5) tm = 0.5; |
dima285 | 19:2fe650d29823 | 45 | target.azimuth = current.azimuth + (max_dist_angle)/57.3; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi; |
dima285 | 20:e73f49ba5001 | 46 | target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200); |
dima285 | 20:e73f49ba5001 | 47 | target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200); |
dima285 | 20:e73f49ba5001 | 48 | target.path = current.path + tm; |
dima285 | 19:2fe650d29823 | 49 | } |
dima285 | 19:2fe650d29823 | 50 | else if(min_dist > 0){ |
dima285 | 20:e73f49ba5001 | 51 | target.azimuth = current.azimuth + (min_dist_angle-15)/57.3 ; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi; |
dima285 | 19:2fe650d29823 | 52 | target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200); |
dima285 | 19:2fe650d29823 | 53 | target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200); |
dima285 | 20:e73f49ba5001 | 54 | target.path = current.path - 0.2; |
dima285 | 17:bd6b6ac89e0e | 55 | } |
dima285 | 20:e73f49ba5001 | 56 | delta.path = target.path - current.path; |
dima285 | 20:e73f49ba5001 | 57 | delta.azimuth = target.azimuth - current.azimuth; if(delta.azimuth > pi) delta.azimuth -= 2*pi; if(delta.azimuth < -pi) delta.azimuth += 2*pi; |
dima285 | 20:e73f49ba5001 | 58 | delta.x = target.x - current.x; |
dima285 | 20:e73f49ba5001 | 59 | delta.y = target.y - current.y; |
dima285 | 20:e73f49ba5001 | 60 | if (delta.path > 0) target.dir = 1; else target.dir = 0; |
dima285 | 20:e73f49ba5001 | 61 | wifi.printf("!***==========***!\r\n"); |
dima285 | 19:2fe650d29823 | 62 | } |
dima285 | 19:2fe650d29823 | 63 | |
dima285 | 19:2fe650d29823 | 64 | void vstavai(){ |
dima285 | 19:2fe650d29823 | 65 | //motor_speed[0] = 0; |
dima285 | 19:2fe650d29823 | 66 | //motor_speed[1] = 0; |
dima285 | 19:2fe650d29823 | 67 | fall_timer = -20; |
dima285 | 19:2fe650d29823 | 68 | if (ax < 0){ |
dima285 | 19:2fe650d29823 | 69 | set_motor_speed(-0.2,-0.2); |
dima285 | 19:2fe650d29823 | 70 | wait(1); |
dima285 | 19:2fe650d29823 | 71 | } |
dima285 | 19:2fe650d29823 | 72 | set_motor_speed(0,0); |
dima285 | 19:2fe650d29823 | 73 | target.x = current.x; |
dima285 | 19:2fe650d29823 | 74 | target.y = current.y; |
dima285 | 19:2fe650d29823 | 75 | target.azimuth = current.azimuth; |
dima285 | 19:2fe650d29823 | 76 | } |
dima285 | 19:2fe650d29823 | 77 | |
dima285 | 17:bd6b6ac89e0e | 78 | /*if (max_dist < 0.3) tupic = 1; |
dima285 | 17:bd6b6ac89e0e | 79 | if (tupic == 1) { |
dima285 | 19:2fe650d29823 | 80 | target.azimuth += pi * (rand() % 10 + 5)/10; if (target.azimuth > pi) target.azimuth -= 2*pi; |
dima285 | 17:bd6b6ac89e0e | 81 | target.x = current.x; |
dima285 | 17:bd6b6ac89e0e | 82 | target.y = current.y; |
dima285 | 17:bd6b6ac89e0e | 83 | } */ |
dima285 | 17:bd6b6ac89e0e | 84 | |
Stas285 | 15:960b922433d1 | 85 | /* |
dima285 | 8:891e4f54e9e2 | 86 | void go_no_wait(int cm, bool brake = 1){ |
dima285 | 8:891e4f54e9e2 | 87 | if(cm != 0) { |
dima285 | 8:891e4f54e9e2 | 88 | stop_flag = 0; infinite_flag = 0; |
dima285 | 8:891e4f54e9e2 | 89 | motor_enable = 1; |
dima285 | 8:891e4f54e9e2 | 90 | target_path =old_path + cm; old_path = target_path; |
dima285 | 8:891e4f54e9e2 | 91 | if(cm > 0) path_dir = 1; else path_dir = 0; |
dima285 | 8:891e4f54e9e2 | 92 | angle_task = 0; |
dima285 | 8:891e4f54e9e2 | 93 | motor_busy = 1; |
dima285 | 8:891e4f54e9e2 | 94 | //while(motor_busy == 1){proc_counter++;} |
dima285 | 8:891e4f54e9e2 | 95 | } |
dima285 | 8:891e4f54e9e2 | 96 | } |
Stas285 | 0:e9488589a8ee | 97 | |
Stas285 | 0:e9488589a8ee | 98 | void turn(int deg, bool brake = 1){ |
Stas285 | 0:e9488589a8ee | 99 | if(deg != 0) { |
Stas285 | 1:e2a6e523bf1f | 100 | stop_flag = 0; infinite_flag = 0; |
Stas285 | 0:e9488589a8ee | 101 | motor_enable = 1; |
Stas285 | 0:e9488589a8ee | 102 | target_angle = old_angle + deg; old_angle = target_angle; |
Stas285 | 0:e9488589a8ee | 103 | if(deg > 0) angle_dir = 1; else angle_dir = 0; |
Stas285 | 0:e9488589a8ee | 104 | angle_task = 1; |
Stas285 | 0:e9488589a8ee | 105 | motor_busy = 1; |
Stas285 | 0:e9488589a8ee | 106 | while(motor_busy == 1){proc_counter++;} |
Stas285 | 0:e9488589a8ee | 107 | old_path = current_path; |
Stas285 | 0:e9488589a8ee | 108 | } |
Stas285 | 0:e9488589a8ee | 109 | } |
Stas285 | 0:e9488589a8ee | 110 | |
Stas285 | 0:e9488589a8ee | 111 | void stop(){ |
Stas285 | 0:e9488589a8ee | 112 | motor_enable = 1; |
Stas285 | 0:e9488589a8ee | 113 | stop_flag = 1; |
Stas285 | 0:e9488589a8ee | 114 | motor_busy = 1; |
Stas285 | 0:e9488589a8ee | 115 | while(motor_busy == 1){} |
Stas285 | 6:6e89cdc3db92 | 116 | target_path = current_path; |
Stas285 | 4:904b737ef08a | 117 | //motor_enable = 0; |
Stas285 | 0:e9488589a8ee | 118 | } |
Stas285 | 0:e9488589a8ee | 119 | |
Stas285 | 0:e9488589a8ee | 120 | void dance(float dance_speed,float dance_accel){ |
Stas285 | 0:e9488589a8ee | 121 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 122 | accel=dance_accel; |
Stas285 | 0:e9488589a8ee | 123 | radius=20; |
Stas285 | 0:e9488589a8ee | 124 | |
Stas285 | 0:e9488589a8ee | 125 | go(25); |
Stas285 | 0:e9488589a8ee | 126 | go(-50); |
Stas285 | 0:e9488589a8ee | 127 | go(25); |
Stas285 | 0:e9488589a8ee | 128 | turn(90); |
Stas285 | 0:e9488589a8ee | 129 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 130 | turn(-180); |
Stas285 | 0:e9488589a8ee | 131 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 132 | turn(90); |
Stas285 | 0:e9488589a8ee | 133 | turn(-360); |
Stas285 | 0:e9488589a8ee | 134 | turn(360); |
Stas285 | 0:e9488589a8ee | 135 | |
Stas285 | 0:e9488589a8ee | 136 | go(20); |
Stas285 | 0:e9488589a8ee | 137 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 138 | turn(-90); |
Stas285 | 0:e9488589a8ee | 139 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 140 | turn(-90); |
Stas285 | 0:e9488589a8ee | 141 | speed = -dance_speed; |
Stas285 | 0:e9488589a8ee | 142 | turn(-90); |
Stas285 | 0:e9488589a8ee | 143 | speed = dance_speed; |
Stas285 | 0:e9488589a8ee | 144 | turn(-90); |
Stas285 | 0:e9488589a8ee | 145 | go(-20); |
Stas285 | 0:e9488589a8ee | 146 | |
Stas285 | 0:e9488589a8ee | 147 | radius =0; |
Stas285 | 0:e9488589a8ee | 148 | turn(720); |
Stas285 | 0:e9488589a8ee | 149 | turn(-720); |
dima285 | 10:5bdd3dfd5f59 | 150 | } |
dima285 | 12:721a9ea55e91 | 151 | */ |
dima285 | 10:5bdd3dfd5f59 | 152 |