From aae4cf7af7cd23f71a4218aefbee7697f4d01f89 Mon Sep 17 00:00:00 2001 From: Ulysse Cura Date: Thu, 30 Jan 2025 22:04:05 +0100 Subject: [PATCH] Clean IO file after merging --- IO.hpp | 306 ++------------------------------------------------------- 1 file changed, 7 insertions(+), 299 deletions(-) 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 #include #include @@ -123,8 +127,6 @@ class IO { float err_dir = dir - m_dir; float angular_displacement = angular_speed * static_cast(millis() / 1000 - prev_time / 1000); - //Serial.printf("Angular Speed : %d\nStatic Cast : %d\nprev_time : %d\n millis : %d\n", angular_speed, static_cast(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(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 -#include -#include -#include - -#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(millis() / 1000 - prev_time / 1000); - - Serial.printf("Angular Speed : %d\nStatic Cast : %d\nprev_time : %d\n millis : %d\n", angular_speed, static_cast(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(m_speed + correction); - int m2_speed = static_cast(m_speed - correction); - - if(m1_speed < 0) - { - m1_speed = 0; - m2_speed = static_cast(m_speed - 2 * correction); - } - else if(m2_speed < 0) - { - m2_speed = 0; - m1_speed = static_cast(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