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

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

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

#1 anton » 08.11.2015, 20:04

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

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

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


Исходный код программы: $this->bbcode_second_pass_code('', '#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:
$this->bbcode_second_pass_code('', '// 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 минут)

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

cron