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 <ESP32Servo.h>
#include <Adafruit_SSD1306.h>
#include <cmath>
#define PIN_TIRETTE 5
@ -12,7 +11,7 @@
#define DANCING_ACTION_DELTA_ANGLE 2
#define GAIN_KD 100
#define GAIN_KD 50
#define ANGULAR_SPEED 10 // °/s
using std::abs;
@ -25,23 +24,26 @@ enum class Axes {
class Motor {
public:
int init(int pin)
int init(int pin, int channel)
{
m_pin = pin;
m_channel = channel;
pinMode(m_pin, OUTPUT);
analogWriteFrequency(m_pin, 5000);
analogWriteResolution(m_pin, 12);
analogWrite(m_pin, 0);
ledcAttachChannel(m_pin, 5000, 12, m_channel);
ledcWriteChannel(m_channel, 0);
//ledcSetup(m_channel, 5000, 12); // Channel, Frequency, Resolution
//ledcAttachPin(m_pin, m_channel); // Attach pin to channel
return 0;
}
void setSpeed(int speed)
{
analogWrite(m_pin, speed);
if(speed > 4095) speed = 4095;
ledcWriteChannel(m_channel, speed);
}
private:
@ -120,7 +122,7 @@ 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());
//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)
{
@ -131,7 +133,7 @@ class IO {
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();
}
@ -175,14 +177,17 @@ class IO {
return &m_screen;
}
Motor m_motors[2];
Motor *getMotor(int motor)
{
return &(m_motors[motor]);
}
private:
// Init Motors
void m_initMotors()
{
m_motors[0].init(PIN_MOTOR1);
m_motors[1].init(PIN_MOTOR2);
m_motors[0].init(PIN_MOTOR1, 0);
m_motors[1].init(PIN_MOTOR2, 1);
}
// Init Servo
@ -250,14 +255,16 @@ class IO {
if(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)
{
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[1].setSpeed(m2_speed);
}
@ -272,6 +279,7 @@ class IO {
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};