diff --git a/README.md b/README.md index fe1f6ab..af7436a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,2 @@ -# PAMI-2025 -Code pour les PAMI de 2025 +# PAMI-2026 +Code pour les PAMI de 2026 diff --git a/include/Servo.h b/include/Servo.h new file mode 100644 index 0000000..bd09bc2 --- /dev/null +++ b/include/Servo.h @@ -0,0 +1,18 @@ +#ifndef SERVO_H +#define SERVO_H + +#define MAX_US 544 +#define MIN_US 2400 + +class Servo { +private: + const int channel; + const int pin; + const int min; + const int max; + +public: +}; + + +#endif //SERVO_H diff --git a/include/main.h b/include/main.h index 9dd2932..e821a71 100644 --- a/include/main.h +++ b/include/main.h @@ -1,8 +1,7 @@ #ifndef MAIN_H #define MAIN_H -enum Direction -{ +enum Direction { FORWARD, BACKWARD, LEFT, @@ -10,18 +9,16 @@ enum Direction STOP }; -enum StepType -{ +enum StepType { STEP_FORWARD, STEP_ROTATE, STEP_FORWARD_UNTIL_END, STEP_BACKWARD }; -struct Step -{ +struct Step { StepType type; - float value; // cm pour STEP_FORWARD, deg pour ROTATE, ignoré pour UNTIL_FALL + float value; // cm pour STEP_FORWARD, deg pour ROTATE, ignoré pour UNTIL_FALL int32_t speed; // Vitesse en pas / seconde }; @@ -34,24 +31,22 @@ struct Step // PINs réels #ifndef SIMULATOR -// Moteur 1 - Gauche +// Moteur 1 - Droite #define M1_DIR_PIN 19 #define M1_STEP_PIN 23 #define M1_ENABLE_PIN 26 -// Moteur 2 - Droite +// Moteur 2 - Gauche #define M2_DIR_PIN 21 #define M2_STEP_PIN 25 #define M2_ENABLE_PIN 27 // Capteur à ultrasons -#define TRIG_PIN 15 -#define ECHO_PIN 14 - -// Capteur de chute -#define FALL_PIN 13 - -#define SERVO_PIN 12 +#define TRIG_PIN 13 +#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 @@ -88,10 +83,11 @@ struct Step #define DRIVER1_ADDR 0b01 #define DRIVER2_ADDR 0b10 +// TODO : Changer les constantes avec nouvelles dim // Constantes -#define WHEEL_DIAMETER 5.5 // cm -#define WHEEL_BASE 7.2 // cm +#define WHEEL_DIAMETER 5.6 // cm +#define WHEEL_BASE 7 // cm #define STEPS_PER_REV 200.0 #define MICROSTEPPING 8.0 -#endif // MAIN_H \ No newline at end of file +#endif // MAIN_H diff --git a/include/utils.h b/include/utils.h index 498b70d..e99efcf 100644 --- a/include/utils.h +++ b/include/utils.h @@ -1,10 +1,14 @@ #ifndef UTILS_H #define UTILS_H -void enableDrivers(); +void enableMotorDrivers(); + void disableDrivers(); int getStepsForDistance(float cm); + int getRotationSteps(float angleDeg); -#endif // UTILS_H \ No newline at end of file +void setServoAngle(int angle, int channel = 0); + +#endif // UTILS_H diff --git a/src/Servo.cpp b/src/Servo.cpp new file mode 100644 index 0000000..105bad0 --- /dev/null +++ b/src/Servo.cpp @@ -0,0 +1 @@ +#include "Servo.h" diff --git a/src/main.cpp b/src/main.cpp index 6665e08..17386a5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,12 +4,16 @@ #include "main.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 4 +#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) @@ -23,22 +27,28 @@ volatile int32_t steps_target = 0; volatile int32_t steps_done = 0; bool movementInProgress = false; -// Les différents scénario possibles : le superstar et les autres +// Les différents scénarios possibles #if PAMI_NUM == 1 -// Chaque étape du scénario +// 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_FORWARD_UNTIL_END, 50, 2000}}; + {STEP_FORWARD, 105, 2500}, + {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 1000}, // Tourner à gauche si côté bleu, droite si jaune + {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 Step scenario[] = { - {STEP_FORWARD, 35, 2500}, - {STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500}, - {STEP_FORWARD, 80 - 10 * PAMI_NUM, 3000}, - {STEP_ROTATE, PAMI_SIDE == 1 ? 45 - 10 * (PAMI_NUM - 2) : -45 + 10 * (PAMI_NUM - 2), 500}, - {STEP_FORWARD, 60 - 10 * PAMI_NUM, 3000}, - {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 500}, - {STEP_BACKWARD, 50 - 10 * PAMI_NUM, 1000}}; + {STEP_FORWARD, 35, 2500}, + {STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500}, + {STEP_FORWARD, 80 - 10 * PAMI_NUM, 3000} +}; #endif const int scenarioLength = sizeof(scenario) / sizeof(Step); @@ -46,53 +56,48 @@ int currentScenarioStep = 0; bool scenarioInProgress = false; // Arrêt des moteurs -void stopMotors() -{ +void stopMotors() { speed_steps_per_sec = 0; } -// Servo moteur -const int angleCentre = 90; // Position centrale -const int amplitude = 10; // Amplitude de mouvement -const unsigned long intervalleServo = 1000; // Intervalle en millisecondes -unsigned long tempsServoPrecedent = 0; -bool directionServo = true; // true = vers le haut, false = vers le bas - // Fonction d'initialisation -void setup() -{ +void setup() { Serial.begin(115200); + // Bouton d'arrêt d'urgence (HIGH si appuyé) pinMode(EMG_PIN, INPUT); + // Moteur 1 (Droite) pinMode(M1_STEP_PIN, OUTPUT); pinMode(M1_DIR_PIN, OUTPUT); pinMode(M1_ENABLE_PIN, OUTPUT); + // Moteur 2 (Gauche) pinMode(M2_STEP_PIN, OUTPUT); pinMode(M2_DIR_PIN, OUTPUT); pinMode(M2_ENABLE_PIN, OUTPUT); + // Tirette pinMode(TIRETTE_PIN, INPUT_PULLUP); // Capteur à ultrasons pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT); - // Capteur de chute - pinMode(FALL_PIN, INPUT); // Servo moteur - // pinMode(SERVO_PIN, OUTPUT); + pinMode(SERVO_PIN, OUTPUT); ledcSetup(0, 50, 16); ledcAttachPin(SERVO_PIN, 0); - enableDrivers(); + setServoAngle(130); + + enableMotorDrivers(); stopMotors(); - scenarioInProgress = true; + scenarioInProgress = false; currentScenarioStep = 0; Serial.println("Setup complete"); + Serial.println("Waiting for the trigger initialization"); } -void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir) -{ +void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir) { // Le nombre de pas à faire steps_target = steps; steps_done = 0; @@ -106,9 +111,7 @@ void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir) lastDirection = forwardDir ? FORWARD : BACKWARD; } -void rotateAsync(float angleDeg, int32_t speed, bool toRight) -{ - +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; @@ -123,18 +126,14 @@ void rotateAsync(float angleDeg, int32_t speed, bool toRight) } // Non-blocking stepper update function -void updateSteppers() -{ +void updateSteppers() { uint32_t now = micros(); // Moteur 1 - if (speed_steps_per_sec != 0 && (steps_done < steps_target)) - { + 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 (now - last_step_time1 >= interval1) { digitalWrite(M1_STEP_PIN, HIGH); digitalWrite(M2_STEP_PIN, HIGH); delayMicroseconds(2); // Short pulse width for step signal @@ -146,9 +145,7 @@ void updateSteppers() } // Comptage des pas (on considère le moteur le plus lent pour terminer l'étape) - if (movementInProgress && steps_done >= steps_target) - { - + if (movementInProgress && steps_done >= steps_target) { stopMotors(); movementInProgress = false; Serial.println("Etape terminé"); @@ -156,8 +153,7 @@ void updateSteppers() } // Vérifie si les moteurs sont en mouvement -bool isMoving() -{ +bool isMoving() { return speed_steps_per_sec != 0; } @@ -166,8 +162,7 @@ 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) -{ +long readDistanceCM(uint8_t trigPin, uint8_t echoPin) { digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); @@ -178,19 +173,6 @@ long readDistanceCM(uint8_t trigPin, uint8_t echoPin) return distance; } -// Vérification de la distance du sol pour éviter les chutes -void checkFall() -{ - // On a fini notre chemin si on arrive en bout de table (détection de distance du sol) - if (digitalRead(FALL_PIN) == HIGH && movementInProgress) - { - stopMotors(); - movementInProgress = false; - scenarioInProgress = false; - Serial.println("Bout de table détecté : Arrêt"); - } -} - bool recherchePlace = false; int rechercheStep = 0; // Nombre de pas restants à faire après la reprise de place @@ -199,152 +181,70 @@ 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}}; + {STEP_FORWARD, 20}, + {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90} +}; -void detectObstacles() -{ +void detectObstacles() { unsigned long now = millis(); - if (now - lastObstacleCheckTime >= obstacleCheckInterval) - { + if (now - lastObstacleCheckTime >= obstacleCheckInterval) { lastObstacleCheckTime = now; -#ifndef SIMULATOR - // On vérifie si on est en train de tomber seulement dans la phase finale - if (PAMI_NUM == 1 && currentScenarioStep > 1) - { - checkFall(); - } -#endif - if (lastDirection == RIGHT || lastDirection == LEFT) - { + // TODO : Threashold pour rotation ? + if (lastDirection == RIGHT || lastDirection == LEFT) { return; } long distance = readDistanceCM(TRIG_PIN, ECHO_PIN); - if (distance > 0 && distance < obstacleThresholdCM) - { - if (isMoving()) - { + if (distance > 0 && distance < obstacleThresholdCM) { + if (isMoving()) { lastSpeed = speed_steps_per_sec; stopMotors(); Serial.println("Obstacle détecté : Arrêt"); - // Si on est dans STEP_FORWARD_UNTIL_END, on lance la recherche de place - if (scenarioInProgress && currentScenarioStep > 0 && scenario[currentScenarioStep - 1].type == STEP_FORWARD_UNTIL_END && !recherchePlace) - { - recherchePlace = true; - rechercheStep = 0; - stepsRemainingPlace = steps_target - steps_done; // On garde en mémoire les pas restants - Serial.println("Début recherche de place..."); - // On tourne pour éviter le robot adverse - if (PAMI_SIDE == 1) - { - rotateAsync(90, 1000, true); // Tourner à droite si côté bleu - } - else - { - rotateAsync(90, 1000, false); // Tourner à gauche si côté jaune - } - } - // Si les PAMIs du bas arrivent sur leur phase finale et détecte un robot, alors on s'arrête et se met en position finale - if (PAMI_NUM > 1 && currentScenarioStep == 2) - { - recherchePlace = true; - Serial.println("Début recherche de place..."); - } } - } - else - { - if (!isMoving() && movementInProgress) - { - 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("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; + } } } } } // On gère le scénario du robot, retourne 0 si le scénario est terminé, 1 si en cours -int processScenario() -{ +int processScenario() { if (!scenarioInProgress) return 0; if (movementInProgress) return 1; - // Si on est en phase de recherche de place - if (recherchePlace) - { - // Les Pamis du bas - if (PAMI_NUM > 1) - { - currentScenarioStep++; - recherchePlace = false; - } - // PAMI superstar du haut - else if (rechercheStep < NB_RECHERCHE_STEPS) - { - Step &step = rechercheScenario[rechercheStep]; - switch (step.type) - { - case STEP_FORWARD: - moveAsyncSteps(getStepsForDistance(step.value), step.speed, true); - break; - case STEP_ROTATE: - if (step.value >= 0) - rotateAsync(step.value, 1000, true); - else - rotateAsync(-step.value, 1000, false); - break; - default: - break; - } - rechercheStep++; - } - else - { - // Après le sous-scénario, on tente d'avancer à nouveau - moveAsyncSteps(stepsRemainingPlace, 2500, true); - recherchePlace = false; - rechercheStep = 0; - } - return 1; - } - - if (currentScenarioStep >= scenarioLength) - { + if (currentScenarioStep >= scenarioLength) { Serial.println("Scénario terminé !"); scenarioInProgress = false; return 0; } Step &step = scenario[currentScenarioStep]; - switch (step.type) - { - case STEP_FORWARD: - moveAsyncSteps(getStepsForDistance(step.value), step.speed, true); - break; - 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); - break; - case STEP_FORWARD_UNTIL_END: - moveAsyncSteps(getStepsForDistance(step.value), step.speed, true); - break; + switch (step.type) { + case STEP_FORWARD: + moveAsyncSteps(getStepsForDistance(step.value), step.speed, true); + break; + 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); + break; } currentScenarioStep++; @@ -352,54 +252,49 @@ int processScenario() } bool tirettePose = false; -void loop() -{ - if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose) - { +void loop() { + if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose) { // Tirette posée tirettePose = true; - } - - else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose) - { + Serial.println("Trigger initialized"); + } else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose) { // Tirette activée - uint32_t startDelay = START_DELAY + (PAMI_NUM - 1) * 2000; + 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); - uint32_t startTime = millis(); + Serial.println("Starting the script"); + startTime = millis(); - while (true) - { + scenarioInProgress = true; + while (true) { updateSteppers(); // Mise à jour des moteurs asynchrone - detectObstacles(); // Vérification des obstacles + // 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) - { + if (processScenario() == 0 || millis() - startTime >= 100000 - startDelay) { Serial.println("Scénario terminé, arrêt des moteurs."); + // TODO : Faire bouger le servo a la fin stopMotors(); - uint32_t duty; - while (true) - { - - if (!digitalRead(EMG_PIN)) - { - duty = (uint32_t)((pow(2, 16) - 1) * 0.05); - ledcWrite(0, duty); + while (true) { + if (!digitalRead(EMG_PIN)) { + Serial.println("Servo run"); + setServoAngle(45); delay(500); - duty = (uint32_t)((pow(2, 16) - 1) * 0.1); - ledcWrite(0, duty); + setServoAngle(135); delay(500); - } - else - { + } else { + Serial.println("Restart"); ESP.restart(); break; } } - }; + } } } } diff --git a/src/utils.cpp b/src/utils.cpp index 0f07877..7781e67 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -2,35 +2,40 @@ #include "utils.h" #include "main.h" -void enableDrivers() -{ +void enableMotorDrivers() { digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver digitalWrite(M2_ENABLE_PIN, LOW); } -void disableDrivers() -{ +void disableDrivers() { digitalWrite(M1_ENABLE_PIN, HIGH); digitalWrite(M2_ENABLE_PIN, HIGH); } -int getStepsForDistance(float cm) -{ - double wheel_circumference_cm = M_PI * (double)WHEEL_DIAMETER; - double steps_per_cm = ((double)STEPS_PER_REV * (double)MICROSTEPPING) / wheel_circumference_cm; - int total_steps = (int)round(cm * steps_per_cm); - int correction = (int)round(1.5 * steps_per_cm); +int getStepsForDistance(float cm) { + double wheel_circumference_cm = M_PI * (double) WHEEL_DIAMETER; + double steps_per_cm = ((double) STEPS_PER_REV * (double) MICROSTEPPING) / wheel_circumference_cm; + int total_steps = (int) round(cm * steps_per_cm); + int correction = (int) round(1.5 * steps_per_cm); return total_steps - correction; // return cm * 740; // Valeur calculée à la main pour une vitesse de 5000 :) } -int getRotationSteps(float angleDeg) -{ - - double wheelTurnPerRotation = (double)WHEEL_BASE / (double)WHEEL_DIAMETER; - double microStepsPerRotation = wheelTurnPerRotation * (double)STEPS_PER_REV * (double)MICROSTEPPING; +int getRotationSteps(float angleDeg) { + double wheelTurnPerRotation = (double) WHEEL_BASE / (double) WHEEL_DIAMETER; + double microStepsPerRotation = wheelTurnPerRotation * (double) STEPS_PER_REV * (double) MICROSTEPPING; double microStepsForAngle = microStepsPerRotation * (angleDeg / 360.0); - return (int)round(microStepsForAngle); -} \ No newline at end of file + 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); +}