From 8dea440f6bda2cb5253dd7be4ea2c27d1306fc2e Mon Sep 17 00:00:00 2001
From: Ulysse Cura <ulysse.cura@gmail.com>
Date: Wed, 29 Jan 2025 18:12:22 +0100
Subject: [PATCH] =?UTF-8?q?Asservissement=20fonctionnel=20mais=20changemen?=
 =?UTF-8?q?t=20de=20direction=20en=20fonction=20du=20temps=20d=C3=A9fectue?=
 =?UTF-8?q?ux,=20tests=20en=20cour...?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 IO.hpp   | 17 +++++++++++++----
 Main.ino |  8 ++++----
 2 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/IO.hpp b/IO.hpp
index 755d822..f70e42e 100644
--- a/IO.hpp
+++ b/IO.hpp
@@ -112,24 +112,33 @@ class IO {
     m_motors[1].setSpeed(0);
   }
 
-  void setDir(float dir)
+  void setDirWithAngularSpeed(float dir, float angular_speed = ANGULAR_SPEED)
   {
     static unsigned long prev_time {millis()};
     float err_dir = dir - m_dir;
-    float max_angular_displacement = ANGULAR_SPEED * (millis() - prev_time);
+    float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000);
 
-    if(std::abs(err_dir) < max_angular_displacement)
+    Serial.printf("Angular Speed : %d\nStatic Cast : %d\nprev_time : %d\n millis : %d\n", angular_speed, static_cast<float>(millis() / 1000 - prev_time / 1000), prev_time, millis());
+
+    if(std::abs(err_dir) < angular_displacement)
     {
       m_dir = dir;
     }
     else
     {
-      m_dir += max_angular_displacement * (err_dir < 0) ? -1 : 1;
+      m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
     }
 
+    Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement);
+
     prev_time = millis();
   }
 
+  void setDir(float dir)
+  {
+    m_dir = dir;
+  }
+
   void setSpeed(float speed)
   {
     m_speed = speed;
diff --git a/Main.ino b/Main.ino
index 3830f75..ce680fa 100644
--- a/Main.ino
+++ b/Main.ino
@@ -55,13 +55,13 @@ void setup()
     while(true);
   }
 
-  my_IO.getScreen()->drawBitmap(0, 0, Imgs::riombotique, 52, 64, WHITE);
-  my_IO.getScreen()->drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
-
-  //my_IO.motorControlOn();
+  my_IO.getScreen()->drawBitmap(38, 0, Res::Imgs::riombotique, 52, 64, WHITE);
+  //my_IO.getScreen()->drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
 
   Serial.println("Initalisation Finished");
 
+  my_IO.motorControlOn();
+
 #ifdef UNIT_TESTS
   Serial.println("UNIT_TESTS");
 #endif