diff --git a/IO.hpp b/IO.hpp
index 1d365b0..1a386e4 100644
--- a/IO.hpp
+++ b/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
+};
\ No newline at end of file