Diff: main.cpp

Revision:

6:59fbbeaac2af

Parent:

5:bc42c03f2a23

Child:

7:f1c122bc63c8

--- a/main.cpp	Fri May 07 14:58:46 2021 +0000
+++ b/main.cpp	Fri May 07 17:19:07 2021 +0000
@@ -19,10 +19,12 @@
 
 int main()
 {
-    
+    //printf("Inicio\n\r");
     pc.baud(115200);
     init_communication(&pc);
     
+    pc.printf("Inicio\n\r");
+    
     DigitalIn UserButton(USER_BUTTON); // Initialize Button
     DigitalOut myled(LED1); // Initialize LED
     
@@ -35,6 +37,7 @@
     lidar.begin(se_lidar);
     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
@@ -42,9 +45,15 @@
     for(int i = 0; i < 40; i++)
         for(int j = 0; j < 40; j++)
             MapaLog[i][j] = 0;
+    for(int i = 0; i < 40; i++){
+        for(int j = 0; j < 40; j++)
+            pc.printf("%.1f ", MapaLog[i][j]);
+        pc.printf("\n\r");
+    }
     
     float Mapa40[40][40];
     
+    pc.printf("Inicializacao 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]},
@@ -66,7 +75,7 @@
         }
     }
     
-    lidar.startThreadScan();
+    //lidar.startThreadScan();
     
     pc.printf("Program started.\n\r");
     
@@ -78,7 +87,7 @@
             }
             pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
             
-            float radians = (data.angle * PI)/180.0;
+            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;
@@ -88,20 +97,25 @@
             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, data.distance);
+            //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));
             
             leituras++;
         }
+        pc.printf("ciclo.\n\r");
     }
     
     // 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]));
+            //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_map(Mapa40[j][i]); // envia linha em linha (j i)
+            send_odometry(1, 2, 5, 4, 10,10, 30);
         }
     }
+    
        
     /*
     while(1) {