feat: code for dots reader

This commit is contained in:
fablab 2024-07-02 14:22:58 +02:00
parent 28b4ad9606
commit f1b7e18bd4
5 changed files with 245 additions and 28 deletions

View file

@ -1,6 +1,11 @@
{ {
// See http://go.microsoft.com/fwlink/?LinkId=827846 // See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format // for the documentation about the extensions.json format
"recommendations": ["platformio.platformio-ide", "wokwi.wokwi-vscode"], "recommendations": [
"unwantedRecommendations": ["ms-vscode.cpptools-extension-pack"] "platformio.platformio-ide",
"wokwi.wokwi-vscode"
],
"unwantedRecommendations": [
"ms-vscode.cpptools-extension-pack"
]
} }

View file

@ -1,26 +1,26 @@
// Libraries locales pour configurer et contrôler le robot. // // Libraries locales pour configurer et contrôler le robot.
#include "config.h" // Configuration des entrées/sorties et delais. // #include "config.h" // Configuration des entrées/sorties et delais.
#include "controls.h" // Fonctions pour se déplacer. // #include "controls.h" // Fonctions pour se déplacer.
#include "convert.h" // Utilitaires pour convertir des mesures en steps. // #include "convert.h" // Utilitaires pour convertir des mesures en steps.
// Code exécuté au démarrage (paramétrage) : // // Code exécuté au démarrage (paramétrage) :
void setup() // void setup()
{ // {
setupPins(); // Configure les pins de l'ESP32 // setupPins(); // Configure les pins de l'ESP32
Serial.begin(115200); // Démarrage d'une communication Série avec l'ordinateur s'il est connecté // Serial.begin(115200); // Démarrage d'une communication Série avec l'ordinateur s'il est connecté
} // }
// Code exécuté en boucle jusqu'à l'extinction du robot // // Code exécuté en boucle jusqu'à l'extinction du robot
void loop() // void loop()
{ // {
// Exemple de commande : // // Exemple de commande :
for (int i = 0; i < 50; i++) // for (int i = 0; i < 50; i++)
{ // {
forward(); // forward();
} // }
int rotation_steps = convertRotToSteps(46); // int rotation_steps = convertRotToSteps(46);
for (int i = 0; i < rotation_steps; i++) // for (int i = 0; i < rotation_steps; i++)
{ // {
left(); // left();
} // }
} // }

View file

@ -0,0 +1,5 @@
// Librairies locales
#include "config_gommettes.h" // Configurations des paramètres
#include "controls_gommette.h" // Fonctions de déplacement
#include "convert.h" // Fonctions de conversions en steps

View file

@ -0,0 +1,43 @@
/************************************
Fichier de configuration du robot
************************************/
// Définition des ports pour contrôler les moteurs
// Moteur gauche :
#define IN1a 14
#define IN2a 12
#define IN3a 27
#define IN4a 26
// Moteur droit :
#define IN1b 21
#define IN2b 19
#define IN3b 18
#define IN4b 5
// Moteur lecteur de gommettes :
#define IN1r 17
#define IN2r 16
#define IN3r 4
#define IN4r 0
// Pompe :
#define IN1p 22
// Led :
#define IN3p 23
// Ports du lecteur à infrarouges :
#define clock 34
#define bit1 35
#define bit2 32
#define bit3 33
#define bit4 25
// Port du bouton de démarrage:
#define button 13
// Définition des différents délais
// Temps d'attente entre deux impulsions :
#define DELAY_TIME 3
// Période de clignotement de la led :
#define period 1000
// Nombre de pas pour les mouvements :
#define StepsPump 900
#define StepsReader 70

View file

