Робот на Arduino с ультразвуковым локатором

Список форумов Общий раздел Программирование

Описание: Обучение программированию, интересные задачи, полезные ресурсы

#1 anton » 08.11.2015, 20:04

Сделал робота на Arduino-Uno с датчиками для огибания препятствий: ультразвуковой дальномер на сервомашинке и фронтальный инфракрасный датчик для близкой дистанции.

Вкратце опишу алгоритм работы робота (более подробно можно прочитать в комментариях к исходному коду программы). Ультразвуковой дальномер сканирует пространство перед роботом, заполняя массив результатами измерений. Затем в массиве находятся кластеры пустот (т.е. зоны с максимальной дальностью). По среднему значению из кластера находится азимут, на который должен довернуть робот, чтобы избежать столкновения. Значение азимута подставляется в формулы для расчета скоростей левого и правого двигателей. Инфракрасный датчик работает независимо и срабатывает на дистанциях около 1 см.

Видео, демонстрирующее возможности робота:
phpBB [media]


Исходный код программы:
Код: Выделить всё
#include <IRremote.h> //использование библиотек
#include <Ultrasonic.h>
#include <Servo.h>

#define ROTATION_DELAY_90 300 //задержка поворота на 90 градусов
#define ROTATION_DELAY_180 700 //задержка поворота на 180 градусов
#define ROTATION_DELAY_45 150 //задержка поворота на 45 градусов

#define RANGE 25 //дальность срабатывания ультразвукового датчика в сантиметрах

#define BUTTON_CH_MINUS 0xE318261B //коды кнопок для ик-пульта
#define BUTTON_CH 0x511DBB
#define BUTTON_CH_PLUS 0xEE886D7F
#define BUTTON_PREV 0x52A3D41F
#define BUTTON_NEXT 0xD7E84B1B
#define BUTTON_PAUSE 0x20FE4DBB
#define BUTTON_VOL_MINUS 0xF076C13B
#define BUTTON_VOL_PLUS 0xA3C8EDDB
#define BUTTON_EQ 0xE5CFBD7F
#define BUTTON_0 0xC101E57B
#define BUTTON_100 0x97483BFB
#define BUTTON_200 0xF0C41643
#define BUTTON_1 0x9716BE3F
#define BUTTON_2 0x3D9AE3F7
#define BUTTON_3 0x6182021B
#define BUTTON_4 0x8C22657B
#define BUTTON_5 0x488F3CBB
#define BUTTON_6 0x449E79F
#define BUTTON_7 0x32C6FDF7
#define BUTTON_8 0x1BC0157B
#define BUTTON_9 0x3EC3FC1B

#define BEEP 1 //порт управления зуммером

#define LEFT_MOTOR_SPEED 5 //порт(шим) управления скоростью левого двигателя
#define RIGHT_MOTOR_SPEED 6 //порт(шим) управления скоростью правого двигателя
#define LEFT_MOTOR_CONTROL_1 2 //порты управления левым двигателем
#define LEFT_MOTOR_CONTROL_2 4
#define RIGHT_MOTOR_CONTROL_1 7 //порты управления правым двигателем
#define RIGHT_MOTOR_CONTROL_2 8

#define SERVO_CONTROL 9 //порт управления сервомашинкой
#define ULTRASONIC_CONTROL_1 12 //порты управления ультразвуковым датчиком
#define ULTRASONIC_CONTROL_2 13
#define IR_CONTROL 10 //порт управления инфракрасным датчиком расстояния
#define IR_RECIVE 11 //порт управления ик-приемника сигнала от пульта

#define DEFAULT_LEFT_SPEED 50 //базовые скорости двигателей (от 0 до 255)
#define DEFAULT_RIGHT_SPEED 50

#define MAX_LEFT_SPEED 255 //максимальные мкорости двигателей
#define MAX_RIGHT_SPEED 255

#define MIN_LEFT_SPEED 50 //минимальные скорости двигателей
#define MIN_RIGHT_SPEED 50

IRrecv irrecv(IR_RECIVE); //инициализация ик-приемника
decode_results results;

Ultrasonic ultrasonic(ULTRASONIC_CONTROL_1, ULTRASONIC_CONTROL_2); //инициализация ультразвукового датчика
Servo servo;

int rotate_count; //объявление переменных
bool automatic;
int currentLeftSpeed;
int currentRightSpeed;
int ranges[19];
int dir;
long dist;
int maximumInAzimut;
int maximumInHorizont;
int azimut;
float factor = 2.83333f;
bool rtl = true;
bool needRotateLeft = true;

