Diff: main.cpp

Revision:

10:6c8ea68e9bac

Parent:

9:76b59c5220f1

Child:

11:58187c7de709

--- a/main.cpp	Tue May 11 17:53:17 2021 +0000
+++ b/main.cpp	Wed May 12 19:00:45 2021 +0000
@@ -18,7 +18,7 @@
 PwmOut rplidar_motor(D3);
 
 float MapaLog[40][40] = {0};
-float Mapa40[40][40] = {0.5};
+float Mapa40[40][40];
 
 void bresenham(float poseX, float poseY, float xf, float yf, float z);
 
@@ -37,16 +37,20 @@
     //float odomX, odomY, odomTheta;
     struct RPLidarMeasurement data;
     
+    //Inicializar Mapa das probabilidades a 0.5
+    for(int i=0; i<40; i++)
+        for(int j=0; j<40; j++)
+            Mapa40[i][j]=0.5;
+    
     // Lidar initialization
     rplidar_motor.period(0.001f);
     //rplidar_motor.write(0.5f);
     lidar.begin(se_lidar);
     lidar.setAngle(0, 360);
     
-    int pose[3] = {20, 20}; // Ponto Inicial
-    float p_angulo = 0;
-    int LidarP[2]; // pontos na plataforma
-    int LidarW[2]; // pontos no mundo
+    float pose[3] = {20, 20, 0}; // Ponto Inicial
+    float LidarP[2]; // pontos na plataforma
+    float LidarW[2]; // pontos no mundo
     
     /*pc.printf("Inicializacao MapaLog\n\r");
     for(int i = 0; i < 40; i++){
@@ -57,8 +61,8 @@
     
     
     // matriz rotacao world plataforma
-    float R_WP[3][3]= {{cos(p_angulo), -sin(p_angulo), pose[0]},
-                        {sin(p_angulo), cos(p_angulo), pose[1]},
+    float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
+                        {sin(pose[2]), cos(pose[2]), pose[1]},
                         {0, 0, 1}};
     
     setSpeeds(0, 0);
@@ -80,26 +84,22 @@
     lidar.startThreadScan();
     
     pc.printf("Entrar no ciclo\n\r");
-    while(leituras < 500){
+    while(leituras < 1000){
         if(lidar.pollSensorData(&data) == 0)
-        {            
-            /*if (UserButton == 0) { // Button is pressed
-                break;
-            }*/
-            
+        {           
             //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
             
             float radians = (data.angle * static_cast<float>(PI))/180.0f;
             
-            LidarP[0] = -data.distance*cos(radians)- 2.8f;
-            LidarP[1] = -data.distance*sin(radians)- 1.5f;
+            LidarP[0] = -data.distance*sin(radians)- 2.8f;
+            LidarP[1] = -data.distance*cos(radians)- 1.5f;
 
             //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
-            bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance);
+            bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5);
             
             leituras++;
         }
@@ -112,20 +112,16 @@
     rplidar_motor.write(0.0f);
     for(int i=0; i<40; i++){
         for(int j=0; j<40; j++){
-            pc.printf("%f|", Mapa40[i][j]);
-            //send_map(Mapa40[j][i]); // envia linha em linha (j i)
-            //send_odometry(1, 2, Mapa40[j][i], j, i,10, 30); // faz prints estranhos no Putty
+            //pc.printf("%0.3f ", Mapa40[i][j]);
+            send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty
+            wait(0.1);
         }
-        pc.printf("\n-----------------------------\n\r");
+        //pc.printf("\n\r");
+        //pc.printf("\n-----------------------------\n\r");
     }
 }
 
 
-
-
-
-
-
 void bresenham(float poseX, float poseY, float xf, float yf, float z){
     int T, E, A, B;
     int x = static_cast<int>(poseX);
@@ -169,7 +165,7 @@
         if (x >= 0 && y >= 0 && x < 40 && y < 40){
             // Mapear mapa do Logaritmo
             MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
-            pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
+            //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
             Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
         }