Diff: main.cpp

Revision:

8:ad8766cf2ec0

Parent:

7:f1c122bc63c8

Child:

9:76b59c5220f1

--- a/main.cpp	Mon May 10 15:23:24 2021 +0000
+++ b/main.cpp	Tue May 11 15:05:49 2021 +0000
@@ -17,6 +17,9 @@
 BufferedSerial se_lidar(PA_9, PA_10);
 PwmOut rplidar_motor(D3);
 
+float MapaLog[40][40] = {0};
+float Mapa40[40][40];
+
 int main()
 {
     //printf("Inicio\n\r");
@@ -38,32 +41,21 @@
     lidar.begin(se_lidar);
     lidar.setAngle(0,360);
     
-    pc.printf("Inicializacao de variaveis\n\r");
     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];
-    pc.printf("Inicializacao MapaLog a 0\n\r");
+    
+    /*pc.printf("Inicializacao MapaLog\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;
         }
-        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("Inicializar Rotacao\n\r");
-    // matriz rotacao world plataforma 
+    // 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}};
@@ -84,19 +76,16 @@
         }
     }
     
-    pc.printf("StartThreadScan.\n\r");
-    
     lidar.startThreadScan();
     
-    pc.printf("Program started.\n\r");
-    
-    while(1){
-        pc.printf("ciclo\n\r");
+    pc.printf("Entrar no ciclo\n\r");
+    while(leituras < 500){
         if(lidar.pollSensorData(&data) == 0)
-        {
-            if(leituras == 100){
+        {            
+            /*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;
@@ -109,36 +98,20 @@
             LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
             
             // pontos onde o feixe passou
-            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);
+            bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance);
             
             leituras++;
         }
-        //pc.printf("ciclo.\n\r");
     }
     
     // Converter o logaritmo para o mapa 40
     
+    rplidar_motor.write(0.0f);
     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])));
+            pc.printf("%f", Mapa40[i][j]);
             //send_map(Mapa40[j][i]); // envia linha em linha (j i)
-            //send_odometry(1, 2, 5, 4, 10,10, 30); // faz prints estranhos no Putty
+            //send_odometry(1, 2, Mapa40[j][i], 4, 10,10, 30); // faz prints estranhos no Putty
         }
     }
-    
-       
-    /*
-    while(1) {
-        // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
-        if(lidar.pollSensorData(&data) == 0)
-        {
-            //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