Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motion.h@22:14e85f2068c7, 2019-07-23 (annotated)

Committer:
dima285
Date:
Tue Jul 23 12:55:23 2019 +0000
Branch:
svoe
Revision:
22:14e85f2068c7
Parent:
20:e73f49ba5001
Child:
23:bc05a104be10
23.07.2019

Who changed what in which revision?

UserRevisionLine numberNew 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;
dima285 22:14e85f2068c7 8 int fail_counter = 0;
Stas285 0:e9488589a8ee 9
dima285 20:e73f49ba5001 10 /*void go(float distance_m, bool brake = 1){
Stas285 15:960b922433d1 11 target.path = current.path + distance_m;
Stas285 15:960b922433d1 12 // wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,distance_m,current_speed);
Stas285 15:960b922433d1 13 // while(abs(target.path - current.path) > 0.01){
Stas285 15:960b922433d1 14 // gyro_process();
Stas285 15:960b922433d1 15 // wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);}
dima285 20:e73f49ba5001 16 }*/
dima285 17:bd6b6ac89e0e 17
dima285 22:14e85f2068c7 18 void correct_obstacle(){ //gabariti
dima285 19:2fe650d29823 19 for(int point = 0;point <= 12;point ++){
dima285 22:14e85f2068c7 20 gabarit_obstacle[point] = obstacle[point];
dima285 19:2fe650d29823 21 }
dima285 19:2fe650d29823 22 for(int point = 0;point <= 12;point ++){
dima285 19:2fe650d29823 23 for(int test_point = 0;test_point <= 12;test_point ++){
dima285 17:bd6b6ac89e0e 24 switch(abs(test_point - point)){
dima285 22:14e85f2068c7 25 case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = obstacle[test_point]; break;
dima285 22:14e85f2068c7 26 case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.85 * obstacle[test_point];break;
dima285 22:14e85f2068c7 27 case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.7 * obstacle[test_point];break;
dima285 22:14e85f2068c7 28 case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.5 * obstacle[test_point];break;
dima285 17:bd6b6ac89e0e 29 }
dima285 17:bd6b6ac89e0e 30 }
dima285 17:bd6b6ac89e0e 31 }
dima285 17:bd6b6ac89e0e 32 /*for(int i = 0;i <= 12;i++){
dima285 17:bd6b6ac89e0e 33 obstacle[i] = corrected_obstacle[i];
dima285 17:bd6b6ac89e0e 34 }*/
dima285 22:14e85f2068c7 35 for(int point = 0;point <= 12;point ++){
dima285 22:14e85f2068c7 36 corrected_obstacle[point] = 0;
dima285 22:14e85f2068c7 37 float tm = 0;
dima285 22:14e85f2068c7 38 for(int test_point = 0;test_point <= 12;test_point ++){
dima285 22:14e85f2068c7 39 corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
dima285 22:14e85f2068c7 40 tm += 1.0/(abs(point-test_point)+1);
dima285 22:14e85f2068c7 41 }
dima285 22:14e85f2068c7 42 corrected_obstacle[point] /= tm;
dima285 22:14e85f2068c7 43 }
dima285 17:bd6b6ac89e0e 44 }
dima285 17:bd6b6ac89e0e 45
dima285 17:bd6b6ac89e0e 46
dima285 17:bd6b6ac89e0e 47 void analyze_obstacle(){
dima285 19:2fe650d29823 48 min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6];
dima285 19:2fe650d29823 49 for(int point = 2;point <= 10;point ++){
dima285 19:2fe650d29823 50 if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;}
dima285 19:2fe650d29823 51 if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;}
dima285 19:2fe650d29823 52 }
dima285 22:14e85f2068c7 53 if (max_dist > 300) max_dist = 300;
dima285 19:2fe650d29823 54 if (min_dist > 10) {
dima285 22:14e85f2068c7 55 float tm = float(max_dist - 30) / 100; //if (tm > 0.5) tm = 0.5;
dima285 19:2fe650d29823 56 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 57 target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200);
dima285 20:e73f49ba5001 58 target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200);
dima285 20:e73f49ba5001 59 target.path = current.path + tm;
dima285 22:14e85f2068c7 60 timeout_counter = 250;
dima285 19:2fe650d29823 61 }
dima285 19:2fe650d29823 62 else if(min_dist > 0){
dima285 20:e73f49ba5001 63 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 64 target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200);
dima285 19:2fe650d29823 65 target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200);
dima285 20:e73f49ba5001 66 target.path = current.path - 0.2;
dima285 22:14e85f2068c7 67 timeout_counter = 250;
dima285 17:bd6b6ac89e0e 68 }
dima285 20:e73f49ba5001 69 delta.path = target.path - current.path;
dima285 20:e73f49ba5001 70 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 71 delta.x = target.x - current.x;
dima285 20:e73f49ba5001 72 delta.y = target.y - current.y;
dima285 20:e73f49ba5001 73 if (delta.path > 0) target.dir = 1; else target.dir = 0;
dima285 22:14e85f2068c7 74 //wifi.printf("!***==========***!\r\n");
dima285 19:2fe650d29823 75 }
dima285 19:2fe650d29823 76
dima285 19:2fe650d29823 77 void vstavai(){
dima285 19:2fe650d29823 78 //motor_speed[0] = 0;
dima285 19:2fe650d29823 79 //motor_speed[1] = 0;
dima285 19:2fe650d29823 80 fall_timer = -20;
dima285 19:2fe650d29823 81 if (ax < 0){
dima285 19:2fe650d29823 82 set_motor_speed(-0.2,-0.2);
dima285 19:2fe650d29823 83 wait(1);
dima285 19:2fe650d29823 84 }
dima285 19:2fe650d29823 85 set_motor_speed(0,0);
dima285 19:2fe650d29823 86 target.x = current.x;
dima285 19:2fe650d29823 87 target.y = current.y;
dima285 22:14e85f2068c7 88 target.path = current.path;
dima285 19:2fe650d29823 89 target.azimuth = current.azimuth;
dima285 22:14e85f2068c7 90 fail_counter++;
dima285 22:14e85f2068c7 91 }
dima285 22:14e85f2068c7 92
dima285 22:14e85f2068c7 93 void free_walk(){
dima285 22:14e85f2068c7 94 if (motion_mode == GO){
dima285 22:14e85f2068c7 95 if (timeout_counter-- == 0) {
dima285 22:14e85f2068c7 96 target.path = current.path - 0.2;
dima285 22:14e85f2068c7 97 timeout_counter = 250;
dima285 22:14e85f2068c7 98 }
dima285 22:14e85f2068c7 99 }
dima285 22:14e85f2068c7 100
dima285 22:14e85f2068c7 101 if (delay_counter-- == 3) {
dima285 22:14e85f2068c7 102 glaz.startContinuous(100);
dima285 22:14e85f2068c7 103 echo_start();
dima285 22:14e85f2068c7 104 }
dima285 22:14e85f2068c7 105 if (delay_counter == 0) {
dima285 22:14e85f2068c7 106 if(motion_mode > STOP) {if(coord_ready) {motion_mode++;if (motion_mode > GO) motion_mode = STOP; delay2_counter = 15;}}
dima285 22:14e85f2068c7 107 else{
dima285 22:14e85f2068c7 108 if (delay2_counter > 0) delay2_counter--;//delay before full stop and scan
dima285 22:14e85f2068c7 109 else{
dima285 22:14e85f2068c7 110 obstacle_glaz[serva_counter] = glaz.read()/10;//cm //Function
dima285 22:14e85f2068c7 111 glaz.stopContinuous();
dima285 22:14e85f2068c7 112 obstacle_echo[serva_counter] = echo_cm;
dima285 22:14e85f2068c7 113 if ((obstacle_glaz[serva_counter] > obstacle_echo[serva_counter]) && (obstacle_echo[serva_counter] < 100))
dima285 22:14e85f2068c7 114 obstacle[serva_counter] = obstacle_echo[serva_counter];
dima285 22:14e85f2068c7 115 else obstacle[serva_counter] = obstacle_glaz[serva_counter];
dima285 22:14e85f2068c7 116 if(serva_counter < 12)serva(-90 + (serva_counter++)*15);
dima285 22:14e85f2068c7 117 else{
dima285 22:14e85f2068c7 118 serva_counter = 0;
dima285 22:14e85f2068c7 119 serva(-90);
dima285 22:14e85f2068c7 120 correct_obstacle();
dima285 22:14e85f2068c7 121 analyze_obstacle();
dima285 22:14e85f2068c7 122 echo_transmit(13);
dima285 22:14e85f2068c7 123 //wifi.putc(current.x * 10); wifi.putc(current.y * 10); wifi.putc(current.azimuth * 40); wifi.putc(target.x * 10); wifi.putc(target.y * 10); wifi.putc(target.azimuth * 40);
dima285 22:14e85f2068c7 124 motion_mode = ROTATE;
dima285 22:14e85f2068c7 125 //motion_timeout = 250;
dima285 22:14e85f2068c7 126 //wait(2);
dima285 22:14e85f2068c7 127 }
dima285 22:14e85f2068c7 128 }
dima285 22:14e85f2068c7 129 }
dima285 22:14e85f2068c7 130 delay_counter = 7;
dima285 22:14e85f2068c7 131 }
dima285 22:14e85f2068c7 132
dima285 22:14e85f2068c7 133 }
dima285 22:14e85f2068c7 134
dima285 22:14e85f2068c7 135 void motion_1D(){
dima285 22:14e85f2068c7 136 static int k;
dima285 22:14e85f2068c7 137 if (k++ > 2400) k = 0; else { target.path = traj_1D[int(k/200)];}
dima285 19:2fe650d29823 138 }
dima285 19:2fe650d29823 139
dima285 17:bd6b6ac89e0e 140 /*if (max_dist < 0.3) tupic = 1;
dima285 17:bd6b6ac89e0e 141 if (tupic == 1) {
dima285 19:2fe650d29823 142 target.azimuth += pi * (rand() % 10 + 5)/10; if (target.azimuth > pi) target.azimuth -= 2*pi;
dima285 17:bd6b6ac89e0e 143 target.x = current.x;
dima285 17:bd6b6ac89e0e 144 target.y = current.y;
dima285 17:bd6b6ac89e0e 145 } */
dima285 17:bd6b6ac89e0e 146
Stas285 15:960b922433d1 147 /*
dima285 8:891e4f54e9e2 148 void go_no_wait(int cm, bool brake = 1){
dima285 8:891e4f54e9e2 149 if(cm != 0) {
dima285 8:891e4f54e9e2 150 stop_flag = 0; infinite_flag = 0;
dima285 8:891e4f54e9e2 151 motor_enable = 1;
dima285 8:891e4f54e9e2 152 target_path =old_path + cm; old_path = target_path;
dima285 8:891e4f54e9e2 153 if(cm > 0) path_dir = 1; else path_dir = 0;
dima285 8:891e4f54e9e2 154 angle_task = 0;
dima285 8:891e4f54e9e2 155 motor_busy = 1;
dima285 8:891e4f54e9e2 156 //while(motor_busy == 1){proc_counter++;}
dima285 8:891e4f54e9e2 157 }
dima285 8:891e4f54e9e2 158 }
Stas285 0:e9488589a8ee 159
Stas285 0:e9488589a8ee 160 void turn(int deg, bool brake = 1){
Stas285 0:e9488589a8ee 161 if(deg != 0) {
Stas285 1:e2a6e523bf1f 162 stop_flag = 0; infinite_flag = 0;
Stas285 0:e9488589a8ee 163 motor_enable = 1;
Stas285 0:e9488589a8ee 164 target_angle = old_angle + deg; old_angle = target_angle;
Stas285 0:e9488589a8ee 165 if(deg > 0) angle_dir = 1; else angle_dir = 0;
Stas285 0:e9488589a8ee 166 angle_task = 1;
Stas285 0:e9488589a8ee 167 motor_busy = 1;
Stas285 0:e9488589a8ee 168 while(motor_busy == 1){proc_counter++;}
Stas285 0:e9488589a8ee 169 old_path = current_path;
Stas285 0:e9488589a8ee 170 }
Stas285 0:e9488589a8ee 171 }
Stas285 0:e9488589a8ee 172
Stas285 0:e9488589a8ee 173 void stop(){
Stas285 0:e9488589a8ee 174 motor_enable = 1;
Stas285 0:e9488589a8ee 175 stop_flag = 1;
Stas285 0:e9488589a8ee 176 motor_busy = 1;
Stas285 0:e9488589a8ee 177 while(motor_busy == 1){}
Stas285 6:6e89cdc3db92 178 target_path = current_path;
Stas285 4:904b737ef08a 179 //motor_enable = 0;
Stas285 0:e9488589a8ee 180 }
Stas285 0:e9488589a8ee 181
Stas285 0:e9488589a8ee 182 void dance(float dance_speed,float dance_accel){
Stas285 0:e9488589a8ee 183 speed = dance_speed;
Stas285 0:e9488589a8ee 184 accel=dance_accel;
Stas285 0:e9488589a8ee 185 radius=20;
Stas285 0:e9488589a8ee 186
Stas285 0:e9488589a8ee 187 go(25);
Stas285 0:e9488589a8ee 188 go(-50);
Stas285 0:e9488589a8ee 189 go(25);
Stas285 0:e9488589a8ee 190 turn(90);
Stas285 0:e9488589a8ee 191 speed = -dance_speed;
Stas285 0:e9488589a8ee 192 turn(-180);
Stas285 0:e9488589a8ee 193 speed = dance_speed;
Stas285 0:e9488589a8ee 194 turn(90);
Stas285 0:e9488589a8ee 195 turn(-360);
Stas285 0:e9488589a8ee 196 turn(360);
Stas285 0:e9488589a8ee 197
Stas285 0:e9488589a8ee 198 go(20);
Stas285 0:e9488589a8ee 199 speed = -dance_speed;
Stas285 0:e9488589a8ee 200 turn(-90);
Stas285 0:e9488589a8ee 201 speed = dance_speed;
Stas285 0:e9488589a8ee 202 turn(-90);
Stas285 0:e9488589a8ee 203 speed = -dance_speed;
Stas285 0:e9488589a8ee 204 turn(-90);
Stas285 0:e9488589a8ee 205 speed = dance_speed;
Stas285 0:e9488589a8ee 206 turn(-90);
Stas285 0:e9488589a8ee 207 go(-20);
Stas285 0:e9488589a8ee 208
Stas285 0:e9488589a8ee 209 radius =0;
Stas285 0:e9488589a8ee 210 turn(720);
Stas285 0:e9488589a8ee 211 turn(-720);
dima285 10:5bdd3dfd5f59 212 }
dima285 12:721a9ea55e91 213 */
dima285 10:5bdd3dfd5f59 214