Diff: main.cpp

Revision:

4:256f2cbe3fdd

Parent:

3:0a718d139ed1

Child:

5:bc42c03f2a23

--- a/main.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/main.cpp	Thu May 06 16:03:09 2021 +0000
@@ -4,6 +4,10 @@
 #include "rplidar.h"
 #include "Robot.h"
 #include "Communication.h"
+#include "Functions.h"
+
+#include <stdlib.h>
+#include <stdio.h>
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 RPLidar lidar;
@@ -12,27 +16,116 @@
 
 int main()
 {
+    
     float odomX, odomY, odomTheta;
     struct RPLidarMeasurement data;
     
     pc.baud(115200);
     init_communication(&pc);
-
+    pc.printf("Program started.\n\r");
+    
     // Lidar initialization
     rplidar_motor.period(0.001f);
+    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 Mapa40[40][40];
+    float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},// matriz rotacao world plataforma 
+                        {sin(pose[2]), cos(pose[2]), pose[1]},
+                        {0, 0, 1}};
+    
+    int dim; // guarda a dimensao (numero de linhas) de pointsVec
+    
+    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.
+            
+            //radians = ( data.angle * pi ) / 180;
+            
+            //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;
 
-    pc.printf("Program started.\n");
+            //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]);
+            }
+            
+            
+            /* 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;*/
+            
+        //}
         
-    lidar.startThreadScan();
+    //}
+    
+    // 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
+        }
+    }
+       
+    /*
     while(1) {
         // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
         if(lidar.pollSensorData(&data) == 0)
         {
-            pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+            //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