Sourino - Arduino-робот для игр детей и котят


Sourino (от французского — «мышь») — маленький Arduino-робот, изготовленный при помощи 3D-печати, который поможет играть детям и котятам.
В основе робота — контроллер Arduino Nano, и три ультразвуковых датчика расстояния (HC-SR04), необходимых для для автономной навигации робота. В верхней части робота располагается инфракрасный приёмник, предназначенный для дистанционного управления роботом. В движение робот приводится с помощью двух небольших мотор-редукторов, управляемых через драйвер двигателей на L298N.

Код прошивки робота — Sourino_software.ino

#include <IRremote.h>

const char DIN_RECEPTEUR_INFRAROUGE = 2;

IRrecv MonRecepteurInfraRouge(DIN_RECEPTEUR_INFRAROUGE);

decode_results MessageRecu;

// Définition des variables :
String mode = "Stop";
const int vitmax = 100;
const int vitref = 75;
const int vitmin = 55;
int vitesse = vitmin; //La vitesse est réglable de 0 à 1023
int vitesse_int = 0; //La vitesse du moteur intérieur du virage
float ValUSG = 0;
float ValUSM = 0;
float ValUSD = 0;
const int K = 1; // Coefficient pour faire varier la vitesse en fonction de la distance
unsigned long t0 = 0; // temps de début de compteur
unsigned long t = 0; // temps écoulé dans la boucle (chien de garde)
int retour = 0; // return value 0 = Bien passé, 1 = Appui OK, 2 = timeout, 3 = appui #
int vitesse_plus_mt_droit = 0;
int vitesse_moins_mt_droit = 0;
int vitesse_plus_mt_gauche = 0;
int vitesse_moins_mt_gauche = 0;
String action;
const int plus_mt_droit = 9;
const int moins_mt_droit = 10;
const int plus_mt_gauche = 6;
const int moins_mt_gauche = 5;
const float D1 = 75.00;
const float D2 = 50.00;
const float D3 = 20.00;

// Définition des fonctions
float MesureDistUS (String Capteur) {
  int PinTrig;
  int PinEcho;
  long Temps;
  float Distance;
  
  if(Capteur == "G") {
    PinTrig = 3;
    PinEcho = 4;
  }
  else if(Capteur == "M") {
    PinTrig = 7;
    PinEcho = 8;
  }
  else if(Capteur == "D") {
    PinTrig = 11;
    PinEcho = 12;
  }
  do{
    digitalWrite (PinTrig,HIGH);
    delayMicroseconds (10);
    digitalWrite (PinTrig,LOW);
    
    Temps = pulseIn (PinEcho,HIGH);
  }while(Temps == 0);
  Temps = Temps/2;
  Distance = (Temps*340)/10000.0;
  return Distance;
};

int Cas1 () { // Tout droit à fond
  vitesse_plus_mt_droit = vitmax;
  vitesse_moins_mt_droit = 0;
  vitesse_plus_mt_gauche = vitmax;
  vitesse_moins_mt_gauche = 0;
  return 0;
};

int Cas2 (float USM) { // Tout droit en ralentissant proportionnellement à la distance avec l'obstacle 
  float V = K * USM;
  if (V > vitmax) {V = vitmax;}
  if (V < vitmin) {V = vitmin;}
  vitesse_plus_mt_droit = V;
  vitesse_moins_mt_droit = 0;
  vitesse_plus_mt_gauche = V;
  vitesse_moins_mt_gauche = 0;
  return 0;
};

int Cas3 () { // Tourne légèrement à gauche
  vitesse_plus_mt_droit = vitmax;
  vitesse_moins_mt_droit = 0;
  vitesse_plus_mt_gauche = vitref;
  vitesse_moins_mt_gauche = 0;
  return 0;
};

int Cas4 () { // Tourne légèrement à droite
  vitesse_plus_mt_droit = vitref;
  vitesse_moins_mt_droit = 0;
  vitesse_plus_mt_gauche = vitmax;
  vitesse_moins_mt_gauche = 0;
  return 0;
};

int Cas5 (float USG, float USD) { // Tourne de façon à passer entre les obstacles
  // Vitesse droite proportionelle à distance obstacle gauche et inversement
  float vG = K * USD;
  if (vG > vitmax) {vG = vitmax;}
  if (vG < vitmin) {vG = vitmin;}
  float vD = K * USG;
  if (vD > vitmax) {vD = vitmax;}
  if (vD < vitmin) {vD = vitmin;}
  vitesse_plus_mt_droit = vD;
  vitesse_moins_mt_droit = 0;
  vitesse_plus_mt_gauche = vG;
  vitesse_moins_mt_gauche = 0;
  return 0;
};

