Petit programme de télécommande de robots, en complément de la version développée par Guillaume.
Il s'adresse aux robots de type deux roues motrices avec roues folles.
Avec quelques ajouts et une approche un peu différente. Les données sont traitées coté joystick ce qui permet d'avoir en retour console les infos envoyés au robot. Puissance et direction des roues.
-Calibrage automatique du point mort du joystick au démarrage.
-Réglage des mini et maxi du potentiomètre (utile avec certains potentiomètres qui ne couvrent pas toute la plage de tension).
-Réglage de l'équilibrage (lorsqu'on utilise des moteurs droit et gauche différents).
-Puissance proportionnelle au déplacement du potentiomètre (vs tension proportionnelle au déplacement du potentiomètre).
-Réglage de la sensibilité de la direction.
-Témoin de signal radio sur le robot.
-Augmentation de la portée (par réglage du débit).
Le code coté joystick:
Code : Tout sélectionner
/*
* Pilote robot à deux roues motrices avec joystick Funduino (ou autre, adaptable) + nFR24L01+, coté joystick. Version 1 Magnéto, usage libre.
* commutateur du shield sur 5V
* Lors de la mise sous tension ou d'un reset, on laisse le potentiomètre au repos (lecture des points morts X et Y pour calibrage automatique).
*
*/
#include <SPI.h>
#include <RF24.cpp> // URL des librairies: https://github.com/TMRh20/RF24
// Il faut installer les fichiers: RF24_config.h RF24.h RF24.cpp nRF24L01.h dans le répertoire "include" par défaut.
// Attention! Il existe plusieurs variantes de librairies du même nom qui ne sont pas toujours totalement compatibles entre-elles.
#define POT_X A0 // axe direction
#define POT_Y A1 // axe avance
#define BOUTON_A 2 // bouton du haut
#define BOUTON_B 3 // bouton de droite
#define BOUTON_C 4 // bouton du bas
#define BOUTON_D 5 // bouton de gauche
#define BOUTON_E 6 // bouton noté E
#define BOUTON_F 7 // bouton noté F
#define BOUTON_POT 8 // bouton poussoir potentiomètre
#define GAUCHE 0
#define DROITE 1
// variables modifiables:
const bool parler = true; // true: retour console (pour contrôle), false: silencieux (plus rapide)
const bool calibrer = false; // true pour calibrer le potentiomètre. false par défaut.
// lors du calibrage, on monte le potentiomètre Y au maxi, puis on inscrit la valeur lue dans la variable Ymax ci-dessous
// puis on descend le potentiomètre Y au mini et on inscrit la valeur lue dans la variable Ymin
const int Ymax = 1010; // valeur par défaut: 1010. A remplacer par la valeur maxi lue lors du calibrage (calibrer à true), on peut retirer quelques unités
const int Ymin = 10; // valeur par défaut: 10. A remplacer par la valeur mini lue lors du calibrage (calibrer à true), on peut ajouter quelques unités
const bool lineaire = true; // true: racine carrée pour obtenir une puissance moteur proportionnelle au déplacement du potentiomètre.
// false: tension moteur proportionnelle au déplacement du potentiomètre. Défaut: true (plus nerveux)
const byte seuil = 4; // seuil de déplacement du potentiomètre à partir du point mort avant action moteurs, défaut 4
const float sensDir = 0.3; // réglage de la sensibilité de la direction. plus la valeur est élevée, plus le robot partira rapidement de coté. A tester selon robot.
const float equilibrage = 1; // Si le robot tire à gauche augmenter la valeur, s'il tire à droite, la réduire. Pratique lorsqu'on utilise deux moteurs de puissance différentes. Défaut: 1.
const byte canal = 102; // choisir un canal d'émission radio (entre 100 et 125, au dessus des canaux wifi)
const byte pipe[5] = {0xE8,0xE8,0x12,0x34,0x56}; // choisir un canal logique unique pour chaque robot
// fin variables modifiables.
RF24 radio(9,10); // CE, CS conexion par défaut du schield Funduino
void calibrage() {
for(;;) {
Serial.print("Y: ");
Serial.println(analogRead(POT_Y));
delay(1000);
}
}
void setup() {
Serial.begin(250000);
pinMode(POT_X, INPUT);
pinMode(POT_Y, INPUT);
pinMode(BOUTON_A, INPUT);
pinMode(BOUTON_B, INPUT);
pinMode(BOUTON_C, INPUT);
pinMode(BOUTON_D, INPUT);
pinMode(BOUTON_E, INPUT);
pinMode(BOUTON_F, INPUT);
pinMode(BOUTON_POT, INPUT);
analogRead(POT_Y); // lecture à blanc pour initialisation ADC
if (calibrer)
calibrage();
radio.begin();
radio.setPALevel(RF24_PA_MAX); // RF24_PA_MIN, RF24_PA_LOW, RF24_PA_HIGH, RF24_PA_MAX. On règle à la puissance maxi (1mW).
radio.setChannel(canal);
radio.setDataRate(RF24_250KBPS); // On réduit le débit pour doubler la portée. Attention! ne fonctionne qu'avec la version "+" du nRF24L01+. Sinon on met la ligne en commentaire.
radio.openWritingPipe(pipe);
}
void loop() {
static int pmX = analogRead(POT_X); // point mort X
static int pmY = analogRead(POT_Y); // point mort Y
byte RFdata[4]; // données de transmission radio: data[0]: puissance moteurs gauche de 0 à 255, data[1]: puissance moteurs droit de 0 à 255, data[2]: dirMot, data[3]: boutons
byte boutons = 0, dirMot = 0; // position des boutons (A = b6, B = b5 [...] POT = b0) 1 = appuyé, sens des moteurs, gauche = b0, droit = b1, 1 = avancer, 0 = reculer.
float PmotG = 0, PmotD = 0; // puissance moteurs gauche et droit, de 0 à 1;
int echelleX, echelleY; // réglage de l'échelle en fonction de la plage du potentiomètre Y et de la sensibilité de direction
int potX = analogRead(POT_X);
int potY = analogRead(POT_Y);
boutons = !digitalRead(BOUTON_A)<<6 | !digitalRead(BOUTON_B)<<5 | !digitalRead(BOUTON_C)<<4 | !digitalRead(BOUTON_D)<<3 | !digitalRead(BOUTON_E)<<2 | !digitalRead(BOUTON_F)<<1 | !digitalRead(BOUTON_POT);
Ymax - pmY > pmY - Ymin ? echelleY = pmY - Ymin - seuil : echelleY = Ymax - pmY - seuil;
echelleX = (int) (echelleY / sensDir);
if (potY - pmY > seuil) // avancer
PmotG = PmotD = (float) (potY - pmY - seuil) / echelleY;
else if (pmY - potY > seuil) // reculer
PmotG = PmotD = (float) (potY - pmY + seuil) / echelleY;
if ((potY - pmY > seuil) || (pmY - potY > seuil) || (potX - pmX > seuil) || (pmX - potX > seuil)) { // tourner
PmotG += (float) (potX - pmX) / echelleX;
PmotD -= (float) (potX - pmX) / echelleX;
}
PmotG < 0 ? PmotG = -PmotG : dirMot |= 1<<GAUCHE;
PmotD < 0 ? PmotD = -PmotD : dirMot |= 1<<DROITE;
if (lineaire) {
PmotG = sqrt(PmotG);
PmotD = sqrt(PmotD);
}
PmotG *= equilibrage;
if (PmotG > 1) { // gestion des dépassements du maxi
PmotD /= PmotG;
PmotG = 1;
}
if (PmotD > 1) {
PmotG /= PmotD;
PmotD = 1;
}
RFdata[0] = (byte) (255 * PmotG); // formatage des données à envoyer au robot
RFdata[1] = (byte) (255 * PmotD);
RFdata[2] = dirMot;
RFdata[3] = boutons;
radio.enableDynamicAck();
radio.write(RFdata, 4, 1); // pas d'ACK, plus rapide (~1ms vs 50ms)
if (parler) {
dirMot & 1<<GAUCHE ? Serial.print("AVG ") : Serial.print("ARG ");
Serial.print(RFdata[0]);
dirMot & 1<<DROITE ? Serial.print(" AVD ") : Serial.print(" ARD ");
Serial.print(RFdata[1]);
Serial.print(" Y: ");
Serial.print(potY);
Serial.print(" X: ");
Serial.print(potX);
if (boutons & 1<<6) Serial.print(" HAUT");
if (boutons & 1<<5) Serial.print(" DROITE");
if (boutons & 1<<4) Serial.print(" BAS");
if (boutons & 1<<3) Serial.print(" GAUCHE");
if (boutons & 1<<2) Serial.print(" E");
if (boutons & 1<<1) Serial.print(" F");
if (boutons & 1<<0) Serial.print(" POT");
Serial.println("");
}
delay(20); // intervalle de temps entre deux trames
}
Code : Tout sélectionner
/*
* Pilote robot à deux roues motrices avec joystick Funduino (ou autre, adaptable) + nFR24L01+, coté robot. Version 1 Magnéto, usage libre.
*
* Schema du brochage:
* 0 RX
* 1 TX
* 2 PONT_GAUCHE_A Moteur gauche (même brochage des moteurs que sur la version de Guillaume)
* 3 PWM_GAUCHE Moteur gauche
* 4 PONT_GAUCHE_B Moteur gauche
* 5 TEMOIN_RF haut lorsque le signal radio arrive. Bas après une perte de signal de plus de 100ms
* 6 PWM_DROIT Moteur droit
* 7 PONT_DROIT_A Moteur droit
* 8 PONT_DROIT_B Moteur droit
* 9 CE nRF standby, facultatif, peut être maintenu à +5V
* 10 CS nRF
* 11 CLK nRF SPI, fixe, ne peut être changé (tout modèles Arduino sauf Mega)
* 12 MOSI nRF SPI, fixe, ne peut être changé
* 13 MISO nRF SPI, fixe, ne peut être changé
* A0 BOUTON_A mis à 5V lorsque le bouton A est appuyé, sinon à 0V. A modifier selon usage du robot
* A1 BOUTON_B ""
* A2 BOUTON_C ""
* A3 BOUTON_D ""
* A4 BOUTON_E ""
* A5 BOUTON_F ""
* A6 BOUTON_POT ""
* A7 RFU
*/
#include <SPI.h>
#include <RF24.cpp>
// On choisi le brochage selon usage et disponibilité
#define PONT_GAUCHE_A 2
#define PWM_GAUCHE 3 // choisir une sortie PWM
#define PONT_GAUCHE_B 4
#define TEMOIN_RF 5
#define PWM_DROIT 6 // choisir une sortie PWM
#define PONT_DROIT_A 7
#define PONT_DROIT_B 8
#define CE 9
#define CS 10
#define BOUTON_A A0 // bouton du haut
#define BOUTON_B A1 // bouton de droite
#define BOUTON_C A2 // bouton du bas
#define BOUTON_D A3 // bouton de gauche
#define BOUTON_E A4 // bouton noté E
#define BOUTON_F A5 // bouton noté F
#define BOUTON_POT A6 // bouton poussoir potentiomètre
// variables modifiables:
const bool parler = true; // true: retour console (pour contrôle), false: silencieux (plus rapide)
const byte canal = 102; // prendre le même canal de réception radio que celui de l'émetteur
const byte pipe[5] = {0xE8,0xE8,0x12,0x34,0x56}; // prendre le même canal logique que celui de l'émetteur
// fin variables modifiables.
RF24 radio(CE,CS);
void setup() {
Serial.begin(250000);
pinMode(PONT_GAUCHE_A, OUTPUT);
pinMode(PONT_GAUCHE_B, OUTPUT);
pinMode(PWM_GAUCHE, OUTPUT);
pinMode(PONT_DROIT_A, OUTPUT);
pinMode(PONT_DROIT_B, OUTPUT);
pinMode(PWM_DROIT, OUTPUT);
pinMode(TEMOIN_RF, OUTPUT);
pinMode(BOUTON_A, OUTPUT);
pinMode(BOUTON_B, OUTPUT);
pinMode(BOUTON_C, OUTPUT);
pinMode(BOUTON_D, OUTPUT);
pinMode(BOUTON_E, OUTPUT);
pinMode(BOUTON_F, OUTPUT);
pinMode(BOUTON_POT, OUTPUT);
digitalWrite(PONT_GAUCHE_A, LOW);
digitalWrite(PONT_GAUCHE_B, LOW);
digitalWrite(PWM_GAUCHE, LOW);
digitalWrite(PONT_DROIT_A, LOW);
digitalWrite(PONT_DROIT_B, LOW);
digitalWrite(PWM_DROIT, LOW);
digitalWrite(TEMOIN_RF, LOW);
digitalWrite(BOUTON_A, LOW);
digitalWrite(BOUTON_B, LOW);
digitalWrite(BOUTON_C, LOW);
digitalWrite(BOUTON_D, LOW);
digitalWrite(BOUTON_E, LOW);
digitalWrite(BOUTON_F, LOW);
digitalWrite(BOUTON_POT, LOW);
radio.begin();
radio.setChannel(canal);
radio.setDataRate(RF24_250KBPS); // On utilise le même débit que pour l'émetteur. Attention! ne fonctionne qu'avec la version "+" du nRF24L01+. Sinon on met la ligne en commentaire.
radio.openReadingPipe(1,pipe);
radio.startListening();
}
void loop() {
static unsigned long t1 = millis(), t2, tempsMort; // tempsMort: temps entre deux trames
static byte boutons = 0, dirMot = 0; // position des boutons (A = b6, B = b5 [...] POT = b0) 1 = appuyé, sens des moteurs, gauche = b0, droit = b1, 1 = avancer, 0 = reculer.
static byte PWMmotG = 0, PWMmotD = 0; // puissance moteurs gauche et droit, de 0 à 255;
static byte RFdata[4] = {0, 0, 0, 0}; // données de transmission radio: data[0]: puissance moteurs gauche de 0 à 255, data[1]: puissance moteurs droit de 0 à 255, data[2]: dirMot, data[3]: boutons
static bool ArretUrgence = true;
t2 = millis();
tempsMort = t2 - t1;
if (radio.available() > 0) {
radio.read(RFdata, 4);
if (parler) {
RFdata[2] & 1 ? Serial.print("AVG ") : Serial.print("ARG ");
Serial.print(RFdata[0]);
RFdata[2] & 2 ? Serial.print(" AVD ") : Serial.print(" ARD ");
Serial.print(RFdata[1]);
if (RFdata[3] & 1<<6) Serial.print(" HAUT");
if (RFdata[3] & 1<<5) Serial.print(" DROITE");
if (RFdata[3] & 1<<4) Serial.print(" BAS");
if (RFdata[3] & 1<<3) Serial.print(" GAUCHE");
if (RFdata[3] & 1<<2) Serial.print(" E");
if (RFdata[3] & 1<<1) Serial.print(" F");
if (RFdata[3] & 1<<0) Serial.print(" POT");
Serial.print(" Delai: "); // depuis la dernière trame
Serial.println(tempsMort);
}
t1 = t2;
if (ArretUrgence) {
ArretUrgence = false;
// on ajoute ici le code à exécuter après un arrêt d'urgence si besoin
}
if (RFdata[2] & 1) { // Si le moteur gauche tourne à l'envers, on inverse LOW et HIGH (ou le câblage)
digitalWrite(PONT_GAUCHE_A, LOW);
digitalWrite(PONT_GAUCHE_B, HIGH);
} else {
digitalWrite(PONT_GAUCHE_A, HIGH);
digitalWrite(PONT_GAUCHE_B, LOW);
}
analogWrite(PWM_GAUCHE, RFdata[0]);
if (RFdata[2] & 2) { // Si le moteur droit tourne à l'enver, on inverse LOW et HIGH (ou le câblage)
digitalWrite(PONT_DROIT_A, LOW);
digitalWrite(PONT_DROIT_B, HIGH);
} else {
digitalWrite(PONT_DROIT_A, HIGH);
digitalWrite(PONT_DROIT_B, LOW);
}
analogWrite(PWM_DROIT, RFdata[1]);
// A modifier et compléter selon usage du robot
RFdata[3] & 1<<6 ? digitalWrite(BOUTON_A, HIGH) : digitalWrite(BOUTON_A, LOW);
RFdata[3] & 1<<5 ? digitalWrite(BOUTON_B, HIGH) : digitalWrite(BOUTON_B, LOW);
RFdata[3] & 1<<4 ? digitalWrite(BOUTON_C, HIGH) : digitalWrite(BOUTON_C, LOW);
RFdata[3] & 1<<3 ? digitalWrite(BOUTON_D, HIGH) : digitalWrite(BOUTON_D, LOW);
RFdata[3] & 1<<2 ? digitalWrite(BOUTON_E, HIGH) : digitalWrite(BOUTON_E, LOW);
RFdata[3] & 1<<1 ? digitalWrite(BOUTON_F, HIGH) : digitalWrite(BOUTON_F, LOW);
RFdata[3] & 1<<0 ? digitalWrite(BOUTON_POT, HIGH) : digitalWrite(BOUTON_POT, LOW);
// fin de code à modifier
}
tempsMort < 100 ? digitalWrite(TEMOIN_RF, HIGH) : digitalWrite(TEMOIN_RF, LOW); // on fixe un délai maxi entre deux trames (en ms) pour maintenir le témoin de signal allumé
if (tempsMort > 500 && !ArretUrgence) { // on fixe un délai maxi entre deux trames (en ms) avant d'enclencher l'arrêt d'urgence
ArretUrgence = true;
if (parler)
Serial.println("Arret d'urgence");
digitalWrite(PONT_GAUCHE_A, LOW); // on bloque les ponts en H des moteurs
digitalWrite(PONT_GAUCHE_B, LOW);
digitalWrite(PONT_DROIT_A, LOW);
digitalWrite(PONT_DROIT_B, LOW);
digitalWrite(PWM_GAUCHE, LOW);
digitalWrite(PWM_DROIT, LOW);
// Ajouter ici le code à exécuter lors d'un arrêt d'urgence
}
}
Vos ajouts, suggestions et retours d'expériences sont les bienvenus.
@+ Magnéto
