Clean IO file after merging

IO.hpp

@@ -1,4 +1,8 @@
-<<<<<<< HEAD
+/*
+  Copyright 2025
+              Thibaut Ferrand / Ulysse Cura
+                                           */
+
 #include <Adafruit_SSD1306.h>
 #include <CodeCell.h>
 #include <ESP32Servo.h>
@@ -123,8 +127,6 @@ 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());
-
     if(abs(err_dir) < angular_displacement)
     {
       m_dir = dir;
@@ -134,8 +136,6 @@ class IO {
       m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
     }
 
-    //Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement);
-
     prev_time = millis();
   }
 
@@ -200,6 +200,7 @@ class IO {
     m_servo.write(87);
   }
 
+  // Init Screen
   int m_initScreen()
   {
     if(!m_screen.begin(SSD1306_SWITCHCAPVCC, 0x3C))
@@ -264,8 +265,6 @@ class IO {
       //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);
   }
@@ -290,295 +289,4 @@ class IO {
   float m_init_x, m_init_y, m_init_z;
   float m_dir {0};
   float m_speed {0};
-};
+};
-=======
-/*
-  Copyright 2025
-              Thibaut Ferrand / Ulysse Cura
-                                           */
-
-#include "esp32-hal.h"
-#include <CodeCell.h>
-#include <ESP32Servo.h>
-#include <Adafruit_SSD1306.h>
-#include <cmath>
-
-#define PIN_TIRETTE 5
-#define PIN_BUTTON_COLOR 6
-#define PIN_MOTOR1 1
-#define PIN_MOTOR2 2
-#define PIN_SERVO 7
-
-#define DANCING_ACTION_DELTA_ANGLE 2
-
-#define GAIN_KD 100
-#define ANGULAR_SPEED 10 // °/s
-
-using std::abs;
-
-enum class Axes {
-  X,
-  Y,
-  Z
-};
-
-class Motor {
- public:
-  int init(int pin)
-  {
-    m_pin = pin;
-
-    pinMode(m_pin, OUTPUT);
-
-    analogWriteFrequency(m_pin, 5000);
-    analogWriteResolution(m_pin, 12);
-
-    analogWrite(m_pin, 0);
-
-    return 0;
-  }
-
-  void setSpeed(int speed)
-  {
-    analogWrite(m_pin, speed);
-  }
-
- private:
-  int m_pin;
-  int m_channel;
-};
-
-class IO {
- public:
-  IO(CodeCell *code_cell) : m_code_cell(code_cell)
-  {}
-
-  int init()
-  {
-    int nb_errors {0};
-
-    pinMode(PIN_TIRETTE, INPUT_PULLUP);
-    pinMode(PIN_BUTTON_COLOR, INPUT_PULLUP);
-
-    m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);
-
-    m_initMotors();
-    m_initServo();
-
-    nb_errors += m_initScreen();
-
-    return nb_errors;
-  }
-
-  int update()
-  {
-    m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
-    m_is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == HIGH);
-
-    if(m_is_motor_control_activated)
-    {
-      m_updateMotorsControl();
-    }
-
-    if(m_is_dancing)
-    {
-      m_updateDancingAction();
-    }
-
-    m_updateScreen();
-
-    return 0;
-  }
-
-  bool isTirettePulled()
-  {
-    return m_is_tirette_pulled;
-  }
-
-  bool isSelectedColorBlue()
-  {
-    return m_is_color_blue;
-  }
-
-  void motorControlOn()
-  {
-    m_is_motor_control_activated = true;
-  }
-
-  void motorControlOff()
-  {
-    m_is_motor_control_activated = false;
-
-    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()};
-    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());
-
-    if(abs(err_dir) < angular_displacement)
-    {
-      m_dir = dir;
-    }
-    else
-    {
-      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;
-  }
-  
-  float getAngle(Axes axis)
-  {
-    float x, y, z;
-
-    m_code_cell->Motion_RotationRead(x, y, z);
-
-    switch(axis)
-    {
-     case Axes::X:
-      return x - m_init_x;
-
-     case Axes::Y:
-      return y - m_init_y;
-
-     default:
-      return z - m_init_z;
-    }
-  }
-
-  void startDancingAction()
-  {
-    m_is_dancing = true;
-  }
-
-  Adafruit_SSD1306 *getScreen()
-  {
-    return &m_screen;
-  }
-
-  Motor m_motors[2];
-
- private:
-  // Init Motors
-  void m_initMotors()
-  {
-    m_motors[0].init(PIN_MOTOR1);
-    m_motors[1].init(PIN_MOTOR2);
-  }
-
-  // Init Servo
-  void m_initServo()
-  {
-    m_servo.setPeriodHertz(50);
-    m_servo.attach(PIN_SERVO, 500, 2400);
-
-    m_servo.write(87);
-  }
-
-  int m_initScreen()
-  {
-    if(!m_screen.begin(SSD1306_SWITCHCAPVCC, 0x3C))
-    {
-      Serial.println("SSD1306 allocation failed.");
-      return -1;
-    }
-
-    m_screen.fillScreen(SSD1306_BLACK);
-
-    return 0;
-  }
-
-  void m_updateDancingAction()
-  {
-    static unsigned long prev_time {millis()};
-    static unsigned long delta_time {0};
-    static int actual_angle {87};
-    static int8_t actual_dir {-1};
-
-    delta_time += millis() - prev_time;
-
-    if(delta_time >= 10)
-    {
-      delta_time = 0;
-
-      actual_angle += actual_dir * DANCING_ACTION_DELTA_ANGLE;
-    }
-
-    if(actual_angle <= 43)
-    {
-      actual_dir = 1;
-    }
-    else if(actual_angle >= 130)
-    {
-      actual_dir = -1;
-    }
-
-    m_servo.write(actual_angle);
-
-    prev_time = millis();
-  }
-
-  void m_updateMotorsControl()
-  {
-    float actual_angle = getAngle(Axes::Z);
-    float error = m_dir - actual_angle;
-
-    float correction = error * GAIN_KD;
-
-    int m1_speed = static_cast<int>(m_speed + correction);
-    int m2_speed = static_cast<int>(m_speed - correction);
-
-    if(m1_speed < 0)
-    {
-      m1_speed = 0;
-      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);
-    }
-
-    m_motors[0].setSpeed(m1_speed);
-    m_motors[1].setSpeed(m2_speed);
-  }
-
-  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_dancing {false};
-
-  Servo m_servo;
-
-  Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};
-
-  CodeCell *m_code_cell;
-
-  float m_init_x, m_init_y, m_init_z;
-  float m_dir {0};
-  float m_speed {0};
-};
->>>>>>> refs/remotes/origin/main