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