Diff: main.cpp

Revision:

9:76b59c5220f1

Parent:

8:ad8766cf2ec0

Child:

10:6c8ea68e9bac

--- a/main.cpp	Tue May 11 15:05:49 2021 +0000
+++ b/main.cpp	Tue May 11 17:53:17 2021 +0000
@@ -18,7 +18,9 @@
 PwmOut rplidar_motor(D3);
 
 float MapaLog[40][40] = {0};
-float Mapa40[40][40];
+float Mapa40[40][40] = {0.5};
+
+void bresenham(float poseX, float poseY, float xf, float yf, float z);
 
 int main()
 {
@@ -39,9 +41,9 @@
     rplidar_motor.period(0.001f);
     //rplidar_motor.write(0.5f);
     lidar.begin(se_lidar);
-    lidar.setAngle(0,360);
+    lidar.setAngle(0, 360);
     
-    int pose[3] = {20,20}; // Ponto Inicial
+    int pose[3] = {20, 20}; // Ponto Inicial
     float p_angulo = 0;
     int LidarP[2]; // pontos na plataforma
     int LidarW[2]; // pontos no mundo
@@ -53,14 +55,13 @@
         }
     }*/
     
-    float Mapa40[40][40];
     
     // 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]},
                         {0, 0, 1}};
     
-    setSpeeds(0,0);
+    setSpeeds(0, 0);
     
     int leituras = 0;
     
@@ -86,7 +87,7 @@
                 break;
             }*/
             
-            pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
+            //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;
             
@@ -106,12 +107,73 @@
     
     // Converter o logaritmo para o mapa 40
     
+    pc.printf("\nFIM DO CICLO\n========================\n\n\r");
+    
     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]);
+            pc.printf("%f|", Mapa40[i][j]);
             //send_map(Mapa40[j][i]); // envia linha em linha (j i)
-            //send_odometry(1, 2, Mapa40[j][i], 4, 10,10, 30); // faz prints estranhos no Putty
+            //send_odometry(1, 2, Mapa40[j][i], j, i,10, 30); // faz prints estranhos no Putty
         }
+        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);
+    int y = static_cast<int>(poseY);
+    int dx = static_cast<int>(abs(xf - poseX));
+    int dy = static_cast<int>(abs(yf - poseY));
+    
+    int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
+    int s2 = static_cast<int>((yf - poseY)/dy);
+    
+    int interchange = 0;
+    
+    if (dy > dx){
+        T = dx;
+        dx = dy;
+        dy = T;
+        interchange = 1;
+    }
+    
+    E = 2*dy - dx;
+    A = 2*dy;
+    B = 2*dy - 2*dx;
+    
+    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;
+        }
+        
+        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])));
+            Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
+        }
+        
+        
+                  
     }
 }
\ No newline at end of file