Clean IO file after merging
This commit is contained in:
parent
1e2a6f4ba7
commit
aae4cf7af7
306
IO.hpp
306
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
|
||||
};
|
||||
Loading…
Reference in New Issue