Revision 9:76b59c5220f1, committed 2021-05-11

Comitter:

henkiwan

Date:

Tue May 11 17:53:17 2021 +0000

Parent:

8:ad8766cf2ec0

Child:

10:6c8ea68e9bac

Commit message:

yah; ; temos de ver o log;

Changed in this revision

--- a/Communication.cpp	Tue May 11 15:05:49 2021 +0000
+++ b/Communication.cpp	Tue May 11 17:53:17 2021 +0000
@@ -2,7 +2,7 @@
 #include "mbed.h"
 #include "MessageBuilder.h"
 
-const char max_len = 30;
+//const char max_len = 30;
 Serial *serial_object;
 MessageBuilder bin_msg;
 

--- a/Functions.cpp	Tue May 11 15:05:49 2021 +0000
+++ b/Functions.cpp	Tue May 11 17:53:17 2021 +0000
@@ -47,7 +47,7 @@
     
     //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
         //L = L0;
-    if ((z < z_max) && (abs(r-z_max) < alfa/2.0))
+    if ((z < z_max) && (abs(r-z) < alfa/2.0))
         L = Locc;
     else if (r <= z)
         L = Lfree;
@@ -58,53 +58,3 @@
     
 }
 
-void bresenham(float poseX, float poseY, float xf, float yf, float z){
-    int T, E, A, B;
-    int x = static_cast<int>(poseX);
-    int y = static_cast<int>(poseY);
-    int dx = static_cast<int>(abs(xf - poseX));
-    int dy = static_cast<int>(abs(yf - poseY));
-    
-    int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
-    int s2 = static_cast<int>((yf - poseY)/dy);
-    
-    int interchange = 0;
-    
-    if (dy > dx){
-        T = dx;
-        dx = dy;
-        dy = T;
-        interchange = 1;
-    }
-    
-    E = 2*dy - dx;
-    A = 2*dy;
-    B = 2*dy - 2*dx;
-    
-    for (int i = 0; i<dx; i++){
-        if (E < 0){
-            if (interchange == 1){
-                y = y + s2;
-            }
-            else{
-                x = x + s1;
-            }
-            E = E + A;
-        }
-        
-        else{
-            y = y + s2;
-            x = x + s1;
-            E = E + B;
-        }
-        
-        if (x >= 0 && y >= 0 && x < 40 && y < 40){
-            // Mapear mapa do Logaritmo
-            MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
-            Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
-        }
-        
-        
-                  
-    }
-}

--- a/Functions.h	Tue May 11 15:05:49 2021 +0000
+++ b/Functions.h	Tue May 11 17:53:17 2021 +0000
@@ -8,4 +8,4 @@
 void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[]);
 void JanelaAtivaVFF(float pose[], float Mapa_40[40][40], float poseFinal[], float FCRepul, float FCAtracao, float RectSize, float *ForcaResult);
 float Algorith_Inverse(float xi, float yi, float xt, float yt, float z);
-void bresenham(float xi, float yi, float xf, float yf, float z);
+//void bresenham(float xi, float yi, float xf, float yf, float z);

--- a/main.cpp	Tue May 11 15:05:49 2021 +0000
+++ b/main.cpp	Tue May 11 17:53:17 2021 +0000
@@ -18,7 +18,9 @@
 PwmOut rplidar_motor(D3);
 
 float MapaLog[40][40] = {0};
-float Mapa40[40][40];
+float Mapa40[40][40] = {0.5};
+
+void bresenham(float poseX, float poseY, float xf, float yf, float z);
 
 int main()
 {
@@ -39,9 +41,9 @@
     rplidar_motor.period(0.001f);
     //rplidar_motor.write(0.5f);
     lidar.begin(se_lidar);
-    lidar.setAngle(0,360);
+    lidar.setAngle(0, 360);
     
-    int pose[3] = {20,20}; // Ponto Inicial
+    int pose[3] = {20, 20}; // Ponto Inicial
     float p_angulo = 0;
     int LidarP[2]; // pontos na plataforma
     int LidarW[2]; // pontos no mundo
@@ -53,14 +55,13 @@
         }
     }*/
     
-    float Mapa40[40][40];
     
     // 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}};
     
-    setSpeeds(0,0);
+    setSpeeds(0, 0);
     
     int leituras = 0;
     
@@ -86,7 +87,7 @@
                 break;
             }*/
             
-            pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
+            //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;
             
@@ -106,12 +107,73 @@
     
     // Converter o logaritmo para o mapa 40
     
+    pc.printf("\nFIM DO CICLO\n========================\n\n\r");
+    
     rplidar_motor.write(0.0f);
     for(int i=0; i<40; i++){
         for(int j=0; j<40; j++){
-            pc.printf("%f", Mapa40[i][j]);
+            pc.printf("%f|", Mapa40[i][j]);
             //send_map(Mapa40[j][i]); // envia linha em linha (j i)
-            //send_odometry(1, 2, Mapa40[j][i], 4, 10,10, 30); // faz prints estranhos no Putty
+            //send_odometry(1, 2, Mapa40[j][i], j, i,10, 30); // faz prints estranhos no Putty
         }
+        pc.printf("\n-----------------------------\n\r");
+    }
+}
+
+
+
+
+
+
+
+void bresenham(float poseX, float poseY, float xf, float yf, float z){
+    int T, E, A, B;
+    int x = static_cast<int>(poseX);
+    int y = static_cast<int>(poseY);
+    int dx = static_cast<int>(abs(xf - poseX));
+    int dy = static_cast<int>(abs(yf - poseY));
+    
+    int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
+    int s2 = static_cast<int>((yf - poseY)/dy);
+    
+    int interchange = 0;
+    
+    if (dy > dx){
+        T = dx;
+        dx = dy;
+        dy = T;
+        interchange = 1;
+    }
+    
+    E = 2*dy - dx;
+    A = 2*dy;
+    B = 2*dy - 2*dx;
+    
+    for (int i = 0; i<dx; i++){
+        if (E < 0){
+            if (interchange == 1){
+                y = y + s2;
+            }
+            else{
+                x = x + s1;
+            }
+            E = E + A;
+        }
+        
+        else{
+            y = y + s2;
+            x = x + s1;
+            E = E + B;
+        }
+        
+        if (x >= 0 && y >= 0 && x < 40 && y < 40){
+            // Mapear mapa do Logaritmo
+            MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
+            pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
+            Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
+        }
+        
+        
+                  
     }
 }
\ No newline at end of file