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

LMotorShield lms;
int gp2;
int gp3;
int gp4;
int gp5;
int gp24;
int gp25;

void setup() {
{
Serial.begin(9600);
lms.begin(LMS_SERVOS);
}
pinMode(3,OUTPUT); // Motor A скорость
pinMode(7,OUTPUT); // Motor A направление
pinMode(11,OUTPUT); // Motor B скорость
pinMode(8,OUTPUT); // Motor B направление
pinMode(2,OUTPUT); // Motor A2
pinMode(4,OUTPUT); // Motor B1
}
void Forward() { // Подпрограмма движения робота вперед
digitalWrite(3,HIGH);
digitalWrite(7,LOW);
digitalWrite(11,HIGH);
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()
{
int i;
for (i=0;i<5;i++){ // Повторить 5 раз для фильтрации шума
gp2=(gp2+analogRead(0));
}
gp2=gp2/5;
gp3=(2914/(gp2+5))+1;
Serial.println(gp3);

if (gp3<10){
motorStop();

lms.servoWrite(1,90);
delay(500);

lms.servoWrite(1,10);
delay(500);
for (i=0;i<5;i++){ // Повторить 5 раз для фильтрации шума
gp24=(gp24+analogRead(0));
}
gp24=gp24/5;
gp4=(2914/(gp24+5))+2;
Serial.println(gp4);

lms.servoWrite(1,175);
delay(500);
int i;
for (i=0;i<5;i++){ // Повторить 5 раз для фильтрации шума
gp25=(gp25+analogRead(0));
}
gp25=gp25/5;
gp5=(2914/(gp25+5))+2;
Serial.println(gp5);

lms.servoWrite(1,90);
delay(500);

if (gp4<12&&gp5<12){
motorRun();
Backward();
delay(1000);
Spin_Left();
delay(1118);
}
else {
if (gp4<12){
motorRun();
Forward();
delay(300);
Spin_Left();
delay(569);
}
if (gp5<12){
motorRun();
Forward();
delay(300);
Spin_Right();
delay(569);
}
if (gp4>12&&gp5>12){
motorRun();
Spin_Left();
delay(569);
}}}
else
{
motorRun();
Forward(); // Двигаться вперед
}}