Dima Dudin / Mbed 2 deprecated DD_Robot

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?

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;
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