PAMI_Super_Star_2025/IO.hpp

350 lines
6.5 KiB
C++

/*
Copyright 2025
Thibaut Ferrand / Ulysse Cura
*/
#include <Adafruit_SSD1306.h>
#include <CodeCell.h>
#include <ESP32Servo.h>
#include <cmath>
#include "Ressources.hpp"
#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 50
#define ANGULAR_SPEED 120 // °/s
using std::abs, std::signbit;
enum class Axes {
X,
Y,
Z
};
class Motor {
public:
int init(int pin, int channel)
{
m_pin = pin;
m_channel = channel;
pinMode(m_pin, OUTPUT);
ledcAttachChannel(m_pin, 5000, 12, m_channel);
ledcWriteChannel(m_channel, 0);
return 0;
}
void setSpeed(int speed)
{
if(speed > 4095) speed = 4095;
ledcWriteChannel(m_channel, 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);
initGyroscope();
m_initMotors();
m_initServo();
nb_errors += m_initScreen();
if(!nb_errors)
{
m_screen.setTextSize(1);
m_screen.setTextColor(WHITE, BLACK);
m_screen.setFont(&Res::Fonts::Freshman12pt7b);
}
return nb_errors;
}
void initGyroscope()
{
m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);
}
int update()
{
static unsigned long last_color_change_time {0};
static bool has_color_changed {true};
static bool is_color_blue {true};
if(m_is_color_blue != is_color_blue)
{
m_screen.clearDisplay();
m_screen.setCursor(0, 20);
if(is_color_blue)
{
m_screen.print("Blue\nTeam");
}
else
{
m_screen.print("Yellow\nTeam");
}
last_color_change_time = millis();
has_color_changed = true;
}
if(millis() - last_color_change_time > 1000 && has_color_changed)
{
m_screen.clearDisplay();
m_screen.drawBitmap(38, 0, Res::Imgs::riombotique, 52, 64, WHITE);
//m_screen.drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
//m_screen.drawBitmap(27, 0, Res::Imgs::diable_gaga, 75, 64, WHITE);
has_color_changed = false;
}
m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == HIGH);
m_is_color_blue = is_color_blue;
is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == LOW);
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() - prev_time) / 1000.0f;
if(abs(err_dir) < abs(angular_displacement))
{
m_dir = dir;
}
else
{
m_dir += angular_displacement * ((!signbit(err_dir)) * 2 - 1);
}
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;
float angle;
m_code_cell->Motion_RotationRead(x, y, z);
switch(axis)
{
case Axes::X:
angle = x - m_init_x;
if(angle < -180) angle += 360;
if(angle > 180) angle -= 360;
return angle;
case Axes::Y:
angle = y - m_init_y;
if(angle < -180) angle += 360;
if(angle > 180) angle -= 360;
return angle;
default:
angle = z - m_init_z;
if(angle < -180) angle += 360;
if(angle > 180) angle -= 360;
return angle;
}
}
void startDancingAction(int dancing_action_delta_angle = DANCING_ACTION_DELTA_ANGLE)
{
m_dancing_action_delta_angle = dancing_action_delta_angle;
m_is_dancing = true;
}
Adafruit_SSD1306 *getScreen()
{
return &m_screen;
}
Motor *getMotor(int motor)
{
return &(m_motors[motor]);
}
private:
// Init Motors
void m_initMotors()
{
m_motors[0].init(PIN_MOTOR1, 0);
m_motors[1].init(PIN_MOTOR2, 1);
}
// Init Servo
void m_initServo()
{
m_servo.setPeriodHertz(50);
m_servo.attach(PIN_SERVO, 500, 2400);
m_servo.write(87);
}
// Init Screen
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 * m_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};
Motor m_motors[2];
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};
int m_dancing_action_delta_angle {DANCING_ACTION_DELTA_ANGLE};
};