Робот-копилка

// Пины для подключения дальномера, светодиода, сервопривода//
#define trigPin 9   //
#define echoPin 8   //
#define led 10      //
#define servopin 7  //

//основные параметры
#define chewing 4   // Сколько раз пережёвываем проглоченное
#define blizost 10  // Расстояние, до которого будет реагировать датчик (в сантиметрах)

//состояния открытия рта. Всего их три: закрыт, открыт, приоткрыт. 
//Настраиваются индивидуально под каждый сервопривод (значения от 0 до 180 градусов).
#define mouthClose 90 // Рот закрыт
#define mouthOpen 30  // Рот открыт
#define mouthAjar 70  // Рот приоткрыт

#include <Servo.h>    // Подключаем библиотеку сервопривода
Servo Sergo;        

void setup() {
  Serial.begin(9600);      //Инициализируем последовательный порт чтобы выводить в него расстояния определенное датчиком
  
  pinMode(trigPin, OUTPUT); //
  pinMode(echoPin, INPUT);

  Sergo.attach(servopin);   // в скобках номера пина к которому подключен сервопривод
  sleep();                  // установим сервопривод в исходное положение, т.е. рот закрыт
}

void loop() {
  int duration, distance;
  digitalWrite(trigPin, LOW);     // Для большей точности установим значение LOW на пине Trig
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);    // Теперь установим высокий уровень на пине Trig
  delayMicroseconds(10);          
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH); // Узнаем длительность высокого сигнала на пине Echo
  //Переводим скорость звука из 343 м/с в см/мкс: s = duration/29
  //Поскольку расстояние пройдено дважды, то s = duration / (29*2)

  distance = duration / 58;       // Рассчитываем расстояние в сантиметрах
  Serial.print(distance);         // Выведем значение в Serial Monitor
  Serial.println(" cm");

  if (distance < blizost) {       // Если расстояние до предмета меньше заданного, то проглатываем
    om_nom_nom();
    digitalWrite(led, HIGH);
  }
  delay(100);
}

void om_nom_nom() {
  digitalWrite(led, HIGH);  // Зажигаем лампочкой
  Sergo.attach(servopin);   // Подключаем сервопривод
  delay(300);

  Sergo.write(mouthOpen);          // открываем рот и ждем пока предмет свалится в него
  delay(700);

  // жуем //
  for (int x = 0; x < chewing; x++) {
    Sergo.write(mouthAjar);
    delay(250);
    Sergo.write(mouthOpen);
    delay(250);
    Serial.println("nyam");
  }
  sleep();  //спим
}

void sleep(){
  Sergo.write(mouthClose);
  delay(250);

  Sergo.detach();         // отключаем питание сервопривода, иначе он будет жужжать
  digitalWrite(led, LOW); // гасим светодиод
}