Robot BotPaP V2 (avec tête mobile)

Publié le 3 décembre 2012 par ludovic

Voici la version 2 du robot. Cette version intégre 3 modifications :

Un ajout sur le chassis permettant aux roues arrières de ne pas se bloquer devant un obstacle non vue ;
Un servo moteur anime la rotation gauche droite de la tête (capteur sharp télémètre infra rouge) ;
Un joystick servant pour l'heure d'interface homme / machine mais pourra être utilisé comme détecteur de choc. Cette modification n'est pas indispensable.

Constitution :

- 2 moteurs pas à pas (genre ceux que l'on trouve dans une imprimante jet d'encre ou photocopieur ou machine à affranchir le courrier) ;

- 2 cicuits ULN2803 (ampli 500mA) pour l'alimenation des pàp + 8 résistances 1k ohms ;

- 2 roues récupérées d'une imprimante qui servaient de guides ;

- 1 platine arduino nano ;

- 1 télémètre sharp analogique (2cm à 30cm) ;

- 1 chassis fait de chute d'epoxy et de plastique ;

- 1 roulette de placard (rayon brico) ;

- 1 servomoteur micro pour l'animation latérale du télémètre ;

- 1 batterie LiPo 12V 550mAh.

FACULTATIF :

- 1 écran LCD 2x16 ;

- 1 joystick récupéré d'un numchuck de manette wii.

Principe de fonctionnement :

Les bibliothèques arduino stepper (moteur pas à pas), liquidcristal, et servo sont utilisées.

Le programme est composé de fonction permettant l'animation des moteurs pàp :

- Roule(valeur). Valeur représente une distance en cm à parcourir ;

- Rota_large(angle). Angle représente l'angle de rotation, seule la roue extérieure est animée ;

- Rota_serree(Angle). Angle représente l'angle de rotation, les 2 roues arrières sont animées en contre sens afi nde faire pivoter le robot sur lui même (pas exactement, puisque une dérive du centre du pivot apparait, loi trigo) ;

- Speed(valeur). Valeur est la vitesse de commutation des pàp.

Une fonction permettant de déterminer la trajectoire :

- Look(). Le servo anime la rotation du télémètre par pas de 20mm à gauche et à droite. Si le télémètre mesure une distance acceptable alors le robot tourne au pas retenu.

Des fonctions diverses :

- Arrondi(valeur_float). Valeur float est une valeur décimale qui arrondi à l'entier supérieur ou inférieur contrairement à la fonction par défaut int() qui arrondi systématiquement à l'entier inférieur ;

- IHM(string, valeur). Permet l'affichage sur LCD sans écrire tout le code à chaque fois.

Des fonctions en cours de développement :

- RtH(). Retour à la maison en reparcourant le chemin emprunté ;

- Clr_parcours(). Permet d'effacer le parcours.

Il y a possibilité de ne pas mettre de servo. La rotation du télémètre se fait alors par une rotation serree du robot (sur lui même). C'est moins pratique et précis puisqu'une dérive apparait. Cette dérive est modélisable (trigonométrie un peu complexe tout de même).

Le code arduino :