void LeftMotorForward() //функция движения вперед левого двигателя
{
  digitalWrite(LEFT_MOTOR_CONTROL_1, HIGH);
  digitalWrite(LEFT_MOTOR_CONTROL_2, LOW);
}

void LeftMotorBackward() //функция движения назад левого двигателя
{
  digitalWrite(LEFT_MOTOR_CONTROL_1, LOW);
  digitalWrite(LEFT_MOTOR_CONTROL_2, HIGH);
}

void RightMotorForward() //функция движения вперед правого двигателя
{
  digitalWrite(RIGHT_MOTOR_CONTROL_1, LOW);
  digitalWrite(RIGHT_MOTOR_CONTROL_2, HIGH);
}

void RightMotorBackward() //функция движения назад правого двигателя
{
  digitalWrite(RIGHT_MOTOR_CONTROL_1, HIGH);
  digitalWrite(RIGHT_MOTOR_CONTROL_2, LOW);
}

void LeftMotorSpeed(int spd) //функция задачи скорости для левого двигателя
{
  analogWrite(LEFT_MOTOR_SPEED, spd);
}

void RightMotorSpeed(int spd) //функция задачи скорости для правого двигателя
{
  analogWrite(RIGHT_MOTOR_SPEED, spd);
}

void Reverse() //функция движения робота назад
{
  RightMotorBackward();
  LeftMotorBackward();
  RightMotorSpeed(currentRightSpeed);
  LeftMotorSpeed(currentLeftSpeed);
}

void RotateRight() //функция вращения робота направо
{
  LeftMotorForward();
  RightMotorBackward();
  RightMotorSpeed(MAX_RIGHT_SPEED);
  LeftMotorSpeed(MAX_LEFT_SPEED);
}

void RotateLeft() //функция вращения робота налево
{
  RightMotorForward();
  LeftMotorBackward();
  RightMotorSpeed(MAX_RIGHT_SPEED);
  LeftMotorSpeed(MAX_LEFT_SPEED);
}

void Stop() //функция остановки робота
{
  RightMotorSpeed(0);
  LeftMotorSpeed(0);
}

void Run() //фукнция движения робота вперед
{
  RightMotorForward();
  LeftMotorForward();
}

void HeadFront() //фукнция разворота сервомашинки вперед
{
  servo.write(90);
}

bool GetFrontBarrier_IR() //функция проверки на препятствие по ик-датчику (его срабатываение)
{
  return digitalRead(IR_CONTROL) == LOW;
}

bool GetFrontBarrier() //функция проверки на препятствие спереди
{
  dist = GetDistance(90); //замер дистанции с ультразвукового датчика
  bool b = GetFrontBarrier_IR(); //сработка ик-датчика
  Serial.println(b); //отладочная печать в порт
  return (dist <= RANGE) || b; //условие нахождения препятствия и возврат результата
}

void SetMaxSpeed() //функция установки максимальной скорости
{
  currentLeftSpeed = MAX_LEFT_SPEED;
  currentRightSpeed = MAX_LEFT_SPEED;
}

void SetMinSpeed() //функция установки минимальной скорости
{
  currentLeftSpeed = MIN_LEFT_SPEED;
  currentRightSpeed = MIN_LEFT_SPEED;
}

void RotateLeft180() //функция разворота на 180 градусов налево
{
  RotateLeft();
  delay(ROTATION_DELAY_180);
  Stop();
}

void RotateRight180() //функция разворота на 180 градусов направо
{
  RotateRight();
  delay(ROTATION_DELAY_180);
  Stop();
}

void GetRanges() //функция сканирования пространства
{
  if (rtl) //rtl - флаг сканирования справа или слева
  {
    for (int i = 1; i < 18; i++) //проход по горизонту с шагом по 10 градусов и замер дистанции по ультразвуковому датчику
      ranges[i] = GetDistance(i * 10); //в GetDistance передается угол необходимого поворота сервомашинки, заполняется массив ranges
    rtl = false;
    LightOn();
  }
  else
  {
    for (int i = 17; i >= 1; i--) //обратный проход по горизонту
      ranges[i] = GetDistance(i * 10);
    rtl = true;
    LightOff();
  }
}

