Diff: main.cpp

Revision:

5:bc42c03f2a23

Parent:

4:256f2cbe3fdd

Child:

6:59fbbeaac2af

--- a/main.cpp	Thu May 06 16:03:09 2021 +0000
+++ b/main.cpp	Fri May 07 14:58:46 2021 +0000
@@ -5,10 +5,13 @@
 #include "Robot.h"
 #include "Communication.h"
 #include "Functions.h"
+#include "math.h"
 
 #include <stdlib.h>
 #include <stdio.h>
 
+#define PI 3.1415926535
+
 Serial pc(SERIAL_TX, SERIAL_RX);
 RPLidar lidar;
 BufferedSerial se_lidar(PA_9, PA_10);
@@ -17,103 +20,86 @@
 int main()
 {
     
-    float odomX, odomY, odomTheta;
-    struct RPLidarMeasurement data;
-    
     pc.baud(115200);
     init_communication(&pc);
-    pc.printf("Program started.\n\r");
+    
+    DigitalIn UserButton(USER_BUTTON); // Initialize Button
+    DigitalOut myled(LED1); // Initialize LED
+    
+    //float odomX, odomY, odomTheta;
+    struct RPLidarMeasurement data;
     
     // Lidar initialization
     rplidar_motor.period(0.001f);
-    rplidar_motor.write(0.5f);
+    //rplidar_motor.write(0.5f);
     lidar.begin(se_lidar);
     lidar.setAngle(0,360);
     
     float pose[3] = {20,20,0}; // Ponto Inicial
-    //int** pointsVec; // ponteiro duplo para a tabela dimensional que guarda os valores da funcao bresenham
     float LidarP[2]; // pontos na plataforma
     float LidarW[2]; // pontos no mundo
-    float MapaLog[40][40] = {0};
+    float MapaLog[40][40];
+    for(int i = 0; i < 40; i++)
+        for(int j = 0; j < 40; j++)
+            MapaLog[i][j] = 0;
+    
     float Mapa40[40][40];
-    float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},// matriz rotacao world plataforma 
+    
+    // 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]},
                         {0, 0, 1}};
     
-    int dim; // guarda a dimensao (numero de linhas) de pointsVec
+    setSpeeds(0,0);
+    
+    int leituras = 0;
+    
+    pc.printf("waiting...\n\r");
+    
+    int start = 0;
+    while(start != 1) {
+        myled=1;
+        if (UserButton == 0) { // Button is pressed
+            myled = 0;
+            start = 1;
+            rplidar_motor.write(0.5f);
+        }
+    }
+    
+    lidar.startThreadScan();
     
     pc.printf("Program started.\n\r");
     
-    //lidar.startThreadScan();
-    
-    setSpeeds(0,0);
-    
-    float dist = 10;
-    float angle = 0;
-    
-    //while(1){
-        //if(lidar.pollSensorData(&data) == 0)
-        //{
-            //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
+    while(1){
+        if(lidar.pollSensorData(&data) == 0)
+        {
+            if(leituras == 100){
+                break;
+            }
+            pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
             
-            //radians = ( data.angle * pi ) / 180;
+            float radians = (data.angle * PI)/180.0;
             
-            //LidarP[0] = -data.distance*cos(radians)- 2.8f;
-            //LidarP[1] = -data.distance*sin(radians)- 1.5f;
-            LidarP[0] = -dist*cos(angle)- 2.8f;
-            LidarP[1] = -dist*sin(angle)- 1.5f;
+            LidarP[0] = -data.distance*cos(radians)- 2.8f;
+            LidarP[1] = -data.distance*sin(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
-            pointsVec = bresenham(pose[0], pose[1], LidarW[0], LidarW[1], &dim);
-            
-            for(int i=0; i<dim; i++){
-                pc.printf("%d, %d\n", pointsVec[i][0], pointsVec[i][1]);
-            }
-            
+            bresenham(pose[0], pose[1], LidarW[0], LidarW[1], MapaLog, data.distance);
             
-            /* para estes valores o resultado é o seguinte
-            dist: 10.000000   angle: 0.000000
-
-            dim: 13
-            19, 20
-            18, 20
-            17, 20
-            16, 20
-            15, 19
-            14, 19
-            13, 19
-            12, 19
-            11, 19
-            10, 19
-            9, 19
-            8, 19
-            7, 18*/
-            
-            // Mapear mapa do Logaritmo
-            //Mapping(MapaLog, pose[0], pose[1], pointsVec, data.distance, dim);           
-                        
-            /* //libertar espaco da variavel pointsVec
-            for (int h = 0; h < height; h++){
-                delete [] pointsVec[h];
-            }
-            delete [] pointsVec;
-            pointsVec = 0;*/
-            
-        //}
-        
-    //}
+            leituras++;
+        }
+    }
     
     // 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]));
             //printf("%.2f\n", 1 - 1/(1+exp(MapaLog[i][j])));
-            send_map(Mapa40[j][i]); // envia linha em linha
+            send_map(Mapa40[j][i]); // envia linha em linha (j i)
         }
     }