Arduino.ru

 Всем привет я хотела обратиться за помощью к Вам по скетчу я заметила совсем не давно что ИК дальномер работает не совсем правильно(или точно) он как бы срабатывает с опозданиями, или даже бывает что остановится в 10см от препятствия замиряет растояния и тут же едит в стенку и только тогда уже начинает разворачиватся.

#include <Servo.h> 
#include <LMotorShield.h> 
LMotorShield lms; 
int gp2; 
int gp3; 
int gp4; 
int gp5;  

void setup() { 
  
Serial.begin(9600); 
lms.begin(LMS_SERVOS);  
pinMode(3,OUTPUT); // Motor A1 
pinMode(7,OUTPUT); // Motor A2 
pinMode(11,OUTPUT);// Motor B1 
pinMode(8,OUTPUT); // Motor B2 
} 
void Forward() { // Подпрограмма движения робота вперед 
digitalWrite(3,HIGH); 
digitalWrite(7,LOW); 
digitalWrite(11,HIGH); 
digitalWrite(8,LOW); 
} 
void Backward(){ // Подпрограмма движения робота назад 
digitalWrite(3,LOW); 
digitalWrite(7,HIGH); 
digitalWrite(11,LOW); 
digitalWrite(8,HIGH); 
} 
void motorStop(){ // Подпрограмма остановки мотора 

digitalWrite(3,LOW); 
digitalWrite(7,LOW); 
digitalWrite(11,LOW); 
digitalWrite(8,LOW); 
} 
 
void Spin_Left(){ // Вращение в лево 
digitalWrite(3,LOW); 
digitalWrite(7,HIGH); 
digitalWrite(11,HIGH); 
digitalWrite(8,LOW); 
} 
void Spin_Right(){ // Вращение в право 
digitalWrite(3,HIGH); 
digitalWrite(7,LOW); 
digitalWrite(11,LOW); 
digitalWrite(8,HIGH); 
}
void loop() 
{ 
int i; 
for (i=0;i<5;i++){ // Повторить 5 раз для фильтрации шума 
gp2=analogRead(0); 
Serial.println(gp2); 

if (gp2>430){ 
motorStop(); 

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

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

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

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

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