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

INIT PAMI 2026

Browse Source
This commit is contained in:
Florent
2026-03-07 12:27:25 +01:00
parent 5789b3179f
commit ccc9c03168
7 changed files with 167 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

@@ -1,2 +1,2 @@
# PAMI-2025
Code pour les PAMI de 2025
# PAMI-2026
Code pour les PAMI de 2026

18
include/Servo.h Normal file
View File

@@ -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

30
include/main.h
View File

@@ -1,8 +1,7 @@
#ifndef MAIN_H
#define MAIN_H
enum Direction
{
enum Direction {
FORWARD,
BACKWARD,
LEFT,
@@ -10,16 +9,14 @@ 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
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,9 +83,10 @@ 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

6
include/utils.h
View File

@@ -1,10 +1,14 @@
#ifndef UTILS_H
#define UTILS_H
void enableDrivers();
void enableMotorDrivers();
void disableDrivers();
int getStepsForDistance(float cm);
int getRotationSteps(float angleDeg);
void setServoAngle(int angle, int channel = 0);
#endif // UTILS_H

1
src/Servo.cpp Normal file
View File

@@ -0,0 +1 @@
#include "Servo.h"

261
src/main.cpp
View File

@@ -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, 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, 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
@@ -200,64 +182,28 @@ int stepsRemainingPlace = 0;
const int NB_RECHERCHE_STEPS = 3;
Step rechercheScenario[NB_RECHERCHE_STEPS] = {
{STEP_FORWARD, 20},
{STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90}};
{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)
{
} else {
if (!isMoving() && movementInProgress) {
Serial.println("Plus d'obstacle : Reprise du mouvement");
switch (lastDirection) {
case FORWARD:
moveAsyncSteps(steps_target - steps_done, lastSpeed, true);
break;
@@ -267,69 +213,26 @@ void detectObstacles()
default:
break;
}
Serial.println("Plus d'obstacle : Reprise du mouvement");
}
}
}
}
// 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)
{
switch (step.type) {
case STEP_FORWARD:
moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
break;
@@ -342,9 +245,6 @@ int processScenario()
else
rotateAsync(-step.value, step.speed, false);
break;
case STEP_FORWARD_UNTIL_END:
moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
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;
}
}
};
}
}
}
}

37
src/utils.cpp
View File

@@ -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);
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);
}