int GetDistance(int dir) //функция замера дистанции
{
  if (GetFrontBarrier_IR()) //проверка на препятствие инфракрасным датчиком
  {
    LongBeep();
    Reverse();
    delay(1000);
    Stop();
    LongBeep();
    RotateLeft180();
  }

  servo.write(dir); //поворот сервомашинки на угол
  delay(10); //задержка в миллисекундах
  int dist1 = ultrasonic.Ranging(1); //замер №1
  delay(5);
  int dist2 = ultrasonic.Ranging(1); //замер №2
  dist = min(dist1, dist2); //выбор минимального значения из замеров, чтобы отфильтровать погрешность
  return dist;
}

int GetDirection() //функция выбора направления движения робота
{
  int prevmax = maximumInHorizont; //сохранение предыдущего значения максимальной дистанции по горизонту
  int prevazimut = azimut; //сохранение предущего азимута направления движения
  maximumInHorizont = 0; //обнуление максимального значения по горизонту
  azimut = 0; //обнуление максимального значения по горизонту
  for (int i = 2; i < 17; i++) //проход по массиву дистанций
  {
    if (ranges[i] > maximumInHorizont) //входное условие для кластерного анализа
    {
      if ((ranges[i - 1]  > ranges[i] * 0.7f) && (ranges[i + 1] > ranges[i] * 0.7f)) //выявление кластера пустот, где 0.7 - это допустимое отклонение расстояния
      {
        maximumInHorizont = ranges[i]; //присвоение максимального значения (или среднего из кластера пустоты)
        azimut = i; //сохранение азимута (направления), где был обнаружен кластер пустот
      }
    }
  }
  if (maximumInHorizont == 0) //случай, если кластер не был обнаружен
  {
    azimut = prevazimut; //использование старых азимута и максимального заначения дальности
    maximumInHorizont = prevmax;
  }
  return azimut * 10; //возвращается азимут в градусах
}

void CheckButtons() //обработка сигналов с пульта
{
  if (irrecv.decode(&results))
  {   
    if (results.value == BUTTON_EQ)
    {
      automatic = true; //вход в автоматический режим
    }
    else if (results.value == BUTTON_100)
    {
      currentRightSpeed = currentRightSpeed + 50;
    }
    else if (results.value == BUTTON_200)
    {
      currentLeftSpeed = currentLeftSpeed + 50;
    }
    else if (results.value == BUTTON_2)
    {
      Run();
    }
    else if (results.value ==  BUTTON_4)
    {
      RotateLeft();
    }
    else if (results.value == BUTTON_5)
    {
      Stop();
      automatic = false;
    }
    else if (results.value == BUTTON_6)
    {
      RotateRight();
    }
    else if (results.value ==  BUTTON_7)
    {
      RotateLeft180();
    }
    else if (results.value ==  BUTTON_8)
    {
      Reverse();
    }
    else if (results.value ==  BUTTON_9)
    {
      RotateRight180();
    }
    irrecv.resume();
  }
}

void SmartMode() //автоматический режим
{
  Run(); //начало движения вперед
  int AzimutDrive = 0; //обнуляется значение направления движения
  GetRanges(); //сканируется пространство
  ShortBeep();
  AzimutDrive = GetDirection(); //находится азимут движения
  if (maximumInHorizont < RANGE) //случай, если расстояние до объекта меньше допустимого (т.е. робот находится в замкнутом пространстве)
    if (needRotateLeft) //флаг, необходимый для чередования поворотов налево/направо
    {
      needRotateLeft = false;
      RotateLeft180();
    }
    else
    {
      needRotateLeft = true;
      RotateRight180();
    }
  AzimutDrive = 90 - AzimutDrive; //изменение диапазона направления движения на -90..90
  currentLeftSpeed = min(255, (int)(255 + (AzimutDrive * factor))); //корректировка скорости левого двигателя по формуле, зависящей от азимута, где находится кластер пустот
  currentRightSpeed = min(255, (int)(255 - (AzimutDrive * factor))); //корректировка скорости правого двигателя по формуле, зависящей от азимута, где находится кластер пустот
  //factor - константа, подобранная экспериментально
  LeftMotorSpeed(currentLeftSpeed); //присвоение новой скорости двигателям
  RightMotorSpeed(currentRightSpeed);
}

void ShortBeep() //короткий зум
{
  digitalWrite(BEEP, HIGH);
  delay(5);             
  digitalWrite(BEEP, LOW);
}

