Роботизированная машина. Часть II: программа

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

Обращаю внимание, что изменилась разводка соединительных кабелей по сравнению с исходной статьёй (например, освобождены аппаратные RX/TX). Мне не хочется рисовать схему соединений, надеюсь, что и так всё понятно из шапки программы. На всякий случай прикладываю фотографию, на которой хорошо виден Sensor Shield и подходящие к нему провода.

Алгоритм работы в общих чертах остался прежним: машина едет вперёд до тех пор, пока не "увидит" преграду, затем откатывается назад и поворачивается вокруг своей оси, а потом продолжает движение.

Однако, теперь добавлены нововведения, которые делают поведение машины забавным и витиеватым. 

1) Добавлен режим "Паника", в который робот переходит, если несколько раз подряд наталкивается на препятствия. Паника заключается в долгом вращении вокруг своей оси и включении сирены, мигает двухцветный диод и машина издаёт тревожные звуки виу-виу. Для описания условий перехода в данный режим пришлось ввести уникальную систему штрафов, которые назначаются за встречу с препятствием.

2) Добавлен выбор лучшего направления движения после отката от препятствия (поворот на дискретный угол и проверка расстояния - продолжается до тех пор, пока не будет выбрано удачное направление). Это нововведение позволило избавиться от бестолкового поведения, когда робот откатывался от препятствия, но поворачивался неудачно, поэтому снова тыкался в него же.

На картинке видно, что робот загнан в угол, пытается выбрать лучшее направление направление движения. Но он перебирает их случайным образом, не запоминая неудачный выбор, поэтому через некоторое время у него не получается выбраться, начинается бесконечное метание в углу. Примерно через 3-4 отскока от препятствия подряд робот переходит в режим паники. Включается вся иллюминация и начинается дикое вращение.

Заключительная стадия режима "Паника" (на GIF не показана) - это методичный последовательный перебор всех возможных вариантов движения, в итоге выбирается направление движения, в котором расстояние до преграды максимально. Это позволяет реально выпутываться из самых сложных конфигураций стен, включая шторы и ножки нескольких табуреток.

Текст программы приведён ниже. Прошил - и готово!

// 4WD RoboCar
// Sonar (NO Servo) + HeadLight + Syrens + LED13 (as backlight) + photosensor
// 2017-December-31
// v.0.5с (хорошая версия)

// Global variables:
byte Critical = 13;     // Критическое расстояние до препятствия в [см]
byte randomNumber;      // Случайное число
byte Cost = 60;         // Штраф (назначается за неспособность ехать вперёд)
byte Profit = 180;      // "Прибыль", которая плавно растёт при движении вперёд
byte velocity = 220;    // Скорость моторов [1..255]

// Setup the servo motor
// #include 
// Servo myservo;
// int servposnum = 0;
// int servpos = 0;

// Описываем подключение драйвера двигателей
// A - правый борт
// В -  левый борт

int enableB = 3; //~ Остался!
int pinB2   = 4; // 
int pinB1   = 5; // 
int enableA = 6; //~ Остался!
int pinA1   = 7; //~ 
int pinA2   = 8; //

#define illumination A0 // подключаем составной светодиод

// Подключаем ультразвуковой датчик
#define trigPin 9
#define echoPin 10

#define light 11 // На этот вывод подключены фары

int Buzzer = 12;// Подключаем зуммер

// Фоторезистор подключен к АЦП
#define PHOTO_SENSOR A5

#define Sweep 8000 // скорость нарастания и убывания частоты
#define Woo_wait_sec 2 // сколько с. длится гудение на макс. частоте

void setup() {
  // Определяем направление работы линий
   pinMode (enableA, OUTPUT);
   pinMode (pinA1, OUTPUT);
   pinMode (pinA2, OUTPUT);
   pinMode (enableB, OUTPUT);
   pinMode (pinB1, OUTPUT);
   pinMode (pinB2, OUTPUT); 
   pinMode(trigPin, OUTPUT);
   pinMode(echoPin, INPUT);
   pinMode(13 , OUTPUT);
   pinMode(light , OUTPUT);
   pinMode(illumination , OUTPUT);

enableMotors();
SayBeep();
delay(2000);
digitalWrite(13,  LOW); // Выключаем встроенный диод
// Serial.begin(9600);     // Отправляем данные на ПЭВМ
}

