From 955be6486c3714dd2a6bf80f53798674025bd063 Mon Sep 17 00:00:00 2001
From: EnricoGuccii <partyka.003@proton.me>
Date: Sat, 30 May 2026 19:12:12 +0200
Subject: motors driver fix

---
 lib/motors/motors.cpp | 143 +++++++++++++++++++++-----------------------------
 1 file changed, 59 insertions(+), 84 deletions(-)

(limited to 'lib')

diff --git a/lib/motors/motors.cpp b/lib/motors/motors.cpp
index 843de22..1ad4fb2 100644
--- a/lib/motors/motors.cpp
+++ b/lib/motors/motors.cpp
@@ -8,139 +8,114 @@
 #define MOTOR_FREQ 5000
 #define MOTOR_RES 10
 
-int MOTORS::scaleSpeed(int speed)
-{
+MOTORS::MOTORS() {}
+
+int MOTORS::scaleSpeed(int speed) {
     return map(speed, 0, 100, 0, (1 << MOTOR_RES) - 1);
 }
 
-MOTORS::MOTORS()
-{
-}
+void MOTORS::init() {
+    ledcSetup(LEFT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+    ledcAttachPin(EN12, LEFT_CHANNEL);
 
-void MOTORS::stop()
-{
-    ledcWrite(RIGHT_CHANNEL, 0);
-    ledcWrite(LEFT_CHANNEL, 0);
+    ledcSetup(RIGHT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
+    ledcAttachPin(EN34, RIGHT_CHANNEL);
+
+    pinMode(IN1, OUTPUT);
+    pinMode(IN2, OUTPUT);
+    pinMode(IN3, OUTPUT);
+    pinMode(IN4, OUTPUT);
 }
 
-void MOTORS::rightStop()
-{
+void MOTORS::stop() {
+    ledcWrite(LEFT_CHANNEL, 0);
     ledcWrite(RIGHT_CHANNEL, 0);
 }
 
-void MOTORS::leftStop()
-{
+void MOTORS::leftStop() {
     ledcWrite(LEFT_CHANNEL, 0);
 }
 
-void MOTORS::init()
-{
-    ledcSetup(LEFT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
-    ledcAttachPin(EN34, LEFT_CHANNEL);
-
-    ledcSetup(RIGHT_CHANNEL, MOTOR_FREQ, MOTOR_RES);
-    ledcAttachPin(EN12, RIGHT_CHANNEL);
-
-    pinMode(IN1, OUTPUT);
-    pinMode(IN2, OUTPUT);
-    pinMode(IN3, OUTPUT);
-    pinMode(IN4, OUTPUT);
+void MOTORS::rightStop() {
+    ledcWrite(RIGHT_CHANNEL, 0);
 }
 
-void MOTORS::leftBackward(int speed, int duration)
-{
+void MOTORS::leftBackward(int speed, int duration) {
     digitalWrite(IN1, HIGH);
     digitalWrite(IN2, LOW);
-    ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+    ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
 
-    if (duration > 0)
-    {
-        delay(duration);
-        stop();
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS);
+        leftStop();
     }
 }
 
-void MOTORS::leftForward(int speed, int duration)
-{
+void MOTORS::leftForward(int speed, int duration) {
     digitalWrite(IN1, LOW);
     digitalWrite(IN2, HIGH);
-    ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed));
+    ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
 
-    if (duration > 0)
-    {
-        delay(duration);
-        stop();
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS);
+        leftStop();
     }
 }
 
-void MOTORS::rightBackward(int speed, int duration)
-{
+void MOTORS::rightBackward(int speed, int duration) {
     digitalWrite(IN3, HIGH);
     digitalWrite(IN4, LOW);
-    ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
-    ledcWrite(RIGHT_CHANNEL, 0);
+    ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed)); 
 
-    if (duration > 0)
-    {
-        delay(duration);
-        stop();
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS);
+        rightStop();
     }
 }
 
-void MOTORS::rightForward(int speed, int duration)
-{
+void MOTORS::rightForward(int speed, int duration) {
     digitalWrite(IN3, LOW);
     digitalWrite(IN4, HIGH);
-    ledcWrite(LEFT_CHANNEL, scaleSpeed(speed));
-    ledcWrite(RIGHT_CHANNEL, 0);
+    ledcWrite(RIGHT_CHANNEL, scaleSpeed(speed)); 
 
-    if (duration > 0)
-    {
-        delay(duration);
-        stop();
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS);
+        rightStop();
     }
 }
 
-void MOTORS::forward(int speed, int duration)
-{
-    leftForward(speed);
-    rightForward(speed);
-    if (duration > 0)
-    {
-        delay(duration);
-        stop();
+void MOTORS::forward(int speed, int duration) {
+    leftForward(speed, 0);  
+    rightForward(speed, 0); 
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS); 
+        stop(); 
     }
 }
 
-void MOTORS::backward(int speed, int duration)
-{
-    leftBackward(speed);
-    rightBackward(speed);
-    if (duration > 0)
-    {
-        delay(duration);
+void MOTORS::backward(int speed, int duration) {
+    leftBackward(speed, 0);
+    rightBackward(speed, 0);
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS);
         stop();
     }
 }
 
-void MOTORS::leftTurn(int speed, int duration)
-{
-    leftForward(speed);
-    rightBackward(speed);
-    if (duration > 0)
-    {
-        delay(duration);
+void MOTORS::leftTurn(int speed, int duration) {
+    leftBackward(speed, 0);
+    rightForward(speed, 0);
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS);
         stop();
     }
 }
 
-void MOTORS::rightTurn(int speed, int duration)
-{
-    leftBackward(speed);
-    rightForward(speed);
-    if (duration > 0)
-    {
-        delay(duration);
+void MOTORS::rightTurn(int speed, int duration) {
+    leftForward(speed, 0);
+    rightBackward(speed, 0);
+    if (duration > 0) {
+        vTaskDelay(duration / portTICK_PERIOD_MS);
         stop();
     }
 }
-- 
cgit v1.2.3