void LongBeep() //длинный зум
{
  digitalWrite(BEEP, HIGH);
  delay(100);             
  digitalWrite(BEEP, LOW); 
}

void setup()
{
  Serial.begin(9600); //инициализация COM-порта
  irrecv.enableIRIn(); //инициализация приемника сигнала от пульта
  pinMode(IR_CONTROL , INPUT); //установка порта для ик-датчика расстояния

  servo.attach(SERVO_CONTROL); //установка порта для сервомашинки
  servo.write(90); //начальное положение сервомашинки

  //установка портов для управления двигателями
  pinMode(LEFT_MOTOR_CONTROL_1, OUTPUT);
  pinMode(LEFT_MOTOR_CONTROL_2, OUTPUT);
  pinMode(LEFT_MOTOR_SPEED, OUTPUT);

  pinMode(RIGHT_MOTOR_CONTROL_1, OUTPUT);
  pinMode(RIGHT_MOTOR_CONTROL_2, OUTPUT);
  pinMode(RIGHT_MOTOR_SPEED, OUTPUT);

  //установка портов для управления зуммером
  pinMode(BEEP, OUTPUT);
  digitalWrite(BEEP, LOW);
 
  //установка базовых значений
  rotate_count = 0;
  automatic = false;

  currentLeftSpeed = DEFAULT_LEFT_SPEED;
  currentRightSpeed = DEFAULT_RIGHT_SPEED;
  maximumInHorizont = 30;
  azimut = 90;
}

void loop()
{
  CheckButtons(); //проверка кнопок

  if (automatic)
  {
    SmartMode(); //запуск автоматического режима
  }
}
anton
Администратор
Аватара
Откуда: Россия, Ростов-на-Дону
Репутация: 33

  • 1

#2 anton » 28.11.2015, 20:49

Новая версия робота с поддержкой Bluetooth:
Код: Выделить всё
// bluetooth модуль подключен к 0 и 1 портам, весь обмен с модулем осуществляется через стандартный COM-порт Serial

#include <IRremote.h> //библиотека ИК-пульта
#include <Ultrasonic.h> // библиотека УЗ сонара
#include <Servo.h> // библиотека управления сервой

#define VERSION "ANTONSDROID v1.0" // название и версия программы для передачи по bluetooth
#define MAX_BT_COMMAND_LENGTH 20 // максимальный размер команды по bluetooth

#define ROTATION_DELAY_90 300 //задержка поворота на 90 градусов
#define ROTATION_DELAY_180 700 //задержка поворота на 180 градусов
#define ROTATION_DELAY_45 150 //задержка поворота на 45 градусов

#define RANGE 25 //дальность срабатывания ультразвукового датчика в сантиметрах

#define BUTTON_CH_MINUS 0xE318261B //коды кнопок для ик-пульта
#define BUTTON_CH 0x511DBB
#define BUTTON_CH_PLUS 0xEE886D7F
#define BUTTON_PREV 0x52A3D41F
#define BUTTON_NEXT 0xD7E84B1B
#define BUTTON_PAUSE 0x20FE4DBB
#define BUTTON_VOL_MINUS 0xF076C13B
#define BUTTON_VOL_PLUS 0xA3C8EDDB
#define BUTTON_EQ 0xE5CFBD7F
#define BUTTON_0 0xC101E57B
#define BUTTON_100 0x97483BFB
#define BUTTON_200 0xF0C41643
#define BUTTON_1 0x9716BE3F
#define BUTTON_2 0x3D9AE3F7
#define BUTTON_3 0x6182021B
#define BUTTON_4 0x8C22657B
#define BUTTON_5 0x488F3CBB
#define BUTTON_6 0x449E79F
#define BUTTON_7 0x32C6FDF7
#define BUTTON_8 0x1BC0157B
#define BUTTON_9 0x3EC3FC1B

#define BEEP 3 //порт управления зуммером

#define LEFT_MOTOR_SPEED 5 //порт(шим) управления скоростью левого двигателя
#define RIGHT_MOTOR_SPEED 6 //порт(шим) управления скоростью правого двигателя
#define LEFT_MOTOR_CONTROL_1 2 //порты управления левым двигателем
#define LEFT_MOTOR_CONTROL_2 4
#define RIGHT_MOTOR_CONTROL_1 7 //порты управления правым двигателем
#define RIGHT_MOTOR_CONTROL_2 8

