Après plusieurs micro-ateliers, le pilotage du Youpi c’est étoffé.

  • Une carte Arduino 2560 pilote maintenant le robot pour intégrer la gestion des capteurs. (le plan du cable est décrit dans le programme)
  • Un vieux clavier (PS2) permet désormais de sélectionner : le mouvement, son sens, sa vitesse et marche/arrêt.
  • Un écran LCD 2×16 caractères, pilotable par une liaison I2C, permet de visualiser le tout, de plus une page spéciale permet de connaître l’état des capteurs (très capricieux…).

Pour éviter de casser le Youpi (vécu en testant le capteur poignet, en mouvement…) , la prochaine étape serait d’ajouter des capteurs fin de courses et d’améliorer la détection des capteurs déjà présents. (à suivre… 😉 )

robot youpi robot youpi robot youpi robot youpi

/*
  Programme de base pour controler un robot Youpi avec une Arduino 2560
  Le 10/05/2015
  AlGau
  http://calvix.org
  
  Voir aussi : http://youpi.forler.ch/
 */
 
  
#include <PS2Keyboard.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

/*
 Youpi
*/
// déclaration des n° de sorties utilisées
// broches arduino		broches DB25 femelle coté robot
int bit_01 = 36;	//			21
int bit_02 = 34;	//			09
int bit_03 = 32;	//			22
int bit_04 = 30;	//			10
int bit_05 = 28;	//			23							
int bit_06 = 26;	//			11
int bit_07 = 24;	//			24
int bit_08 = 22;	//			12
// déclaration des entrées utilisées
int capteur_01 = 52;	// base			25
int capteur_02 = 50;  	// épaule		14
int capteur_03 = 48;  	// coude		15
int capteur_04 = 46;  	// poignet		16
int capteur_05 = 44;  	// rotation pince	17
int capteur_06 = 42;  	// fermeture pince	18
//				masse arduino	01

boolean marche = false;     //  false = arret moteur ; true = marche moteur
int mouvement = 0b00000000;            // xxxxx000 = base ; xxxxx001 = épaule ; xxxxx010 = coude ; xxxxx011 = poignet ; xxxxx100 = rotation poignet ; xxxxx101 = pince
int sens_mouvement = 0b00000000;         // xxxxxxx0 = base ; xxxxxx0x = épaule ; xxxxx0xx = coude ; xxxx0xxx = poignet ; xxx0xxxx = rotation poignet ; xx0xxxxx = pince
int vitesse[] = {5, 1, 1, 1, 1, 1};

char* myStrings[]={"Base", "Epaule", "Coude", "Poignet", "Rot. pince","Fermeture"};

boolean etat_capteurs = false;  // true  = affiche l'état des capteurs

int capteurs = 0b00000000;
int mem_capteurs = 0b00000000;

// délai entre l'envoi de deux octets
int delai = 5;
/*******************************************/


// LCD
// le SDA sur A4 sur Dueminalove
// le SCL sur A5 sur Dueminalove
// le SDA sur 20 sur Méga 2560
// le SCL sur 21 sur Méga 2560
// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x20, 16, 2);


// clavier
const int DataPin = 4;
const int IRQpin =  3;

PS2Keyboard keyboard;


/*
  Initialisation
*/
void setup() {
  // afficheur LCD
  lcd.begin();

  // console série
  Serial.begin(9600);

  // clavier
  keyboard.begin(DataPin, IRQpin);
  
  // initialisation des sorties pour le robot Youpi
  pinMode(bit_01, OUTPUT);
  pinMode(bit_02, OUTPUT);
  pinMode(bit_03, OUTPUT);
  pinMode(bit_04, OUTPUT);
  pinMode(bit_05, OUTPUT);
  pinMode(bit_06, OUTPUT);
  pinMode(bit_07, OUTPUT);
  pinMode(bit_08, OUTPUT);
  // initialisation des entrées pour les capteurs du robot Youpi  
  pinMode(capteur_01, INPUT);  // base
  pinMode(capteur_02, INPUT);  // épaule
  pinMode(capteur_03, INPUT);  // coude
  pinMode(capteur_04, INPUT);  // poignet
  pinMode(capteur_05, INPUT);  // rotation pince
  pinMode(capteur_06, INPUT);  // fermeture pince

  initialise_Youpi();
}

