Diff: main.cpp

Revision:

7:f1c122bc63c8

Parent:

6:59fbbeaac2af

Child:

8:ad8766cf2ec0

--- a/main.cpp	Fri May 07 17:19:07 2021 +0000
+++ b/main.cpp	Mon May 10 15:23:24 2021 +0000
@@ -23,6 +23,7 @@
     pc.baud(115200);
     init_communication(&pc);
     
+    pc.printf("======================\n\r");
     pc.printf("Inicio\n\r");
     
     DigitalIn UserButton(USER_BUTTON); // Initialize Button
@@ -38,25 +39,33 @@
     lidar.setAngle(0,360);
     
     pc.printf("Inicializacao de variaveis\n\r");
-    float pose[3] = {20,20,0}; // Ponto Inicial
-    float LidarP[2]; // pontos na plataforma
-    float LidarW[2]; // pontos no mundo
+    int pose[3] = {20,20}; // Ponto Inicial
+    float p_angulo = 0;
+    int LidarP[2]; // pontos na plataforma
+    int LidarW[2]; // pontos no mundo
+    pc.printf("Inicializacao MapaLog\n\r");
     float MapaLog[40][40];
-    for(int i = 0; i < 40; i++)
-        for(int j = 0; j < 40; j++)
+    pc.printf("Inicializacao MapaLog a 0\n\r");
+    for(int i = 0; i < 40; i++){
+        pc.printf("%d\n\r", i);
+        for(int j = 0; j < 40; j++){
+            pc.printf("%d ", j);
             MapaLog[i][j] = 0;
-    for(int i = 0; i < 40; i++){
+        }
+        pc.printf("\n\r");
+    }
+    /*for(int i = 0; i < 40; i++){
         for(int j = 0; j < 40; j++)
             pc.printf("%.1f ", MapaLog[i][j]);
         pc.printf("\n\r");
-    }
-    
+    }*/
+    pc.printf("Inicializar Mapa40\n\r");
     float Mapa40[40][40];
     
-    pc.printf("Inicializacao Rotacao\n\r");
+    pc.printf("Inicializar Rotacao\n\r");
     // matriz rotacao world plataforma 
-    float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
-                        {sin(pose[2]), cos(pose[2]), pose[1]},
+    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);
@@ -75,11 +84,14 @@
         }
     }
     
-    //lidar.startThreadScan();
+    pc.printf("StartThreadScan.\n\r");
+    
+    lidar.startThreadScan();
     
     pc.printf("Program started.\n\r");
     
     while(1){
+        pc.printf("ciclo\n\r");
         if(lidar.pollSensorData(&data) == 0)
         {
             if(leituras == 100){
@@ -97,12 +109,13 @@
             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], pose[1], LidarW[0], LidarW[1], MapaLog, static_cast<float>(data.distance));
-            //cona(pose[0], pose[1], LidarW[0], LidarW[1], static_cast<float>(data.distance));
+            pc.printf("Entrar no bresenham\n\r");
+            bresenham(pose[0], pose[1], LidarW[0], LidarW[1], MapaLog, data.distance);
+            //test2(pose[0], pose[1], LidarW[0], LidarW[1], data.distance);
             
             leituras++;
         }
-        pc.printf("ciclo.\n\r");
+        //pc.printf("ciclo.\n\r");
     }
     
     // Converter o logaritmo para o mapa 40
@@ -112,7 +125,7 @@
             //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 (j i)
-            send_odometry(1, 2, 5, 4, 10,10, 30);
+            //send_odometry(1, 2, 5, 4, 10,10, 30); // faz prints estranhos no Putty
         }
     }