Робот некорректно воспринимает команды: влево, вправо, назад

Здравствуйте!
Помогите понять в чём причина того, что робот не правильно воспринимает команду: ехать вправо, ехать назад и ехать влево. Код написан правильно (приведён ниже), но вместо поворотов робот всегда едет вперед только с замедляя скорость в то время когда ему нужно поворачивать. Да забыл указать, что это происходит после того как подключишь ультразвуковой дальномер hc-sr04.
Без дальномера робот правильно выполняет повороты и едет назад. Дальномер тоже исправен, робот доезжая до препятствия за 10мм должен отъехать назад, считать расстояние и повернуться направо. Вместо этого он едет всё равно вперёд только более медленно. Бьюсь 4 день, не понимаю в чём дело, по отдельности всё работает, вместе едет только вперёд!!!
Перечень компонентов:
1. Arduino Uno
2. Motor shield от Амперки (с зелёными клеммниками) - 2 шт.
3. Ультразвуковой дальномер HC-SR04
4. Troyka Shield от Амперки
5. Роботизированная платформа с 4 моторами постоянного тока
6. Батарейный отсек 4 AA - 2шт

Код:

#define trigPin 14
#define echoPin 15

#define CON_MOTOR1 0
#define CON_MOTOR2 0

// Motor shield использует четыре контакта 4, 5, 6, 7 для управления моторами
// 4 и 7 — для направления, 5 и 6 — для скорости
#define SPEED_1 5
#define DIR_1 4

#define SPEED_2 6
#define DIR_2 7

// Возможные направления движения робота
#define FORWARD 0
#define BACKWARD 1
#define LEFT 2
#define RIGHT 3

int d1, d2;

int get_distance(){
long duration;
int d_1, d_2, average = -1;
// -------------------
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
d_1 = (int) duration / 58.2;
// -------------------
// -------------------
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
d_2 = (int) duration / 58.2;
// -------------------
average = (int) (d_1 + d_2) / 2;
Serial.print("distance: ");
Serial.println(average);
return average;
}

void go(int newDirection, int speed)
{
boolean motorDirection_1, motorDirection_2;

switch ( newDirection ) {

case FORWARD:
motorDirection_1 = true;
motorDirection_2 = true;
break;
case BACKWARD:
motorDirection_1 = false;
motorDirection_2 = false;
break;
case LEFT:
motorDirection_1 = true;
motorDirection_2 = false;
break;
case RIGHT:
motorDirection_1 = false;
motorDirection_2 = true;
break;
}
motorDirection_1 = CON_MOTOR1 ^ motorDirection_1;
motorDirection_2 = CON_MOTOR2 ^ motorDirection_2;
analogWrite(SPEED_1, speed);
analogWrite(SPEED_2, speed);

digitalWrite(DIR_1, motorDirection_1);
digitalWrite(DIR_2, motorDirection_2);

}
void setup()
{
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

}
void loop(){
if (get_distance() < 10){
go(BACKWARD, 50);
} else if (get_distance() < 20){
go(RIGHT, 90);
delay(500);
go(RIGHT, 90);
} else {
go(FORWARD, 255);
delay(100);
}
delay(500);
go(FORWARD, 90);
}

Программа пишет что код верный. Возможно проблема в подключении, но я не могу понять какая. Очень прошу помочь!!!

 

Тэги где!!!

 

Спасибо, буду разбираться!!!

 

Всё равно не получается, может с подключением пинов в коде не правильное?

 

Вставил! Всё равно едет вперёд и не поворачивает! Видимо дело в подключении. Спасибо большое!

 

Код (C++):

void loop() {
  float dist_cm = ultrasonic.Ranging(CM);
  if (dist_cm < 15.0) {
    go(FORWARD, 0);
    delay(100);
    go(RIGHT, 90);
    delay(500);
  } else {
    go(FORWARD, 255);
  }
}

При работающем дальномере не получилось добиться того что бы он повернул на право, но удалось сделать так чтобы он остановился.

 

Не очень читабельно....
Вот это:

Код (C++):

#define CON_MOTOR1 0
#define CON_MOTOR2 0

Потом (зачем такие операции XOR - простите не понял):

Код (C++):

motorDirection_1 = CON_MOTOR1 ^ motorDirection_1;
motorDirection_2 = CON_MOTOR2 ^ motorDirection_2;

Если у вас уже и так определения:

Код (C++):

witch ( newDirection ) {

case FORWARD:
motorDirection_1 = true;
motorDirection_2 = true;
break;
case BACKWARD:
motorDirection_1 = false;
motorDirection_2 = false;
break;
case LEFT:
motorDirection_1 = true;
motorDirection_2 = false;
break;
case RIGHT:
motorDirection_1 = false;
motorDirection_2 = true;
break;
}

По моему вот это "закручено":
motorDirection_1 = CON_MOTOR1 ^ motorDirection_1;
motorDirection_2 = CON_MOTOR2 ^ motorDirection_2;
Но может вы преследуете какую-то цель. Попробуйте поменять с 90 до... ну хотя бы до 128 или более - потому как простое движение у вас 255

Код (C++):

go(RIGHT, 90);

И простите если не прав.
И ещё....:

Код (C++):

void loop() {
  float dist_cm = ultrasonic.Ranging(CM);
  if (dist_cm < 15.0) {
    go(FORWARD, 0);
    delay(100);
    go(RIGHT, 90);
    delay(500);
  } else {
    go(FORWARD, 255);
  }
}

В каждом цикле у вас каждый раз остановка и поворот на право с малым временем между командами....

 

Я понял причину почему робот не поворачивает! Когда я проверял работу моторов без дальномера то светодиоды направления на Motor Shield горели зелёным, а вместе с дальномером они горели красным. Возможно когда светодиода горят зелёным, то робот различает направление, а красным когда не различает направление. Проверьте, пожалуйста, правильная ли это гипотеза и можно ли её исправить?

 

Проверьте если не сложно через Serial значение дистанции.... ну хотя бы вставив в цикл Serial.println(дистанция);. А то совсем непонятно.

 

Спасибо всем за помощь!!! Мне удалось устранить свою проблему! Теперь всё отлично, робот обьезжает препятствия!!!