Revision 4:256f2cbe3fdd, committed 2021-05-06

Comitter:

ppovoa

Date:

Thu May 06 16:03:09 2021 +0000

Parent:

3:0a718d139ed1

Child:

5:bc42c03f2a23

Commit message:

Implementacao do preenchimento do mapa de probabilidades (erro de Stackoverflow)

Changed in this revision

--- a/Communication.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/Communication.cpp	Thu May 06 16:03:09 2021 +0000
@@ -32,4 +32,13 @@
     bin_msg.add(theta);
 
     write_bytes(bin_msg.message, bin_msg.length());
+}
+
+void send_map(float Mapa)
+{
+    bin_msg.reset();
+    bin_msg.add('O');
+    bin_msg.add(Mapa);
+
+    write_bytes(bin_msg.message, bin_msg.length());
 }
\ No newline at end of file

--- a/Communication.h	Wed Apr 24 16:00:07 2019 +0000
+++ b/Communication.h	Thu May 06 16:03:09 2021 +0000
@@ -6,5 +6,6 @@
 void init_communication(Serial *serial_in);
 void write_bytes(char *ptr, unsigned char len);
 void send_odometry(int value1, int value2, int ticks_left, int ticks_right, float x, float y, float theta);
+void send_map(float Mapa);
 
 #endif

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Functions.h	Thu May 06 16:03:09 2021 +0000
@@ -0,0 +1,9 @@
+ #include <math.h>
+#include "mbed.h"
+
+void velRobot2velWheels(float vRobot, float wRobot, float wheelsRadius, float wheelsDistance, float w[2]);
+void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[]);
+void JanelaAtivaVFF(float pose[], float Mapa_40[40][40], float poseFinal[], float FCRepul, float FCAtracao, float RectSize, float *ForcaResult);
+int** bresenham(float xi, float yi, float x1, float y1, int *dim);
+float Algorith_Inverse(float xi, float yi, float xt, float yt, float z);
+void Mapping(float MapaLog[40][40], float xi, float yi, int** pointsVec, float z, int dim);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Lidar.lib	Thu May 06 16:03:09 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/fabiofaria/code/DEEC_STM32_for_Romi_and_Rplidar/#0a718d139ed1

--- a/main.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/main.cpp	Thu May 06 16:03:09 2021 +0000
@@ -4,6 +4,10 @@
 #include "rplidar.h"
 #include "Robot.h"
 #include "Communication.h"
+#include "Functions.h"
+
+#include <stdlib.h>
+#include <stdio.h>
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 RPLidar lidar;
@@ -12,27 +16,116 @@
 
 int main()
 {
+    
     float odomX, odomY, odomTheta;
     struct RPLidarMeasurement data;
     
     pc.baud(115200);
     init_communication(&pc);
-
+    pc.printf("Program started.\n\r");
+    
     // Lidar initialization
     rplidar_motor.period(0.001f);
+    rplidar_motor.write(0.5f);
     lidar.begin(se_lidar);
     lidar.setAngle(0,360);
+    
+    float pose[3] = {20,20,0}; // Ponto Inicial
+    //int** pointsVec; // ponteiro duplo para a tabela dimensional que guarda os valores da funcao bresenham
+    float LidarP[2]; // pontos na plataforma
+    float LidarW[2]; // pontos no mundo
+    float MapaLog[40][40] = {0};
+    float Mapa40[40][40];
+    float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},// matriz rotacao world plataforma 
+                        {sin(pose[2]), cos(pose[2]), pose[1]},
+                        {0, 0, 1}};
+    
+    int dim; // guarda a dimensao (numero de linhas) de pointsVec
+    
+    pc.printf("Program started.\n\r");
+    
+    //lidar.startThreadScan();
+    
+    setSpeeds(0,0);
+    
+    float dist = 10;
+    float angle = 0;
+    
+    //while(1){
+        //if(lidar.pollSensorData(&data) == 0)
+        //{
+            //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
+            
+            //radians = ( data.angle * pi ) / 180;
+            
+            //LidarP[0] = -data.distance*cos(radians)- 2.8f;
+            //LidarP[1] = -data.distance*sin(radians)- 1.5f;
+            LidarP[0] = -dist*cos(angle)- 2.8f;
+            LidarP[1] = -dist*sin(angle)- 1.5f;
 
-    pc.printf("Program started.\n");
+            //W_P = R_WP * p_P
+            LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
+            LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
+            
+            // pontos onde o feixe passou
+            pointsVec = bresenham(pose[0], pose[1], LidarW[0], LidarW[1], &dim);
+            
+            for(int i=0; i<dim; i++){
+                pc.printf("%d, %d\n", pointsVec[i][0], pointsVec[i][1]);
+            }
+            
+            
+            /* para estes valores o resultado é o seguinte
+            dist: 10.000000   angle: 0.000000
+
+            dim: 13
+            19, 20
+            18, 20
+            17, 20
+            16, 20
+            15, 19
+            14, 19
+            13, 19
+            12, 19
+            11, 19
+            10, 19
+            9, 19
+            8, 19
+            7, 18*/
+            
+            // Mapear mapa do Logaritmo
+            //Mapping(MapaLog, pose[0], pose[1], pointsVec, data.distance, dim);           
+                        
+            /* //libertar espaco da variavel pointsVec
+            for (int h = 0; h < height; h++){
+                delete [] pointsVec[h];
+            }
+            delete [] pointsVec;
+            pointsVec = 0;*/
+            
+        //}
         
-    lidar.startThreadScan();
+    //}
+    
+    // Converter o logaritmo para o mapa 40
     
+    for(int i=0; i<40; i++){
+        for(int j=0; j<40; j++){
+            Mapa40[j][i] = 1 - 1/(1+exp(MapaLog[j][i]));
+            //printf("%.2f\n", 1 - 1/(1+exp(MapaLog[i][j])));
+            send_map(Mapa40[j][i]); // envia linha em linha
+        }
+    }
+       
+    /*
     while(1) {
         // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
         if(lidar.pollSensorData(&data) == 0)
         {
-            pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+            //if (data.angle > 0 and data.angle < 15)
+                pc.printf("%f\t%f\t%d\t%c\n\r", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+                send_odometry(1, 2, countsLeft, countsRight, odomX, odomY, odomTheta);
         }
        wait(0.01); 
-    }
+    }*/
 }
\ No newline at end of file