Роботизированная машина. Часть III: Bluetooth

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

Первое, что необходимо сделать, это дополнить робота модулем HC-06 для общения по Bluetooth. Предварительно потребуется выполнить настройку HC-06. Можно использовать модуль HC-05, но он дороже, а его функционал окажется избыточным для данной задачи.

Следующий шаг - установить на телефон бесплатную программу Bluetooth RC Controller. Программа имитирует основные органы управления пульта и отправляет команды по Bluetooth в текстовом виде.

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

• Передние фары - у нас как раз уже есть голубой светодиод, который и назвался фарой!
• Задние фонари - у нас для этого использовался встроенный светодиод
• Гудок - зуммер тоже имеется!
• Аварийка - у нас это будет составной светодиод, который сигнализировал о "панике"
• Кнопка с шестерёнкой - это вызов меню настройки самого пульта
• Бегунок изменения скоростей - о, вот тут пришлось слегка напрячься, чтобы дополнить код автомобиля
• Ну а ниже кнопки Вперёд / Назад / Влево / Вправо - можно удерживать одновременно две.

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

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

*

// 4WD RoboCar
// Sonar + Bluetooth
// 2018-January-20
// v.11a (bluetooth)
// (c) 2018, Vladimir E. DRACH

// Global variables:
int  Critical = 14;     // Критическое расстояние до препятствия в [см]
int  BT_Step  = 10;     // Время движения при получении одного СИМВОЛА по BT [мс]
byte randomNumber;      // Случайное число
byte Cost = 60;         // Штраф (назначается за неспособность ехать вперёд)
byte Profit = 180;      // Очки, т.е. "прибыль", которая плавно растёт при движении вперёд
byte velocity = 220;    // Скорость моторов [1..255]
                            // Подобрать экспериментально:
const byte SPEED_MIN = 100; // минимальная скорость моторов, если меньше - моторы не смогут вращаться
const byte SPEED_MAX = 250; // максимальная скорость моторов

// для управления по Bluetooth
char btCommand = 'S';
// счетчики для определения потери связи с Bluetooth
unsigned long btTimer0 = 2000;  //Stores the time (in millis since execution started)
unsigned long btTimer1 = 0;     //Stores the time when the last command was received from the phone

// Описываем подключение драйвера двигателей
// 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;// Подключаем зуммер 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 (13 ,     OUTPUT);
   pinMode (light ,  OUTPUT);
   pinMode (trigPin, OUTPUT);
   pinMode (echoPin, INPUT) ;
   pinMode (illumination, OUTPUT);
 enableMotors();
 SayBeep();
 delay(2000);
 bii();
 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(); 
 // SetVelocity(SPEED_MAX, SPEED_MAX); не очень работает :(
}

void SetVelocity(int A, int B)
{         analogWrite (enableA, A);
          analogWrite (enableB, B); }

void CheckLight () {
  int val = analogRead(PHOTO_SENSOR);
  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_measured;
     distance_measured = distance();
      // Serial.print(distance_0);
      // Serial.println(" сантиметров. ");
     SetVelocity (velocity, velocity);
      // Движемся вперёд, пока расстояние до преграды > критического [cm]
     while (distance_measured > Critical)
     {
     CheckLight ();  // проверяем, не пора ли зажигать фары?
     goForward(30); // Едем вперёд некоторое время
     randomNumber = random(1,100); // передёрнули затвор генератора ПСЧ
     if (Profit < 254 ) { Profit++; };
     // Serial.print(Profit);
     // Serial.println(" очков. ");
     distance_measured = 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);
    // Serial.println(" очков. ");
    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) Пытаемся выбрать лучшее направление!
     
    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);
     // Serial.println(" очков. ");
    } 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 boo(){
      tone(Buzzer, 600, 200);
      delay (300);
      tone(Buzzer, 410, 600);
      delay (600);
}

void bii(){
      tone(Buzzer, 1611, 90);
      delay (150);
      tone(Buzzer, 1611, 90);
      delay (150);
      tone(Buzzer, 2111, 400);
      delay (440);
}

void SafeForward (int duration){
  int distance_measured;
  distance_measured = distance();
  if ( distance_measured > Critical )
      {goForward(duration);}
      else {
      FullStop(10);
      boo(); }
}

void BluetoothControl() {
     for(int i = 0; i <= 4; i++){
     // digitalWrite(illumination, HIGH); // включаем мигалку
     digitalWrite(13,  HIGH); // Включаем встроенный диод!
     FullStop(80);
     // digitalWrite(illumination, LOW); // вЫключаем мигалку
     digitalWrite(13,  LOW); // Включаем встроенный диод!
     FullStop(900);
     velocity = SPEED_MIN + 10;         // Выставлям скорость поменьше, т.к.
     SetVelocity(velocity, velocity);   // на Android будет миниальная скорость по умолчанию!
     }
     do{
     // CheckLight ();  // проверяем, не пора ли зажигать фары?
     aquire();
     }
     while ( 1==1 );
}

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

