funcs.cpp@6:df6b8b2468d8, 2021-05-18 (annotated)
- Committer:
- xaficz
- Date:
- Tue May 18 00:04:27 2021 +0000
- Revision:
- 6:df6b8b2468d8
- Child:
- 7:5fa6f21eb739
gbhuj
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| xaficz | 6:df6b8b2468d8 | 1 | #include "mbed.h" |
| xaficz | 6:df6b8b2468d8 | 2 | #include "BufferedSerial.h" |
| xaficz | 6:df6b8b2468d8 | 3 | #include "rplidar.h" |
| xaficz | 6:df6b8b2468d8 | 4 | #include "Robot.h" |
| xaficz | 6:df6b8b2468d8 | 5 | #include "Communication.h" |
| xaficz | 6:df6b8b2468d8 | 6 | #include <string.h> |
| xaficz | 6:df6b8b2468d8 | 7 | #include <stdio.h> |
| xaficz | 6:df6b8b2468d8 | 8 | #include <math.h> |
| xaficz | 6:df6b8b2468d8 | 9 | |
| xaficz | 6:df6b8b2468d8 | 10 | #define pi 3.14159265359 |
| xaficz | 6:df6b8b2468d8 | 11 | |
| xaficz | 6:df6b8b2468d8 | 12 | |
| xaficz | 6:df6b8b2468d8 | 13 | int x_c; |
| xaficz | 6:df6b8b2468d8 | 14 | int y_c; |
| xaficz | 6:df6b8b2468d8 | 15 | int next_x; |
| xaficz | 6:df6b8b2468d8 | 16 | int next_y; |
| xaficz | 6:df6b8b2468d8 | 17 | float auxx; |
| xaficz | 6:df6b8b2468d8 | 18 | float auxy; |
| xaficz | 6:df6b8b2468d8 | 19 | double dist_odometria; |
| xaficz | 6:df6b8b2468d8 | 20 | |
| xaficz | 6:df6b8b2468d8 | 21 | |
| xaficz | 6:df6b8b2468d8 | 22 | //Funcao para criar um mapa simples com um quadrado no meio |
| xaficz | 6:df6b8b2468d8 | 23 | void read_map(int Map_Matrix[80][80]){ |
| xaficz | 6:df6b8b2468d8 | 24 | for(int i = 0; i < 80; i++){ |
| xaficz | 6:df6b8b2468d8 | 25 | for(int j = 0; j < 80; j++){ |
| xaficz | 6:df6b8b2468d8 | 26 | if(i == 0 || j==0 || i==79 || j==79){ |
| xaficz | 6:df6b8b2468d8 | 27 | Map_Matrix[i][j] = 1; |
| xaficz | 6:df6b8b2468d8 | 28 | } |
| xaficz | 6:df6b8b2468d8 | 29 | else{ |
| xaficz | 6:df6b8b2468d8 | 30 | Map_Matrix[i][j] = 0; |
| xaficz | 6:df6b8b2468d8 | 31 | if((i >= 2 && j>=2 && i<=10 && j<=10) || (i >=69 && j>=69 && i<=77 && j<=77) || (i >=69 && j>=69 && i<=10 && j<=10) || (i >= 2 && j>=2 && i<=77 && j<=77) || (i >= 2 && j>=2 && i<=10 && j<=10)){ |
| xaficz | 6:df6b8b2468d8 | 32 | Map_Matrix[i][j] = 1; |
| xaficz | 6:df6b8b2468d8 | 33 | } |
| xaficz | 6:df6b8b2468d8 | 34 | } |
| xaficz | 6:df6b8b2468d8 | 35 | } |
| xaficz | 6:df6b8b2468d8 | 36 | } |
| xaficz | 6:df6b8b2468d8 | 37 | } |
| xaficz | 6:df6b8b2468d8 | 38 | |
| xaficz | 6:df6b8b2468d8 | 39 | |
| xaficz | 6:df6b8b2468d8 | 40 | int distancia_prevista (int x , int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80]){ |
| xaficz | 6:df6b8b2468d8 | 41 | |
| xaficz | 6:df6b8b2468d8 | 42 | // x_celula --> posição odometria x |
| xaficz | 6:df6b8b2468d8 | 43 | // y_celula --> posição odometria y |
| xaficz | 6:df6b8b2468d8 | 44 | // Distance --> Leitura do lidar |
| xaficz | 6:df6b8b2468d8 | 45 | |
| xaficz | 6:df6b8b2468d8 | 46 | int find = 0; |
| xaficz | 6:df6b8b2468d8 | 47 | next_x = x; |
| xaficz | 6:df6b8b2468d8 | 48 | next_y = y; |
| xaficz | 6:df6b8b2468d8 | 49 | |
| xaficz | 6:df6b8b2468d8 | 50 | x_c = x_celula; |
| xaficz | 6:df6b8b2468d8 | 51 | y_c = y_celula; |
| xaficz | 6:df6b8b2468d8 | 52 | |
| xaficz | 6:df6b8b2468d8 | 53 | //:::::::::::: calcular estimativa :::::::::::::::::: |
| xaficz | 6:df6b8b2468d8 | 54 | while(find == 0) |
| xaficz | 6:df6b8b2468d8 | 55 | |
| xaficz | 6:df6b8b2468d8 | 56 | if(Map_Matrix[x_c][y_c] == 1){ |
| xaficz | 6:df6b8b2468d8 | 57 | find = 1; |
| xaficz | 6:df6b8b2468d8 | 58 | } |
| xaficz | 6:df6b8b2468d8 | 59 | else |
| xaficz | 6:df6b8b2468d8 | 60 | next_x = next_x + 5*cos(Angle); // meti ele a andar para a frente de 5 em 5 cm |
| xaficz | 6:df6b8b2468d8 | 61 | next_y = next_y + 5*sin(Angle); // isto pode causar algum erro |
| xaficz | 6:df6b8b2468d8 | 62 | |
| xaficz | 6:df6b8b2468d8 | 63 | auxx=next_x/5; |
| xaficz | 6:df6b8b2468d8 | 64 | auxy= next_y/5; |
| xaficz | 6:df6b8b2468d8 | 65 | x_c = ceil(auxx)-1; |
| xaficz | 6:df6b8b2468d8 | 66 | y_c = ceil(auxy) - 1; |
| xaficz | 6:df6b8b2468d8 | 67 | |
| xaficz | 6:df6b8b2468d8 | 68 | |
| xaficz | 6:df6b8b2468d8 | 69 | |
| xaficz | 6:df6b8b2468d8 | 70 | dist_odometria = ((next_x-x)*(next_x-x)) + ((next_y-y)*(next_y-y)); |
| xaficz | 6:df6b8b2468d8 | 71 | dist_odometria = sqrt(dist_odometria); |
| xaficz | 6:df6b8b2468d8 | 72 | |
| xaficz | 6:df6b8b2468d8 | 73 | return(dist_odometria); |
| xaficz | 6:df6b8b2468d8 | 74 | }// fim do while |
| xaficz | 6:df6b8b2468d8 | 75 | |
| xaficz | 6:df6b8b2468d8 | 76 | |
| xaficz | 6:df6b8b2468d8 | 77 | // usei o next_x_cel com erro de que ele faz de 5 em 5 cm |
| xaficz | 6:df6b8b2468d8 | 78 | // mas tb posso meter de 1 em 1 ou ate menos |
| xaficz | 6:df6b8b2468d8 | 79 |