Робот ведет себя неадекватно

NaughtyFox
Offline
Зарегистрирован: 28.12.2019

По моей программе робот должен доезжать до ближайшейго препятствия и объехать вокруг него. Я установил на робота серво вместе с ультразвуковым датчиком расстояния. Для начала я сделал скетч с циклом, в котором робот должен был ехать вперед, пока расстояние будет больше определенного. Тогда все работало нормально. Потом я дописал этот скетч, после чего робот стал как бы дергаться, и вал серво тоже дрожит. При этом робот периодически двигается вперед, но сразу же останавливается. вот текст программы:

#define M11 5
#define M12 6
#define M21 11
#define M22 10
#include <Ultrasonic.h>
#include <Servo.h>

float dist, rast;                    //Функции для измерения расстояния и положения серво
Ultrasonic dalnomer(12, 13);       
Servo MyServo;

void setup() {
  MyServo.attach(9);
  pinMode(M11, OUTPUT);              //Контакты для двигателей
  pinMode(M12, OUTPUT);
  pinMode(M21, OUTPUT);
  pinMode(M22, OUTPUT);  
}

void loop() {
  MyServo.write(90);                 //Серво смотрит прямо
  
  do{
    dist = dalnomer.read(CM);        //Считываем расстояние с дальномера
    goPered(255);                    //Едем вперед
  }while(dist>5);                    //Пока расстояние больше 5 см
  goStop();                          //Остановка
  delay(1000);                       //Задержка

                                      //Если убрать код начиная отсюда
  MyServo.write(180);                 //Поворачиваем серво налево
  dist = dalnomer.read(CM);           //Считываем расстояние с дальномера
  if(dist>30){                        //Проверка на отсутствие стенки
    do{
      dist = dalnomer.read(CM);
      goPered(255);                  
    }while(dist>10);                  //Езда вперед
  }
  else{                               //То же самое, только в правую сторону
    MyServo.write(180);
    dist = dalnomer.read(CM);
    if(dist>30){
      do{
        dist = dalnomer.read(CM);
        goPered(255);    
      }while(dist>10);
    }
  }

  rast = MyServo.read();            //На какой угол повернут серво?

  if(rast==0){                      //Если налево, то
    MyServo.write(180);             //поворачиваем направо
dist = dalnomer.read(CM);       //проверка на отсутствие стенки
    if(dist>30){                    //если нет, то
      goPravo(255); delay(1000);    //Поворачиваем направо
      MyServo.write(90);            //Серво смотрит прямо
    }
  }

  if(rast==180){                    //Симметричная ситуация
     MyServo.write(0);
     dist = dalnomer.read(CM);
     if(dist>30){
       goLevo(255); delay(1000);
       MyServo.write(90);
     }
  }
}                                    //до сюда, то робот выполняет то, что было в начале функции loop, как планировалось
				     //то есть едет вперед до ближайшего препятствия. По плану, если присоединить код который я дописал,
				     //он должен проверить если возможность повернуть, и после этого обогнуть препятствие. Но вместо этого,
				     //он даже не двигается с места

int goPered(int Skorost){            //Движение вперед
  analogWrite(M12, LOW);
  analogWrite(M21, LOW);
  analogWrite(M11, Skorost);
  analogWrite(M22, Skorost);}

int goNazad(int Skorost){            //Движение назад
  analogWrite(M11, LOW);
  analogWrite(M22, LOW);
  analogWrite(M12, Skorost);
  analogWrite(M21, Skorost);}

int goLevo(int Skorost){             //Поворот налево
  analogWrite(M12, LOW);
  analogWrite(M22, LOW);
  analogWrite(M11, Skorost);
  analogWrite(M21, LOW);}

int goPravo(int Skorost){            //Поворот направо
  analogWrite(M11, LOW);
  analogWrite(M21, LOW);
  analogWrite(M12, Skorost);
  analogWrite(M22, LOW);}

void goStop(){                       //Остановка
  digitalWrite(M12, LOW);
  digitalWrite(M11, LOW);
  digitalWrite(M21, LOW);
  digitalWrite(M22, LOW);}
      MyServo.write(90);
    }
  }

Срочно нужна помощь, если дадите совет - буду благодарен

P.S. Драйвер двигателя - мостовой, на основе микросхемы L293;

P.P.S. До проверки всей программы, робот доезжал до стенки нормально. теперь и этот кусок кода не работает(Я ничего в нем не менял) 