int Cas6 (float USG, float USM, float USD) { // Avance et tourne en s'éloignant de l'obstacle le plus proche
  vitesse_moins_mt_droit = 0;
  vitesse_moins_mt_gauche = 0;
  float V = K * USM;
  if (V > vitmax) {V = vitmax;}
  if (V < vitmin) {V = vitmin;}
  if (USG < USD) {
    if (V * 0.8 < vitmin) {
      vitesse_plus_mt_droit = 0;
    }
    else {
      vitesse_plus_mt_droit = V * 0.8;
    }
    vitesse_plus_mt_gauche = V;
  }
  else {
    if (V * 0.8 < vitmin) {
      vitesse_plus_mt_gauche = 0;
    }
    else {
      vitesse_plus_mt_gauche = V * 0.8;
    }
    vitesse_plus_mt_droit = V;
  }
  return 0;
};

int Cas7 () { // Tourne sur place vers la droite jusqu'a ce que l'obstacle est disparut ou qu'un obstacle apparaissent à droite
  vitesse_plus_mt_droit = 0;
  vitesse_moins_mt_droit = vitmin;
  vitesse_plus_mt_gauche = vitmin;
  vitesse_moins_mt_gauche = 0;
  return 0;
};

int Cas8 () { // Tourne sur place vers la gauche jusqu'a ce que l'obstacle est disparut ou qu'un obstacle apparaissent à gauche
  vitesse_plus_mt_droit = vitmin;
  vitesse_moins_mt_droit = 0;
  vitesse_plus_mt_gauche = 0;
  vitesse_moins_mt_gauche = vitmin;
  return 0;
};

int Cas9 (float USG, float USD) { // Tourne vers là ou l'obstacle est le plus loin
  if(USG > USD) {
    vitesse_plus_mt_droit = vitmin;
    vitesse_moins_mt_droit = 0;
    vitesse_plus_mt_gauche = 0;
    vitesse_moins_mt_gauche = vitmin;
  }
  else {
    vitesse_plus_mt_droit = 0;
    vitesse_moins_mt_droit = vitmin;
    vitesse_plus_mt_gauche = vitmin;
    vitesse_moins_mt_gauche = 0;
  }
  return 0;
};

int Cas10 () { // Souris coincée dans un cul de sac ou dans un angle
  t0 = millis();
  do{
    ValUSG = MesureDistUS("G");
    ValUSD = MesureDistUS("D");
    if(ValUSG > ValUSD) {
      vitesse_plus_mt_droit = vitmin;
      vitesse_moins_mt_droit = 0;
      vitesse_plus_mt_gauche = 0;
      vitesse_moins_mt_gauche = vitmin;
    }
    else {
      vitesse_plus_mt_droit = 0;
      vitesse_moins_mt_droit = vitmin;
      vitesse_plus_mt_gauche = vitmin;
      vitesse_moins_mt_gauche = 0;
    }
    analogWrite(plus_mt_droit, vitesse_plus_mt_droit);
    analogWrite(moins_mt_droit, vitesse_moins_mt_droit);
    analogWrite(plus_mt_gauche, vitesse_plus_mt_gauche);
    analogWrite(moins_mt_gauche, vitesse_moins_mt_gauche);
          
    t = millis() - t0;
    if(t >= 10000) {
      return 2;
      break;
      
    }
    if (MonRecepteurInfraRouge.decode(&MessageRecu)) {
      if (MessageRecu.value == 0xFF38C7) {
        break;
      }
    }
  }while(true == true);
  if(t <= 10000) {
    return 1;
  }
}

