Gonçalo Lopes / Mbed 2 deprecated VFH

Dependencies:   mbed

funcs.cpp@0:f623431aed01, 2021-05-13 (annotated)

Committer:
xaficz
Date:
Thu May 13 15:19:19 2021 +0000
Revision:
0:f623431aed01
vfh

Who changed what in which revision?

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