void aquire ()
{
  if (Serial.available() > 0) {
    btTimer1 = millis();
    btCommand = Serial.read();
    switch (btCommand){
    case 'F':
      goForward(BT_Step); // Можно просто ехать вперёд наобум,
      // SafeForward (BT_Step); // а можно включить сонар
      break;
    case 'B':
      goBackward(BT_Step);
      break;
    case 'L':
      rotateLeft(BT_Step);
      break;
    case 'R':
      rotateRight(BT_Step);
      break;
    case 'S':
      FullStop(BT_Step);
      break;
    case 'G':  // Вперёд, подкручивая вправо
      if (velocity < SPEED_MAX-99) {
      SetVelocity((velocity+99), (velocity-99));}
      else {SetVelocity(SPEED_MAX, SPEED_MIN);} // выставили дифференциал
      goForward (BT_Step);
      SetVelocity(velocity, velocity); // вернули скорость
      break;
    case 'I':  // Вперёд, подкручивая влево
      if (velocity < SPEED_MAX-99) {
      SetVelocity((velocity-99), (velocity+99));}
      else {SetVelocity(SPEED_MIN,SPEED_MAX);} // выставили дифференциал
      goForward (BT_Step);
      SetVelocity(velocity, velocity); // вернули скорость
      break;
    /*case 'J':  //BR
      if (velocity < SPEED_MAX-80) {
      SetVelocity((velocity-80), (velocity+80));}
      else {SetVelocity((velocity-80),SPEED_MAX);} // выставили дифференциал
      goBackward(BT_Step);
      SetVelocity(velocity, velocity); // вернули скорость
      break;
    case 'H':  //BL
      if (velocity < SPEED_MAX-80) {
      SetVelocity((velocity+80), (velocity-80));}
      else {SetVelocity(SPEED_MAX, (velocity-80));} // выставили дифференциал
      goBackward(BT_Step);
      SetVelocity(velocity, velocity); // вернули скорость
      break;
      */
    case 'W':  //  Зажгли фары
      digitalWrite(light, HIGH);
      break;
   case 'w':  // Погасили фары
      digitalWrite(light, LOW);
      break;
   case 'D':  // Everything OFF
      FullStop(100);
      break;
   case 'X': // аварийка
      digitalWrite(illumination, HIGH);
      break;
   case 'x': // аварийка
      digitalWrite(illumination, LOW);
      break;      
   case 'U':  // Back ON
      digitalWrite(13,  HIGH);  // Включаем встроенный диод
      break;
   case 'u':  // Back OFF
      digitalWrite(13,  LOW);    // Выключаем встроенный диод
      break;
   case 'V': // Пискнуть весело (в оригинале - гудок ВКЛ)
      bii();
      break;
   case 'v': // Пискнуть грустно (в оригинале (гудок ВЫКЛ)
      boo();
      break; 
   default:  // Get SPEED_CURRENT
      if ( btCommand == 'q' ){
         velocity = SPEED_MAX;
         SetVelocity(velocity, velocity);
         } else {
        // Символы '0' - '9' эквивалентны кодам integer 48 - 57 соответственно
        if ( (btCommand >= 48) && (btCommand <= 57) ) {
          // Subtracting 48 changes the range from 48-57 to 0-9.
          // Multiplying by 25 changes the range from 0-9 to 0-225.
          velocity = SPEED_MIN + (btCommand - 48) * 15;
          SetVelocity(velocity, velocity);
        }
      } // else
    } // switch
  } // if (Serial.available() > 0)
  else {
    btTimer0 = millis();  // Узнаём текущее время (millis since execution started)
    //Check if it has been 500ms since we received last btCommand.
    if ((btTimer0 - btTimer1) > 800)   {
      //More than 800 ms have passed since last btCommand received, car is out of range.
      FullStop(1000);
      digitalWrite(illumination, HIGH); // включаем мигалку
      boo ();
      FullStop(4000);
      Syren();
      FullStop(8000);
    }
  }
}

void loop() {
     CheckLight ();  // проверяем, не пора ли зажигать фары?
     // Serial.println("System ready...");
     if ( distance() > Critical )
        {                     // Путь вперёд свободен,
          SelfControl();      // робот отправляется в самостоятельное путешествие
        } else {
          BluetoothControl(); // Впереди преграда, отдаём управление водителю
        }
     }

*

Выбор режима (ручное управление или демо) выполняется в самом начале за счёт проверки расстояния до преграды. При включении машина смотрит в стену - ручное управление по Bluetooth. При включении нет преград перед машиной - самостоятельное движение (демо-режим).