SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

JanelaAtivaVFF.cpp@13:20e124fba426, 2021-05-15 (annotated)

Committer:
henkiwan
Date:
Sat May 15 03:17:01 2021 +0000
Revision:
13:20e124fba426
Parent:
12:348038b466a3 
Que merda;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henkiwan 12:348038b466a3 1 #include <math.h>
henkiwan 12:348038b466a3 2 #include <stdio.h>
henkiwan 12:348038b466a3 3 #include <stdlib.h>
henkiwan 12:348038b466a3 4
henkiwan 12:348038b466a3 5 void JanelaAtivaVFF(float pose[], float Mapa_40[40][40], float poseFinal[], float FCRepul, float FCAtracao, float RectSize, float *ForcaResult)
henkiwan 12:348038b466a3 6 {
henkiwan 12:348038b466a3 7 float x0, y0;
henkiwan 12:348038b466a3 8 int cima, baixo, esq, dir;
henkiwan 12:348038b466a3 9
henkiwan 12:348038b466a3 10 x0 = static_cast<float>(ceil(pose[0]/5.0f));
henkiwan 12:348038b466a3 11 y0 = static_cast<float>(ceil(pose[1]/5.0f)); // para corresponder ao Y do plot (de baixo para cima)
henkiwan 12:348038b466a3 12
henkiwan 12:348038b466a3 13 // condições caso a janela ativa esteja fora da matriz 40 por 40
henkiwan 12:348038b466a3 14 // se estiver fora entao fica no limite da matriz
henkiwan 12:348038b466a3 15 cima = (int)(y0-floor(RectSize/2.0f));
henkiwan 12:348038b466a3 16 if(cima < 0)
henkiwan 12:348038b466a3 17 cima = 0;
henkiwan 12:348038b466a3 18
henkiwan 12:348038b466a3 19 baixo = (int)(y0+floor(RectSize/2.0f));
henkiwan 12:348038b466a3 20 if(baixo > 39)
henkiwan 12:348038b466a3 21 baixo = 39;
henkiwan 12:348038b466a3 22
henkiwan 12:348038b466a3 23 esq = (int)(x0-floor(RectSize/2.0f));
henkiwan 12:348038b466a3 24 if(esq < 0)
henkiwan 12:348038b466a3 25 esq = 0;
henkiwan 12:348038b466a3 26
henkiwan 12:348038b466a3 27 dir = (int)(x0+floor(RectSize/2.0f));
henkiwan 12:348038b466a3 28 if(dir > 39)
henkiwan 12:348038b466a3 29 dir = 39;
henkiwan 12:348038b466a3 30
henkiwan 12:348038b466a3 31 // Posicao do robo real (cm)
henkiwan 12:348038b466a3 32 x0 = pose[0];
henkiwan 12:348038b466a3 33 y0 = pose[1];
henkiwan 12:348038b466a3 34
henkiwan 12:348038b466a3 35 // Inicialização de variaveis
henkiwan 12:348038b466a3 36 float fr[2] = {0, 0};
henkiwan 12:348038b466a3 37 float fa[2] = {0, 0};
henkiwan 13:20e124fba426 38 //float FR_magn = 0;
henkiwan 13:20e124fba426 39 //float FR_angle = 0;
henkiwan 13:20e124fba426 40 //float FA_angle = 0;
henkiwan 12:348038b466a3 41
henkiwan 12:348038b466a3 42 float xt, yt, dist;
henkiwan 12:348038b466a3 43
henkiwan 12:348038b466a3 44 // confirmar (x_max - x_min)
henkiwan 12:348038b466a3 45 /*for (int i = 0; i <= (baixo-cima); i++){
henkiwan 12:348038b466a3 46 for (int j = 0; j <= (dir-esq); j++){
henkiwan 12:348038b466a3 47 Janela[i][j] = Mapa_40[cima+i][esq+j];
henkiwan 12:348038b466a3 48 }
henkiwan 12:348038b466a3 49 }
henkiwan 12:348038b466a3 50 */
henkiwan 12:348038b466a3 51 for (int i = cima; i <= baixo; i++){
henkiwan 12:348038b466a3 52 for (int j = esq; j <= dir; j++){
henkiwan 13:20e124fba426 53 //if(Mapa_40[i][j]==1){
henkiwan 12:348038b466a3 54 // Posicao do pixel real (cm)
henkiwan 12:348038b466a3 55 //printf("%f\n\r", Mapa_40[i][j]);
henkiwan 12:348038b466a3 56 xt = (5.0f*(static_cast<float>(j)))-2.5f;
henkiwan 12:348038b466a3 57 yt = (5.0f*(static_cast<float>(i)))-2.5f;
henkiwan 12:348038b466a3 58
henkiwan 12:348038b466a3 59 // Distância do Robo ao Pixel
henkiwan 12:348038b466a3 60 dist = sqrt(pow(yt-y0,2) + pow(xt-x0,2));
henkiwan 13:20e124fba426 61
henkiwan 13:20e124fba426 62 // só as celulas acima de 0.6 que exercem uma forca repulsiva
henkiwan 13:20e124fba426 63 if (Mapa_40[i][j] > 0.6f){
henkiwan 13:20e124fba426 64 fr[0] = fr[0] + Mapa_40[i][j]*FCRepul*(xt-x0)/pow(dist,3);
henkiwan 13:20e124fba426 65 fr[1] = fr[1] + Mapa_40[i][j]*FCRepul*(yt-y0)/pow(dist,3);
henkiwan 13:20e124fba426 66 }
henkiwan 13:20e124fba426 67 //}
henkiwan 12:348038b466a3 68 }
henkiwan 12:348038b466a3 69 }
henkiwan 12:348038b466a3 70
henkiwan 12:348038b466a3 71 //printf("Cima: %d, Baixo: %d, Esq= %d, Dir= %d\n\r", cima, baixo, esq, dir);
henkiwan 12:348038b466a3 72 //printf("Mapa40[28][0]= %f, Mapa_40[28][0]==1: %d\n\r", Mapa_40[28][0], Mapa_40[28][0]==1);
henkiwan 12:348038b466a3 73
henkiwan 13:20e124fba426 74 //FA_angle = atan2((poseFinal[1]-y0), (poseFinal[0]-x0));
henkiwan 12:348038b466a3 75
henkiwan 13:20e124fba426 76 dist = sqrt(pow(poseFinal[1]-y0,2) + pow(poseFinal[0]-x0,2));
henkiwan 13:20e124fba426 77
henkiwan 13:20e124fba426 78 fa[0] = FCAtracao * ((poseFinal[0]-x0)/dist);
henkiwan 13:20e124fba426 79 fa[1] = FCAtracao * ((poseFinal[1]-y0)/dist);
henkiwan 12:348038b466a3 80
henkiwan 12:348038b466a3 81 ForcaResult[0] = fa[0];
henkiwan 12:348038b466a3 82 ForcaResult[1] = fa[1];
henkiwan 12:348038b466a3 83 ForcaResult[2] = fr[0];
henkiwan 12:348038b466a3 84 ForcaResult[3] = fr[1];
henkiwan 12:348038b466a3 85
henkiwan 12:348038b466a3 86 }