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

Asservissement fonctionnel !

This commit is contained in:
Ulysse Cura 2025-01-30 22:00:16 +01:00
parent 2d7be3ade0
commit f7fe86d877
1 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
IO.hpp
View File

@ -1,7 +1,6 @@
#include "esp32-hal.h" #include <Adafruit_SSD1306.h>
#include <CodeCell.h> #include <CodeCell.h>
#include <ESP32Servo.h> #include <ESP32Servo.h>
#include <Adafruit_SSD1306.h>
#include <cmath> #include <cmath>
#define PIN_TIRETTE 5 #define PIN_TIRETTE 5
@ -12,7 +11,7 @@
#define DANCING_ACTION_DELTA_ANGLE 2 #define DANCING_ACTION_DELTA_ANGLE 2
#define GAIN_KD 100 #define GAIN_KD 50
#define ANGULAR_SPEED 10 // °/s #define ANGULAR_SPEED 10 // °/s
using std::abs; using std::abs;
@ -25,23 +24,26 @@ enum class Axes {
class Motor { class Motor {
public: public:
int init(int pin) int init(int pin, int channel)
{ {
m_pin = pin; m_pin = pin;
m_channel = channel;
pinMode(m_pin, OUTPUT); pinMode(m_pin, OUTPUT);
analogWriteFrequency(m_pin, 5000); ledcAttachChannel(m_pin, 5000, 12, m_channel);
analogWriteResolution(m_pin, 12); ledcWriteChannel(m_channel, 0);
//ledcSetup(m_channel, 5000, 12); // Channel, Frequency, Resolution
analogWrite(m_pin, 0); //ledcAttachPin(m_pin, m_channel); // Attach pin to channel
return 0; return 0;
} }
void setSpeed(int speed) void setSpeed(int speed)
{ {
analogWrite(m_pin, speed); if(speed > 4095) speed = 4095;
ledcWriteChannel(m_channel, speed);
} }
private: private:
@ -120,7 +122,7 @@ class IO {
float err_dir = dir - m_dir; float err_dir = dir - m_dir;
float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000); 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()); //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)
{ {
@ -131,7 +133,7 @@ class IO {
m_dir += angular_displacement * (err_dir < 0) ? -1 : 1; m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
} }
Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement); //Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement);
prev_time = millis(); prev_time = millis();
} }
@ -175,14 +177,17 @@ class IO {
return &m_screen; return &m_screen;
} }
Motor m_motors[2]; Motor *getMotor(int motor)
{
return &(m_motors[motor]);
}
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
@ -250,14 +255,16 @@ class IO {
if(m1_speed < 0) if(m1_speed < 0)
{ {
m1_speed = 0; m1_speed = 0;
m2_speed = static_cast<int>(m_speed - 2 * correction); //m2_speed = static_cast<int>(m_speed - 2 * correction);
} }
else if(m2_speed < 0) else if(m2_speed < 0)
{ {
m2_speed = 0; m2_speed = 0;
m1_speed = static_cast<int>(m_speed + 2 * correction); //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[0].setSpeed(m1_speed);
m_motors[1].setSpeed(m2_speed); m_motors[1].setSpeed(m2_speed);
} }
@ -272,6 +279,7 @@ class IO {
bool m_is_color_blue {false}; bool m_is_color_blue {false};
bool m_is_dancing {false}; bool m_is_dancing {false};
Motor m_motors[2];
Servo m_servo; Servo m_servo;
Adafruit_SSD1306 m_screen {128, 64, &Wire, -1}; Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};