Clean IO file after merging

# Conflicts:
#    IO.hpp

IO.hpp

@@ -1,289 +1,292 @@
-/*
+/*
-  Copyright 2025
+  Copyright 2025
-              Thibaut Ferrand / Ulysse Cura
+              Thibaut Ferrand / Ulysse Cura
-                                           */
+                                           */
-
+
-#include "esp32-hal.h"
+#include <Adafruit_SSD1306.h>
-#include <CodeCell.h>
+#include <CodeCell.h>
-#include <ESP32Servo.h>
+#include <ESP32Servo.h>
-#include <Adafruit_SSD1306.h>
+#include <cmath>
-#include <cmath>
+
-
+#define PIN_TIRETTE 5
-#define PIN_TIRETTE 5
+#define PIN_BUTTON_COLOR 6
-#define PIN_BUTTON_COLOR 6
+#define PIN_MOTOR1 1
-#define PIN_MOTOR1 1
+#define PIN_MOTOR2 2
-#define PIN_MOTOR2 2
+#define PIN_SERVO 7
-#define PIN_SERVO 7
+
-
+#define DANCING_ACTION_DELTA_ANGLE 2
-#define DANCING_ACTION_DELTA_ANGLE 2
+
-
+#define GAIN_KD 50
-#define GAIN_KD 100
+#define ANGULAR_SPEED 10 // °/s
-#define ANGULAR_SPEED 10 // °/s
+
-
+using std::abs;
-using std::abs;
+
-
+enum class Axes {
-enum class Axes {
+  X,
-  X,
+  Y,
-  Y,
+  Z
-  Z
+};
-};
+
-
+class Motor {
-class Motor {
+ public:
- public:
+  int init(int pin, int channel)
-  int init(int pin)
+  {
-  {
+    m_pin = pin;
-    m_pin = pin;
+    m_channel = channel;
-
+
-    pinMode(m_pin, OUTPUT);
+    pinMode(m_pin, OUTPUT);
-
+
-    analogWriteFrequency(m_pin, 5000);
+    ledcAttachChannel(m_pin, 5000, 12, m_channel);
-    analogWriteResolution(m_pin, 12);
+    ledcWriteChannel(m_channel, 0);
-
+    //ledcSetup(m_channel, 5000, 12); // Channel, Frequency, Resolution
-    analogWrite(m_pin, 0);
+    //ledcAttachPin(m_pin, m_channel); // Attach pin to channel
-
+
-    return 0;
+    return 0;
-  }
+  }
-
+
-  void setSpeed(int speed)
+  void setSpeed(int speed)
-  {
+  {
-    analogWrite(m_pin, speed);
+    if(speed > 4095) speed = 4095;
-  }
+
-
+    ledcWriteChannel(m_channel, speed);
- private:
+  }
-  int m_pin;
+
-  int m_channel;
+ private:
-};
+  int m_pin;
-
+  int m_channel;
-class IO {
+};
- public:
+
-  IO(CodeCell *code_cell) : m_code_cell(code_cell)
+class IO {
-  {}
+ public:
-
+  IO(CodeCell *code_cell) : m_code_cell(code_cell)
-  int init()
+  {}
-  {
+
-    int nb_errors {0};
+  int init()
-
+  {
-    pinMode(PIN_TIRETTE, INPUT_PULLUP);
+    int nb_errors {0};
-    pinMode(PIN_BUTTON_COLOR, INPUT_PULLUP);
+
-
+    pinMode(PIN_TIRETTE, INPUT_PULLUP);
-    m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);
+    pinMode(PIN_BUTTON_COLOR, INPUT_PULLUP);
-
+
-    m_initMotors();
+    m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);
-    m_initServo();
+
-
+    m_initMotors();
-    nb_errors += m_initScreen();
+    m_initServo();
-
+
-    return nb_errors;
+    nb_errors += m_initScreen();
-  }
+
-
+    return nb_errors;
-  int update()
+  }
-  {
+
-    m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
+  int update()
-    m_is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == HIGH);
+  {
-
+    m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
-    if(m_is_motor_control_activated)
+    m_is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == HIGH);
-    {
+
-      m_updateMotorsControl();
+    if(m_is_motor_control_activated)
-    }
+    {
-
+      m_updateMotorsControl();
-    if(m_is_dancing)
+    }
-    {
+
-      m_updateDancingAction();
+    if(m_is_dancing)
-    }
+    {
-
+      m_updateDancingAction();
-    m_updateScreen();
+    }
-
+
-    return 0;
+    m_updateScreen();
-  }
+
-
+    return 0;
-  bool isTirettePulled()
+  }
-  {
+
-    return m_is_tirette_pulled;
+  bool isTirettePulled()
-  }
+  {
-
+    return m_is_tirette_pulled;
-  bool isSelectedColorBlue()
+  }
-  {
+
-    return m_is_color_blue;
+  bool isSelectedColorBlue()
-  }
+  {
-
+    return m_is_color_blue;
-  void motorControlOn()
+  }
-  {
+
-    m_is_motor_control_activated = true;
+  void motorControlOn()
-  }
+  {
-
+    m_is_motor_control_activated = true;
-  void motorControlOff()
+  }
-  {
+
-    m_is_motor_control_activated = false;
+  void motorControlOff()
-
+  {
-    m_motors[0].setSpeed(0);
+    m_is_motor_control_activated = false;
-    m_motors[1].setSpeed(0);
+
-  }
+    m_motors[0].setSpeed(0);
-
+    m_motors[1].setSpeed(0);
-  void setDirWithAngularSpeed(float dir, float angular_speed = ANGULAR_SPEED)
+  }
-  {
+
-    static unsigned long prev_time {millis()};
+  void setDirWithAngularSpeed(float dir, float angular_speed = ANGULAR_SPEED)
-    float err_dir = dir - m_dir;
+  {
-    float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000);
+    static unsigned long prev_time {millis()};
-
+    float err_dir = dir - m_dir;
-    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());
+    float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000);
-
+
-    if(abs(err_dir) < angular_displacement)
+    if(abs(err_dir) < angular_displacement)
-    {
+    {
-      m_dir = dir;
+      m_dir = dir;
-    }
+    }
-    else
+    else
-    {
+    {
-      m_dir += 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();
-
+  }
-    prev_time = millis();
+
-  }
+  void setDir(float dir)
-
+  {
-  void setDir(float dir)
+    m_dir = dir;
-  {
+  }
-    m_dir = dir;
+
-  }
+  void setSpeed(float speed)
-
+  {
-  void setSpeed(float speed)
+    m_speed = speed;
-  {
+  }
-    m_speed = speed;
+  
-  }
+  float getAngle(Axes axis)
-  
+  {
-  float getAngle(Axes axis)
+    float x, y, z;
-  {
+
-    float x, y, z;
+    m_code_cell->Motion_RotationRead(x, y, z);
-
+
-    m_code_cell->Motion_RotationRead(x, y, z);
+    switch(axis)
-
+    {
-    switch(axis)
+     case Axes::X:
-    {
+      return x - m_init_x;
-     case Axes::X:
+
-      return x - m_init_x;
+     case Axes::Y:
-
+      return y - m_init_y;
-     case Axes::Y:
+
-      return y - m_init_y;
+     default:
-
+      return z - m_init_z;
-     default:
+    }
-      return z - m_init_z;
+  }
-    }
+
-  }
+  void startDancingAction()
-
+  {
-  void startDancingAction()
+    m_is_dancing = true;
-  {
+  }
-    m_is_dancing = true;
+
-  }
+  Adafruit_SSD1306 *getScreen()
-
+  {
-  Adafruit_SSD1306 *getScreen()
+    return &m_screen;
-  {
+  }
-    return &m_screen;
+
-  }
+  Motor *getMotor(int motor)
-
+  {
-  Motor m_motors[2];
+    return &(m_motors[motor]);
-
+  }
- private:
+
-  // Init Motors
+ private:
-  void m_initMotors()
+  // 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
+
-  void m_initServo()
+  // Init Servo
-  {
+  void m_initServo()
-    m_servo.setPeriodHertz(50);
+  {
-    m_servo.attach(PIN_SERVO, 500, 2400);
+    m_servo.setPeriodHertz(50);
-
+    m_servo.attach(PIN_SERVO, 500, 2400);
-    m_servo.write(87);
+
-  }
+    m_servo.write(87);
-
+  }
-  int m_initScreen()
+
-  {
+  // Init Screen
-    if(!m_screen.begin(SSD1306_SWITCHCAPVCC, 0x3C))
+  int m_initScreen()
-    {
+  {
-      Serial.println("SSD1306 allocation failed.");
+    if(!m_screen.begin(SSD1306_SWITCHCAPVCC, 0x3C))
-      return -1;
+    {
-    }
+      Serial.println("SSD1306 allocation failed.");
-
+      return -1;
-    m_screen.fillScreen(SSD1306_BLACK);
+    }
-
+
-    return 0;
+    m_screen.fillScreen(SSD1306_BLACK);
-  }
+
-
+    return 0;
-  void m_updateDancingAction()
+  }
-  {
+
-    static unsigned long prev_time {millis()};
+  void m_updateDancingAction()
-    static unsigned long delta_time {0};
+  {
-    static int actual_angle {87};
+    static unsigned long prev_time {millis()};
-    static int8_t actual_dir {-1};
+    static unsigned long delta_time {0};
-
+    static int actual_angle {87};
-    delta_time += millis() - prev_time;
+    static int8_t actual_dir {-1};
-
+
-    if(delta_time >= 10)
+    delta_time += millis() - prev_time;
-    {
+
-      delta_time = 0;
+    if(delta_time >= 10)
-
+    {
-      actual_angle += actual_dir * DANCING_ACTION_DELTA_ANGLE;
+      delta_time = 0;
-    }
+
-
+      actual_angle += actual_dir * DANCING_ACTION_DELTA_ANGLE;
-    if(actual_angle <= 43)
+    }
-    {
+
-      actual_dir = 1;
+    if(actual_angle <= 43)
-    }
+    {
-    else if(actual_angle >= 130)
+      actual_dir = 1;
-    {
+    }
-      actual_dir = -1;
+    else if(actual_angle >= 130)
-    }
+    {
-
+      actual_dir = -1;
-    m_servo.write(actual_angle);
+    }
-
+
-    prev_time = millis();
+    m_servo.write(actual_angle);
-  }
+
-
+    prev_time = millis();
-  void m_updateMotorsControl()
+  }
-  {
+
-    float actual_angle = getAngle(Axes::Z);
+  void m_updateMotorsControl()
-    float error = m_dir - actual_angle;
+  {
-
+    float actual_angle = getAngle(Axes::Z);
-    float correction = error * GAIN_KD;
+    float error = m_dir - actual_angle;
-
+
-    int m1_speed = static_cast<int>(m_speed + correction);
+    float correction = error * GAIN_KD;
-    int m2_speed = static_cast<int>(m_speed - correction);
+
-
+    int m1_speed = static_cast<int>(m_speed + correction);
-    if(m1_speed < 0)
+    int m2_speed = static_cast<int>(m_speed - correction);
-    {
+
-      m1_speed = 0;
+    if(m1_speed < 0)
-      m2_speed = static_cast<int>(m_speed - 2 * correction);
+    {
-    }
+      m1_speed = 0;
-    else if(m2_speed < 0)
+      //m2_speed = static_cast<int>(m_speed - 2 * correction);
-    {
+    }
-      m2_speed = 0;
+    else if(m2_speed < 0)
-      m1_speed = static_cast<int>(m_speed + 2 * correction);
+    {
-    }
+      m2_speed = 0;
-
+      //m1_speed = static_cast<int>(m_speed + 2 * correction);
-    m_motors[0].setSpeed(m1_speed);
+    }
-    m_motors[1].setSpeed(m2_speed);
+
-  }
+    m_motors[0].setSpeed(m1_speed);
-
+    m_motors[1].setSpeed(m2_speed);
-  void m_updateScreen()
+  }
-  {
+
-    m_screen.display();
+  void m_updateScreen()
-  }
+  {
-
+    m_screen.display();
-  bool m_is_motor_control_activated {false};
+  }
-  bool m_is_tirette_pulled {false};
+
-  bool m_is_color_blue {false};
+  bool m_is_motor_control_activated {false};
-  bool m_is_dancing {false};
+  bool m_is_tirette_pulled {false};
-
+  bool m_is_color_blue {false};
-  Servo m_servo;
+  bool m_is_dancing {false};
-
+
-  Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};
+  Motor m_motors[2];
-
+  Servo m_servo;
-  CodeCell *m_code_cell;
+
-
+  Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};
-  float m_init_x, m_init_y, m_init_z;
+
-  float m_dir {0};
+  CodeCell *m_code_cell;
-  float m_speed {0};
+
-};
+  float m_init_x, m_init_y, m_init_z;
+  float m_dir {0};
+  float m_speed {0};
+};