Densl
Offline
Зарегистрирован: 28.11.2018

А в 33 строке у вас правильное условие? Что-то мне кажется оно нелогичным.

NaughtyFox
Offline
Зарегистрирован: 28.12.2019

Да ведь он даже двигаться не начинает, то есть первый цикл не проходит. Да и то что в условии такое расстояние, пока не важно, потому что он должен хотя бы доехать до стенки и развернуться. Я это потом подкорректирую

Densl
Offline
Зарегистрирован: 28.11.2018

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

NaughtyFox
Offline
Зарегистрирован: 28.12.2019

Заметил, что когда колеса крутятся, LD светодиод на плате (желтый) горит. А когда он не горит, колеса не крутятся. О чем это может говорить? 

sadman41
Онлайн
Зарегистрирован: 19.10.2016

Как Вы думаете, сколько разновидностей плат с L239D и светодиодами можно прикупить на Aliexpress - одну или десяток?

kalapanga
Offline
Зарегистрирован: 23.10.2016

Раз не работает даже тот код, который якобы работал, так может аппаратная проблема?

Перепроверьте все подключения, их правильность и качество. От чего питается вся эта конструкция? Может тупо батарейка села? Да и что за конструкция-то? Какие моторы? Чем управляются? Как подключены (схема)? 

Только не говорите, что стандартные моторы, через стандартный шилд, по стандартной схеме. Нет таких!

Возвращайтесь к минимальному коду, добивайтесь, чтобы он заработал. 

ЕвгенийП
ЕвгенийП аватар
Онлайн
Зарегистрирован: 25.05.2015

NaughtyFox,

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

Я, конечно, понимаю, что скорее всего Вы что-то намудрили с текстом при выкладывании, но

1. Как нам с этим разбираться? Откуда нам знать что там у Вас на самом деле в коде?

2. Такие ошибки говорят об общей аккуратности работы с программой, которую Вам необходимо прокачивать, иначе ни одна программа у Вас никогда не заработает.

3. Никогда не используйте фразу "Я ничего в нем не менял" - вызывает в зависимости от настроение либо саркастическую ухмылку, либо раздражение.

andriano
andriano аватар
Offline
Зарегистрирован: 20.06.2015

Densl пишет:

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

Не уверен, что подключить робота к ПК хорошая идея, но направление мысли правильное - для отладки необходима индикация режимов. Собственно, именно с этой целью на робота обычно устанавливают дисплей: либо Нокиевский, либо какой-нибудь вариант 128х64. По минимуму - хотя бы одноразрядный 7-сегментный индикатор.

Densl
Offline
Зарегистрирован: 28.11.2018

Получается робот такой не надежный что этот экран очень востребован.

NaughtyFox
Offline
Зарегистрирован: 28.12.2019
 MyServo.write(90);
  do{
    dist = dalnomer.read(CM);
    Serial.println(dist);
    goPered(255); 
  }while(dist>5);
  goStop();
  MyServo.write(180);

Решил добавлять строки по отдельности к уже работающему коду. После добавления 8 строчки (поворот серво), у меня код выше работает хорошо, но серво в конце не поворачивается. Решил проверить серво на другом скетче - нормально поворачивается

ЕвгенийП
ЕвгенийП аватар
Онлайн
Зарегистрирован: 25.05.2015

NaughtyFox пишет:
код работает хорошо, но серво не поворачивается
Глубокая мысль. Я её не понял.

qwone
qwone аватар
Offline
Зарегистрирован: 03.07.2016

ЕвгенийП пишет:

NaughtyFox пишет:
код работает хорошо, но серво не поворачивается
Глубокая мысль. Я её не понял.

Все ясно и понятно. Писайтелю кода надо заплатить. А те кто сервы производит штрафануть. Понаделают китайских серв, а потом программисты мучаются, а им за выполненную работу не платят. А скоро НГ,сроки горят.

inspiritus
Offline
Зарегистрирован: 17.12.2012

Возможно все это еще и на бредборде дюпонами собрано :)

NaughtyFox
Offline
Зарегистрирован: 28.12.2019

Я добавил задержки, теперь код работает правильно. Спасибо!

mykaida
mykaida аватар
Offline
Зарегистрирован: 12.07.2018

NaughtyFox пишет:

Я добавил задержки, теперь код работает правильно. Спасибо!

Ну вот - delay() рулит, да и похрен, что пока механизм отрабатывает кнопки не нажимаются!