Arduino.ru

Здравствуйте. Решил сделать робота-машинку, сделал по примеру этой статьи http://payaem.ru/avtonomnaya-mashinka.html

Проблема: Загрузил код, все вроде бы хорошо, но, после того как робот-машинка натыкается на препятствие останавливается, поворачивает колеса и все, ни назад не едет, ни вперед влево или вправо. Проше говоря, робот не избегает препятствия. 

Буду очень благодарен за помощь. В заранее СПАСИБО! 

#include <Ultrasonic.h>

int motorPin1 =  9;    
int motorPin2 =  10;    
int motorPin3 =  7;    
int motorPin4 =  8;    

int steeringMotorSpeed = 150;  


int avoidDistance = 80;
int endAvoidDistance = 100;

int goToReverseDistance = 55;
int reverseTime = 2000;

 int fullTrottleMinDistance = 250;


int fullTrottleSpeed = 220;
int cruiseSpeed = 160;
int avoidSpeed = 120;
int reverseSpeed = 130;

int sensorDelay = 100;  

long randNumber;
int avoidDirection = 1;
int avoidCirclesBeforeDirectionChange = 10;
int countDown = 1;
int distanceCountDown = 1;

#include "Ultrasonic.h"
Ultrasonic ultrasonic(12,13);

int distance = (ultrasonic.Ranging(CM));



void setup()   {

 
  Serial.begin(9600);
  
  randomSeed(analogRead(0));
  
  
  pinMode(motorPin1, OUTPUT); 
  pinMode(motorPin2, OUTPUT);  
  pinMode(motorPin3, OUTPUT); 
  pinMode(motorPin4, OUTPUT);  
}


void loop()                     
{
   // Serial.println("start loop  ");
    
   // Serial.println(ultrasonic.Ranging(CM));

//choseMode();



distance = (ultrasonic.Ranging(CM));


  if(distance <= goToReverseDistance){
    avoidDirection = avoidDirection * -1;
    
//   while(distance < avoidDistance) {
    // Serial.println("start reverse  ");
      if(avoidDirection == 1){                      
        analogWrite(motorPin3, steeringMotorSpeed);
        digitalWrite(motorPin4, LOW);  
      } else {                                      
        analogWrite(motorPin4, steeringMotorSpeed); 
        digitalWrite(motorPin3, LOW); 
      }
    analogWrite(motorPin2, reverseSpeed);            
    digitalWrite(motorPin1, LOW); 
    delay(reverseTime);                              
 //   delay(sensorDelay);
    distance = (ultrasonic.Ranging(CM));
 //   }

 
    digitalWrite(motorPin2, LOW);                    
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
    
 
    avoidDirection = avoidDirection * -1;             
                               
    distance = (ultrasonic.Ranging(CM));
  }


        distance = (ultrasonic.Ranging(CM));
        
  if(distance < avoidDistance){
  // Serial.println("start avoid  ");

    distance = (ultrasonic.Ranging(CM));
    countDown = avoidCirclesBeforeDirectionChange;
    distanceCountDown = distance;
    
  while(distance <= endAvoidDistance && distance > goToReverseDistance){
        if(avoidDirection == 1){
            analogWrite(motorPin3, steeringMotorSpeed); 
            digitalWrite(motorPin4, LOW);
        } else {
            analogWrite(motorPin4, steeringMotorSpeed); 
            digitalWrite(motorPin3, LOW);
        }
        
        analogWrite(motorPin1, avoidSpeed); 
        digitalWrite(motorPin2, LOW);        
        delay(sensorDelay);
        distance = (ultrasonic.Ranging(CM));
          if(distance < distanceCountDown) { countDown--; }           
          if(countDown < 1) {
              avoidDirection = avoidDirection * -1;   
              countDown = avoidCirclesBeforeDirectionChange;
              distanceCountDown = distance;  
          }
    // Serial.println(distance);  
    // Serial.println(avoidDirection);  
    }   // end while (avoid) 
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
  }


        distance = (ultrasonic.Ranging(CM));
        
    while(distance > avoidDistance  && distance < fullTrottleMinDistance){
        analogWrite(motorPin1, cruiseSpeed); 
        digitalWrite(motorPin2, LOW);       
        delay(sensorDelay);   
        distance = (ultrasonic.Ranging(CM));
    // Serial.println("cruise");
    // Serial.println(distance);  
    }   // end while
    
    
        distance = (ultrasonic.Ranging(CM));
        
    while(distance > fullTrottleMinDistance){
        analogWrite(motorPin1, fullTrottleSpeed);  
        digitalWrite(motorPin2, LOW);      
        delay(sensorDelay);   
        distance = (ultrasonic.Ranging(CM));
              // Serial.println("FULL");
              // Serial.println(distance);  
    }   // end while

      // Serial.println("REstart loop  ");
    
   // Serial.println(distance);
  

}