From f7fe86d877fded894fd809668abb4322e867cc59 Mon Sep 17 00:00:00 2001
From: Ulysse Cura <ulysse.cura@gmail.com>
Date: Thu, 30 Jan 2025 22:00:16 +0100
Subject: [PATCH] Asservissement fonctionnel !

---
 IO.hpp | 40 ++++++++++++++++++++++++----------------
 1 file changed, 24 insertions(+), 16 deletions(-)

diff --git a/IO.hpp b/IO.hpp
index 14ea13b..c02e28c 100644
--- a/IO.hpp
+++ b/IO.hpp
@@ -1,7 +1,6 @@
-#include "esp32-hal.h"
+#include <Adafruit_SSD1306.h>
 #include <CodeCell.h>
 #include <ESP32Servo.h>
-#include <Adafruit_SSD1306.h>
 #include <cmath>
 
 #define PIN_TIRETTE 5
@@ -12,7 +11,7 @@
 
 #define DANCING_ACTION_DELTA_ANGLE 2
 
-#define GAIN_KD 100
+#define GAIN_KD 50
 #define ANGULAR_SPEED 10 // °/s
 
 using std::abs;
@@ -25,23 +24,26 @@ enum class Axes {
 
 class Motor {
  public:
-  int init(int pin)
+  int init(int pin, int channel)
   {
     m_pin = pin;
+    m_channel = channel;
 
     pinMode(m_pin, OUTPUT);
 
-    analogWriteFrequency(m_pin, 5000);
-    analogWriteResolution(m_pin, 12);
-
-    analogWrite(m_pin, 0);
+    ledcAttachChannel(m_pin, 5000, 12, m_channel);
+    ledcWriteChannel(m_channel, 0);
+    //ledcSetup(m_channel, 5000, 12); // Channel, Frequency, Resolution
+    //ledcAttachPin(m_pin, m_channel); // Attach pin to channel
 
     return 0;
   }
 
   void setSpeed(int speed)
   {
-    analogWrite(m_pin, speed);
+    if(speed > 4095) speed = 4095;
+
+    ledcWriteChannel(m_channel, speed);
   }
 
  private:
@@ -120,7 +122,7 @@ class IO {
     float err_dir = dir - m_dir;
     float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000);
 
-    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());
+    //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(abs(err_dir) < angular_displacement)
     {
@@ -131,7 +133,7 @@ class IO {
       m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
     }
 
-    Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement);
+    //Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement);
 
     prev_time = millis();
   }
@@ -175,14 +177,17 @@ class IO {
     return &m_screen;
   }
 
-  Motor m_motors[2];
+  Motor *getMotor(int motor)
+  {
+    return &(m_motors[motor]);
+  }
 
  private:
   // Init Motors
   void m_initMotors()
   {
-    m_motors[0].init(PIN_MOTOR1);
-    m_motors[1].init(PIN_MOTOR2);
+    m_motors[0].init(PIN_MOTOR1, 0);
+    m_motors[1].init(PIN_MOTOR2, 1);
   }
 
   // Init Servo
@@ -250,14 +255,16 @@ class IO {
     if(m1_speed < 0)
     {
       m1_speed = 0;
-      m2_speed = static_cast<int>(m_speed - 2 * correction);
+      //m2_speed = static_cast<int>(m_speed - 2 * correction);
     }
     else if(m2_speed < 0)
     {
       m2_speed = 0;
-      m1_speed = static_cast<int>(m_speed + 2 * correction);
+      //m1_speed = static_cast<int>(m_speed + 2 * correction);
     }
 
+    //Serial.printf("Motor 1 Speed : %d\nMotor 2 Speed : %d\nGeneral Speed : %f\nCorrection : %f\nError : %f\nActual Angle : %f\n", m1_speed, m2_speed, m_speed, correction, error, actual_angle);
+
     m_motors[0].setSpeed(m1_speed);
     m_motors[1].setSpeed(m2_speed);
   }
@@ -272,6 +279,7 @@ class IO {
   bool m_is_color_blue {false};
   bool m_is_dancing {false};
 
+  Motor m_motors[2];
   Servo m_servo;
 
   Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};