#define SERVO_CONTROL 9 //порт управления сервомашинкой
#define ULTRASONIC_CONTROL_1 12 //порты управления ультразвуковым датчиком
#define ULTRASONIC_CONTROL_2 13
#define IR_CONTROL 10 //порт управления инфракрасным датчиком расстояния
#define IR_RECIVE 11 //порт управления ик-приемника сигнала от пульта

#define DEFAULT_LEFT_SPEED 50 //базовые скорости двигателей (от 0 до 255)
#define DEFAULT_RIGHT_SPEED 50

#define MAX_LEFT_SPEED 255 //максимальные мкорости двигателей
#define MAX_RIGHT_SPEED 255

#define MIN_LEFT_SPEED 50 //минимальные скорости двигателей
#define MIN_RIGHT_SPEED 50

//объявление переменных

char command_buf[MAX_BT_COMMAND_LENGTH]; // массив для получения команды по bluetooth
int throttleLeft, throttleRight, throttleServo; // величины для левого, правого двигателя и сервы в процентах - приходят по bluetooth

IRrecv irrecv(IR_RECIVE); //инициализация ик-приемника
decode_results results;

Ultrasonic ultrasonic(ULTRASONIC_CONTROL_1, ULTRASONIC_CONTROL_2); //инициализация сонара
Servo servo;

int rotate_count;
bool automatic;
int currentLeftSpeed;
int currentRightSpeed;
int ranges[19];
int dir;
long dist;
int maximumInAzimut;
int maximumInHorizont;
int azimut;
float factor = 2.83333f;
bool rtl = true;
bool needRotateLeft = true;
bool motion_dir_right;
bool motion_dir_left;

void LeftMotorForward() //функция движения вперед левого двигателя
{
  digitalWrite(LEFT_MOTOR_CONTROL_1, HIGH);
  digitalWrite(LEFT_MOTOR_CONTROL_2, LOW);
  motion_dir_left = true;
}

void LeftMotorBackward() //функция движения назад левого двигателя
{
  digitalWrite(LEFT_MOTOR_CONTROL_1, LOW);
  digitalWrite(LEFT_MOTOR_CONTROL_2, HIGH);
  motion_dir_left = false;
}

void RightMotorForward() //функция движения вперед правого двигателя
{
  digitalWrite(RIGHT_MOTOR_CONTROL_1, LOW);
  digitalWrite(RIGHT_MOTOR_CONTROL_2, HIGH);
  motion_dir_right = true;
}

void RightMotorBackward() //функция движения назад правого двигателя
{
  digitalWrite(RIGHT_MOTOR_CONTROL_1, HIGH);
  digitalWrite(RIGHT_MOTOR_CONTROL_2, LOW);
  motion_dir_right = false;
}

void LeftMotorSpeed(int spd) //функция задачи скорости для левого двигателя
{
  analogWrite(LEFT_MOTOR_SPEED, spd);
}

void RightMotorSpeed(int spd) //функция задачи скорости для правого двигателя
{
  analogWrite(RIGHT_MOTOR_SPEED, spd);
}

void Reverse() //функция движения робота назад
{
  RightMotorBackward();
  LeftMotorBackward();
  RightMotorSpeed(currentRightSpeed);
  LeftMotorSpeed(currentLeftSpeed);
}

void RotateRight() //функция вращения робота направо
{
  LeftMotorForward();
  RightMotorBackward();
  RightMotorSpeed(MAX_RIGHT_SPEED);
  LeftMotorSpeed(MAX_LEFT_SPEED);
}

void RotateLeft() //функция вращения робота налево
{
  RightMotorForward();
  LeftMotorBackward();
  RightMotorSpeed(MAX_RIGHT_SPEED);
  LeftMotorSpeed(MAX_LEFT_SPEED);
}

void Stop() //функция остановки робота
{
  RightMotorSpeed(0);
  LeftMotorSpeed(0);
}

void Run() //фукнция движения робота вперед
{
  RightMotorForward();
  LeftMotorForward();
}

void HeadFront() //фукнция разворота сервомашинки вперед
{
  servo.write(90);
}

bool GetFrontBarrier_IR() //функция проверки на препятствие по ик-датчику (его срабатываение)
{
  return digitalRead(IR_CONTROL) == LOW;
}

bool GetFrontBarrier() //функция проверки на препятствие спереди
{
  dist = GetDistance(90); //замер дистанции с ультразвукового датчика
  bool b = GetFrontBarrier_IR(); //сработка ик-датчика
  Serial.println(b); //отладочная печать в порт
  return (dist <= RANGE) || b; //условие нахождения препятствия и возврат результата
}