// Описываем варианты работы моторов
void motorAforward() {
 digitalWrite (pinA1, HIGH);
 digitalWrite (pinA2, LOW);
}
void motorBforward() {
 digitalWrite (pinB1, LOW);
 digitalWrite (pinB2, HIGH);
}
void motorAbackward() {
 digitalWrite (pinA1, LOW);
 digitalWrite (pinA2, HIGH);
}
void motorBbackward() {
 digitalWrite (pinB1, HIGH);
 digitalWrite (pinB2, LOW);
}
void motorAstop() {
 digitalWrite (pinA1, HIGH);
 digitalWrite (pinA2, HIGH);
}
void motorBstop() {
 digitalWrite (pinB1, HIGH);
 digitalWrite (pinB2, HIGH);
}
void motorAon() {
 digitalWrite (enableA, HIGH);
}
void motorBon() {
 digitalWrite (enableB, HIGH);
}
void motorAoff() {
 digitalWrite (enableA, LOW);
}
void motorBoff() {
 digitalWrite (enableB, LOW);
}

// Описываем варианты движения машины
void goForward (int duration) {
 motorAforward();
 motorBforward();
 delay (duration);
}
void goBackward (int duration) {
 motorAbackward();
 motorBbackward();
 delay (duration);
}
void rotateRight (int duration) {
 motorAbackward();
 motorBforward();
 delay (duration);
}
void rotateLeft (int duration) {
 motorAforward();
 motorBbackward();
 delay (duration);
}
void FullStop (int duration) {
 motorAstop();
 motorBstop();
 delay (duration);
}
void disableMotors() {
 motorAoff();
 motorBoff();
}
void enableMotors() {
 motorAon();
 motorBon();
}

void CheckLight () {
  int val = analogRead(PHOTO_SENSOR);
  /* if (val > 999) {
    digitalWrite(13, HIGH); // слишком светло
    panic();                // это повод
    digitalWrite(13,  LOW); // паниковать
  } */
  if (val < 500) {
    // Темновато, включаем фары
    digitalWrite(light, HIGH);
   } else {
    // Светло, выключаем фары
    digitalWrite(light,  LOW);
   }
}

// Пользуемся УЗ датчиком расстояния
int distance() {
  int duration, distance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1; // Переводим в сантиметры
  return distance;
}

// Функция запуска автомобиля
void launch() {
int distance_0;
distance_0 = distance();
//  Serial.print(distance_0);
//  Serial.println(" сантиметров. ");
// Движемся вперёд, пока расстояние до преграды > критического [cm]
while (distance_0 > Critical)
  {
     CheckLight ();  // проверяем, не пора ли зажигать фары?
     analogWrite (enableA, velocity);
     analogWrite (enableB, velocity);
     goForward(30); // Едем вперёд некоторое время
     randomNumber = random(1,100); // передёрнули затвор генератора ПСЧ
     if (Profit < 254 ) { Profit++; };
     // Serial.print(Profit);
     distance_0 = distance();
  }
  FullStop(100); // Останов, т.к. впереди помеха
}

void Woo(int freq, long duration){ // первый параметр частота, чем ниже он тем выше частота, второй длительность
  long time = duration/2/freq;
  for(long t = 0; t < time; t++)
  { digitalWrite(Buzzer, HIGH);
    delayMicroseconds(freq);
    digitalWrite(Buzzer, LOW);
    delayMicroseconds(freq); }
}

void Syren() {
    for(int i = 0; i <= 1; i++){         // делаем виу-виу 2 раза
    for(int f = 2000; f >= 100; f=f-40){ // нарастание частоты
    Woo(f, Sweep); }
    // Woo(400, Woo_wait_sec*100);    // сколько длится гудение на максимальной частоте
    for(int f = 100; f <= 2000; f=f+40){ // убывание частоты
    Woo(f, Sweep); }
    }
    }