@ -0,0 +1,164 @@
/*******************************************
Initialisation et fonctions de déplacement
*******************************************/
/**
* @brief Configure les ports de contrôle des moteurs sont des sorties (output).
*
*/
void setupPins()
{
pinMode(clock,INPUT);
pinMode(bit1,INPUT);
pinMode(bit2,INPUT);
pinMode(bit3,INPUT);
pinMode(bit4,INPUT);
pinMode(IN1a, OUTPUT);
pinMode(IN2a, OUTPUT);
pinMode(IN3a, OUTPUT);
pinMode(IN4a, OUTPUT);
pinMode(IN1b, OUTPUT);
pinMode(IN2b, OUTPUT);
pinMode(IN3b, OUTPUT);
pinMode(IN4b, OUTPUT);
pinMode(IN1r, OUTPUT);
pinMode(IN2r, OUTPUT);
pinMode(IN3r, OUTPUT);
pinMode(IN4r, OUTPUT);
pinMode(IN1p, OUTPUT);
pinMode(IN3p, OUTPUT);
pinMode(button, INPUT_PULLUP);
}
/**
* @brief Applique une configuration aux moteurs.
* @brief Chaque configuration est sur 1 octet.
*
* Exemple
* ```
* writeMotor(0b0010, 0b1100, 0b0000);
* ```
*
* @param left Configuration du moteur droit (ex: `0b1100`).
* @param right Configuration du moteur droit (ex: `0b0101`).
* @param reader Configuration du moteur de lecture des gommettes (ex: `0b0101`).
*/
void writeMotor(uint8_t left, uint8_t right, uint8_t reader)
{
digitalWrite(IN4a, left & 0b1000);
digitalWrite(IN3a, left & 0b0100);
digitalWrite(IN2a, left & 0b0010);
digitalWrite(IN1a, left & 0b0001);
digitalWrite(IN4b, right & 0b1000);
digitalWrite(IN3b, right & 0b0100);
digitalWrite(IN2b, right & 0b0010);
digitalWrite(IN1b, right & 0b0001);
digitalWrite(IN4r, reader & 0b1000);
digitalWrite(IN3r, reader & 0b0100);
digitalWrite(IN2r, reader & 0b0010);
digitalWrite(IN1r, reader & 0b0001);
}
/**
* @brief Commande pour faire avancer le robot.
*
*/
void forward()
{
writeMotor(0b1000, 0b0001, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0100, 0b0010, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0010, 0b0100, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0001, 0b1000, 0b0000);
delay(DELAY_TIME);
}
/**
* @brief Commande pour faire reculer le robot.
*
*/
void backward()
{
writeMotor(0b0001, 0b1000, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0010, 0b0100, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0100, 0b0010, 0b0000);
delay(DELAY_TIME);
writeMotor(0b1000, 0b0001, 0b0000);
delay(DELAY_TIME);
}
/**
* @brief Commande pour faire tourner le robot à droite.
*
*/
void right()
{
writeMotor(0b1000, 0b1000, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0100, 0b0100, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0010, 0b0010, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0001, 0b0001, 0b0000);
delay(DELAY_TIME);
}
/**
* @brief Commande pour faire tourner le robot à gauche.
*
*/
void left()
{
writeMotor(0b0001, 0b0001, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0010, 0b0010, 0b0000);
delay(DELAY_TIME);
writeMotor(0b0100, 0b0100, 0b0000);
delay(DELAY_TIME);
writeMotor(0b1000, 0b1000, 0b0000);
delay(DELAY_TIME);
}
/**
* @brief Commande pour faire avancer la page de gommettes.
*
*/
void forwardReader()
{
writeMotor(0b0000, 0b0000, 0b0001);
delay(DELAY_TIME);
writeMotor(0b0000, 0b0000, 0b0010);
delay(DELAY_TIME);
writeMotor(0b0000, 0b0000, 0b0100);
delay(DELAY_TIME);
writeMotor(0b0000, 0b0000, 0b1000);
delay(DELAY_TIME);
}
/**
* @brief Comande pour activer la pompe à eau
*/
void pump()
{
digitalWrite(IN1p, HIGH);
delay(DELAY_TIME);
}
/**
* @brief Commande pour arrêter les moteurs du robot.
*
*/
void stopMotors()
{
writeMotor(0b0000, 0b0000, 0b0000);
}