Diff: funcs.cpp
- Revision:
- 0:f623431aed01
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/funcs.cpp Thu May 13 15:19:19 2021 +0000
@@ -0,0 +1,225 @@
+#include "mbed.h"
+#include "math.h"
+#include <stdio.h>
+#include "funcs.h"
+
+static const double pi = 3.14159265358;
+
+Serial pc1(SERIAL_TX, SERIAL_RX);
+
+
+//Funcao para criar um mapa simples com um quadrado no meio
+void read_map1(int Map_Matrix[80][80]){
+ for(int i = 0; i < 80; i++){
+ for(int j = 0; j < 80; j++){
+ Map_Matrix[i][j] = 0;
+
+ if((i >= 30 || j >= 30) || (i >= 5 && j >=5 && i <= 10 && j <= 10) || (i >= 5 && j >= 20 && i <= 10 && j <= 25)){
+ Map_Matrix[i][j] = 1;
+ }
+ }
+ }
+}
+
+//Funcao para obter as matrizes de Seccoes e Amplitudes
+void get_Sector_Matrix(int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15]){
+ double division_angle = 2*pi/24;
+ double Aux_Angle_Matrix[15][15];
+ double a = sqrt((double)2)*7;
+ double b = 1;
+ int i;
+
+ //Atribui angulos aos varios pontos da matriz
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ Aux_Angle_Matrix[x][y] = atan2((double)(y-7),(double)(x-7));
+ }
+ }
+
+ //Colocar a matriz de seccoes toda a 0
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ Sector_Matrix_Sections[x][y] = 0;
+ }
+ }
+
+ //Atribuir o indice da seccao em funcao dos angulos obtidos anteriormente
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ i = 0;
+ for(double ca = division_angle-pi; ca < pi; ca = ca+division_angle){
+ 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){
+ Sector_Matrix_Sections[x][y] = i;
+ }
+ i = i+1;
+ }
+ }
+ }
+
+
+ //Atribuir a amplitude a cada elemento da matriz
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ Sector_Matrix_Abs[x][y] = a-b*sqrt((double)((7-y)*(7-y) + (7-x)*(7-x)));
+ }
+ }
+ /*
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ pc1.printf("%d,",Sector_Matrix_Sections[x][y]);
+ }
+ pc1.printf("\n");
+ }
+ */
+}
+
+//Funcao para obter a matriz das redondezas do robo
+void get_Surroundings_Matrix(int Surroundings_Matrix[15][15], double actual_position_x, double actual_position_y, int Map_Matrix[80][80]){
+ int map_position_x = floor(actual_position_x/50);
+ int map_position_y = floor(actual_position_y/50);
+
+ int i = 0;
+ int j = 0;
+ for(int x = map_position_x-8; x < map_position_x+8; x++){
+ for(int y = map_position_y-8; y < map_position_y+8; y++){
+ if(x < 0 || x > 79 || y < 0 || y > 79){
+ Surroundings_Matrix[i][j] = 1;
+ }
+ else{
+ Surroundings_Matrix[i][j] = Map_Matrix[x][y];
+ }
+ j = j + 1;
+ }
+ i = i + 1;
+ j = 0;
+ }
+ pc1.printf("matriz surroundings:\n");
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ pc1.printf("%d,", Surroundings_Matrix[x][y]);
+ }
+ pc1.printf("\n");
+ }
+
+}
+
+//Funcao auxiliar de get_orientation_angle para encontrar os indices do histograma
+void find_index(int given_index, int l, int out_index[9]){
+ int n = 0;
+ for(int i = given_index-l; i <= given_index+l; i++){
+ if(i < 0){
+ out_index[n] = 22 + i;
+ n++;
+ }
+ else{
+ if(i > 22){
+ out_index[n] = i - 23;
+ n++;
+ }
+ else{
+ out_index[n] = i;
+ n++;
+ }
+ }
+ }
+
+}
+
+//Funcao para obter o angulo de orientacao seguinte
+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){
+ 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};
+ double Sector_Sum_Vector_Filtered[23];
+ double auxiliar_angle[23];
+ double angle_difference_end[23];
+ double section_angle = 2*pi/23;
+ double end_angle;
+ int index = 0;
+ int out_index[23];
+ int ind;
+ int l = 4;
+
+ double Surroundings_Matrix_Aux[15][15];
+
+ //Atribuimos os valores de amplitudes aos pontos da matriz das redondezas
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ Surroundings_Matrix_Aux[x][y] = Surroundings_Matrix[x][y]*Sector_Matrix_Abs[x][y];
+ }
+ }
+ /*
+ pc1.printf("matriz abs:\n");
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ pc1.printf("%f,", Surroundings_Matrix_Aux[x][y]);
+ }
+ pc1.printf("\n");
+ }
+ */
+
+ //Criamos o vetor do histograma das seccoes
+ for(int x = 0; x < 15; x++){
+ for(int y = 0; y < 15; y++){
+ index = Sector_Matrix_Sections[x][y];
+
+ Sector_Sum_Vector[index] = Sector_Sum_Vector[index] + Surroundings_Matrix_Aux[x][y];
+ }
+ }
+
+ /*
+ pc1.printf("tabela s filtro:\n");
+ for(int n = 0; n < 23; n++){
+ pc1.printf("%f\n", Sector_Sum_Vector[n]);
+ }
+ */
+
+ //Filtramos o histograma
+ for(int i = 0; i < 23; i++){
+ find_index(i,l,out_index);
+
+ for(int c = 0; c < l; c++){
+ ind = out_index[c];
+ Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (c+1)*Sector_Sum_Vector[ind];
+ }
+
+ Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l+1)*Sector_Sum_Vector[l];
+
+ for(int c = 0; c < l; c++){
+ ind = out_index[c+l+1];
+ Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l-c)*Sector_Sum_Vector[ind];
+ }
+
+ Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i]/(2*l+1);
+ }
+
+ /*
+ pc1.printf("\ntabela c f:\n");
+ for(int n = 0; n < 23; n++){
+ pc1.printf("%f\n", Sector_Sum_Vector_Filtered[n]);
+ }
+ */
+
+ for(int i = 0; i < 23; i++){
+ if(Sector_Sum_Vector_Filtered[i] < threshold){
+ auxiliar_angle[i] = i*section_angle - pi; // subtraimos o pi
+ } // para que os angulos rodem 180 graus
+ else{
+ auxiliar_angle[i] = 0; // trajectoria negada
+ }
+ }
+
+ end_angle = atan2((double)(end_position_y-actual_position_y),(double)(end_position_x-actual_position_x));
+
+ //Escolhemos a secccao disponivel cujo angulo e mais proximo do angulo final
+ for(int i = 0; i < 23; i++){
+ angle_difference_end[i] = sqrt((double)(auxiliar_angle[i] - end_angle)*(auxiliar_angle[i] - end_angle));
+ }
+
+ ind = 0;
+ for(int i = 0; i < 23; i++){
+ if(angle_difference_end[i] < angle_difference_end[ind]){
+ ind = i;
+ }
+ }
+
+ return(auxiliar_angle[ind]);
+}