Gonçalo Lopes / Mbed OS 3_VFH

3_VFH

Dependencies:   BufferedSerial

funcs.cpp@4:213afe3d5c4b, 2021-05-03 (annotated)

Committer:
xaficz
Date:
Mon May 03 15:45:29 2021 +0000
Revision:
4:213afe3d5c4b
3_VFH

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xaficz 4:213afe3d5c4b 1 #include "mbed.h"
xaficz 4:213afe3d5c4b 2 #include "math.h"
xaficz 4:213afe3d5c4b 3 #include <stdio.h>
xaficz 4:213afe3d5c4b 4 #include "funcs.h"
xaficz 4:213afe3d5c4b 5
xaficz 4:213afe3d5c4b 6 static const double pi = 3.14159265358;
xaficz 4:213afe3d5c4b 7
xaficz 4:213afe3d5c4b 8 Serial pc1(SERIAL_TX, SERIAL_RX);
xaficz 4:213afe3d5c4b 9
xaficz 4:213afe3d5c4b 10
xaficz 4:213afe3d5c4b 11 //Funcao para obter as matrizes de Seccoes e Amplitudes
xaficz 4:213afe3d5c4b 12 void get_Sector_Matrix(int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15]){
xaficz 4:213afe3d5c4b 13 double division_angle = 2*pi/24;
xaficz 4:213afe3d5c4b 14 double Aux_Angle_Matrix[15][15];
xaficz 4:213afe3d5c4b 15 double a = sqrt((double)2)*7;
xaficz 4:213afe3d5c4b 16 double b = 1;
xaficz 4:213afe3d5c4b 17 int i;
xaficz 4:213afe3d5c4b 18
xaficz 4:213afe3d5c4b 19 //Atribui angulos aos varios pontos da matriz
xaficz 4:213afe3d5c4b 20 for(int x = 0; x < 15; x++){
xaficz 4:213afe3d5c4b 21 for(int y = 0; y < 15; y++){
xaficz 4:213afe3d5c4b 22 Aux_Angle_Matrix[x][y] = atan2((double)(y-7),(double)(x-7));
xaficz 4:213afe3d5c4b 23 }
xaficz 4:213afe3d5c4b 24 }
xaficz 4:213afe3d5c4b 25
xaficz 4:213afe3d5c4b 26 //Colocar a matriz de seccoes toda a 0
xaficz 4:213afe3d5c4b 27 for(int x = 0; x < 15; x++){
xaficz 4:213afe3d5c4b 28 for(int y = 0; y < 15; y++){
xaficz 4:213afe3d5c4b 29 Sector_Matrix_Sections[x][y] = 0;
xaficz 4:213afe3d5c4b 30 }
xaficz 4:213afe3d5c4b 31 }
xaficz 4:213afe3d5c4b 32
xaficz 4:213afe3d5c4b 33 //Atribuir o indice da seccao em funcao dos angulos obtidos anteriormente
xaficz 4:213afe3d5c4b 34 for(int x = 0; x < 15; x++){
xaficz 4:213afe3d5c4b 35 for(int y = 0; y < 15; y++){
xaficz 4:213afe3d5c4b 36 i = 0;
xaficz 4:213afe3d5c4b 37 for(double ca = division_angle-pi; ca < pi; ca = ca+division_angle){
xaficz 4:213afe3d5c4b 38 if(Sector_Matrix_Sections[x][y] == 0 && (Aux_Angle_Matrix[x][y] <= ca || Aux_Angle_Matrix[x][y] == pi) && Aux_Angle_Matrix[x][y] >= ca-division_angle){
xaficz 4:213afe3d5c4b 39 Sector_Matrix_Sections[x][y] = i;
xaficz 4:213afe3d5c4b 40 }
xaficz 4:213afe3d5c4b 41 i = i+1;
xaficz 4:213afe3d5c4b 42 }
xaficz 4:213afe3d5c4b 43 }
xaficz 4:213afe3d5c4b 44 }
xaficz 4:213afe3d5c4b 45
xaficz 4:213afe3d5c4b 46 //Atribuir a amplitude a cada elemento da matriz
xaficz 4:213afe3d5c4b 47 for(int x = 0; x < 15; x++){
xaficz 4:213afe3d5c4b 48 for(int y = 0; y < 15; y++){
xaficz 4:213afe3d5c4b 49 Sector_Matrix_Abs[x][y] = a-b*sqrt((double)((7-y)*(7-y) + (7-x)*(7-x)));
xaficz 4:213afe3d5c4b 50 }
xaficz 4:213afe3d5c4b 51 }
xaficz 4:213afe3d5c4b 52 }
xaficz 4:213afe3d5c4b 53
xaficz 4:213afe3d5c4b 54
xaficz 4:213afe3d5c4b 55 //Funcao para obter a matriz das redondezas do robo
xaficz 4:213afe3d5c4b 56 void get_Surroundings_Matrix(int Surroundings_Matrix[15][15], double actual_position_x, double actual_position_y, int Map_Matrix[80][80]){
xaficz 4:213afe3d5c4b 57 int map_position_x = floor(actual_position_x/50);
xaficz 4:213afe3d5c4b 58 int map_position_y = floor(actual_position_y/50);
xaficz 4:213afe3d5c4b 59
xaficz 4:213afe3d5c4b 60 int i = 0;
xaficz 4:213afe3d5c4b 61 int j = 0;
xaficz 4:213afe3d5c4b 62 for(int x = map_position_x-8; x < map_position_x+8; x++){
xaficz 4:213afe3d5c4b 63 for(int y = map_position_y-8; y < map_position_y+8; y++){
xaficz 4:213afe3d5c4b 64 if(x < 0 || x > 79 || y < 0 || y > 79){
xaficz 4:213afe3d5c4b 65 Surroundings_Matrix[i][j] = 1;
xaficz 4:213afe3d5c4b 66 }
xaficz 4:213afe3d5c4b 67 else{
xaficz 4:213afe3d5c4b 68 Surroundings_Matrix[i][j] = Map_Matrix[x][y];
xaficz 4:213afe3d5c4b 69 }
xaficz 4:213afe3d5c4b 70 j = j + 1;
xaficz 4:213afe3d5c4b 71 }
xaficz 4:213afe3d5c4b 72 i = i + 1;
xaficz 4:213afe3d5c4b 73 j = 0;
xaficz 4:213afe3d5c4b 74 }
xaficz 4:213afe3d5c4b 75 }
xaficz 4:213afe3d5c4b 76
xaficz 4:213afe3d5c4b 77 //Funcao auxiliar de get_orientation_angle para encontrar os indices do histograma
xaficz 4:213afe3d5c4b 78 void find_index(int given_index, int l, int out_index[9]){
xaficz 4:213afe3d5c4b 79 int n = 0;
xaficz 4:213afe3d5c4b 80 for(int i = given_index-l; i <= given_index+l; i++){
xaficz 4:213afe3d5c4b 81 if(i < 0){
xaficz 4:213afe3d5c4b 82 out_index[n] = 22 + i;
xaficz 4:213afe3d5c4b 83 n++;
xaficz 4:213afe3d5c4b 84 }
xaficz 4:213afe3d5c4b 85 else{
xaficz 4:213afe3d5c4b 86 if(i > 22){
xaficz 4:213afe3d5c4b 87 out_index[n] = i - 23;
xaficz 4:213afe3d5c4b 88 n++;
xaficz 4:213afe3d5c4b 89 }
xaficz 4:213afe3d5c4b 90 else{
xaficz 4:213afe3d5c4b 91 out_index[n] = i;
xaficz 4:213afe3d5c4b 92 n++;
xaficz 4:213afe3d5c4b 93 }
xaficz 4:213afe3d5c4b 94 }
xaficz 4:213afe3d5c4b 95 }
xaficz 4:213afe3d5c4b 96 }
xaficz 4:213afe3d5c4b 97
xaficz 4:213afe3d5c4b 98 //Funcao para obter o angulo de orientacao seguinte
xaficz 4:213afe3d5c4b 99 double get_orientation_angle(double actual_position_x, double actual_position_y, double end_position_x, double end_position_y, int Surroundings_Matrix[15][15], int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15], double threshold){
xaficz 4:213afe3d5c4b 100 double Sector_Sum_Vector[23] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
xaficz 4:213afe3d5c4b 101 double Sector_Sum_Vector_Filtered[23];
xaficz 4:213afe3d5c4b 102 double auxiliar_angle[23];
xaficz 4:213afe3d5c4b 103 double angle_difference_end[23];
xaficz 4:213afe3d5c4b 104 double section_angle = 2*pi/23;
xaficz 4:213afe3d5c4b 105 double end_angle;
xaficz 4:213afe3d5c4b 106 int index = 0;
xaficz 4:213afe3d5c4b 107 int l = 4;
xaficz 4:213afe3d5c4b 108 int out_index[(2*l)+1];
xaficz 4:213afe3d5c4b 109 int ind;
xaficz 4:213afe3d5c4b 110
xaficz 4:213afe3d5c4b 111 double Surroundings_Matrix_Aux[15][15];
xaficz 4:213afe3d5c4b 112
xaficz 4:213afe3d5c4b 113 //Atribuimos os valores de amplitudes aos pontos da matriz das redondezas
xaficz 4:213afe3d5c4b 114 for(int x = 0; x < 15; x++){
xaficz 4:213afe3d5c4b 115 for(int y = 0; y < 15; y++){
xaficz 4:213afe3d5c4b 116 Surroundings_Matrix_Aux[x][y] = Surroundings_Matrix[x][y]*Sector_Matrix_Abs[x][y];
xaficz 4:213afe3d5c4b 117 }
xaficz 4:213afe3d5c4b 118 }
xaficz 4:213afe3d5c4b 119
xaficz 4:213afe3d5c4b 120 //Criamos o vetor do histograma das seccoes
xaficz 4:213afe3d5c4b 121 for(int x = 0; x < 15; x++){
xaficz 4:213afe3d5c4b 122 for(int y = 0; y < 15; y++){
xaficz 4:213afe3d5c4b 123 index = Sector_Matrix_Sections[x][y];
xaficz 4:213afe3d5c4b 124 Sector_Sum_Vector[index] = Sector_Sum_Vector[index] + Surroundings_Matrix_Aux[x][y];
xaficz 4:213afe3d5c4b 125 }
xaficz 4:213afe3d5c4b 126 }
xaficz 4:213afe3d5c4b 127
xaficz 4:213afe3d5c4b 128 //Filtramos o histograma
xaficz 4:213afe3d5c4b 129 for(int i = 0; i < 23; i++){
xaficz 4:213afe3d5c4b 130 find_index(i,l,out_index);
xaficz 4:213afe3d5c4b 131
xaficz 4:213afe3d5c4b 132 for(int c = 0; c < l; c++){
xaficz 4:213afe3d5c4b 133 ind = out_index[c];
xaficz 4:213afe3d5c4b 134 Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (c+1)*Sector_Sum_Vector[ind];
xaficz 4:213afe3d5c4b 135 }
xaficz 4:213afe3d5c4b 136
xaficz 4:213afe3d5c4b 137 Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l+1)*Sector_Sum_Vector[l];
xaficz 4:213afe3d5c4b 138
xaficz 4:213afe3d5c4b 139 for(int c = 0; c < l; c++){
xaficz 4:213afe3d5c4b 140 ind = out_index[c+l+1];
xaficz 4:213afe3d5c4b 141 Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l-c)*Sector_Sum_Vector[ind];
xaficz 4:213afe3d5c4b 142 }
xaficz 4:213afe3d5c4b 143
xaficz 4:213afe3d5c4b 144 Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i]/(2*l+1);
xaficz 4:213afe3d5c4b 145 }
xaficz 4:213afe3d5c4b 146
xaficz 4:213afe3d5c4b 147
xaficz 4:213afe3d5c4b 148 for(int i = 0; i < 23; i++){
xaficz 4:213afe3d5c4b 149 if(Sector_Sum_Vector_Filtered[i] < threshold){
xaficz 4:213afe3d5c4b 150 auxiliar_angle[i] = i*section_angle - pi;
xaficz 4:213afe3d5c4b 151 }
xaficz 4:213afe3d5c4b 152 else{
xaficz 4:213afe3d5c4b 153 auxiliar_angle[i] = 999;
xaficz 4:213afe3d5c4b 154 }
xaficz 4:213afe3d5c4b 155 }
xaficz 4:213afe3d5c4b 156
xaficz 4:213afe3d5c4b 157 end_angle = atan2((double)(end_position_y-actual_position_y),(double)(end_position_x-actual_position_x));
xaficz 4:213afe3d5c4b 158
xaficz 4:213afe3d5c4b 159 //Escolhemos a secccao disponivel cujo angulo e mais proximo do angulo final
xaficz 4:213afe3d5c4b 160 for(int i = 0; i < 23; i++){
xaficz 4:213afe3d5c4b 161 angle_difference_end[i] = sqrt((double)(auxiliar_angle[i] - end_angle)*(auxiliar_angle[i] - end_angle));
xaficz 4:213afe3d5c4b 162 }
xaficz 4:213afe3d5c4b 163
xaficz 4:213afe3d5c4b 164 ind = 0;
xaficz 4:213afe3d5c4b 165 for(int i = 0; i < 23; i++){
xaficz 4:213afe3d5c4b 166 if(angle_difference_end[i] < angle_difference_end[ind]){
xaficz 4:213afe3d5c4b 167 ind = i;
xaficz 4:213afe3d5c4b 168 }
xaficz 4:213afe3d5c4b 169 }
xaficz 4:213afe3d5c4b 170
xaficz 4:213afe3d5c4b 171 if(auxiliar_angle[ind] == 999) auxiliar_angle[ind] = 0; // caso de erro andar em frente
xaficz 4:213afe3d5c4b 172
xaficz 4:213afe3d5c4b 173 return(auxiliar_angle[ind]);
xaficz 4:213afe3d5c4b 174 }
xaficz 4:213afe3d5c4b 175
xaficz 4:213afe3d5c4b 176 void update_map(double Log_Map_Matrix[80][80], int Aux_Matrix[80][80], int Map_Matrix[80][80], double distance, double theta, double actual_position_x, double actual_position_y){
xaficz 4:213afe3d5c4b 177 double inc = 1;
xaficz 4:213afe3d5c4b 178 int x, y;
xaficz 4:213afe3d5c4b 179 actual_position_x = floor(actual_position_x/50); // saber a celula
xaficz 4:213afe3d5c4b 180 actual_position_y = floor(actual_position_y/50);
xaficz 4:213afe3d5c4b 181 double ang = theta;
xaficz 4:213afe3d5c4b 182 for(double r = inc; r < (distance-inc); r += inc){
xaficz 4:213afe3d5c4b 183 x = actual_position_x+floor(r*cos(ang));
xaficz 4:213afe3d5c4b 184 y = actual_position_y+floor(r*sin(ang));
xaficz 4:213afe3d5c4b 185 if(x >= 0 && y >= 0 && x < 80 && y < 80){
xaficz 4:213afe3d5c4b 186 if(Aux_Matrix[x][y] != 1){
xaficz 4:213afe3d5c4b 187 Log_Map_Matrix[x][y] = Log_Map_Matrix[x][y] - 0.65;
xaficz 4:213afe3d5c4b 188 Aux_Matrix[x][y] = 1;
xaficz 4:213afe3d5c4b 189 if((1-(1/(1+exp(Log_Map_Matrix[x][y])))) > 0.5){
xaficz 4:213afe3d5c4b 190 Map_Matrix[x][y] = 1;
xaficz 4:213afe3d5c4b 191 }
xaficz 4:213afe3d5c4b 192 else{
xaficz 4:213afe3d5c4b 193 Map_Matrix[y][x] = 0;
xaficz 4:213afe3d5c4b 194 }
xaficz 4:213afe3d5c4b 195 }
xaficz 4:213afe3d5c4b 196 }
xaficz 4:213afe3d5c4b 197 }
xaficz 4:213afe3d5c4b 198
xaficz 4:213afe3d5c4b 199 x = actual_position_x+floor(distance*cos(ang));
xaficz 4:213afe3d5c4b 200 y = actual_position_y+floor(distance*sin(ang));
xaficz 4:213afe3d5c4b 201 if(x >= 0 && y >= 0 && x < 80 && y < 80){
xaficz 4:213afe3d5c4b 202 if(Aux_Matrix[x][y] != 1){
xaficz 4:213afe3d5c4b 203 Log_Map_Matrix[x][y] = Log_Map_Matrix[x][y] + 0.65;
xaficz 4:213afe3d5c4b 204 Aux_Matrix[x][y] = 1;
xaficz 4:213afe3d5c4b 205 if((1-(1/(1+exp(Log_Map_Matrix[x][y])))) > 0.5){
xaficz 4:213afe3d5c4b 206 Map_Matrix[x][y] = 1;
xaficz 4:213afe3d5c4b 207 }
xaficz 4:213afe3d5c4b 208 else{
xaficz 4:213afe3d5c4b 209 Map_Matrix[y][x] = 0;
xaficz 4:213afe3d5c4b 210 }
xaficz 4:213afe3d5c4b 211 }
xaficz 4:213afe3d5c4b 212 }
xaficz 4:213afe3d5c4b 213
xaficz 4:213afe3d5c4b 214 //for(double ang=(theta-alpha); ang <= (theta+alpha); ang += alpha){
xaficz 4:213afe3d5c4b 215 for(double r = 0; r < (distance-inc); r += inc){
xaficz 4:213afe3d5c4b 216 x = actual_position_x+floor(r*cos(ang));
xaficz 4:213afe3d5c4b 217 y = actual_position_y+floor(r*sin(ang));
xaficz 4:213afe3d5c4b 218 if(x >= 0 && y >= 0 && x < 80 && y < 80){
xaficz 4:213afe3d5c4b 219 Aux_Matrix[x][y] = 0;
xaficz 4:213afe3d5c4b 220 }
xaficz 4:213afe3d5c4b 221 }
xaficz 4:213afe3d5c4b 222
xaficz 4:213afe3d5c4b 223 x = actual_position_x+floor(distance*cos(theta));
xaficz 4:213afe3d5c4b 224 y = actual_position_y+floor(distance*sin(theta));
xaficz 4:213afe3d5c4b 225 if(x >= 0 && y >= 0 && x < 80 && y < 80){
xaficz 4:213afe3d5c4b 226 Aux_Matrix[x][y] = 0;
xaficz 4:213afe3d5c4b 227 }
xaficz 4:213afe3d5c4b 228 }
xaficz 4:213afe3d5c4b 229