Revision 8:ad8766cf2ec0, committed 2021-05-11

Comitter:

ppovoa

Date:

Tue May 11 15:05:49 2021 +0000

Parent:

7:f1c122bc63c8

Child:

9:76b59c5220f1

Commit message:

SRA - erro no sendodometry

Changed in this revision

--- a/Functions.cpp	Mon May 10 15:23:24 2021 +0000
+++ b/Functions.cpp	Tue May 11 15:05:49 2021 +0000
@@ -1,7 +1,6 @@
-
 #include <math.h>
-//#include <cmath>
 #include <stdio.h>
+#include "Functions.h"
 
 void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
 {
@@ -59,18 +58,15 @@
     
 }
 
-void bresenham(int poseX, int poseY, int xf, int yf, float (&MapaLog)[40][40], float z){
-//void cona(float poseX, float poseY, float x1, float y1, float z){
-    printf("bresenham!\n\r");
-    /*
+void bresenham(float poseX, float poseY, float xf, float yf, float z){
     int T, E, A, B;
-    int x = poseX;
-    int y = poseY;
-    int dx = abs(xf - poseX);
-    int dy = abs(yf - poseY);
+    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 = (xf - poseX)/dx; // substitui o sign() do matlab
-    int s2 = (yf - poseY)/dy;
+    int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
+    int s2 = static_cast<int>((yf - poseY)/dy);
     
     int interchange = 0;
     
@@ -102,10 +98,13 @@
             E = E + B;
         }
         
-        // Mapear mapa do Logaritmo
-        //MapaLog[x][y] = MapaLog[x][y]; //+ Algorith_Inverse(poseX, poseY, x, y, z);        
+        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	Mon May 10 15:23:24 2021 +0000
+++ b/Functions.h	Tue May 11 15:05:49 2021 +0000
@@ -1,8 +1,11 @@
 #include "mbed.h"
 
+extern float MapaLog[40][40], Mapa40[40][40];
+
+//float MapaLog[40][40];
+
 void velRobot2velWheels(float vRobot, float wRobot, float wheelsRadius, float wheelsDistance, float w[2]);
 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(int xi, int yi, int xf, int yf, float (&MapaLog)[40][40], float z);
-//void test2(float xi, float yi, float x1, float y1, float z);
+void bresenham(float xi, float yi, float xf, float yf, float z);

--- 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