String prog_US () {
  String toto = "Auto";
  Serial.println ("En mode Auto");
  do {
    t0 = millis();
    retour = 0;

    ValUSG = MesureDistUS("G");
    ValUSM = MesureDistUS("M");
    ValUSD = MesureDistUS("D");

    if(ValUSG > D2 && ValUSM > D1 && ValUSD > D2)
    {
      retour = Cas1();
    }
    else if(ValUSG > D2 && (ValUSM < D1 && ValUSM > D3) && ValUSD > D2)
    {
      retour = Cas2(ValUSM);
    }
    else if(ValUSG > D2 && ValUSM > D3 && (ValUSD > D3 && ValUSD < D2))
    {
      retour = Cas3();
    }
    else if((ValUSG < D2 && ValUSG > D3) && ValUSM > D3 && ValUSD > D2)
    {
      retour = Cas4();
    }
    else if((ValUSG < D2 && ValUSG > D3) && ValUSM > D1 && (ValUSD > D3 && ValUSD < D2))
    {
      retour = Cas5(ValUSG, ValUSD);
    }
    else if((ValUSG < D2 && ValUSG > D3) && (ValUSM < D1 && ValUSM > D3) && (ValUSD < D2 && ValUSD > D3))
    {
      retour = Cas6(ValUSG, ValUSM, ValUSD);
    }
    else if(ValUSG < D3 && ValUSM > D3 && ValUSD > D3)
    {
      retour = Cas7();
    }
    else if(ValUSG > D3 && ValUSM > D3 && ValUSD < D3)
    {
      retour = Cas8();
    }
    else if(ValUSG > D3 && ValUSM < D3 && ValUSD > D3)
    {
      retour = Cas9(ValUSG, ValUSD);
    }
    else if(ValUSG < D3 && ValUSM < D3 && ValUSD < D3)
    {
      retour = Cas10();
    }
    /*else //Cas non prévu arret immédiat + msg d'erreur
    {
      analogWrite(plus_mt_droit, 0);
      analogWrite(moins_mt_droit, 0);
      analogWrite(plus_mt_gauche, 0);
      analogWrite(moins_mt_gauche, 0);
      Serial.println ("Cas non prévu");
      break;
    }*/
    analogWrite(plus_mt_droit, vitesse_plus_mt_droit);
    analogWrite(moins_mt_droit, vitesse_moins_mt_droit);
    analogWrite(plus_mt_gauche, vitesse_plus_mt_gauche);
    analogWrite(moins_mt_gauche, vitesse_moins_mt_gauche);
    
    if (retour == 2) { // time-out du cas 10
      toto = "Stop";
    }
      
    if (MonRecepteurInfraRouge.decode(&MessageRecu)) {
      if (MessageRecu.value == 0xFF38C7) {  //OK
        toto ="Stop";
      }
      if (MessageRecu.value == 0xFFB04F) {  //#
        toto ="Tele";
      }
      MonRecepteurInfraRouge.resume();
    }
      
  }while(toto == "Auto");
  analogWrite(plus_mt_droit, 0);
  analogWrite(moins_mt_droit, 0);
  analogWrite(plus_mt_gauche, 0);
  analogWrite(moins_mt_gauche, 0);
  return toto;
}


