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

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 # PAMI-2026
Code pour les PAMI de 2025 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 #ifndef MAIN_H
#define MAIN_H #define MAIN_H
enum Direction enum Direction {
{
FORWARD, FORWARD,
BACKWARD, BACKWARD,
LEFT, LEFT,
@@ -10,16 +9,14 @@ enum Direction
STOP STOP
}; };
enum StepType enum StepType {
{
STEP_FORWARD, STEP_FORWARD,
STEP_ROTATE, STEP_ROTATE,
STEP_FORWARD_UNTIL_END, STEP_FORWARD_UNTIL_END,
STEP_BACKWARD STEP_BACKWARD
}; };
struct Step struct Step {
{
StepType type; 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 int32_t speed; // Vitesse en pas / seconde
@@ -34,24 +31,22 @@ struct Step
// PINs réels // PINs réels
#ifndef SIMULATOR #ifndef SIMULATOR
// Moteur 1 - Gauche // Moteur 1 - Droite
#define M1_DIR_PIN 19 #define M1_DIR_PIN 19
#define M1_STEP_PIN 23 #define M1_STEP_PIN 23
#define M1_ENABLE_PIN 26 #define M1_ENABLE_PIN 26
// Moteur 2 - Droite // Moteur 2 - Gauche
#define M2_DIR_PIN 21 #define M2_DIR_PIN 21
#define M2_STEP_PIN 25 #define M2_STEP_PIN 25
#define M2_ENABLE_PIN 27 #define M2_ENABLE_PIN 27
// Capteur à ultrasons // Capteur à ultrasons
#define TRIG_PIN 15 #define TRIG_PIN 13
#define ECHO_PIN 14 #define ECHO_PIN 12
// Servo Moteur
// Capteur de chute #define SERVO_PIN 15
#define FALL_PIN 13 // FIXME : Je sais pas, loop sous le bouton d'arret d'urgence
#define SERVO_PIN 12
#define EMG_PIN 5 #define EMG_PIN 5
#endif #endif
@@ -88,9 +83,10 @@ struct Step
#define DRIVER1_ADDR 0b01 #define DRIVER1_ADDR 0b01
#define DRIVER2_ADDR 0b10 #define DRIVER2_ADDR 0b10
// TODO : Changer les constantes avec nouvelles dim
// Constantes // Constantes
#define WHEEL_DIAMETER 5.5 // cm #define WHEEL_DIAMETER 5.6 // cm
#define WHEEL_BASE 7.2 // cm #define WHEEL_BASE 7 // cm
#define STEPS_PER_REV 200.0 #define STEPS_PER_REV 200.0
#define MICROSTEPPING 8.0 #define MICROSTEPPING 8.0

6
include/utils.h
View File

@@ -1,10 +1,14 @@
#ifndef UTILS_H #ifndef UTILS_H
#define UTILS_H #define UTILS_H
void enableDrivers(); void enableMotorDrivers();
void disableDrivers(); void disableDrivers();
int getStepsForDistance(float cm); int getStepsForDistance(float cm);
int getRotationSteps(float angleDeg); int getRotationSteps(float angleDeg);
void setServoAngle(int angle, int channel = 0);
#endif // UTILS_H #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 "main.h"
#include "utils.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 // Côté bleu = 1 et Côté jaune = 2
#define PAMI_SIDE 1 #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 // 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 #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; uint32_t startTime = 0;
volatile int32_t speed_steps_per_sec = 0; // target speed (signed) 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; volatile int32_t steps_done = 0;
bool movementInProgress = false; bool movementInProgress = false;
// Les différents scénario possibles : le superstar et les autres // Les différents scénarios possibles
#if PAMI_NUM == 1 #if PAMI_NUM == 1
// Chaque étape du scénario // Chaque étape du scénario écureuil ninja
// TODO
Step scenario[] = { Step scenario[] = {
{STEP_FORWARD, 105, 2500}, {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}, // 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 #else
Step scenario[] = { Step scenario[] = {
{STEP_FORWARD, 35, 2500}, {STEP_FORWARD, 35, 2500},
{STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500}, {STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500},
{STEP_FORWARD, 80 - 10 * PAMI_NUM, 3000}, {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}};
#endif #endif
const int scenarioLength = sizeof(scenario) / sizeof(Step); const int scenarioLength = sizeof(scenario) / sizeof(Step);
@@ -46,53 +56,48 @@ int currentScenarioStep = 0;
bool scenarioInProgress = false; bool scenarioInProgress = false;
// Arrêt des moteurs // Arrêt des moteurs
void stopMotors() void stopMotors() {
{
speed_steps_per_sec = 0; 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 // Fonction d'initialisation
void setup() void setup() {
{
Serial.begin(115200); Serial.begin(115200);
// Bouton d'arrêt d'urgence (HIGH si appuyé)
pinMode(EMG_PIN, INPUT); pinMode(EMG_PIN, INPUT);
// Moteur 1 (Droite)
pinMode(M1_STEP_PIN, OUTPUT); pinMode(M1_STEP_PIN, OUTPUT);
pinMode(M1_DIR_PIN, OUTPUT); pinMode(M1_DIR_PIN, OUTPUT);
pinMode(M1_ENABLE_PIN, OUTPUT); pinMode(M1_ENABLE_PIN, OUTPUT);
// Moteur 2 (Gauche)
pinMode(M2_STEP_PIN, OUTPUT); pinMode(M2_STEP_PIN, OUTPUT);
pinMode(M2_DIR_PIN, OUTPUT); pinMode(M2_DIR_PIN, OUTPUT);
pinMode(M2_ENABLE_PIN, OUTPUT); pinMode(M2_ENABLE_PIN, OUTPUT);
// Tirette
pinMode(TIRETTE_PIN, INPUT_PULLUP); pinMode(TIRETTE_PIN, INPUT_PULLUP);
// Capteur à ultrasons // Capteur à ultrasons
pinMode(TRIG_PIN, OUTPUT); pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT); pinMode(ECHO_PIN, INPUT);
// Capteur de chute
pinMode(FALL_PIN, INPUT);
// Servo moteur // Servo moteur
// pinMode(SERVO_PIN, OUTPUT); pinMode(SERVO_PIN, OUTPUT);
ledcSetup(0, 50, 16); ledcSetup(0, 50, 16);
ledcAttachPin(SERVO_PIN, 0); ledcAttachPin(SERVO_PIN, 0);
enableDrivers(); setServoAngle(130);
enableMotorDrivers();
stopMotors(); stopMotors();
scenarioInProgress = true; scenarioInProgress = false;
currentScenarioStep = 0; currentScenarioStep = 0;
Serial.println("Setup complete"); 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 // Le nombre de pas à faire
steps_target = steps; steps_target = steps;
steps_done = 0; steps_done = 0;
@@ -106,9 +111,7 @@ void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir)
lastDirection = forwardDir ? FORWARD : BACKWARD; 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 // Le nombre de pas à faire
steps_target = getRotationSteps(angleDeg + 10.0); // 10.0 correspond à une correction trouvée à la main :) steps_target = getRotationSteps(angleDeg + 10.0); // 10.0 correspond à une correction trouvée à la main :)
steps_done = 0; steps_done = 0;
@@ -123,18 +126,14 @@ void rotateAsync(float angleDeg, int32_t speed, bool toRight)
} }
// Non-blocking stepper update function // Non-blocking stepper update function
void updateSteppers() void updateSteppers() {
{
uint32_t now = micros(); uint32_t now = micros();
// Moteur 1 // 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); uint32_t interval1 = 1000000UL / abs(speed_steps_per_sec);
static uint32_t last_step_time1 = 0; 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(M1_STEP_PIN, HIGH);
digitalWrite(M2_STEP_PIN, HIGH); digitalWrite(M2_STEP_PIN, HIGH);
delayMicroseconds(2); // Short pulse width for step signal 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) // 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(); stopMotors();
movementInProgress = false; movementInProgress = false;
Serial.println("Etape terminé"); Serial.println("Etape terminé");
@@ -156,8 +153,7 @@ void updateSteppers()
} }
// Vérifie si les moteurs sont en mouvement // Vérifie si les moteurs sont en mouvement
bool isMoving() bool isMoving() {
{
return speed_steps_per_sec != 0; return speed_steps_per_sec != 0;
} }
@@ -166,8 +162,7 @@ const int obstacleCheckInterval = 100; // ms between distance checks
unsigned long lastObstacleCheckTime = 0; unsigned long lastObstacleCheckTime = 0;
const int obstacleThresholdCM = 7; // stop if closer than 7 cm 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); digitalWrite(trigPin, LOW);
delayMicroseconds(2); delayMicroseconds(2);
digitalWrite(trigPin, HIGH); digitalWrite(trigPin, HIGH);
@@ -178,19 +173,6 @@ long readDistanceCM(uint8_t trigPin, uint8_t echoPin)
return distance; 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; bool recherchePlace = false;
int rechercheStep = 0; int rechercheStep = 0;
// Nombre de pas restants à faire après la reprise de place // Nombre de pas restants à faire après la reprise de place
@@ -200,64 +182,28 @@ int stepsRemainingPlace = 0;
const int NB_RECHERCHE_STEPS = 3; const int NB_RECHERCHE_STEPS = 3;
Step rechercheScenario[NB_RECHERCHE_STEPS] = { Step rechercheScenario[NB_RECHERCHE_STEPS] = {
{STEP_FORWARD, 20}, {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(); unsigned long now = millis();
if (now - lastObstacleCheckTime >= obstacleCheckInterval) if (now - lastObstacleCheckTime >= obstacleCheckInterval) {
{
lastObstacleCheckTime = now; lastObstacleCheckTime = now;
#ifndef SIMULATOR // TODO : Threashold pour rotation ?
// On vérifie si on est en train de tomber seulement dans la phase finale if (lastDirection == RIGHT || lastDirection == LEFT) {
if (PAMI_NUM == 1 && currentScenarioStep > 1)
{
checkFall();
}
#endif
if (lastDirection == RIGHT || lastDirection == LEFT)
{
return; return;
} }
long distance = readDistanceCM(TRIG_PIN, ECHO_PIN); long distance = readDistanceCM(TRIG_PIN, ECHO_PIN);
if (distance > 0 && distance < obstacleThresholdCM) if (distance > 0 && distance < obstacleThresholdCM) {
{ if (isMoving()) {
if (isMoving())
{
lastSpeed = speed_steps_per_sec; lastSpeed = speed_steps_per_sec;
stopMotors(); stopMotors();
Serial.println("Obstacle détecté : Arrêt"); 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 } else {
{ if (!isMoving() && movementInProgress) {
rotateAsync(90, 1000, false); // Tourner à gauche si côté jaune Serial.println("Plus d'obstacle : Reprise du mouvement");
} switch (lastDirection) {
}
// 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: case FORWARD:
moveAsyncSteps(steps_target - steps_done, lastSpeed, true); moveAsyncSteps(steps_target - steps_done, lastSpeed, true);
break; break;
@@ -267,69 +213,26 @@ void detectObstacles()
default: default:
break; 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 // 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) if (!scenarioInProgress)
return 0; return 0;
if (movementInProgress) if (movementInProgress)
return 1; return 1;
// Si on est en phase de recherche de place if (currentScenarioStep >= scenarioLength) {
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)
{
Serial.println("Scénario terminé !"); Serial.println("Scénario terminé !");
scenarioInProgress = false; scenarioInProgress = false;
return 0; return 0;
} }
Step &step = scenario[currentScenarioStep]; Step &step = scenario[currentScenarioStep];
switch (step.type) switch (step.type) {
{
case STEP_FORWARD: case STEP_FORWARD:
moveAsyncSteps(getStepsForDistance(step.value), step.speed, true); moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
break; break;
@@ -342,9 +245,6 @@ int processScenario()
else else
rotateAsync(-step.value, step.speed, false); rotateAsync(-step.value, step.speed, false);
break; break;
case STEP_FORWARD_UNTIL_END:
moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
break;
} }
currentScenarioStep++; currentScenarioStep++;
@@ -352,54 +252,49 @@ int processScenario()
} }
bool tirettePose = false; bool tirettePose = false;
void loop()
{
if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose) void loop() {
{ if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose) {
// Tirette posée // Tirette posée
tirettePose = true; tirettePose = true;
} Serial.println("Trigger initialized");
} else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose) {
else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose)
{
// Tirette activée // 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); 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 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é // 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."); Serial.println("Scénario terminé, arrêt des moteurs.");
// TODO : Faire bouger le servo a la fin
stopMotors(); stopMotors();
uint32_t duty;
while (true) while (true) {
{ if (!digitalRead(EMG_PIN)) {
Serial.println("Servo run");
if (!digitalRead(EMG_PIN)) setServoAngle(45);
{
duty = (uint32_t)((pow(2, 16) - 1) * 0.05);
ledcWrite(0, duty);
delay(500); delay(500);
duty = (uint32_t)((pow(2, 16) - 1) * 0.1); setServoAngle(135);
ledcWrite(0, duty);
delay(500); delay(500);
} } else {
else Serial.println("Restart");
{
ESP.restart(); ESP.restart();
break; break;
} }
} }
}; }
} }
} }
} }

37
src/utils.cpp
View File

@@ -2,35 +2,40 @@
#include "utils.h" #include "utils.h"
#include "main.h" #include "main.h"
void enableDrivers() void enableMotorDrivers() {
{
digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver
digitalWrite(M2_ENABLE_PIN, LOW); digitalWrite(M2_ENABLE_PIN, LOW);
} }
void disableDrivers() void disableDrivers() {
{
digitalWrite(M1_ENABLE_PIN, HIGH); digitalWrite(M1_ENABLE_PIN, HIGH);
digitalWrite(M2_ENABLE_PIN, HIGH); digitalWrite(M2_ENABLE_PIN, HIGH);
} }
int getStepsForDistance(float cm) int getStepsForDistance(float cm) {
{ double wheel_circumference_cm = M_PI * (double) WHEEL_DIAMETER;
double wheel_circumference_cm = M_PI * (double)WHEEL_DIAMETER; double steps_per_cm = ((double) STEPS_PER_REV * (double) MICROSTEPPING) / wheel_circumference_cm;
double steps_per_cm = ((double)STEPS_PER_REV * (double)MICROSTEPPING) / wheel_circumference_cm; int total_steps = (int) round(cm * steps_per_cm);
int total_steps = (int)round(cm * steps_per_cm); int correction = (int) round(1.5 * steps_per_cm);
int correction = (int)round(1.5 * steps_per_cm);
return total_steps - correction; return total_steps - correction;
// return cm * 740; // Valeur calculée à la main pour une vitesse de 5000 :) // return cm * 740; // Valeur calculée à la main pour une vitesse de 5000 :)
} }
int getRotationSteps(float angleDeg) int getRotationSteps(float angleDeg) {
{ double wheelTurnPerRotation = (double) WHEEL_BASE / (double) WHEEL_DIAMETER;
double microStepsPerRotation = wheelTurnPerRotation * (double) STEPS_PER_REV * (double) MICROSTEPPING;
double wheelTurnPerRotation = (double)WHEEL_BASE / (double)WHEEL_DIAMETER;
double microStepsPerRotation = wheelTurnPerRotation * (double)STEPS_PER_REV * (double)MICROSTEPPING;
double microStepsForAngle = microStepsPerRotation * (angleDeg / 360.0); 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);
} }