#include <Servo.h>
#include <LMotorShield.h>

LMotorShield lms;
int ledPin = 13;                // Кнопка включения мп3
int ledPin1 = 12;                // кнопка перемотки мп3
int i;
int j;
int p[18]; // массив, здесь будем записывать данные с ИК дальномера в разных его положениях   
int gp,gp1,gp2,g;
int b;
void setup() {
{    
     Serial.begin(9600);
    lms.begin(LMS_SERVOS);
}
pinMode(ledPin,OUTPUT);        
pinMode(ledPin1,OUTPUT);      
pinMode(3,OUTPUT); // Motor A скорость
pinMode(7,OUTPUT); // Motor A направление
pinMode(11,OUTPUT); // Motor B скорость
pinMode(8,OUTPUT); // Motor B направление
pinMode(2,OUTPUT); // Motor A стоп, пуск
pinMode(4,OUTPUT); // Motor B стоп, пуск
}
void Forward() { // Подпрограмма движения робота вперед
digitalWrite(3,HIGH);
digitalWrite(7,LOW);
analogWrite(11,240);// здесь скоростью вращения колеса регулируем прямолинейность движения
digitalWrite(8,LOW);
}
void Backward(){ // Подпрограмма движения робота назад
digitalWrite(3,HIGH);
digitalWrite(7,HIGH);
digitalWrite(11,HIGH);
digitalWrite(8,HIGH);
}
void motorStop(){ // Подпрограмма остановки мотора

digitalWrite(4,HIGH);
digitalWrite(2,HIGH);
}
void motorRun(){ // Подпрограмма запуска мотора

digitalWrite(2,LOW);
digitalWrite(4,LOW);
}
void Spin_Left(){ // Вращение в лево
digitalWrite(11,HIGH);
digitalWrite(8,LOW);
digitalWrite(3,HIGH);
digitalWrite(7,HIGH);
}
void Spin_Right(){ // Вращение в право
digitalWrite(3,HIGH);
digitalWrite(7,LOW);
digitalWrite(11,HIGH);
digitalWrite(8,HIGH);
}
void loop()
{
lms.servoWrite(2,15); // раскрываем лапки
   delay(100);   
   lms.servoWrite(4,160); // раскрываем лапки
   delay(100);     
lms.servoWrite(1,0);
delay(200);
j=0;    
for (j=0;j<18;j++){  // для j от 0 до 18
lms.servoWrite(1,j*10); // вращение сервы по 10 градусов

gp=analogRead(0); // считываем данные с ИК радара
delay(50);
p[j]=gp; // записываем это значение в массив
Serial.println(p[j]); // выводим на экран

gp2=(analogRead(2)); // считываем значение датчиков плоскости
gp1=(analogRead(1));
g=min(gp1,gp2);      // выбираем минимальное значение из двух
   if (g<4){           // Если значение близко к 0, то отходим назад и разворачиваемся
     motorRun();
     Backward();
     delay(500);
     Spin_Left();
     delay(600);
     motorStop();
   }

   if (p[j]<160){     // если значение с дальномера меньше 150, (примерно 25 см.), то шаг вперед   
     motorRun();
     Forward();
     delay (50);
     motorStop();}
else {              // Иначе
     digitalWrite(ledPin1, HIGH); // нажимаем кнопку перемотка
     delay(100);
     digitalWrite(ledPin1, LOW); // отпускаем кнопку перемотка
     delay(100);
     
     lms.servoWrite(1,90); // поворачиваем голову вперед
     delay(200);
            
     gp=analogRead(0);    // считываем данные с ИК радара
     Serial.println(gp);
     while(gp<160){       // поворачиваемся, пока не заметим предмет    
     motorRun();
     if (j>9){            // если предмет с лева, то
     Spin_Left();// поворот в сторону объекта
     }
     else {              // иначе поворот вправо
     Spin_Right();
     }
     delay (10);
     motorStop();
     gp=analogRead(0); // считываем данные с ИК радара   
     delay(50);     
     }
       
   gp=analogRead(0); // считываем данные с ИК радара
   delay(50);
   while(gp<400){    //Двигаемся вперед пока предмет не будет на расстоянии 10 см.
   motorRun();    
   Forward();    // движение до объекта
   delay (50); // время движения
   motorStop();
   gp=analogRead(0); // считываем данные с ИК радара
   delay(50);
   }
   lms.servoWrite(1,0); // убираем голову в сторону, чтобы она не мешала захвату предмета и транспортировке
   delay(100);
   motorRun();    
   Forward();    // движение до объекта
   delay (700); // время движения
   motorStop();
     
   lms.servoWrite(2,140); // захват первой лапой
   delay(500);   
   lms.servoWrite(4,15); // захват второй лапой
   delay(100);
       
   gp2=(analogRead(2));
   gp1=(analogRead(1));
   g=min(gp1,gp2);
   while(g>4) // движение после захвата до конца плоскости.
    {
      gp2=(analogRead(2));
      gp1=(analogRead(1));
      Serial.println(gp2);
      g=min(gp1,gp2);
      motorRun();
      Forward();
      delay(5);
      motorStop();}
        
     lms.servoWrite(2,15); // раскрываем лапки
     delay(100);   
     lms.servoWrite(4,160); // раскрываем лапки
     delay(100);       
      
     digitalWrite(ledPin, HIGH); // нажимаем кнопку стоп
     delay(150);
     digitalWrite(ledPin, LOW); // отпускаем кнопку стоп
     delay(100);
      
     motorRun();
     Backward();
     delay(100);
     Spin_Right();
     delay(600);
   }
   }
   }