svoe
Dependencies: mbed mbed-STM32F103C8T6 MPU6050_1
main.cpp@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 | 0:e9488589a8ee | 1 | #include "stm32f103c8t6.h" |
Stas285 | 0:e9488589a8ee | 2 | #include "mbed.h" |
dima285 | 17:bd6b6ac89e0e | 3 | #include "VL53L1X.h" |
Stas285 | 1:e2a6e523bf1f | 4 | #include "common.h" |
Stas285 | 4:904b737ef08a | 5 | #include "gyro.h" |
Stas285 | 0:e9488589a8ee | 6 | #include "motor.h" |
Stas285 | 0:e9488589a8ee | 7 | #include "serva.h" |
Stas285 | 0:e9488589a8ee | 8 | #include "echo.h" |
Stas285 | 0:e9488589a8ee | 9 | #include "motion.h" |
Stas285 | 1:e2a6e523bf1f | 10 | #include "wifi.h" |
Stas285 | 0:e9488589a8ee | 11 | #include "realtime.h" |
Stas285 | 0:e9488589a8ee | 12 | #include "system.h" |
Stas285 | 6:6e89cdc3db92 | 13 | #include "sound.h" |
Stas285 | 0:e9488589a8ee | 14 | |
dima285 | 17:bd6b6ac89e0e | 15 | |
dima285 | 19:2fe650d29823 | 16 | |
Stas285 | 0:e9488589a8ee | 17 | int main() { |
dima285 | 17:bd6b6ac89e0e | 18 | //int k,l; |
Stas285 | 15:960b922433d1 | 19 | wait_ms(500); |
Stas285 | 0:e9488589a8ee | 20 | system_init(); |
Stas285 | 15:960b922433d1 | 21 | start_recognizer(); |
Stas285 | 15:960b922433d1 | 22 | play(1); |
dima285 | 17:bd6b6ac89e0e | 23 | delay_counter = 3; |
dima285 | 17:bd6b6ac89e0e | 24 | motion_flag = 0; |
dima285 | 20:e73f49ba5001 | 25 | motion_mode = STOP; |
dima285 | 17:bd6b6ac89e0e | 26 | serva(-90); |
dima285 | 17:bd6b6ac89e0e | 27 | serva_counter = 0; |
dima285 | 19:2fe650d29823 | 28 | |
dima285 | 17:bd6b6ac89e0e | 29 | //wait(20); |
dima285 | 10:5bdd3dfd5f59 | 30 | |
dima285 | 12:721a9ea55e91 | 31 | while(1){ |
Stas285 | 4:904b737ef08a | 32 | while(realtime_flag == 0){} |
dima285 | 17:bd6b6ac89e0e | 33 | //myled = 0; //inverse connection |
dima285 | 12:721a9ea55e91 | 34 | gyro.read(&gx,&gy,&gz,&ax,&ay,&az); //doesn't work in interrupt // reading - 500 uS |
Stas285 | 15:960b922433d1 | 35 | //constant speed motion: distance_to_target = (x_diff/x_prop) * target.speed |
dima285 | 19:2fe650d29823 | 36 | if (fall_flag == 1) vstavai(); |
dima285 | 20:e73f49ba5001 | 37 | //balance_coord(); |
dima285 | 20:e73f49ba5001 | 38 | balance_motion_PID(); |
dima285 | 17:bd6b6ac89e0e | 39 | if(external_command != 0) command_process(); |
dima285 | 19:2fe650d29823 | 40 | if (delay_counter-- == 3) { |
dima285 | 19:2fe650d29823 | 41 | glaz.startContinuous(100); |
dima285 | 19:2fe650d29823 | 42 | echo_start(); |
dima285 | 19:2fe650d29823 | 43 | } |
dima285 | 17:bd6b6ac89e0e | 44 | if (delay_counter == 0) { |
dima285 | 20:e73f49ba5001 | 45 | if(motion_mode > 0) {if(coord_ready) {motion_mode++;if (motion_mode > 2) motion_mode = 0; delay2_counter = 15;}} |
dima285 | 17:bd6b6ac89e0e | 46 | else{ |
dima285 | 20:e73f49ba5001 | 47 | if (delay2_counter > 0) delay2_counter--;//delay before full stop and scan |
dima285 | 19:2fe650d29823 | 48 | else{ |
dima285 | 19:2fe650d29823 | 49 | obstacle_glaz[serva_counter] = glaz.read()/10;//cm //Function |
dima285 | 19:2fe650d29823 | 50 | glaz.stopContinuous(); |
dima285 | 19:2fe650d29823 | 51 | obstacle_echo[serva_counter] = echo_cm; |
dima285 | 19:2fe650d29823 | 52 | //if (obstacle_glaz[serva_counter] < obstacle_echo[serva_counter]) obstacle[serva_counter] = obstacle_glaz[serva_counter]; else obstacle[serva_counter] = obstacle_echo[serva_counter]; |
dima285 | 19:2fe650d29823 | 53 | obstacle[serva_counter] = obstacle_glaz[serva_counter]; |
dima285 | 19:2fe650d29823 | 54 | if(serva_counter < 12)serva(-90 + (serva_counter++)*15); |
dima285 | 19:2fe650d29823 | 55 | else{ |
dima285 | 19:2fe650d29823 | 56 | serva_counter = 0; |
dima285 | 19:2fe650d29823 | 57 | serva(-90); |
dima285 | 19:2fe650d29823 | 58 | correct_obstacle(); |
dima285 | 19:2fe650d29823 | 59 | analyze_obstacle(); |
dima285 | 20:e73f49ba5001 | 60 | //echo_transmit(13); |
dima285 | 20:e73f49ba5001 | 61 | //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 | 20:e73f49ba5001 | 62 | motion_mode = 1; |
dima285 | 20:e73f49ba5001 | 63 | //motion_timeout = 250; |
dima285 | 19:2fe650d29823 | 64 | //wait(2); |
dima285 | 19:2fe650d29823 | 65 | } |
dima285 | 19:2fe650d29823 | 66 | } |
dima285 | 17:bd6b6ac89e0e | 67 | } |
dima285 | 19:2fe650d29823 | 68 | delay_counter = 7; |
dima285 | 17:bd6b6ac89e0e | 69 | } |
dima285 | 17:bd6b6ac89e0e | 70 | //voice_command_process(); |
dima285 | 17:bd6b6ac89e0e | 71 | //if (k++ > 200) {k = 0;/*play(rand()%10+1);*/} else { target.x = trajectory[int(k/100)][0];target.y = trajectory[int(k/100)][1]; target.azimuth = trajectory[int(k/100)][2];} |
dima285 | 17:bd6b6ac89e0e | 72 | //if (l++ > 25) {l = 0; wifi.printf("%u", glaz.read());} |
dima285 | 17:bd6b6ac89e0e | 73 | realtime_flag = 0;// myled = 1; |
Stas285 | 4:904b737ef08a | 74 | } |
Stas285 | 0:e9488589a8ee | 75 | //while(1){proc_counter++;} |
Stas285 | 0:e9488589a8ee | 76 | } |
Stas285 | 15:960b922433d1 | 77 |