/*
  Initialisation du Youpi
*/
void initialise_Youpi(void) {
// initialisation
    envoie_code(0b01000111);  // 47h
    envoie_code(0b00000000);  // 00h
// sens de rotation des moteurs
    envoie_code(0b10000000);  // 80h
    envoie_code(0b00000000);  // 00h
   
// affichage   
  lcd.clear();      // Clear the screen
  lcd.printstr("Youpi controle"); 
  lcd.setCursor(0, 1);  
  lcd.printstr("AlGau - mai 2015");  
  Serial.println("Youpi controle");    
  Serial.println("AlGau - mai 2015");
}


/*
  active les broches du port //
*/
void envoie_code(int code) {
    if (code & 1)     {  digitalWrite(bit_01, HIGH); } else {  digitalWrite(bit_01, LOW); }
    if (code & 2)     {  digitalWrite(bit_02, HIGH); } else {  digitalWrite(bit_02, LOW); }
    if (code & 4)     {  digitalWrite(bit_03, HIGH); } else {  digitalWrite(bit_03, LOW); }
    if (code & 8)     {  digitalWrite(bit_04, HIGH); } else {  digitalWrite(bit_04, LOW); }
    if (code & 16)   {  digitalWrite(bit_05, HIGH); } else {  digitalWrite(bit_05, LOW); }
    if (code & 32)   {  digitalWrite(bit_06, HIGH); } else {  digitalWrite(bit_06, LOW); }
    if (code & 64)   {  digitalWrite(bit_07, HIGH); } else {  digitalWrite(bit_07, LOW); }
    if (code & 128) {  digitalWrite(bit_08, HIGH); } else {  digitalWrite(bit_08, LOW); }
    delay(vitesse[mouvement]);    // temps d'attente pour régler la vitesse de déplacement
}

/*
  modifie le sens de rotation des moteurs
*/
void sens_rotation(void) {
  if (mouvement  == 0b00000000) {
    sens_mouvement = sens_mouvement ^ 0b00000001;
  } else if (mouvement  == 0b00000001) {
    sens_mouvement = sens_mouvement ^ 0b00000010;
  } else if (mouvement  == 0b00000010) {
    sens_mouvement = sens_mouvement ^ 0b00000100;
  } else if (mouvement  == 0b00000011) {
    sens_mouvement = sens_mouvement ^ 0b00001000;
  } else if (mouvement  == 0b00000100) {
    sens_mouvement = sens_mouvement ^ 0b00010000;
  } else if (mouvement  == 0b00000101) {
    sens_mouvement = sens_mouvement ^ 0b00100000;
  }

    envoie_code(sens_mouvement + 0b10000000);
    envoie_code(sens_mouvement);
    
    delay(100);
}

/*
  Affiche l'etat du robot
*/
void affiche_etat(void) {
  boolean sens = false; 
  
  lcd.clear(); 
  Serial.println(myStrings[mouvement]);
  lcd.printstr(myStrings[mouvement]);  
  lcd.setCursor(11, 0);
  if (marche == true){
    lcd.printstr("Start");    
    Serial.print("Start");
  } else {
    lcd.printstr("Stop");    
    Serial.print("Stop");
  }
  Serial.print("Vitesse : ");
  Serial.println(21 - vitesse[mouvement], DEC);
  lcd.setCursor(0, 1);  
  lcd.printstr("Vit. ");
  lcd.print(21 - vitesse[mouvement]);  
  Serial.print("Sens : ");  
  lcd.setCursor(8, 1);
  lcd.printstr("Sens ");

  if (mouvement  == 0b00000000) {
    sens = sens_mouvement & 0b00000001;
  } else if (mouvement  == 0b00000001) {
    sens = sens_mouvement & 0b00000010;
  } else if (mouvement  == 0b00000010) {
    sens = sens_mouvement & 0b00000100;
  } else if (mouvement  == 0b00000011) {
    sens = sens_mouvement & 0b00001000;
  } else if (mouvement  == 0b00000100) {
    sens = sens_mouvement & 0b00010000;
  } else if (mouvement  == 0b00000101) {
    sens = sens_mouvement & 0b00100000;
  }

  lcd.setCursor(13, 1);

  if (sens) {
    Serial.print("horaire + >");  
    lcd.printstr("+ >");    
  } else {
    Serial.print("anti horaire < -");  
    lcd.printstr("< -");    
  }
 
}


