Diff: Functions.cpp

Revision:

4:256f2cbe3fdd

Child:

5:bc42c03f2a23

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Functions.cpp	Thu May 06 16:03:09 2021 +0000
@@ -0,0 +1,142 @@
+
+#include <math.h>
+#include <cmath>
+
+void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
+{
+    w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
+    w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
+}
+
+
+void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[3])
+{
+    // Deslocamentos
+    float d_l, d_r, desl, delta_ang, delta_x, delta_y;
+    
+    d_l = 2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f );
+    d_r = 2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f );
+    
+    desl = (d_l+d_r)/2.0f;
+    
+    
+    delta_ang = (d_r-d_l)/wheelsDistance;
+    
+    delta_x = desl * cos(pose[2]+delta_ang/2.0f);
+    delta_y = desl * sin(pose[2]+delta_ang/2.0f);
+    
+    
+    pose[0] = pose[0] + delta_x;
+    pose[1] = pose[1] + delta_y;
+    pose[2] = pose[2] + delta_ang;
+}
+
+int** bresenham(float poseX, float poseY, float x1, float y1, int *dim){
+    
+    float T, E, A, B;
+    float x = poseX;
+    float y = poseY;
+    float dx = abs(x1 - poseX);
+    float dy = abs(y1 - poseY);
+    
+    float s1 = (x1 - poseX)/dx; // substitui o sign() do matlab
+    float s2 = (y1 - poseY)/dy;
+    
+    int interchange = 0;
+
+    if (dy > dx){
+        T = dx;
+        dx = dy;
+        dy = T;
+        interchange = 1;
+    }
+    
+    E = 2.0f*dy - dx;
+    A = 2.0f*dy;
+    B = 2.0f*dy - 2.0f*dx;
+    
+    // =========================================
+    //  Inicializar tabela bidimensional a zero
+    // =========================================
+    int width = 2;
+    int height = (int)(double)(dx+0.5);
+    *dim = height;
+    
+    int** pointsVec = 0;
+    pointsVec = new int*[height];
+    
+    for (int h = 0; h < height; h++){
+        pointsVec[h] = new int[width];
+        for (int w = 0; w < width; w++){
+              pointsVec[h][w] = 0;
+        }
+    }
+    // =========================================
+    
+    for (int i = 0; i<dx; i++){
+        if (E < 0){
+            if (interchange == 1){
+                y = y + s2;
+            }
+            else{
+                x = x + s1;
+            }
+            E = E + A;
+        }
+        
+        else{
+            y = y + s2;
+            x = x + s1;
+            E = E + B;
+        }
+        
+        pointsVec[i][0] = static_cast<int>(x); // converte de float para int (confirmar)
+        pointsVec[i][1] = static_cast<int>(y);
+    }
+    
+    return pointsVec;
+}
+
+
+float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){
+    
+    
+    float z_max = 200; // 2 m
+    float alfa = 5; // 5 cm
+    //float beta = 1; // 1 grau
+    float L0 = 0.0;
+    float Locc = 0.65;
+    float Lfree = -0.65;
+    float L;
+    
+    float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) );
+    //phi = atan2( yi-yt, xi-xt ) - theta;
+    
+    //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
+        //L = L0;
+    if ((z < z_max) && (abs(r-z_max) < alfa/2.0))
+        L = Locc;
+    else if (r <= z)
+        L = Lfree;
+    else
+        L = L0;
+    
+    return L;
+    
+}
+
+void Mapping(float MapaLog[40][40], float xi, float yi, int **pointsVec, float z, int dim){
+    
+    int x, y;
+    float L;
+    
+    for(int i=0; i<dim; i++){
+        x = pointsVec[i][0];
+        y = pointsVec[i][1];
+        
+        L = Algorith_Inverse(xi, yi, x, y, z);
+        
+        MapaLog[x][y] = MapaLog[x][y] + L;
+    }
+    
+}
\ No newline at end of file