Arduino.ru

 #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(); // Двигаться вперед 
 }}