Illustration (toujours 2 moteurs pas à pas 7,5°, roue fixe à l'avant) :

#include <Stepper.h>
#include <LiquidCrystal.h>
#include <Servo.h> 
 
#define nstep           48   // step motor
#define IR          0    // analog input
#define STICK_VERT  1    // analog input
#define STICK_HORI  2    // analog input
#define DIST1           80 // distance IR 1° seuil
#define DIST2           160 // distance IR 2° seuil
#define DIST3           300 // distance IR seuil très proche
#define PAS_DEG         1.95 //2 : angle légérement supérieur à 90° pour rota(90). 1,85 angle inférieur à 90°.
#define PASmm           3.351
#define PAS_ROULE   10 // (mm)
#define I_PARCOURS  200
#define INIT_SPEED  35  //mini 30 sinon ca broute
#define MAX_SPEED   70
 
int parcours_roule[I_PARCOURS], parcours_rotation[I_PARCOURS], parcours_rotation_serree[I_PARCOURS];
int signe=1, index_parcours=0, nbpas;
unsigned int  flag_roule=0, distance, carto;
byte Gspeed = 0;
boolean inib_rth = 0;
 
Stepper PaP_G_Av(nstep, 5,4);
Stepper PaP_G_Ar(nstep, 4,5);
Stepper PaP_D_Av(nstep, 2,3);
Stepper PaP_D_Ar(nstep, 3,2);
 
Servo ServoTete;
 
LiquidCrystal lcd(7, 8, 9, 10, 11, 12);


////////////////////////////////////////////////////////////////////////////////////
void setup()
{
  lcd.begin(16, 2);
  ServoTete.attach(6);
  ServoTete.write(90); // en degre, 90 est le milieu (0;180)  
  Clr_Parcours();
  IHM("Parcours clear  ", 0);
  while(analogRead(STICK_VERT) < 700) { ServoTete.write(map(analogRead(STICK_HORI), 47, 965, 0, 180)); IHM("Cap. Infra-Rouge", analogRead(IR)); delay(50); }
  IHM("C'est parti !   ", 0);
  delay(200); 
}
// la boucle while permet le test du servomoteur et du capteur télémètre.


////////////////////////////////////////////////////////////////////////////////
void loop() {
  if (index_parcours < I_PARCOURS) { //mode normal, par défaut roule droit ou tourne en fonction des obstacles
    if      (analogRead(IR) > DIST3) { Roule(-50); }
    else if (analogRead(IR) > DIST1) { Look(); }
    else                                { Gspeed++; Gspeed = constrain (Gspeed, 0, MAX_SPEED); Roule(PAS_ROULE); }
  }
  else if (index_parcours >= I_PARCOURS) { index_parcours = 0; }  // IHM("Retour maison !!", index_parcours); RtH();  // mode retour au point d'origine
  else { IHM("BotPaP est perdu", index_parcours); delay(7000); } // robot perdu
}
 
/////////////////// Fonctions //////////////////////////////
int Arrondi(float nombre_float) {
  float nombre;
  nombre = nombre_float - int(nombre_float);
  if (nombre >= 0.5) { nombre = int(nombre_float) + 1; }
  else { nombre = int(nombre_float); }
  return nombre;
}

void IHM(char str[16], int val) {
  lcd.clear();
  lcd.setCursor(0, 0);
  for (int i = 0; i < 16; i++){
    lcd.print(str[i]);
  }
  lcd.setCursor(0, 1);
  lcd.print(val);
}
void Look() {
  boolean wdt = 0; // watch_dog_time
  unsigned int time1, time = millis();
  carto = 0; signe = 1;
  while (analogRead(IR) > DIST1) {
    carto +=20;
    if (carto >= 90) { signe = -1; carto = 0; }
    ServoTete.write(90 - signe * carto); delay(100);
    time1 = millis()-time;
    if (time1 > 6000) { wdt = 1; break; }
  }
  ServoTete.write(90);
  time = 0; time1 = 0;
  if (wdt == 0) { Rota_large(signe*carto); }
  else { IHM("DEPASSEMENT TIME", time); delay(500); Roule(-70); delay(500);
}
}

void Roule(int Pas_AR) {
  IHM("Roule tt droit  ", index_parcours);
  if (inib_rth == 0) { index_parcours ++; parcours_roule[index_parcours] = Pas_AR; }
  nbpas = Arrondi(Pas_AR/PASmm);
  Speed(INIT_SPEED+Gspeed);
  if (Pas_AR >= 0) {    //avancer
    for (int i=0; i<=nbpas; i++) {
      PaP_G_Av.step(1);
      PaP_D_Av.step(1);
      delay(1);
    }
  }
  else {                //reculer
    IHM("Recule          ", index_parcours);
    Speed(INIT_SPEED-10); delay(100);
    nbpas = abs(nbpas);
    for (int i=0; i<=nbpas; i++) {
    PaP_G_Ar.step(1);
    PaP_D_Ar.step(1);
    delay(1);
    }
  }
}
 
void Rota_large(int angle) {
  IHM("Tourne large :  ", angle);
  Speed(INIT_SPEED-10);
  if (inib_rth == 0) { index_parcours ++; parcours_rotation[index_parcours]
= angle; }
  nbpas = Arrondi(abs(angle)/PAS_DEG);
  if (angle >= 0) { //tourner à gauche
    for (int i=0; i<nbpas; i++) {
      PaP_G_Av.step(1);
    delay(1);
    }
  }
  else {
    nbpas = abs(nbpas);
    for (int i=0; i<nbpas; i++) {
      PaP_D_Av.step(1);
    delay(1);
    }
  }
}
 
void Speed(unsigned int Speed_int) {
  PaP_G_Av.setSpeed(Speed_int);
  PaP_G_Ar.setSpeed(Speed_int);
  PaP_D_Av.setSpeed(Speed_int);
  PaP_D_Ar.setSpeed(Speed_int);
}
 
// option, permet au robot de rentrer à la base (pilotage des pàp en sens inverse)
// le code d'appel ne figure pas dans le code ci dessus
 
void Clr_Parcours() {
  for (int i=0; i<I_PARCOURS; i++) {
  parcours_roule[i]=0; parcours_rotation[i]=0; parcours_rotation_serree[i]=0;
  }
}
 
void RtH() {
  inib_rth = 1; delay(2000);
  Rota_serree(180);
  int index = 0;
  for (int i = 0; i <= index_parcours; i++) {
    index = index_parcours - i;
    if  (parcours_roule[index] != 0) { Roule(parcours_roule[index]); }
    else if (parcours_rotation_serree[index] != 0) { Rota_serree(-parcours_rotation_serree[index]); }
    else if (parcours_rotation[index] != 0) { Rota_large(-parcours_rotation[index]); }
    else { index_parcours = 0; IHM("-----Maison-----", index_parcours); Clr_Parcours(); }
  }
  Rota_serree(180); inib_rth = 0; index_parcours = 0; Clr_Parcours(); delay(4000); 
}
 
void Rota_serree(int angle) {
  IHM("Tourne court :  ", angle);
  Speed(20);
  if (inib_rth == 0) { index_parcours ++; parcours_rotation_serree[index_parcours] = angle; }
  nbpas = Arrondi(abs(angle)*0.278+1.107);
  if (angle >= 0) { //tourner à gauche
    for (int i=0; i<nbpas; i++) {
      PaP_G_Av.step(1);
      PaP_D_Ar.step(1);
      delay(10);
    }
  }
  else {
    nbpas = abs(nbpas);
    for (int i=0; i<nbpas; i++) {
      PaP_G_Ar.step(1);
      PaP_D_Av.step(1);
    delay(10);
    }
  }
}