Gonçalo Lopes / Mbed OS 3

Dependencies:   BufferedSerial

funcs.cpp@7:5fa6f21eb739, 2021-05-24 (annotated)

Committer:
xaficz
Date:
Mon May 24 15:32:13 2021 +0000
Revision:
7:5fa6f21eb739
Parent:
6:df6b8b2468d8 
4__zenaga

Who changed what in which revision?

UserRevisionLine numberNew 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 7:5fa6f21eb739 13 int *x_cel;
xaficz 7:5fa6f21eb739 14 int *y_cel;
xaficz 6:df6b8b2468d8 15 int x_c;
xaficz 6:df6b8b2468d8 16 int y_c;
xaficz 7:5fa6f21eb739 17
xaficz 6:df6b8b2468d8 18 int next_x;
xaficz 6:df6b8b2468d8 19 int next_y;
xaficz 6:df6b8b2468d8 20 float auxx;
xaficz 6:df6b8b2468d8 21 float auxy;
xaficz 7:5fa6f21eb739 22 int dist_odometria;
xaficz 7:5fa6f21eb739 23 int x_end;
xaficz 7:5fa6f21eb739 24 int y_end;
xaficz 6:df6b8b2468d8 25
xaficz 6:df6b8b2468d8 26 //Funcao para criar um mapa simples com um quadrado no meio
xaficz 6:df6b8b2468d8 27 void read_map(int Map_Matrix[80][80]){
xaficz 6:df6b8b2468d8 28 for(int i = 0; i < 80; i++){
xaficz 6:df6b8b2468d8 29 for(int j = 0; j < 80; j++){
xaficz 6:df6b8b2468d8 30 if(i == 0 || j==0 || i==79 || j==79){
xaficz 6:df6b8b2468d8 31 Map_Matrix[i][j] = 1;
xaficz 6:df6b8b2468d8 32 }
xaficz 6:df6b8b2468d8 33 else{
xaficz 6:df6b8b2468d8 34 Map_Matrix[i][j] = 0;
xaficz 6:df6b8b2468d8 35 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 36 Map_Matrix[i][j] = 1;
xaficz 6:df6b8b2468d8 37 }
xaficz 6:df6b8b2468d8 38 }
xaficz 6:df6b8b2468d8 39 }
xaficz 6:df6b8b2468d8 40 }
xaficz 6:df6b8b2468d8 41 }
xaficz 6:df6b8b2468d8 42
xaficz 7:5fa6f21eb739 43 /*
xaficz 7:5fa6f21eb739 44 void distancia_prevista (int x , int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80],float dist_odometria){
xaficz 6:df6b8b2468d8 45
xaficz 6:df6b8b2468d8 46 // x_celula --> posição odometria x
xaficz 6:df6b8b2468d8 47 // y_celula --> posição odometria y
xaficz 6:df6b8b2468d8 48 // Distance --> Leitura do lidar
xaficz 6:df6b8b2468d8 49
xaficz 6:df6b8b2468d8 50 int find = 0;
xaficz 6:df6b8b2468d8 51 next_x = x;
xaficz 6:df6b8b2468d8 52 next_y = y;
xaficz 6:df6b8b2468d8 53
xaficz 6:df6b8b2468d8 54 x_c = x_celula;
xaficz 6:df6b8b2468d8 55 y_c = y_celula;
xaficz 6:df6b8b2468d8 56
xaficz 6:df6b8b2468d8 57 //:::::::::::: calcular estimativa ::::::::::::::::::
xaficz 6:df6b8b2468d8 58 while(find == 0)
xaficz 6:df6b8b2468d8 59
xaficz 6:df6b8b2468d8 60 if(Map_Matrix[x_c][y_c] == 1){
xaficz 6:df6b8b2468d8 61 find = 1;
xaficz 6:df6b8b2468d8 62 }
xaficz 6:df6b8b2468d8 63 else
xaficz 6:df6b8b2468d8 64 next_x = next_x + 5*cos(Angle); // meti ele a andar para a frente de 5 em 5 cm
xaficz 6:df6b8b2468d8 65 next_y = next_y + 5*sin(Angle); // isto pode causar algum erro
xaficz 6:df6b8b2468d8 66
xaficz 6:df6b8b2468d8 67 auxx=next_x/5;
xaficz 6:df6b8b2468d8 68 auxy= next_y/5;
xaficz 6:df6b8b2468d8 69 x_c = ceil(auxx)-1;
xaficz 6:df6b8b2468d8 70 y_c = ceil(auxy) - 1;
xaficz 6:df6b8b2468d8 71
xaficz 6:df6b8b2468d8 72
xaficz 6:df6b8b2468d8 73 dist_odometria = ((next_x-x)*(next_x-x)) + ((next_y-y)*(next_y-y));
xaficz 6:df6b8b2468d8 74 dist_odometria = sqrt(dist_odometria);
xaficz 6:df6b8b2468d8 75
xaficz 7:5fa6f21eb739 76 //return(dist_odometria);
xaficz 6:df6b8b2468d8 77 }// fim do while
xaficz 6:df6b8b2468d8 78
xaficz 6:df6b8b2468d8 79
xaficz 6:df6b8b2468d8 80 // usei o next_x_cel com erro de que ele faz de 5 em 5 cm
xaficz 6:df6b8b2468d8 81 // mas tb posso meter de 1 em 1 ou ate menos
xaficz 6:df6b8b2468d8 82
xaficz 7:5fa6f21eb739 83 */
xaficz 7:5fa6f21eb739 84 /*
xaficz 7:5fa6f21eb739 85 int DistanciaPrevBresh(int x, int y, int x_celula, int y_celula, float angle, int Map_Matrix[80][80] ){
xaficz 7:5fa6f21eb739 86
xaficz 7:5fa6f21eb739 87 if(angle < pi/2 && angle > 3*pi/2)
xaficz 7:5fa6f21eb739 88 x_end = 80;
xaficz 7:5fa6f21eb739 89 else
xaficz 7:5fa6f21eb739 90 x_end = 0;
xaficz 7:5fa6f21eb739 91
xaficz 7:5fa6f21eb739 92 if(angle < 0 && angle < pi)
xaficz 7:5fa6f21eb739 93 y_end = 80;
xaficz 7:5fa6f21eb739 94 else
xaficz 7:5fa6f21eb739 95 y_end = 0;
xaficz 7:5fa6f21eb739 96
xaficz 7:5fa6f21eb739 97
xaficz 7:5fa6f21eb739 98 int find = 0;
xaficz 7:5fa6f21eb739 99
xaficz 7:5fa6f21eb739 100 x_cel = x_bresenham(x_celula, y_celula, x_end, y_end);
xaficz 7:5fa6f21eb739 101 y_cel = y_bresenham(x_celula, y_celula, x_end, y_end);
xaficz 7:5fa6f21eb739 102
xaficz 7:5fa6f21eb739 103
xaficz 7:5fa6f21eb739 104 while(find == 0){
xaficz 7:5fa6f21eb739 105 int i = 0;
xaficz 7:5fa6f21eb739 106 if(Map_Matrix[x_cel[i]][y_cel[i]] == 1){
xaficz 7:5fa6f21eb739 107 find = 1;
xaficz 7:5fa6f21eb739 108 }
xaficz 7:5fa6f21eb739 109 else{
xaficz 7:5fa6f21eb739 110 i=i+1;
xaficz 7:5fa6f21eb739 111 }
xaficz 7:5fa6f21eb739 112 }
xaficz 7:5fa6f21eb739 113
xaficz 7:5fa6f21eb739 114 dist_odometria = ((x_c-x_celula)*(x_c-x_celula)) + ((y_c-y_celula)*(y_c-y_celula));
xaficz 7:5fa6f21eb739 115 dist_odometria = sqrt(dist_odometria);
xaficz 7:5fa6f21eb739 116
xaficz 7:5fa6f21eb739 117 return(dist_odometria);
xaficz 7:5fa6f21eb739 118 }
xaficz 7:5fa6f21eb739 119
xaficz 7:5fa6f21eb739 120 */