void SetMaxSpeed() //функция установки максимальной скорости
{
  currentLeftSpeed = MAX_LEFT_SPEED;
  currentRightSpeed = MAX_LEFT_SPEED;
}

void SetMinSpeed() //функция установки минимальной скорости
{
  currentLeftSpeed = MIN_LEFT_SPEED;
  currentRightSpeed = MIN_LEFT_SPEED;
}

void RotateLeft180() //функция разворота на 180 градусов налево
{
  RotateLeft();
  delay(ROTATION_DELAY_180);
  Stop();
}

void RotateRight180() //функция разворота на 180 градусов направо
{
  RotateRight();
  delay(ROTATION_DELAY_180);
  Stop();
}

void GetRanges() //функция сканирования пространства
{
  if (rtl) //rtl - флаг сканирования справа или слева
  {
    for (int i = 1; i < 18; i++) //проход по горизонту с шагом по 10 градусов и замер дистанции по ультразвуковому датчику
      ranges[i] = GetDistance(i * 10); //в GetDistance передается угол необходимого поворота сервомашинки, заполняется массив ranges
    rtl = false;
    //LightOn();
  }
  else
  {
    for (int i = 17; i >= 1; i--) //обратный проход по горизонту
      ranges[i] = GetDistance(i * 10);
    rtl = true;
    //LightOff();
  }
}

int GetDistance(int dir) //функция замера дистанции
{
  if (GetFrontBarrier_IR()) //проверка на препятствие инфракрасным датчиком
  {
    LongBeep();
    Reverse();
    Serial.println("Backward");
    delay(1000);
    Stop();
    LongBeep();
    RotateLeft180();
  }

  servo.write(dir); //поворот сервомашинки на угол
  delay(10); //задержка в миллисекундах
  int dist1 = ultrasonic.Ranging(1); //замер №1
  delay(5);
  int dist2 = ultrasonic.Ranging(1); //замер №2
  dist = min(dist1, dist2); //выбор минимального значения из замеров, чтобы отфильтровать погрешность
  return dist;
}

int GetDirection() //функция выбора направления движения робота
{
  int prevmax = maximumInHorizont; //сохранение предыдущего значения максимальной дистанции по горизонту
  int prevazimut = azimut; //сохранение предущего азимута направления движения
  maximumInHorizont = 0; //обнуление максимального значения по горизонту
  azimut = 0; //обнуление максимального значения по горизонту
  for (int i = 2; i < 17; i++) //проход по массиву дистанций
  {
    if (ranges[i] > maximumInHorizont) //входное условие для кластерного анализа
    {
      if ((ranges[i - 1]  > ranges[i] * 0.7f) && (ranges[i + 1] > ranges[i] * 0.7f)) //выявление кластера пустот, где 0.7 - это допустимое отклонение расстояния
      {
        maximumInHorizont = ranges[i]; //присвоение максимального значения (или среднего из кластера пустоты)
        azimut = i; //сохранение азимута (направления), где был обнаружен кластер пустот
      }
    }
  }
  if (maximumInHorizont == 0) //случай, если кластер не был обнаружен
  {
    azimut = prevazimut; //использование старых азимута и максимального заначения дальности
    maximumInHorizont = prevmax;
  }
  return azimut * 10; //возвращается азимут в градусах
}

void CheckButtons() //обработка сигналов с пульта
{
  if (irrecv.decode(&results))
  {   
    if (results.value == BUTTON_EQ)
    {
      automatic = true; //вход в автоматический режим
    }
    else if (results.value == BUTTON_100)
    {
      currentRightSpeed = currentRightSpeed + 50;
    }
    else if (results.value == BUTTON_200)
    {
      currentLeftSpeed = currentLeftSpeed + 50;
    }
    else if (results.value == BUTTON_2)
    {
      Run();
    }
    else if (results.value ==  BUTTON_4)
    {
      RotateLeft();
    }
    else if (results.value == BUTTON_5)
    {
      Stop();
      automatic = false;
    }
    else if (results.value == BUTTON_6)
    {
      RotateRight();
    }
    else if (results.value ==  BUTTON_7)
    {
      RotateLeft180();
    }
    else if (results.value ==  BUTTON_8)
    {
      Reverse();
    }
    else if (results.value ==  BUTTON_9)
    {
      RotateRight180();
    }
    irrecv.resume();
  }
}