String prog_IR () {
  String toto = "Tele";
  Serial.println ("En mode Tele");
  do {
      if (MonRecepteurInfraRouge.decode(&MessageRecu)) {
        if (MessageRecu.value == 0xFF18E7) // Si la flèche du haut à été pressée    //
          {                                                                           //
            vitesse_plus_mt_droit = vitesse;                                          //
            vitesse_moins_mt_droit = 0;                                               //
            vitesse_plus_mt_gauche = vitesse;                                         //
            vitesse_moins_mt_gauche = 0;                                              //
            delay(500);                                                               //
            action = "avance";                                                        //
          }                                                                           //
                                                                                //
          if (MessageRecu.value == 0xFF10EF) // Si la flèche de gauche à été pressée  //
          {                                                                           //
            vitesse_plus_mt_droit = vitesse;                                          //
            vitesse_moins_mt_droit = 0;                                               //
            vitesse_plus_mt_gauche = vitesse_int;                                     //
            vitesse_moins_mt_gauche = 0;                                              //
            delay(500);                                                               // Controle de la direction en fonction du bouton préssé
            action = "gauche";                                                        //
          }                                                                           //
                                                                                //
          if (MessageRecu.value == 0xFF5AA5) // Si la flèche de droite à été pressée  //
          {                                                                           //
            vitesse_plus_mt_droit = vitesse_int;                                      //
            vitesse_moins_mt_droit = 0;                                               //
            vitesse_plus_mt_gauche = vitesse;                                         //
            vitesse_moins_mt_gauche = 0;                                              //
            delay(500);                                                               //
            action = "droite";                                                        //
          }                                                                           //
                                                                                //
          if (MessageRecu.value == 0xFF4AB5) // Si la flèche du bas à été pressée     //
          {                                                                           //
            vitesse_plus_mt_droit = 0;                                                //
            vitesse_moins_mt_droit = vitesse;                                         //
            vitesse_plus_mt_gauche = 0;                                               //
            vitesse_moins_mt_gauche = vitesse;                                        //
            delay(500);                                                               //
            action = "arriere";                                                       //
          }                                                                           //
                                                                                //
          if (MessageRecu.value == 0xFF38C7) // Si OK à été pressé                    //
          {                                                                           //
            vitesse_plus_mt_droit = 0;                                                //
            vitesse_moins_mt_droit = 0;                                               //
            vitesse_plus_mt_gauche = 0;                                               //
            vitesse_moins_mt_gauche = 0;                                              //
            action = "stop";                                                          //
          }                                                                           //

          if (MessageRecu.value == 0xFFA25D) // Si le bouton 1 à été préssé //
          {                                                                 //
            vitesse = vitmin;                                               //
            vitesse_int = 0;                                                //
                                                                      //
            if (action == "avance")                                         //
            {                                                               //
              vitesse_plus_mt_droit = vitesse;                              //
              vitesse_moins_mt_droit = 0;                                   //
              vitesse_plus_mt_gauche = vitesse;                             //
              vitesse_moins_mt_gauche = 0;                                  //
            }                                                               //
                                                                      // 
            else if (action == "droite")                                    //
            {                                                               //
              vitesse_plus_mt_droit = vitesse_int;                          //
              vitesse_moins_mt_droit = 0;                                   //
              vitesse_plus_mt_gauche = vitesse;                             //
              vitesse_moins_mt_gauche = 0;                                  //
            }                                                               //
                                                                      //         
            else if (action == "gauche")                                    //
            {                                                               //
              vitesse_plus_mt_droit = vitesse;                              //
              vitesse_moins_mt_droit = 0;                                   //
              vitesse_plus_mt_gauche = vitesse_int;                         //
              vitesse_moins_mt_gauche = 0;                                  //
            }                                                               //
                                                                      //
            else if (action == "arriere")                                   //
            {                                                               //
              vitesse_plus_mt_droit = 0;                                    //
              vitesse_moins_mt_droit = vitesse;                             //
              vitesse_plus_mt_gauche = 0;                                   //
              vitesse_moins_mt_gauche = vitesse;                            //
            }                                                               //
        }                                                                 //
                                                                      //
        if (MessageRecu.value == 0xFF629D) // Si le bouton 2 à été préssé //
        {                                                                 //
          vitesse = vitref;                                               // Réglage des vitesses en fonction du bouton préssé (1,2 ou 3)
          vitesse_int = 0;                                                //
                                                                      //
          if (action == "avance")                                         //
          {                                                               //
            vitesse_plus_mt_droit = vitesse;                              //
            vitesse_moins_mt_droit = 0;                                   //
            vitesse_plus_mt_gauche = vitesse;                             //
            vitesse_moins_mt_gauche = 0;                                  //
          }                                                               //
                                                                      //
          else if (action == "droite")                                    //
          {                                                               //
            vitesse_plus_mt_droit = vitesse_int;                          //
            vitesse_moins_mt_droit = 0;                                   //
            vitesse_plus_mt_gauche = vitesse;                             //
            vitesse_moins_mt_gauche = 0;                                  //
          }                                                               //
                                                                      //
          else if (action == "gauche")                                    //
          {                                                               //
            vitesse_plus_mt_droit = vitesse;                              //
            vitesse_moins_mt_droit = 0;                                   //
            vitesse_plus_mt_gauche = vitesse_int;                         //
            vitesse_moins_mt_gauche = 0;                                  //
          }                                                               //
                                                                      //
          else if (action == "arriere")                                   //
          {                                                               //
            vitesse_plus_mt_droit = 0;                                    //
            vitesse_moins_mt_droit = vitesse;                             //
            vitesse_plus_mt_gauche = 0;                                   //
            vitesse_moins_mt_gauche = vitesse;                            //
          }                                                               //
        }                                                                 //
                                                                      //
        if (MessageRecu.value == 0xFFE21D) // Si le bouton 3 à été préssé //
        {                                                                 //
          vitesse = vitmax;                                               //
          vitesse_int = 50;                                               //
                                                                      //
          if (action == "avance")                                         //
          {                                                               //
            vitesse_plus_mt_droit = vitesse;                              //
            vitesse_moins_mt_droit = 0;                                   //
            vitesse_plus_mt_gauche = vitesse;                             //
            vitesse_moins_mt_gauche = 0;                                  //
          }                                                               //
                                                                      //
          else if (action == "droite")                                    //
          {                                                               //
            vitesse_plus_mt_droit = vitesse_int;                          //
            vitesse_moins_mt_droit = 0;                                   //
            vitesse_plus_mt_gauche = vitesse;                             //
            vitesse_moins_mt_gauche = 0;                                  //
          }                                                               //
                                                                      //
          else if (action == "gauche")                                    //
          {                                                               //
            vitesse_plus_mt_droit = vitesse;                              //
            vitesse_moins_mt_droit = 0;                                   //
            vitesse_plus_mt_gauche = vitesse_int;                         //
            vitesse_moins_mt_gauche = 0;                                  //
          }                                                               //
                                                                      //
          else if (action == "arriere")                                   //
          {                                                               //
            vitesse_plus_mt_droit = 0;                                    //
            vitesse_moins_mt_droit = vitesse;                             //
            vitesse_plus_mt_gauche = 0;                                   //
            vitesse_moins_mt_gauche = vitesse;                            //
          }                                                               //
        }
        if (MessageRecu.value == 0xFF6897) {  //*
          toto ="Auto";
        }
        MonRecepteurInfraRouge.resume();
      }
    analogWrite(plus_mt_droit, vitesse_plus_mt_droit);
    analogWrite(moins_mt_droit, vitesse_moins_mt_droit);
    analogWrite(plus_mt_gauche, vitesse_plus_mt_gauche);
    analogWrite(moins_mt_gauche, vitesse_moins_mt_gauche);
    
  }while(toto == "Tele");
  analogWrite(plus_mt_droit, 0);
  analogWrite(moins_mt_droit, 0);
  analogWrite(plus_mt_gauche, 0);
  analogWrite(moins_mt_gauche, 0);
  return toto;
}


