modelec/pami
1
0
Fork 0
mirror of https://github.com/modelec/pami.git synced 2026-03-18 21:40:35 +01:00
Code Issues Packages Projects Releases Wiki Activity

add multi core, servo class

Browse Source 
Use core 0 for ultrasonic detection
Use a custom class to pilot servo
This commit is contained in:
Florent
2026-03-11 17:36:49 +01:00
parent ccc9c03168
commit 5f36968eaa
7 changed files with 209 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
include/Servo.h
View File

@@ -1,18 +1,40 @@
#ifndef SERVO_H
#define SERVO_H
#ifndef PAMI_SERVO_H
#define PAMI_SERVO_H
#define MAX_US 544
#define MIN_US 2400
#define DEFAULT_MIN_US 544
#define DEFAULT_MAX_US 2400
#define DEFAULT_MIN_ANGLE 0
#define DEFAULT_MAX_ANGLE 180
class Servo {
private:
const int channel;
const int pin;
const int min;
const int max;
int channel{};
int pin{};
int min_us{};
int max_us{};
int min_angle{};
int max_angle{};
void initServo() const;
public:
Servo();
Servo(int pin, int channel, int min_angle = DEFAULT_MIN_ANGLE, int max_angle = DEFAULT_MAX_ANGLE,
int min_us = DEFAULT_MIN_US,
int max_us = DEFAULT_MAX_US);
void writeAngle(int angle) const;
void writeUs(int us) const;
/**
* Write to the servo the value as degree if it's in the degree range of the servo otherwise as us value
* @param value The value to write in angle or us
*/
void write(int value) const;
};
#endif //SERVO_H
#endif //PAMI_SERVO_H

7
include/main.h
View File

