diff --git a/TCPServer.cpp b/TCPServer.cpp
index da5d832..9647cd8 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -171,7 +171,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
             }
             if (args[0] == "0") {
                 if (!handleEmergecnyFlag) {
-                    int angle = TCPUtils::mapValue(value, -32767, 32768, - PI / 2, PI / 2);
+                    int angle = static_cast<int>(TCPUtils::mapValue(value, -32767.0, 32768.0, - PI / 2, PI / 2));
                     this->broadcastMessage("strat;arduino;angle;" + std::to_string(angle) + "\n");
                 }
             }
@@ -181,11 +181,11 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
                 value = -value;
 
                 if (value < 0) {
-                    speed = TCPUtils::mapValue(value, -32767, 0, -310, -70);
+                    speed = static_cast<int>(TCPUtils::mapValue(value, -32767.0, 0.0, -310.0, -70.0));
                 } else if (value == 0) {
                     speed = 0; // Maps 0 to 0
                 } else {
-                    speed = TCPUtils::mapValue(value, 70, 32768, 70, 310);
+                    speed = static_cast<int>(TCPUtils::mapValue(value, 0.0, 32768.0, 70.0, 310.0));
                 }
 
                 if (!handleEmergecnyFlag) {
@@ -201,7 +201,7 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
                 }
             }
             else if (args[0] == "2") {
-                int speed = TCPUtils::mapValue(value, -32767, 32768, -310, 310);
+                int speed = static_cast<int>(TCPUtils::mapValue(value, -32767.0, 32768.0, -310.0, 310.0));
                 this->broadcastMessage("start;arduino;rotate;" + std::to_string(speed));
             }
         }
diff --git a/utils.h b/utils.h
index dba2819..5db47f0 100644
--- a/utils.h
+++ b/utils.h
@@ -26,8 +26,8 @@ namespace TCPUtils {
     std::vector<std::string> split(const std::string& str, const std::string& delimiter);
 
     template<typename T>
-    int mapValue(T v, int v_min, int v_max, int v_min_prime, int v_max_prime) {
-        return v_min_prime + (static_cast<int>(v) - v_min) * (v_max_prime - v_min_prime) / (v_max - v_min);
+    T mapValue(T v, T v_min, T v_max, T v_min_prime, T v_max_prime) {
+        return v_min_prime + (v - v_min) * (v_max_prime - v_min_prime) / (v_max - v_min);
     }
 }