// Définition des fonctions :
void setup() {
  // Début du setup...
  // ...pour tout...
  MonRecepteurInfraRouge.enableIRIn();
  pinMode (plus_mt_droit, OUTPUT);
  pinMode (moins_mt_droit, OUTPUT);
  pinMode (plus_mt_gauche, OUTPUT);
  pinMode (moins_mt_gauche, OUTPUT);
  // ...et pour l'automatisation
  pinMode (3, OUTPUT);
  pinMode (4, INPUT);
  pinMode (7, OUTPUT);
  pinMode (8, INPUT);
  pinMode (11, OUTPUT);
  pinMode (12, INPUT);  
  Serial.begin (9600);
}

void loop() {
if (mode == "Tele") { // Si il faut passer en mode télécommandé
  vitesse_plus_mt_droit = 0;
  vitesse_moins_mt_droit = 0;
  vitesse_plus_mt_gauche = 0;
  vitesse_moins_mt_gauche = 0;  
  mode = prog_IR();
  }
  else if (mode == "Auto") { // Si il faut passer en autonome
    mode = prog_US();
  }
  else if (mode == "Stop") { // Si il faut passer en stop
    Serial.println ("STOP !!!");
    analogWrite(plus_mt_droit, 0);
    analogWrite(moins_mt_droit, 0);
    analogWrite(plus_mt_gauche, 0);
    analogWrite(moins_mt_gauche, 0);
  }
  if (MonRecepteurInfraRouge.decode(&MessageRecu)) {
    if (MessageRecu.value == 0xFFB04F) { // #
      mode ="Tele";    }
    if (MessageRecu.value == 0xFF6897) { // *
      mode = "Auto";
    }
    MonRecepteurInfraRouge.resume();
  }  
}


Ссылки
Sourino – the Best Toy for Cats and Kids

По теме
GoodBoy — робот-собака на Arduino
Свой spotmini для 3D-печати
Доступный Arduino-совместимый робот для рисования
LogoBot — простой робот для изготовления при помощи 3D-печати
Изготовление виброботов при помощи 3D-печати
PANDA — самодельный робот-чистильщик на базе Arduino и 3D-печати
Простой Arduino-робот на одном моторе и сервомашинке
  • 0
  • 2 сентября 2020, 09:46
  • admin

Комментарии (0)

RSS свернуть / развернуть

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.