ulysse
/
PAMI_Super_Star_2025
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: ulysse:aae4cf7af7cd23f71a4218aefbee7697f4d01f89
Branches Tags
ulysse:main
..
compare: ulysse:0f647cec10b88656a2a81067c907cdbd52bec420
Branches Tags
ulysse:main

No commits in common. "aae4cf7af7cd23f71a4218aefbee7697f4d01f89" and "0f647cec10b88656a2a81067c907cdbd52bec420" have entirely different histories.
aae4cf7af7 ... 0f647cec10

1 changed files with 289 additions and 292 deletions
Show Stats Expand all files Collapse all files

581
IO.hpp
View File

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