Arduino.ru

Всем доброго времени суток,

Прошу Вашей помощт, т.к. не могу разобраться что не так в моем первом проекте.

Задача, на сколько я понимаю для большинства, тривиальная. Заставить робота объезжать препятствия. Реализовать я его решил на Arduino Mega 2560, 4wd шасси, драйвер шилд, сервопривод и HC-SR04. Но робот при запуске начинает сканировать пространство в 3 положениях и далее постоянно движется назад, периодически откланяясь, останавляиваясь и далле в том же духе.

#include <AFMotor.h>
#include <Servo.h>

//Создаем объекты для двигателей 
AF_DCMotor motor1(1); //канал М1 на Motor Shield — задний левый 
AF_DCMotor motor2(2); //канал М2 на Motor Shield — задний правый 
AF_DCMotor motor3(3); //канал М3 на Motor Shield — передний левый 
AF_DCMotor motor4(4); //канал М4 на Motor Shield — передний правый 

#define trigPin 35

#define echoPin 36

Servo myservo; // create servo object to control a servo

// a maximum of eight servo objects can be created

int pos = 0; // variable to store the servo position

int servoDirection = 100;

int servoDelay = 20;


void setup()

{

  Serial.begin (9600);

  myservo.attach(10); // attaches the servo on pin 7 to the servo object

  myservo.write(0);


  // Задаем направление движение 
// FORWARD - вперед 
// BACKWARD - назад 
// RELEASE - стоп 



  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

}

void forward() {

motor1.run(FORWARD); 
motor2.run(FORWARD); 
motor3.run(FORWARD); 
motor4.run(FORWARD); 

}

void backward() {

motor1.run(BACKWARD); 
motor2.run(BACKWARD); 
motor3.run(BACKWARD); 
motor4.run(BACKWARD);

}

void turnLeft() {
motor1.run(BACKWARD); 
motor2.run(FORWARD); 
motor3.run(BACKWARD); 
motor4.run(FORWARD);

}

void turnRight() {

motor1.run(FORWARD); 
motor2.run(BACKWARD); 
motor3.run(FORWARD); 
motor4.run(BACKWARD); 

}

int CheckDistance()

{

  long duration, distance;

  digitalWrite(trigPin, LOW); // Added this line

  delayMicroseconds(2); // Added this line

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10); // Added this line

  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);

  distance = (duration / 2) / 29.1;

  return distance;

}

void sweepServo1() {

  for (pos = 10; pos < 40; pos += 1) // goes from 0 degrees to 100 degrees

  { // in steps of 1 degree

    myservo.write(pos); // tell servo to go to position in variable 'pos'

    delay(10);

  }

  int testDistance = CheckDistance(); /// get object distance using ping

  /// if object is more than 30 cm away it is out of range

  if (testDistance >= 30 || testDistance <= 0) /// if object is more than 30 cm away it is out of range

  {

    forward();

  }

  else /// object is closer than 30cm, print distance

  {

    turnRight();

    Serial.print(testDistance);

    Serial.println("cm");

  }

}

void sweepServo2() {

  for (pos = 40; pos < 80; pos += 1) // goes from 0 degrees to 100 degrees

  { // in steps of 1 degree

    myservo.write(pos); // tell servo to go to position in variable 'pos'

    delay(10);

  }

  int testDistance = CheckDistance(); /// get object distance using ping

  /// if object is more than 30 cm away it is out of range

  if (testDistance >= 30 || testDistance <= 0) /// if object is more than 30 cm away it is out of range

  {

    forward();

  }

  else /// object is closer than 30cm, print distance

  {

    turnRight();

    Serial.print(testDistance);

    Serial.println("cm");

  }

}

void sweepServo3() {

  for (pos = 80; pos < 120; pos += 1) // goes from 0 degrees to 100 degrees

  { // in steps of 1 degree

    myservo.write(pos); // tell servo to go to position in variable 'pos'

    delay(10);

  }

  int testDistance = CheckDistance(); /// get object distance using ping

  /// if object is more than 30 cm away it is out of range

  if (testDistance >= 30 || testDistance <= 0) /// if object is more than 30 cm away it is out of range

  {

    forward();

  }

  else /// object is closer than 30cm, print distance

  {

    turnRight();

    Serial.print(testDistance);

    Serial.println("cm");

  }

}

void loop()

{

  sweepServo1();

  sweepServo2();

  sweepServo3();

}

Прошу помощи, что не так в моем коде или сборке?

Заранее спасибо