void CheckBluetooth()
{
  Serial.flush();
  String command_name = GetCommand();
  if (command_name.length() > 0)
  {
    //Serial.println("Received command: " + command_name);
    if (command_name == "STATUS")
    {
      //String ver = VERSION;
      //Serial.println("Software version: " + ver);
      //Serial.println("Left:" + String(throttleLeft) + " Right:" + String(throttleRight) + " Servo:" + String(throttleServo));
      //Serial.println("Status confirmed");
      int dist_ping = ultrasonic.Ranging(1);
      delay(150);
      Serial.println("Distance: " + String(dist_ping));
    }
    else if (command_name == "RUN")
    {
      Run();
      LeftMotorSpeed(150); 
      RightMotorSpeed(150); 
      automatic = false;
      Serial.println("Running on confirmed");

    }
    else if (command_name == "AIMODE")
    {
      automatic = true;
      Serial.println("I'm going to AI mode - confirmed");

    }
    else if (command_name == "HALT")
    {
      Stop();
      HeadFront();
      automatic = false;
      Serial.println("Halt confirmed");
    }
    else
    {
      String motor_select = command_name.substring(0, 1);
      String motor_speed = command_name.substring(1, command_name.length());
      if (motor_speed.length() > 0)
      {
        if (motor_select == "L")
        {
           throttleLeft = motor_speed.toInt();
           Serial.println("Left " + String(throttleLeft) + " confirmed");
           automatic = false;
           if (throttleLeft > 50)
           {
             LeftMotorForward();
             LeftMotorSpeed((throttleLeft - 50) * 5.1f); 
           }
           else
           {
             LeftMotorBackward();
             LeftMotorSpeed((50 - throttleLeft) * 5.1f);
           }
        }
        else if (motor_select == "R")
        {
           throttleRight = motor_speed.toInt();
           Serial.println("Right " + String(throttleRight) + " confirmed");
           automatic = false;
           if (throttleRight > 50)
           {
             RightMotorForward();
             RightMotorSpeed((throttleRight - 50) * 5.1f); 
           }
           else
           {
             RightMotorBackward();
             RightMotorSpeed((50 - throttleRight) * 5.1f);
           }
        }
        else if (motor_select == "S")
        {
           throttleServo = motor_speed.toInt();
           Serial.println("Servo " + String(throttleServo) + " confirmed");
           automatic = false;
           servo.write(180 - (throttleServo * 1.8f));
        }
      }
    }
  }
}

void SmartMode() //автоматический режим
{
  Run(); //начало движения вперед
  int AzimutDrive = 0; //обнуляется значение направления движения
  GetRanges(); //сканируется пространство
  //ShortBeep();
  AzimutDrive = GetDirection(); //находится азимут движения
 
  if (maximumInHorizont < RANGE) //случай, если расстояние до объекта меньше допустимого (т.е. робот находится в замкнутом пространстве)
    if (needRotateLeft) //флаг, необходимый для чередования поворотов налево/направо
    {
      needRotateLeft = false;
      RotateLeft180();
      Serial.println("No space");
    }
    else
    {
      needRotateLeft = true;
      RotateRight180();
      Serial.println("No space");
    }
   
  AzimutDrive = 90 - AzimutDrive; //изменение диапазона направления движения на -90..90
  currentLeftSpeed = min(255, (int)(255 + (AzimutDrive * factor))); //корректировка скорости левого двигателя по формуле, зависящей от азимута, где находится кластер пустот
  currentRightSpeed = min(255, (int)(255 - (AzimutDrive * factor))); //корректировка скорости правого двигателя по формуле, зависящей от азимута, где находится кластер пустот
  //factor - константа, подобранная экспериментально
  LeftMotorSpeed(currentLeftSpeed); //присвоение новой скорости двигателям
  RightMotorSpeed(currentRightSpeed);t

  int telemetry_rightm;
  int telemetry_leftm;
 
  if (motion_dir_right = true)
  {
    telemetry_rightm = int((currentRightSpeed/5.1f) + 50);
  }
  else
  {
    telemetry_rightm = int(50 - (currentRightSpeed/5.1f));
  }

  if (motion_dir_left = true)
  {
    telemetry_leftm = int((currentLeftSpeed/5.1f) + 50);
  }
  else
  {
    telemetry_leftm = int(50 - (currentLeftSpeed/5.1f));
  }
 
  Serial.println("L: " + String(telemetry_leftm) + " R: " + String(telemetry_rightm) + " A: " + String(AzimutDrive));     
}

