Diff: Functions.cpp

Revision:

8:ad8766cf2ec0

Parent:

7:f1c122bc63c8

Child:

9:76b59c5220f1

--- 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]));
+        }
         
+        
+                  
     }
-    */
-
 }