@@ -1,5 +1,5 @@
#ifndef MAIN_H
#define MAIN_H
#ifndef PAMI_MAIN_H
#define PAMI_MAIN_H
enum Direction {
FORWARD,
@@ -46,7 +46,6 @@ struct Step {
#define ECHO_PIN 12
// Servo Moteur
#define SERVO_PIN 15
// FIXME : Je sais pas, loop sous le bouton d'arret d'urgence
#define EMG_PIN 5
#endif
@@ -90,4 +89,4 @@ struct Step {
#define STEPS_PER_REV 200.0
#define MICROSTEPPING 8.0
#endif // MAIN_H
#endif // PAMI_MAIN_H

8
include/utils.h
View File

@@ -1,5 +1,5 @@
#ifndef UTILS_H
#define UTILS_H
#ifndef PAMI_UTILS_H
#define PAMI_UTILS_H
void enableMotorDrivers();
@@ -9,6 +9,4 @@ int getStepsForDistance(float cm);
int getRotationSteps(float angleDeg);
void setServoAngle(int angle, int channel = 0);
#endif // UTILS_H
#endif // PAMI_UTILS_H

1
platformio.ini
View File

@@ -12,3 +12,4 @@
platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200

40
src/Servo.cpp
View File

@@ -1 +1,41 @@
#include "Servo.h"
#include <Arduino.h>
void Servo::initServo() const {
pinMode(pin, OUTPUT);
ledcSetup(channel, 50, 16);
ledcAttachPin(pin, channel);
}
Servo::Servo() = default;
Servo::Servo(int pin, int channel, int min_angle, int max_angle, int min_us, int max_us) {
this->pin = pin;
this->channel = channel;
this->min_us = min_us;
this->max_us = max_us;
this->min_angle = min_angle;
this->max_angle = max_angle;
initServo();
}
void Servo::writeAngle(int angle) const {
if (angle < min_angle) angle = min_angle;
if (angle > max_angle) angle = max_angle;
const int value = map(angle, min_angle, max_angle, min_us, max_us);
writeUs(value);
}
void Servo::writeUs(int us) const {
if (us < min_us) us = min_us;
if (us > max_us) us = max_us;
int duty = us * 65535 / 20000;
ledcWrite(channel, duty);
}
void Servo::write(int value) const {
if (value > min_angle && value < max_angle) return writeAngle(value);
return writeUs(value);
}

297
src/main.cpp
View File

@@ -1,46 +1,33 @@
#include <Arduino.h>
// #define SIMULATOR // Commenter cette ligne pour utiliser le matériel réel
// Définitions des constantes dans main.h
#include "main.h"
#include "Servo.h"
#include "utils.h"
// TODO : Sélection du camps avec un bouton et non un define
// Côté bleu = 1 et Côté jaune = 2
#define PAMI_SIDE 1
// TODO : Brève description de quel pami correspond a quel numéro
// Numéro du pami pour les différents robots car chemins différents
#define PAMI_NUM 2
// FIXME : Mettre au min à START_DELAY 85000 (85s)
#define START_DELAY 5000
//FIXME : Le reset des value devrait pas etre fait dans le setup ? (Si on considère que le code peut s'auto restart)
uint32_t startTime = 0;
volatile int32_t speed_steps_per_sec = 0; // target speed (signed)
uint32_t last_step_time = 0;
// Pour le redémarrage des moteurs après capteur obstacle
// Stepper
volatile int32_t speed_steps_per_sec = 0;
volatile int32_t steps_target = 0;
volatile int32_t steps_done = 0;
volatile bool movementInProgress = false;
Direction lastDirection = STOP;
volatile int32_t lastSpeed = 0;
volatile int32_t steps_target = 0;
volatile int32_t steps_done = 0;
bool movementInProgress = false;
// Les différents scénarios possibles
// Scénario
#if PAMI_NUM == 1
// Chaque étape du scénario écureuil ninja
// TODO
Step scenario[] = {
{STEP_FORWARD, 105, 2500},
{STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 1000}, // Tourner à gauche si côté bleu, droite si jaune
{STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 1000},
{STEP_FORWARD, 50, 2000}
};
#elif PAMI_NUM == 2
// TODO : Voir comment gerer les différence de distance a parcourir selon PAMI_NUM, plein de if ?
Step scenario[] = {
{STEP_BACKWARD, 35, 2500},
// {STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500},
{STEP_BACKWARD, 80 - 10 * PAMI_NUM, 3000}
};
#else
@@ -55,16 +42,47 @@ const int scenarioLength = sizeof(scenario) / sizeof(Step);
int currentScenarioStep = 0;
bool scenarioInProgress = false;
// Arrêt des moteurs
void stopMotors() {
speed_steps_per_sec = 0;
// Ultrason (non bloquant)
volatile unsigned long distanceCM = 1000;
const unsigned int obstacleThresholdCM = 7;
unsigned long lastUltrasonicTrigger = 0;
bool waitingEcho = false;
unsigned long echoStart = 0;
// Servo
const int SERVO_EAT_UP = 135;
const int SERVO_EAT_DOWN = 45;
Servo servo_eat;
// Tirette et EMG
bool tirettePose = false;
uint32_t startTime = 0;
// Tâche ultrason sur Core 0
[[noreturn]] void ultrasonicTask(void *pvParameters) {
for (;;) {
// Trigger
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(20);
digitalWrite(TRIG_PIN, LOW);
// Lecture bloquante
unsigned long duration = pulseIn(ECHO_PIN, HIGH, 20000); // timeout 20ms
unsigned long newDistance = duration * 0.034 / 2; // cm
distanceCM = newDistance;
vTaskDelay(50 / portTICK_PERIOD_MS); // 50ms entre mesures
}
}
// Fonction d'initialisation
// ========================= Setup =========================
void setup() {
Serial.begin(115200);
// Bouton d'arrêt d'urgence (HIGH si appuyé)
pinMode(EMG_PIN, INPUT);
// Pins moteurs
// Moteur 1 (Droite)
pinMode(M1_STEP_PIN, OUTPUT);
pinMode(M1_DIR_PIN, OUTPUT);
@@ -73,70 +91,47 @@ void setup() {
pinMode(M2_STEP_PIN, OUTPUT);
pinMode(M2_DIR_PIN, OUTPUT);
pinMode(M2_ENABLE_PIN, OUTPUT);
// Tirette
pinMode(TIRETTE_PIN, INPUT_PULLUP);
// Capteur à ultrasons
// Capteurs
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
// Tirette de démarrage
pinMode(TIRETTE_PIN, INPUT_PULLUP);
// Bouton d'arrêt d'urgence (HIGH si appuyé)
pinMode(EMG_PIN, INPUT);
// Servo moteur
pinMode(SERVO_PIN, OUTPUT);
// Servo
servo_eat = Servo(SERVO_PIN, 1);
servo_eat.writeAngle(SERVO_EAT_UP);
ledcSetup(0, 50, 16);
ledcAttachPin(SERVO_PIN, 0);
setServoAngle(130);
enableMotorDrivers();
stopMotors();
speed_steps_per_sec = 0;
scenarioInProgress = false;
currentScenarioStep = 0;
// Créer la tâche sur Core 0
xTaskCreatePinnedToCore(
ultrasonicTask,
"UltrasonicTask",
2000,
NULL,
1,
NULL,
0); // Core 0
Serial.println("Setup complete");
Serial.println("Waiting for the trigger initialization");
}
void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir) {
// Le nombre de pas à faire
steps_target = steps;
steps_done = 0;
// La direction du moteur
digitalWrite(M1_DIR_PIN, forwardDir ? HIGH : LOW);
digitalWrite(M2_DIR_PIN, forwardDir ? HIGH : LOW);
speed_steps_per_sec = speed;
movementInProgress = true;
lastDirection = forwardDir ? FORWARD : BACKWARD;
}
void rotateAsync(float angleDeg, int32_t speed, bool toRight) {
// Le nombre de pas à faire
steps_target = getRotationSteps(angleDeg + 10.0); // 10.0 correspond à une correction trouvée à la main :)
steps_done = 0;
// La direction du moteur
digitalWrite(M1_DIR_PIN, toRight ? HIGH : LOW);
digitalWrite(M2_DIR_PIN, toRight ? LOW : HIGH);
speed_steps_per_sec = speed;
movementInProgress = true;
lastDirection = toRight ? RIGHT : LEFT;
}
// Non-blocking stepper update function
// ========================= Steppers =========================
void updateSteppers() {
static uint32_t last_step_time1 = 0;
uint32_t now = micros();
// Moteur 1
if (speed_steps_per_sec != 0 && (steps_done < steps_target)) {
uint32_t interval1 = 1000000UL / abs(speed_steps_per_sec);
static uint32_t last_step_time1 = 0;
if (now - last_step_time1 >= interval1) {
if (speed_steps_per_sec != 0 && steps_done < steps_target) {
uint32_t interval = 1000000UL / abs(speed_steps_per_sec);
if (now - last_step_time1 >= interval) {
digitalWrite(M1_STEP_PIN, HIGH);
digitalWrite(M2_STEP_PIN, HIGH);
delayMicroseconds(2); // Short pulse width for step signal
delayMicroseconds(2);
digitalWrite(M1_STEP_PIN, LOW);
digitalWrite(M2_STEP_PIN, LOW);
last_step_time1 = now;
@@ -144,86 +139,62 @@ void updateSteppers() {
}
}
// Comptage des pas (on considère le moteur le plus lent pour terminer l'étape)
if (movementInProgress && steps_done >= steps_target) {
stopMotors();
speed_steps_per_sec = 0;
movementInProgress = false;
Serial.println("Etape terminé");
Serial.println("Etape terminée");
}
}
// Vérifie si les moteurs sont en mouvement
// Arrêt des moteurs
void stopMotors() {
speed_steps_per_sec = 0;
}
bool isMoving() {
return speed_steps_per_sec != 0;
}
// Capteur à ultrasons
const int obstacleCheckInterval = 100; // ms between distance checks
unsigned long lastObstacleCheckTime = 0;
const int obstacleThresholdCM = 7; // stop if closer than 7 cm
long readDistanceCM(uint8_t trigPin, uint8_t echoPin) {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
long duration = pulseIn(echoPin, HIGH, 20000); // timeout 20 ms
long distance = duration * 0.034 / 2;
return distance;
}
bool recherchePlace = false;
int rechercheStep = 0;
// Nombre de pas restants à faire après la reprise de place
int stepsRemainingPlace = 0;
// Sous-scénario pour la recherche de place
const int NB_RECHERCHE_STEPS = 3;
Step rechercheScenario[NB_RECHERCHE_STEPS] = {
{STEP_FORWARD, 20},
{STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90}
};
// ========================= Détection obstacles =========================
void detectObstacles() {
unsigned long now = millis();
if (now - lastObstacleCheckTime >= obstacleCheckInterval) {
lastObstacleCheckTime = now;
// TODO : Threashold pour rotation ?
if (lastDirection == RIGHT || lastDirection == LEFT) {
return;
}
long distance = readDistanceCM(TRIG_PIN, ECHO_PIN);
if (distance > 0 && distance < obstacleThresholdCM) {
if (distanceCM > 0 && distanceCM < obstacleThresholdCM) {
if (isMoving()) {
lastSpeed = speed_steps_per_sec;
stopMotors();
speed_steps_per_sec = 0;
Serial.println("Obstacle détecté : Arrêt");
Serial.println(distanceCM);
}
} else {
if (!isMoving() && movementInProgress) {
Serial.println("Plus d'obstacle : Reprise du mouvement");
switch (lastDirection) {
case FORWARD:
moveAsyncSteps(steps_target - steps_done, lastSpeed, true);
break;
case BACKWARD:
moveAsyncSteps(steps_target - steps_done, lastSpeed, false);
break;
default:
break;
}
}
}
} else if (!isMoving() && movementInProgress) {
Serial.println("Obstacle évité : reprise mouvement");
Serial.println(distanceCM);
speed_steps_per_sec = lastSpeed;
}
}
// On gère le scénario du robot, retourne 0 si le scénario est terminé, 1 si en cours
// ========================= Scenario =========================
void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir) {
steps_target = steps;
steps_done = 0;
digitalWrite(M1_DIR_PIN, forwardDir ? HIGH : LOW);
digitalWrite(M2_DIR_PIN, forwardDir ? HIGH : LOW);
speed_steps_per_sec = speed;
movementInProgress = true;
lastDirection = forwardDir ? FORWARD : BACKWARD;
}
void rotateAsync(float angleDeg, int32_t speed, bool toRight) {
steps_target = getRotationSteps(angleDeg + 10.0);
steps_done = 0;
digitalWrite(M1_DIR_PIN, toRight ? HIGH : LOW);
digitalWrite(M2_DIR_PIN, toRight ? LOW : HIGH);
speed_steps_per_sec = speed;
movementInProgress = true;
lastDirection = toRight ? RIGHT : LEFT;
}
int processScenario() {
if (!scenarioInProgress)
return 0;
if (movementInProgress)
return 1;
if (!scenarioInProgress) return 0;
if (movementInProgress) return 1;
if (currentScenarioStep >= scenarioLength) {
Serial.println("Scénario terminé !");
@@ -233,65 +204,53 @@ int processScenario() {
Step &step = scenario[currentScenarioStep];
switch (step.type) {
case STEP_FORWARD:
moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
case STEP_FORWARD: moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
break;
case STEP_BACKWARD:
moveAsyncSteps(getStepsForDistance(step.value), step.speed, false);
case STEP_BACKWARD: moveAsyncSteps(getStepsForDistance(step.value), step.speed, false);
break;
case STEP_ROTATE:
if (step.value >= 0)
rotateAsync(step.value, step.speed, true);
else
rotateAsync(-step.value, step.speed, false);
if (step.value >= 0) rotateAsync(step.value, step.speed, true);
else rotateAsync(-step.value, step.speed, false);
break;
}
currentScenarioStep++;
return 1;
}
bool tirettePose = false;
// ========================= Loop principal =========================
void loop() {
// Tirette
if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose) {
// Tirette posée
tirettePose = true;
Serial.println("Trigger initialized");
} else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose) {
// Tirette activée
Serial.println("Trigger removed");
// TODO : Mettre le bon delay selon le num
uint32_t startDelay = START_DELAY + (PAMI_NUM - 1) * /*2000*/0;
Serial.print("Waiting : ");
Serial.println(startDelay);
delay(startDelay);
delay(START_DELAY);
Serial.println("Starting the script");
startTime = millis();
scenarioInProgress = true;
while (true) {
updateSteppers(); // Mise à jour des moteurs asynchrone
updateSteppers();
detectObstacles();
processScenario();
// detectObstacles(); // Vérification des obstacles
// Gestion du scénario puis arrêt si terminé ou si temps dépassé
if (processScenario() == 0 || millis() - startTime >= 100000 - startDelay) {
Serial.println("Scénario terminé, arrêt des moteurs.");
// TODO : Faire bouger le servo a la fin
// Fin de scénario ou timeout
if (!scenarioInProgress || millis() - startTime >= 100000) {
stopMotors();
Serial.println("Script finished, motor stopped");
// Servo en boucle jusqu'à reset
Serial.println("Starting the post game action");
while (true) {
if (!digitalRead(EMG_PIN)) {
Serial.println("Servo run");
setServoAngle(45);
servo_eat.writeAngle(SERVO_EAT_UP);
delay(500);
setServoAngle(135);
servo_eat.writeAngle(SERVO_EAT_DOWN);
delay(500);
} else {
Serial.println("Restart");
Serial.println("Restarting the ESP 32 ...");
ESP.restart();
break;
}
}
}

10
src/utils.cpp
View File

@@ -29,13 +29,3 @@ int getRotationSteps(float angleDeg) {
return (int) round(microStepsForAngle);
}
void setServoAngle(int angle, int channel) {
int value = map(angle, 0, 180, 544, 2400);
// Serial.print("Servo angle: ");
// Serial.println(value);
if (value < 544) value = 544;
if (value > 2400) value = 2400;
int duty = value * 65535 / 20000;
ledcWrite(channel, duty);
}