void ShortBeep() //короткий зум
{
  digitalWrite(BEEP, HIGH);
  delay(5);             
  digitalWrite(BEEP, LOW);
}

void LongBeep() //длинный зум
{
  digitalWrite(BEEP, HIGH);
  delay(100);             
  digitalWrite(BEEP, LOW); 
}

String GetCommand() // получение команды из COM-порта (bluetooth), если команды нет, то возвращается пустая строка
{
  int inputCount = Serial.available();
  if (inputCount > 0)
  {
    int totalByte = Serial.readBytesUntil('\n', command_buf, MAX_BT_COMMAND_LENGTH);
    String command = command_buf;
    command = command.substring(0, totalByte);
    command.replace("\r", "");
    command.replace("\n", "");
    return command;
  }
  else
    return "";
}

// начальная настройка робота
void setup()
{
  Serial.begin(38400); //инициализация COM-порта, скорость 38400 бит в секунду, сюда будет подключен bluetooth модуль
  Serial.println(VERSION); // выводим версиию в COM-порт
  Serial.println("CONNECTED");
  Serial.flush();
  throttleLeft = 50;
  throttleRight = 50;
  throttleServo = 50;

  irrecv.enableIRIn(); //инициализация приемника сигнала от пульта
  pinMode(IR_CONTROL , INPUT); //установка порта для ик-датчика расстояния

  servo.attach(SERVO_CONTROL); //установка порта для сервомашинки
  servo.write(90); //начальное положение сервомашинки

  //установка портов для управления двигателями
  pinMode(LEFT_MOTOR_CONTROL_1, OUTPUT);
  pinMode(LEFT_MOTOR_CONTROL_2, OUTPUT);
  pinMode(LEFT_MOTOR_SPEED, OUTPUT);

  pinMode(RIGHT_MOTOR_CONTROL_1, OUTPUT);
  pinMode(RIGHT_MOTOR_CONTROL_2, OUTPUT);
  pinMode(RIGHT_MOTOR_SPEED, OUTPUT);

  //установка портов для управления зуммером
  pinMode(BEEP, OUTPUT);
  digitalWrite(BEEP, LOW);
 
  //установка базовых значений
  rotate_count = 0;
  automatic = false;

  currentLeftSpeed = DEFAULT_LEFT_SPEED;
  currentRightSpeed = DEFAULT_RIGHT_SPEED;
  maximumInHorizont = 30;
  azimut = 90;
}

void loop()
{
  CheckButtons(); //проверка кнопок
  CheckBluetooth(); // проверка команд от bluetooth
 
  if (automatic)
  {
    SmartMode(); //запуск автоматического режима
  }
}
anton
Администратор
Аватара
Откуда: Россия, Ростов-на-Дону
Репутация: 33

#3 alman » 21.12.2015, 03:52

Очень хорошо! Если не ошибаюсь, то такой способ управления устройством называется "суперцикл", когда в цикле последовательно опрашиваются всё устройства и даются команды. Преимущество суперцикла - простота. Но есть и недостатки - суперцикл не даёт процессору "заснуть" и поэтому процессор греется и жрёт батарейку. Это не так страшно для Ардуино, но потребление мощных процессоров сравнимо с потреблением мотора роботизированной платформы.

Есть и ещё одна проблема, но в данном случае она почти и не проблема. Суть в том, что во время выполнения инструкции delay может быть задержано какое-то другое событие, например анализ сигнала от датчика. Для небольшой роботизированной платформы это не проблема, но на некоторых задачах такие задержки недопустимы.

В общем, самое время попробовать прерывания от таймера и постараться выполнить какую-то задачу с помощью прерывания..
alman
Репутация: 4

#4 difo » 01.04.2017, 23:22

Здравствуйте . Я понимаю что прошло много времени когда вы сделали эту штуковину .Может схему подключения вспомните.Как я понял можно управлять и ИК и по БЛЮТУЗ. Какаой был использован драйвер двигателей.Спасибо за ваш ответ.
Неудача — это возможность начать заново, но уже более мудро
difo
Репутация: 0


Вернуться в Программирование

Кто сейчас на сайте (по активности за 60 минут)

Сейчас этот форум просматривают: 1 гость

cron