void panic() {
 int distance_tmp;
 int distance_new = 32000;
 int angle = 600;  // угол поворота, измеряем в [мс]
    FullStop(100); // Сначала останавливаемся
    Profit = 255;  // Забываем про старые штрафы!
    // Serial.print(Profit);
    digitalWrite(illumination, HIGH); // включаем мигалку
    Syren ();
    Syren ();
    Syren ();
    delay(1000);  // пауза, для отдыха
    // Выполняем манёвры на высокой скорости (ведь паника!)
    analogWrite (enableA, velocity);
    analogWrite (enableB, velocity);
    // 0) Начинаем крутиться волчком в какую-то сторону:
    randomNumber = random(1,100);
    if (randomNumber < 50) { motorAbackward(); motorBforward(); }
                      else { motorBbackward(); motorAforward(); };
    // Истерично продолжаем крутиться, пока гудит сирена:
    Syren ();
    delay (1000); // Ждём 
    FullStop(1000);
    delay (100); // Ждём 
    // 1) Ищем хоть какое-то направление, куда вообще можно ехать
    distance_tmp = distance();
    do {
     rotateRight(angle); // Крутимся
     FullStop(1000); // Ждём и смотрим вдаль
     distance_tmp = distance();
     } while(distance_tmp < Critical); // повторяем поворот, если расстояние всё ещё мало

     SayBeep();
     // rotateLeft(angle); // Возвращаемся на один шаг
 
    // 2) Пытаемся выбрать лучшее направление!
    /* rotateRight(angle); // Крутимся и проверяем дистанцию
    distance_new = distance(); 
    if (distance_new < distance_tmp) // Не угадали, раньше было лучше,
    {rotateLeft(angle); }            // поэтому поворачиваемся обратно */

    /* distance_new = 32000;
    while( distance_new > distance_tmp ) {
    distance_tmp = distance();
    rotateRight(angle); // Крутимся и проверяем дистанцию
    FullStop(900); // Ждём и смотрим вдаль
    distance_new = distance();
    delay (800); // Ждём и смотрим вдаль
    } */

   
    do {
    distance_tmp = distance();
    rotateRight(angle); // Крутимся и проверяем дистанцию
    FullStop(900); // Ждём и смотрим вдаль
    distance_new = distance();
    delay (800); // Ждём и смотрим вдаль
    } while( distance_new > distance_tmp );
    
                                   // Не угадали, раньше было лучше,
    rotateLeft(angle);             // поэтому поворачиваемся обратно
    FullStop(1000); // Восстанавливаем дыхание, успокаиваемся
    digitalWrite(illumination, LOW); // успокоились, выключаем мигалку
    delay (410); // Ждём
    SayBeep();
    }

void rollBack()
    {
    digitalWrite(13,  HIGH); // Включаем встроенный диод!
    // Сначала откатываемся назад на случайное количество шагов
    randomNumber = random(700,2100);
    //analogWrite (enableA, 150);
    //analogWrite (enableB, 189);
    goBackward(randomNumber);
    FullStop(200); // Восстанавливаем дыхание, успокаиваемся
        
    // Случайным образом выбираем направление поворота:
    randomNumber = random(1,100);
       
     do { // поворачиваемся в выбранную сторону на случайный угол
     if (randomNumber < 50) { rotateRight(random(450,1300)); } else { rotateLeft(random(350,1400)); };
     FullStop(1000); // Выключаем моторы
     tone(Buzzer, 600, 333);
     delay (700); // передышка!
     } while( distance() < Critical ); // повторяем поворот, если расстояние всё ещё мало

    analogWrite (enableA, velocity);
    analogWrite (enableB, velocity);
    FullStop(600); // Восстанавливаем дыхание, успокаиваемся
    digitalWrite(13,  LOW); // Выключаем встроенный диод
    // SayBeep();
    delay (200); // передышка!
    } 

void avoid()
{
    CheckLight ();  // проверяем, не пора ли зажигать фары?
    tone(Buzzer, 2100, 110);
    delay (700); // передышка!
    // Штрафуем сами себя:
    if (Profit > Cost) {
    Profit = Profit - Cost; // Ещё есть возможность оплатить штраф,
    rollBack();             // тогда откат назад с разворотом
    // Serial.print(Profit);
    } else {
    // Нельзя штрафовать, всё плохо, значит запутались - паникуем
    panic();
    }
}

void SayBeep(){
    tone(Buzzer, 700, 109);
    delay(200);
    tone(Buzzer, 1200, 109);
    delay(200);
    tone(Buzzer, 2600, 240);
    delay(350);
    noTone(Buzzer);
}

void loop() {
     randomNumber = random(2,5); // вхолостую выбираем псевдо-случайное число
     launch(); // запускаем автомобиль вперёд до встречи с преградой
     avoid();  // откатываемся от препятствия и как-то поворачиваемся
     CheckLight ();  // проверяем, не пора ли зажигать фары?
}

Как видно, программа стала значительно длиннее. Надеюсь, она не потеряла изящности.

В режиме отладки можно отправлять данные в монитор последовательного порта, чтобы оценить динамику набора очков и получения штрафов.

Фактически, наш аппарат перемещается вперёд дискретно, шагами. Одно очко выдаётся за один шаг вперёд. Если впереди препятствие, назначается штраф. Встреча с препятствием показана красной стрелкой на рисунке выше. Затем автомобиль находит направление для дальнейшего движения, теперь очки линейно начисляются за непрерывное движение вперёд.

Для различных помещений и задач можно варьировать как константу штрафа (Cost), так и время выполнения одного "шага". Это позволит переходить реже или чаще в режим паники.