diff --git a/include/Servo.h b/include/Servo.h index bd09bc2..d7b986a 100644 --- a/include/Servo.h +++ b/include/Servo.h @@ -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 diff --git a/include/main.h b/include/main.h index e821a71..1e70bce 100644 --- a/include/main.h +++ b/include/main.h @@ -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 diff --git a/include/utils.h b/include/utils.h index e99efcf..cffd182 100644 --- a/include/utils.h +++ b/include/utils.h @@ -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 diff --git a/platformio.ini b/platformio.ini index 4b30716..8dd1b2d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,3 +12,4 @@ platform = espressif32 board = esp32dev framework = arduino +monitor_speed = 115200 \ No newline at end of file diff --git a/src/Servo.cpp b/src/Servo.cpp index 105bad0..61880af 100644 --- a/src/Servo.cpp +++ b/src/Servo.cpp @@ -1 +1,41 @@ #include "Servo.h" + +#include + +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); +} diff --git a/src/main.cpp b/src/main.cpp index 17386a5..f5a3878 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,46 +1,33 @@ #include -// #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 (isMoving()) { - lastSpeed = speed_steps_per_sec; - stopMotors(); - Serial.println("Obstacle détecté : Arrêt"); - } - } 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; - } - } + if (distanceCM > 0 && distanceCM < obstacleThresholdCM) { + if (isMoving()) { + lastSpeed = speed_steps_per_sec; + speed_steps_per_sec = 0; + Serial.println("Obstacle détecté : Arrêt"); + Serial.println(distanceCM); } + } 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; } } } diff --git a/src/utils.cpp b/src/utils.cpp index 7781e67..be4d8d4 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -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); -}