SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

Functions.cpp@4:256f2cbe3fdd, 2021-05-06 (annotated)

Committer:
ppovoa
Date:
Thu May 06 16:03:09 2021 +0000
Revision:
4:256f2cbe3fdd
Child:
5:bc42c03f2a23
Implementacao do preenchimento do mapa de probabilidades (erro de Stackoverflow)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ppovoa 4:256f2cbe3fdd 1
ppovoa 4:256f2cbe3fdd 2 #include <math.h>
ppovoa 4:256f2cbe3fdd 3 #include <cmath>
ppovoa 4:256f2cbe3fdd 4
ppovoa 4:256f2cbe3fdd 5 void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
ppovoa 4:256f2cbe3fdd 6 {
ppovoa 4:256f2cbe3fdd 7 w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
ppovoa 4:256f2cbe3fdd 8 w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
ppovoa 4:256f2cbe3fdd 9 }
ppovoa 4:256f2cbe3fdd 10
ppovoa 4:256f2cbe3fdd 11
ppovoa 4:256f2cbe3fdd 12 void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[3])
ppovoa 4:256f2cbe3fdd 13 {
ppovoa 4:256f2cbe3fdd 14 // Deslocamentos
ppovoa 4:256f2cbe3fdd 15 float d_l, d_r, desl, delta_ang, delta_x, delta_y;
ppovoa 4:256f2cbe3fdd 16
ppovoa 4:256f2cbe3fdd 17 d_l = 2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f );
ppovoa 4:256f2cbe3fdd 18 d_r = 2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f );
ppovoa 4:256f2cbe3fdd 19
ppovoa 4:256f2cbe3fdd 20 desl = (d_l+d_r)/2.0f;
ppovoa 4:256f2cbe3fdd 21
ppovoa 4:256f2cbe3fdd 22
ppovoa 4:256f2cbe3fdd 23 delta_ang = (d_r-d_l)/wheelsDistance;
ppovoa 4:256f2cbe3fdd 24
ppovoa 4:256f2cbe3fdd 25 delta_x = desl * cos(pose[2]+delta_ang/2.0f);
ppovoa 4:256f2cbe3fdd 26 delta_y = desl * sin(pose[2]+delta_ang/2.0f);
ppovoa 4:256f2cbe3fdd 27
ppovoa 4:256f2cbe3fdd 28
ppovoa 4:256f2cbe3fdd 29 pose[0] = pose[0] + delta_x;
ppovoa 4:256f2cbe3fdd 30 pose[1] = pose[1] + delta_y;
ppovoa 4:256f2cbe3fdd 31 pose[2] = pose[2] + delta_ang;
ppovoa 4:256f2cbe3fdd 32 }
ppovoa 4:256f2cbe3fdd 33
ppovoa 4:256f2cbe3fdd 34 int** bresenham(float poseX, float poseY, float x1, float y1, int *dim){
ppovoa 4:256f2cbe3fdd 35
ppovoa 4:256f2cbe3fdd 36 float T, E, A, B;
ppovoa 4:256f2cbe3fdd 37 float x = poseX;
ppovoa 4:256f2cbe3fdd 38 float y = poseY;
ppovoa 4:256f2cbe3fdd 39 float dx = abs(x1 - poseX);
ppovoa 4:256f2cbe3fdd 40 float dy = abs(y1 - poseY);
ppovoa 4:256f2cbe3fdd 41
ppovoa 4:256f2cbe3fdd 42 float s1 = (x1 - poseX)/dx; // substitui o sign() do matlab
ppovoa 4:256f2cbe3fdd 43 float s2 = (y1 - poseY)/dy;
ppovoa 4:256f2cbe3fdd 44
ppovoa 4:256f2cbe3fdd 45 int interchange = 0;
ppovoa 4:256f2cbe3fdd 46
ppovoa 4:256f2cbe3fdd 47 if (dy > dx){
ppovoa 4:256f2cbe3fdd 48 T = dx;
ppovoa 4:256f2cbe3fdd 49 dx = dy;
ppovoa 4:256f2cbe3fdd 50 dy = T;
ppovoa 4:256f2cbe3fdd 51 interchange = 1;
ppovoa 4:256f2cbe3fdd 52 }
ppovoa 4:256f2cbe3fdd 53
ppovoa 4:256f2cbe3fdd 54 E = 2.0f*dy - dx;
ppovoa 4:256f2cbe3fdd 55 A = 2.0f*dy;
ppovoa 4:256f2cbe3fdd 56 B = 2.0f*dy - 2.0f*dx;
ppovoa 4:256f2cbe3fdd 57
ppovoa 4:256f2cbe3fdd 58 // =========================================
ppovoa 4:256f2cbe3fdd 59 // Inicializar tabela bidimensional a zero
ppovoa 4:256f2cbe3fdd 60 // =========================================
ppovoa 4:256f2cbe3fdd 61 int width = 2;
ppovoa 4:256f2cbe3fdd 62 int height = (int)(double)(dx+0.5);
ppovoa 4:256f2cbe3fdd 63 *dim = height;
ppovoa 4:256f2cbe3fdd 64
ppovoa 4:256f2cbe3fdd 65 int** pointsVec = 0;
ppovoa 4:256f2cbe3fdd 66 pointsVec = new int*[height];
ppovoa 4:256f2cbe3fdd 67
ppovoa 4:256f2cbe3fdd 68 for (int h = 0; h < height; h++){
ppovoa 4:256f2cbe3fdd 69 pointsVec[h] = new int[width];
ppovoa 4:256f2cbe3fdd 70 for (int w = 0; w < width; w++){
ppovoa 4:256f2cbe3fdd 71 pointsVec[h][w] = 0;
ppovoa 4:256f2cbe3fdd 72 }
ppovoa 4:256f2cbe3fdd 73 }
ppovoa 4:256f2cbe3fdd 74 // =========================================
ppovoa 4:256f2cbe3fdd 75
ppovoa 4:256f2cbe3fdd 76 for (int i = 0; i<dx; i++){
ppovoa 4:256f2cbe3fdd 77 if (E < 0){
ppovoa 4:256f2cbe3fdd 78 if (interchange == 1){
ppovoa 4:256f2cbe3fdd 79 y = y + s2;
ppovoa 4:256f2cbe3fdd 80 }
ppovoa 4:256f2cbe3fdd 81 else{
ppovoa 4:256f2cbe3fdd 82 x = x + s1;
ppovoa 4:256f2cbe3fdd 83 }
ppovoa 4:256f2cbe3fdd 84 E = E + A;
ppovoa 4:256f2cbe3fdd 85 }
ppovoa 4:256f2cbe3fdd 86
ppovoa 4:256f2cbe3fdd 87 else{
ppovoa 4:256f2cbe3fdd 88 y = y + s2;
ppovoa 4:256f2cbe3fdd 89 x = x + s1;
ppovoa 4:256f2cbe3fdd 90 E = E + B;
ppovoa 4:256f2cbe3fdd 91 }
ppovoa 4:256f2cbe3fdd 92
ppovoa 4:256f2cbe3fdd 93 pointsVec[i][0] = static_cast<int>(x); // converte de float para int (confirmar)
ppovoa 4:256f2cbe3fdd 94 pointsVec[i][1] = static_cast<int>(y);
ppovoa 4:256f2cbe3fdd 95 }
ppovoa 4:256f2cbe3fdd 96
ppovoa 4:256f2cbe3fdd 97 return pointsVec;
ppovoa 4:256f2cbe3fdd 98 }
ppovoa 4:256f2cbe3fdd 99
ppovoa 4:256f2cbe3fdd 100
ppovoa 4:256f2cbe3fdd 101 float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){
ppovoa 4:256f2cbe3fdd 102
ppovoa 4:256f2cbe3fdd 103
ppovoa 4:256f2cbe3fdd 104 float z_max = 200; // 2 m
ppovoa 4:256f2cbe3fdd 105 float alfa = 5; // 5 cm
ppovoa 4:256f2cbe3fdd 106 //float beta = 1; // 1 grau
ppovoa 4:256f2cbe3fdd 107 float L0 = 0.0;
ppovoa 4:256f2cbe3fdd 108 float Locc = 0.65;
ppovoa 4:256f2cbe3fdd 109 float Lfree = -0.65;
ppovoa 4:256f2cbe3fdd 110 float L;
ppovoa 4:256f2cbe3fdd 111
ppovoa 4:256f2cbe3fdd 112 float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) );
ppovoa 4:256f2cbe3fdd 113 //phi = atan2( yi-yt, xi-xt ) - theta;
ppovoa 4:256f2cbe3fdd 114
ppovoa 4:256f2cbe3fdd 115 //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
ppovoa 4:256f2cbe3fdd 116 //L = L0;
ppovoa 4:256f2cbe3fdd 117 if ((z < z_max) && (abs(r-z_max) < alfa/2.0))
ppovoa 4:256f2cbe3fdd 118 L = Locc;
ppovoa 4:256f2cbe3fdd 119 else if (r <= z)
ppovoa 4:256f2cbe3fdd 120 L = Lfree;
ppovoa 4:256f2cbe3fdd 121 else
ppovoa 4:256f2cbe3fdd 122 L = L0;
ppovoa 4:256f2cbe3fdd 123
ppovoa 4:256f2cbe3fdd 124 return L;
ppovoa 4:256f2cbe3fdd 125
ppovoa 4:256f2cbe3fdd 126 }
ppovoa 4:256f2cbe3fdd 127
ppovoa 4:256f2cbe3fdd 128 void Mapping(float MapaLog[40][40], float xi, float yi, int **pointsVec, float z, int dim){
ppovoa 4:256f2cbe3fdd 129
ppovoa 4:256f2cbe3fdd 130 int x, y;
ppovoa 4:256f2cbe3fdd 131 float L;
ppovoa 4:256f2cbe3fdd 132
ppovoa 4:256f2cbe3fdd 133 for(int i=0; i<dim; i++){
ppovoa 4:256f2cbe3fdd 134 x = pointsVec[i][0];
ppovoa 4:256f2cbe3fdd 135 y = pointsVec[i][1];
ppovoa 4:256f2cbe3fdd 136
ppovoa 4:256f2cbe3fdd 137 L = Algorith_Inverse(xi, yi, x, y, z);
ppovoa 4:256f2cbe3fdd 138
ppovoa 4:256f2cbe3fdd 139 MapaLog[x][y] = MapaLog[x][y] + L;
ppovoa 4:256f2cbe3fdd 140 }
ppovoa 4:256f2cbe3fdd 141
ppovoa 4:256f2cbe3fdd 142 }