Dima Dudin / Mbed 2 deprecated DD_Robot

svoe

Dependencies:   mbed mbed-STM32F103C8T6 MPU6050_1

motion.h@23:bc05a104be10, 2019-08-06 (annotated)

Committer:
dima285
Date:
Tue Aug 06 14:13:55 2019 +0000
Branch:
svoe
Revision:
23:bc05a104be10
Parent:
22:14e85f2068c7 
06.08.19

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