Télécommande Robot Arduino+joystick Funduino, vers. Magnéto

Que ce soit pour se déplacer, pour attaquer ou se défendre, il est nécessaire de contrôler les moteurs et autres actionneurs de votre robot. Pour cela, il est nécessaire de recourir à un minimum d’électronique.
Pour réagir aux différentes commandes, le robot peut donc embarquer un ou plusieurs microcontrôleurs. Dans ce cas, un peu de programmation est nécessaire pour s’assurer du bon comportement du robot.
Le règlement impose que les robots soient pilotés par un ou plusieurs membres de l’équipe. La transmission sera exclusivement de type sans fil. Il est possible d’utiliser des modules semblables au modélisme ou de hacker un autre système comme une manette de console de jeux.

Modérateur : Guillaume

Magnéto
Messages : 2
Inscription : 07 mars 2017, 14:46

Télécommande Robot Arduino+joystick Funduino, vers. Magnéto

Message par Magnéto » 09 mars 2017, 21:26

Bonjour,

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
}

Le code coté robot:

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

Je l'ai testé à blanc mais pas encore sur robot.

Vos ajouts, suggestions et retours d'expériences sont les bienvenus.

@+ Magnéto
Pièces jointes
robot_rx_joystick.ino
(6.81 Kio) Téléchargé 381 fois
robot_tx_joystick.ino
(6.64 Kio) Téléchargé 374 fois

Avatar de l’utilisateur
qiko68
Messages : 154
Inscription : 02 janv. 2015, 14:11
Localisation : Didenheim

Re: Télécommande Robot Arduino+joystick Funduino, vers. Magn

Message par qiko68 » 12 mars 2017, 13:57

je vais voir pour le tester cette semaine sur un robot
- Christophe -

Avatar de l’utilisateur
qiko68
Messages : 154
Inscription : 02 janv. 2015, 14:11
Localisation : Didenheim

Re: Télécommande Robot Arduino+joystick Funduino, vers. Magn

Message par qiko68 » 12 mars 2017, 21:52

Christophe, à la ligne :

Code : Tout sélectionner

const byte pipe[5] = {0xE8,0xE8,0x12,0x34,0x56}; // choisir un canal logique unique pour chaque robot
si je comprend bien, on choisi l'un des 5 canal ?

Mais que ce passe-t-il si plusieurs personnes prennent le même canal logique ?
ne serait-il pas plus sécurisé de prévoir une fonction de type appairage entre robot et émetteur ? via une jonction filaire entre les 2 par exemple ? (du moins le temps de faire un appairage)
il y a que 5 canaux ? ou il peut y en avoir plusieurs?
Il faudra faire systématiquement des vérifications de sécurité pour éviter les accidents par brouillage ou le déclenchement accidentel d'un robot qui serait aux stands
- Christophe -

Magnéto
Messages : 2
Inscription : 07 mars 2017, 14:46

Re: Télécommande Robot Arduino+joystick Funduino, vers. Magn

Message par Magnéto » 14 mars 2017, 11:22

Attention il s'agit là du canal logique! A ne pas confondre avec le canal de fréquence défini par la fonction "radio.setChannel(canal);", initialisée au début du programme.

Code : Tout sélectionner

const byte canal = 102;      // choisir un canal d'émission radio (entre 100 et 125, au dessus des canaux wifi)
Pour éviter le brouillage, il est impératif que les deux robots fonctionnent sur une fréquence différente.
Si deux robots utilisent une même fréquence mais un canal logique (pipe) différent, les signaux envoyés via le mauvais pipe seront simplement ignorés. Mais les deux signaux étant sur la même fréquence, ils vont se brouiller mutuellement.
Si deux robots utilisent le même pipe et la même fréquence, ils vont chacun recevoir les signaux destinés aux deux robots. Mais si la directive de choisir un canal logique unique pour chaque robot est respectée (remplacer "123456" par des chiffres au hasard), cela ne ce produira pas.

Code : Tout sélectionner

const byte pipe[5] = {0xE8,0xE8,0x12,0x34,0x56}; // choisir un canal logique unique pour chaque robot
Il n'y a qu'un seul pipe en émission (constitué par les 5 bytes). En réception, il faut utiliser le même pipe (sinon le robot ignore les trames).

Datasheet du "nRF24L01+" pour ceux qui veulent aller plus loin:
https://www.sparkfun.com/datasheets/Com ... n_v1_0.pdf

Répondre