Diff: main.cpp

Revision:

11:58187c7de709

Parent:

10:6c8ea68e9bac

Child:

12:348038b466a3

--- a/main.cpp	Wed May 12 19:00:45 2021 +0000
+++ b/main.cpp	Thu May 13 16:28:09 2021 +0000
@@ -24,7 +24,6 @@
 
 int main()
 {
-    //printf("Inicio\n\r");
     pc.baud(115200);
     init_communication(&pc);
     
@@ -64,6 +63,7 @@
     float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
                         {sin(pose[2]), cos(pose[2]), pose[1]},
                         {0, 0, 1}};
+    float dist;
     
     setSpeeds(0, 0);
     
@@ -91,18 +91,24 @@
             
             float radians = (data.angle * static_cast<float>(PI))/180.0f;
             
-            LidarP[0] = -data.distance*sin(radians)- 2.8f;
-            LidarP[1] = -data.distance*cos(radians)- 1.5f;
+            dist = static_cast<float>(data.distance/10.0f); //converte de mm para cm
+            
+            LidarP[0] = -dist*sin(radians)- 2.8f;
+            LidarP[1] = -dist*cos(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];
             
+            /*if (data.angle > 0 && data.angle < 5){
+                pc.printf("Pose[0]: %f Pose[1]: %f Dist: %f Ang: %f LidarP[0]: %f LidarP[1]: %f LidarW[0]: %f LidarW[1]: %f\n\r", pose[0], pose[1], data.distance, data.angle, LidarP[0], LidarP[1], LidarW[0], LidarW[1]);
+            }*/
             // pontos onde o feixe passou
-            bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5);
+            bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, dist/5);
             
             leituras++;
         }
+        wait(0.01);
     }
     
     // Converter o logaritmo para o mapa 40