Diff: Functions.cpp

Revision:

12:348038b466a3

Parent:

11:58187c7de709

Child:

13:20e124fba426

--- a/Functions.cpp	Thu May 13 16:28:09 2021 +0000
+++ b/Functions.cpp	Fri May 14 05:52:55 2021 +0000
@@ -1,6 +1,5 @@
+
 #include <math.h>
-#include <stdio.h>
-#include "Functions.h"
 
 void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
 {
@@ -14,16 +13,16 @@
     // Deslocamentos
     float d_l, d_r, desl, delta_ang, delta_x, delta_y;
     
-    d_l = 2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f );
-    d_r = 2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f );
+    d_l = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f ));
+    d_r = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f ));
     
-    desl = (d_l+d_r)/2.0f;
+    desl = (d_l+d_r)/2;
     
     
     delta_ang = (d_r-d_l)/wheelsDistance;
     
-    delta_x = desl * cos(pose[2]+delta_ang/2.0f);
-    delta_y = desl * sin(pose[2]+delta_ang/2.0f);
+    delta_x = desl * cos(pose[2]+delta_ang/2);
+    delta_y = desl * sin(pose[2]+delta_ang/2);
     
     
     pose[0] = pose[0] + delta_x;
@@ -31,8 +30,7 @@
     pose[2] = pose[2] + delta_ang;
 }
 
-
-float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){
+float Algorithm_Inverse(float xi, float yi, float xt, float yt, float z){
     
     float z_max = 200; // 2 m
     float alfa = 6.0f/5.0f;
@@ -56,5 +54,4 @@
     
     return L;
     
-}
-
+}
\ No newline at end of file