/*
  Affiche l'etat des capteurs du robot
*/
void affiche_etat_capteurs(void) {
  int val=0;

  val=digitalRead(capteur_01);
  if (val==HIGH) {  capteurs |= 0b00000001; } else { if (capteurs & 0b00000001) {capteurs ^= 0b00000001; }; }
  val=digitalRead(capteur_02);
  if (val==HIGH) {  capteurs |= 0b00000010; } else { if (capteurs & 0b00000010) {capteurs ^= 0b00000010; }; }
  val=digitalRead(capteur_03);
  if (val==HIGH) {  capteurs |= 0b00000100; } else { if (capteurs & 0b00000100) {capteurs ^= 0b00000100; }; }
  val=digitalRead(capteur_04);
  if (val==HIGH) {  capteurs |= 0b00001000; } else { if (capteurs & 0b00001000) {capteurs ^= 0b00001000; }; }
  val=digitalRead(capteur_05);
  if (val==HIGH) {  capteurs |= 0b00010000; } else { if (capteurs & 0b00010000) {capteurs ^= 0b00010000; }; }
  val=digitalRead(capteur_06);
  if (val==HIGH) {  capteurs |= 0b00100000; } else { if (capteurs & 0b00100000) {capteurs ^= 0b00100000; }; }

  if (capteurs != mem_capteurs) {      // on affiche que s'il y a des changements
    mem_capteurs = capteurs;
    lcd.clear(); 
    lcd.printstr("Ent. 1 2 3 4 5 6");      
    lcd.setCursor(5, 1);
    if (capteurs & 0b00000001) { lcd.printstr("1 ");  } else { lcd.printstr("0 "); }
    if (capteurs & 0b00000010) { lcd.printstr("1 ");  } else { lcd.printstr("0 "); }
    if (capteurs & 0b00000100) { lcd.printstr("1 ");  } else { lcd.printstr("0 "); }
    if (capteurs & 0b00001000) { lcd.printstr("1 ");  } else { lcd.printstr("0 "); }
    if (capteurs & 0b00010000) { lcd.printstr("1 ");  } else { lcd.printstr("0 "); }
    if (capteurs & 0b00100000) { lcd.printstr("1 ");  } else { lcd.printstr("0 "); }    
  }
}


/*
 boucle principale
*/
void loop() {
  if (keyboard.available()) {
    
    // read the next key
    char c = keyboard.read();
    Serial.println(c);
    
    // check for some of the special keys
    if (c == 's') {
      if (marche == false) {
        marche = true; 
      } else {
        marche = false;
      }
    }else if (c == 'i') {
      sens_rotation();                        // inversion du sens de rotation
    }else if (c == 'b') {
      mouvement = 0b00000000;    // binaire 000, Base
    }else if (c == 'e') {
      mouvement = 0b00000001;    // binaire 001, Epaule
    }else if (c == 'c') {
      mouvement = 0b00000010;    // binaire 010, Coude
    }else if (c == 'p') {
      mouvement = 0b00000011;    // binaire 011, Poignet
    }else if (c == 'r') {
      mouvement = 0b00000100;    // binaire 100, Rotation pince
    }else if (c == 'f') {
      mouvement = 0b00000101;    // binaire 101, Fermeture pince
    } else if (c == '-') {
      vitesse[mouvement] += 1;
      if (vitesse[mouvement] > 20) { vitesse[mouvement] = 20; }
    }else if (c == '+') {
      vitesse[mouvement] -= 1;
      if (vitesse[mouvement] < 1) { vitesse[mouvement] = 1; }
    }else if (c == 'h') {
       if (etat_capteurs) { etat_capteurs = false; } else { etat_capteurs = true; mem_capteurs = 0b10000000;}  // change la mémoire capteur pour le premier affichage sans changement d'état
    }else {
      // otherwise, just print all normal characters
      //Serial.println(c);
    }
    if (!etat_capteurs) { affiche_etat();  }    // affiche l'état du robot
  }
  // commande des moteurs
  if (marche == true){
    envoie_code(mouvement + 0b01000000);
    envoie_code(mouvement);
  }
  if (etat_capteurs) { affiche_etat_capteurs(); }
}