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

Размещено на http://www.allbest.ru

Размещено на http://www.allbest.ru

Введение

Задача управления подвижными объектами является одной из основных в робототехнике. Использование таких объектов очень широко: от детских игрушек, до военных дронов и космических аппаратов. Реализация сложных алгоритмов управления дорогими высокоточными механизмами на больших расстояниях под силу крупным научным организациям и конструкторским бюро, но для первоначального знакомства с некоторыми упрощенными основами подобного управления вполне достаточно возможностей недорого контроллера Arduino UNO и доступного Android-устройства.

Обычно сложные и дорогие роботы снабжаются не только ручным управлением, но и возможностью самостоятельно принимать решения и действовать в стандартных ситуациях. Сложность таких действий тоже разнится: от обхождения препятствий в роботе-пылесосе, до автопилотов быстро движущихся боевых дронов или ракет, обрабатывающих большой объем информации, приходящей от множества датчиков и сенсоров, способных принимать самостоятельное решение о поражении цели.

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

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

Задачи были поставлены передо мной следующие:

1. Создать подвижную трехколёсную платформу под управлением контроллера Arduino UNO.

2. Изучить особенности управления платформой. Познакомиться с конструктором для создания приложений Android App Inventor 2.

3. Создать многофункциональный пульт управления для Android.

4. Расширить навыки программирования среды Arduino и приобрести навыки создания приложений Android.

В своей работе я попыталась за счет взаимодействия «умного» пульта управления на Android и подвижной платформы на базе Arduino, создать робот-зонд, который бы обладал возможностью, как действовать самостоятельно, так и управляться оператором. При этом была добавлена возможность отдавать команды оператору голосом, что в определённых ситуациях, является удобным.

Программное обеспечение для робота написано на «Си»-подобном языке в среде разработки Arduino IDE (листинг кода в приложении). Программное обеспечение для пульта дистанционного управления выполнено в Scraсh-среде App Inventor 2.

Автоматическое поведение робота реализовано в двух режимах: первый имитирует объезд препятствий (для удобной наглядной демонстрации в небольших помещениях, этот режим показан в инверсии датчиков: робот не падает со стола), и «Демо» режим, в котором робот показывает возможности движения и световой индикации. Реализован полуавтоматический режим удержания расстояния до цели.

Измерение основного параметра может происходить по команде оператора в ручном режиме. Таким же образом может быть измерена дальность до препятствия. Результаты любых измерений транслируются на пульт оператору.

Световая индикация робота позволяет оператору позиционировать робота, не отвлекаясь на пульт управления, что бывает необходимо при некоторых ответственных обстоятельствах. И помимо этого, позволяет скрытно сигнализировать оператору о некоторых особенностях исполнения программы робота.

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

1. Теоретическая часть

В первую очередь следует описать собранную роботизированною трехколесную платформу -- робота-зонда с двумя ведущими колёсами и одним вращающимся пассивным колесом (далее робот). В качестве радиоканала удалённого управления роботом используется Bluetooth. В качестве многофункционального приемо-передающего пульта управления используется Android-устройство (планшет или телефон). Для простоты и из экономии средств, полезная нагрузка робота ограничена недорогим датчиком температуры и влажности, который достаточен для иллюстрации передачи измеряемого роботом основного параметра оператору. Но робот может быть оборудован и более сложными сенсорами. Кроме того, робот имеет несколько вспомогательных датчиков: два инфракрасных и один подвижный ультразвуковой датчик, - которые позволяют оператору позиционировать робота в необходимом месте для проведения основного измерения.

1.1 Составные части проекта

робот ультразвуковой датчик

1. Arduino-совместимый контроллер 16Hz R3 UNO (далее Arduino UNO).

2. Драйвер Maxim MAX7219 - 2 шт.

3. Светодиодная матрица 8х8, с общими катодами, красного свечения SH1388ASR - 2 шт.

4. Драйвер L293D.

5. Датчик температуры и влажности DHT-11.

6. ИК датчики FC-51 -- 2 шт.

7. Ультразвуковой датчик HC-SR04.

8. Модуль Bluetooth JY-MCU.

9. Микромоторы с редуктором 1:48 6V -2 шт.

10. Сервопривод TowerPRO MG-90.

11. Макетная монтажная плата.

12. Аndroid планшет.

13. Моторы 6В -- 2 шт.

Рисунок 1.1. Структурная схема элементов робота

2. Основная часть

2.1 Алгоритм работы робота

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

2. Ручное подключение к роботу Android-планшета управления по Bluetooth.

3. Запуск бесконечного главного цикла программы, работа которого сводится к чтению состояния последовательного порта, при появлении символа в нём. При отсутствии символа, программа исполняет тот блок кода (режим работы робота), который соответствовал предыдущему символу до тех пор, пока из последовательного порта не будет считан символ приводящий к исполнению другого блока кода. Для реализации этого поведения в программу включена глобальная переменная last_bluetooth_simvol, принимающая изначально значение «T», что соответствует режиму остановки робота, и устанавливаемая в значение соответствующее тому режиму работы, который последним был исполнен.

2.2 Алгоритмы работы режимов робота

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

Режим «А».

Автоматическое движение по столу с контролем края и способностью избегать падение.

1. Проверка срабатывания инфракрасных датчиков, проверка срабатывания отдельных датчиков, в случае отсутствия сработавших датчиков, движение вперёд в течение 20мс и уход на проверку принятого символа последовательного порта..

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

3. Уход на проверку принятого символа последовательного порта.

Режим «F», «B», «R», «L» режимы ручного управления

1. Движение робота в направлении «вперёд», «назад», «вправо», «влево» в течении 25мс сопровождаемое анимацией светодиодных матриц.

2. Уход на проверку принятого символа последовательного порта.

Режим «Т»

Режим остановки

1. Остановка робота на 50мс сопровождаемое анимацией светодиодных матриц.

2. Уход на проверку принятого символа последовательного порта.

Режим «e»

Режим ультразвукового измерения дистанции до объекта

1. Измерение дистанции в диапазоне 0 -- 99 см и индикация этих значений на светодиодные матрицы.

2. Уход на проверку принятого символа последовательного порта.

Режим «n» Режим демонстрации

1. Демонстрация «эпизода», состоящего из краткосрочных команд моторам движения, сервоприводу, светодиодным матрицам.

2. Уход на проверку принятого символа последовательного порта.

3. Переход к следующему эпизоду до достижения последнего «эпизода».

Режим «а» режим сопровождения роботом, попавшем в диапазон контролируемый ультразвуковым сенсором цели

1. Измерение расстояния ультразвуковым сенсором.

2. Сравнение измеренного расстояния с контролируемым диапазоном расстояний. При не попадании в диапазон контроля, обнуление переменной хранения последней сопровождаемой величины расстояния last_distance_sm

3. Запоминание измеренного значения в last_distance_sm при попадании в диапазон впервые, с анимацией светодиодными матрицами и последующим уходом на проверку принятого символа последовательного порта.

4. При не покидании измеренного диапазона расстояний объектом, подача на моторы движения управляющих сигналов, компенсирующих рассогласование между измеренным и контрольным расстоянием, сопровождаемый анимацией светодиодами. При этом более сильное рассогласование компенсируется более сильными компенсирующими воздействиями моторов (компенсация, по силе воздействия имеет три градации, для обслуживания трёх зон по 5см с «мёртвой зоной» 4 см посередине).

5. Уход на проверку принятого символа последовательного порта.

3. Практическая часть

Рисунок 3.1. Робот-зонд (вид спереди)

3.1 Описание особенностей реализации проекта

Питание контроллера и механизмов производится от Li-on батарейного блока 2S и составляет 6,4 -- 8,4 вольта. Силовые цепи драйверов моторов и питаются напрямую, а другая электроника через линейный стабилизатор 5 вольт AMS1117. Таким образом, падение напряжения в процессе эксплуатации робота в данной версии проекта никак не компенсируется. Кроме того, предотвращение глубокого разряда Li-on батарей, тоже программно не отслеживается, а контролируется пока вручную. В следующих версиях платформы планируется автоматизировать эти процессы.

Контроллер Arduino UNO управляет через силовой драйвер L293D двумя коллекторными шестивольтовыми микромоторами с редукторами 1:48. На шпинделях редукторов размещены перфорированные диски с 20 прорезями, которые находятся между датчиков угловых энкодеров RM827. Энкодеры смонтированы, но пока не подключены, они будут использованы для проведения будущих исследований алгоритмов управления движения платформы, оставляя возможности перспективного совершенствования робота.

Два инфракрасных датчика FC-51 размещены спереди платформы на выносных консолях для того, чтобы компенсировать инерцию робота в момент обнаружения препятствия (края стола). В «нормальном» режиме работы («нет края, ехать прямо можно»), датчики передают на цифровой вход контроллера сигнал высокого уровня, в случае обнаружения края стола под датчиком, передаётся низкоуровневый сигнал на контроллер для принятия мер к остановке и изменению направления движения. При этом данное действие сопровождается визуализацией с использованием сервопривода, который поворачивает консоль со светодиодными матрицами в направлении сработавшего инфракрасного датчика, и изменением состояния самих матриц-глаз с обычного, «чуть прищуренного» на «удивлённо-распахнутое» состояние. Эти действия маскируют небольшую задержку, достаточную для работы компенсирующего мотора по изменению направления движения. Таким образом визуализация служит нескольким целям: позволяет произвести задержку по времени главного цикла программы, необходимую для работы мотора в компенсационном режиме, и служит цели подсказки оператору робота точки исполнения алгоритма основной программы. Ну и, конечно, придаёт зрелищности работе всего робота, наделяет его элементами человеческого поведения: «попало что-то под ногу - посмотреть на препятствие».

На работе светодиодных матриц 8х8 в связке с драйвером MAX7219 отдельно останавливаться не буду, так как эта тема была мною раскрыта в моём предыдущем проекте. По итогам предыдущего проекта об управлении одной светодиодной матрицей, уже после защиты проекта, мною был снят и опубликован на Youtube небольшой получасовой учебный фильм. Его можно посмотреть по ссылке: https://youtu.be/BssRFlF3emg. Там я подробно рассказываю и показываю каким образом происходит подключение матриц и управление ими. Единственным отличием от предыдущего проекта, стало каскадирование двух матриц, что привело к необходимости изменить частично функции трансляции состояний матриц с 16-и битных посылок на 32-битные.

Рисунок 3.2. Робот-зонд (вид сбоку)

Ультразвуковой сенсор HC-SR04 закреплён на рычаге сервопривода и дополнительно является консолью для крепления светодиодных матриц. Такое крепление сенсора на сервопривод позволит в последующих версиях платформы реализовать алгоритмы обхода препятствия основанные на оценке и сравнении расстояния до препятствия и пространства слева и справа от него. В настоящем проекте ультразвуковой сенсор используется только для режима сопровождения платформой препятствия непосредственно перед платформой. В этом режиме запоминается расстояние до объекта попавшего в диапазон, и в зависимости от измерения этого расстояния на следующей итерации основного цикла программы, сравнивая это значение с вновь измеренным, принимается решения о направлении движения: либо за объектом (объект удаляется), либо от объекта (объект приближается).

В данной работе я познакомилась с особенностями подключения моторов к контроллеру, при этом преследуя цель попрактиковаться в искусстве пайки, мною было выбрано не готовое решение Shild с драйвером мотора, а непосредственно сама микросхема драйвера L293D, которую я самостоятельно распаяла на монтажную плату и подключила к контроллеру. При этом, такое решение позволило мне оставить для дальнейших исследования свободными 2 и 3 выводы контроллера, куда я в дальнейшем планирую подключить энкодеры углового положения колеса, что позволит в дальнейшем воспользоваться внешними прерываниями, которые в контроллере Arduino UNO можно подключить только к этим выводам. (В настоящем проекте энкодеры установлены на платформу, но не подключены).

Программное обеспечение для робота написано на «Си»-подобном языке в среде разработки Arduino IDE (листинг кода в приложении). Программное обеспечение для пульта дистанционного управления выполнено в Scraсh-среде AppInventor 2.

Рисунок 3.3. Окно программы AppInventor2 с эмуляцией пульта ДУ

3.2 Пульт дистанционного управления для робота-зонда

Управление роботом осуществляется через беспроводный протокол Bluetooth, подключенный к выводам последовательного порта Arduino. Пультом управления роботом может служить любое Аndroid-устройство (версией 3.2 и старше). Для этого на него необходимо загрузить написанное мною в среде-конструкторе App Inventor 2 приложение AlenaBluetooth_35. После установки этого приложения на экране устройства появляется пульт управления и появляется возможность установить Bluetooth-соединение с модулем JY-MCU, являющимся Blluetooth-шлюзом робота. Этот модуль распознаётся как устройство с именем «linvor». Пароль соединения -- 1234.

Кнопки-стрелки в верхней части экрана переводят робота на ручное управление, овальные разноцветные кнопки снизу -- включают один из пяти режимов. Кнопка «А» (красная кнопка) - режим движения по столу и контроля его края, режим «Л» - режим измерения температуры и влажности, режим «Ё» - режим измерения расстояния дальномером режим «Н»- активирует режим демонстрации возможностей светодиодных матриц и сервопривода с небольшими движениями платформы, и режим «А»(синяя кнопка) -- режим сопровождения цели попавшей в диапазон. Каждая из этих кнопок по событию TouchDown посылает один управляющий символ методом SendText объекта BluetoothClient1. При этом событие TouchUp имеют только кнопки-стрелки, которые посылают сигнал остановки на платформу.



Рисунок 3.4. Пульт дистанционного управления для робота-зонда

Таким образом получается, что при «отпускании» пальца от стрелки на экране планшета робот в ручном режиме останавливается, а при отпускании любой цветной овальной кнопки -- ничего в порт не посылается и продолжает исполняться тот режим, который был вызван ранее.

Команды управления роботом принимаются модулем Bluetooth JY-MCU и транслируются на выводы контроллера Arduino с номерами 0 и 1, на которых у него аппаратно реализован интерфейс последовательного порта. Данные считываются программно-аппаратными средствами контроллера Arduino с выводов последовательного порта, и далее происходит выполнение, в соответствии с написанным мною программным отработчиком, полученных условных команд-символов.

Голосовое управление реализовано с использованием App Inventor Speech Recognition - метода основанного на On-line Google API для распознания речи. Голосовые команды распознаются Android, после чего сравниваются с шаблонами фраз в приложении пульта, и в случае совпадения фразы, на Arduino отправляется нужная цифровая команда.



Рисунок 3.5. Схема сигнальных цепей робота

Заключение

В результате работы над проектом был создан универсальный прототип робота-зонда, позволяющего измерять два контрольных параметра (температура и влажность) и позиционировать робота-зонда с помощью дополнительного параметра (удаление от объекта) на расстоянии до 10 метров. Также, реализована возможность автономного обследования роботом закрытой зоны, с передачей сигнала об увеличении контролируемых параметров выше допустимых.

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

В процессе изготовления платформы я впервые для себя самостоятельно применила технологию пайки, ощутимо расширила свои знания программирования на языке С и, так же, впервые для себя познакомилась с конструктором программ для Android App Inventor 2, создав небольшой пульт управления Android для робота.

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

Литература

1. У. Соммер. Программирование микроконтроллерных пплат Arduino/Freeduino «БХВ-Петербург», 2012 ISBN:978-5977507271.

2. Б. Кернегиан, Д. Ритчи. Язык программирования Си. М. «Финансы и статистика», 1992. ISBN:5-279-00-473-1.

3. Arduino (официальный сайт)http://www.arduino.cc

4. Data sheet драйвера MAX7219 http://datasheets.maximintegrated.com/en/ds/MAX7219-MAX7221.pdf

5. Описание светодиодной матрицы SH1388ASR http://arduino.cc/en/Tutorial/RowColumnScanning

6. Статья «Когда не хватает ног» http://avrproject.ru/publ/kak_podkljuchit/kogda_ne_khvataet_nog_chast_3_drajver_indikatorov_max7221/2-1-0-142 (AVRproject.ru)

Приложение

Код программы

#define INPUT4 4//

#define IR_Pin_Forward_Left 9//

#define IR_Pin_Forward_Right 7//

#define INPUT2 11//

#define INPUT1 8//

#define ENABL1 5//

#define ENABL2 6//

#define IR_Pin_Backward_Left 10//

#define INPUT3 12//

#define IR_Pin_Backward_Right 13//

#define Trig 14//A-0

#define Echo 15//A-1

#define Din 16//A-2

#define Clk 17//A-3

#define Load 18//A-4

#define Servo_Pin 19//A-5

#include <Servo.h>

Servo myservo;

int Step_show = 2;

int last_Step_show = 2;

char Napravlenie;

short int Left_Motor_correction = 0;//коррекция для левого мотора.

short int Right_Motor_correction = 30;//коррекцыя для правого мотора.

char bluetooth_simvol;//символ, принемаемый по bluetooth

char last_bluetooth_simvol = 'T';//прошлый символ, принемаемый по bluetooth равен 'T'(стоим).

short int strong = 200;//сила воздействия на оба мотора.

unsigned int angle = 98;

unsigned int impulseTime=0;

unsigned int distance_sm=0;

unsigned int New_distance_sm=0;

unsigned int New_impulseTime=0;

int last_distance_sm;

int sost1;

int sost2;

char data[][8]=

{

{0x3C,0x42,0x81,0x99,0x99,0x81,0x42,0x3C}, // 0 - ГЛАЗ центр.

{0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18}, // 1 - закрыт

{0x3C,0x42,0x99,0x99,0x81,0x81,0x42,0x3C}, // 2 - ГЛАЗ посередине право.

{0x3C,0x42,0x81,0x81,0x99,0x99,0x42,0x3C}, // 3 - ГЛАЗ посередине лево.

{0x3C,0x42,0xB1,0xB1,0x81,0x81,0x42,0x3C}, // 4 - ВЕРХ левый угол.

{0x3C,0x42,0x81,0x8D,0x8D,0x81,0x42,0x3C}, // 5 - ВНИЗ средне.

{0x3C,0x42,0x81,0x81,0x8D,0x8D,0x42,0x3C}, // 6 - ВНИЗ направо

{0x3C,0x42,0x81,0x81,0xB1,0xB1,0x42,0x3C}, // 7 - ВЕРХ направо угол

{0x3C,0x42,0x81,0xB1,0xB1,0x81,0x42,0x3C}, // 8 - ВЕРХ средне

{0x3C,0x42,0x81,0xE1,0xE1,0x81,0x42,0x3C}, //9 - ВЕРХ высоко

{0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C,0x0C}, //10 - НИЖНИЕ моргание.

{0x30,0x30,0x30,0x30,0x30,0x30,0x30,0x30}, // 11 - ВЕРХ МОРГАНИЕ.

{0x00,0x00,0xFF,0x81,0x81,0xFF,0x00,0x00}, //12- 0

{0x00,0x00,0x00,0x20,0x40,0xFF,0x00,0x00}, //13- 1

{0x00,0x00,0xCF,0x89,0x89,0xF9,0x00,0x00}, //14- 2

{0x00,0x00,0x91,0x91,0x91,0xFF,0x00,0x00}, //15- 3

{0x00,0x00,0xF8,0x08,0x08,0xFF,0x00,0x00}, //16- 4

{0x00,0x00,0xF1,0x91,0x91,0x9F,0x00,0x00}, //17- 5

{0x00,0x00,0xFF,0x91,0x91,0x9F,0x00,0x00}, //18- 6

{0x00,0x00,0x80,0x80,0x80,0xFF,0x00,0x00}, //19- 7

{0x00,0x00,0xFF,0x91,0x91,0xFF,0x00,0x00}, //20- 8

{0x00,0x00,0xF1,0x91,0x91,0xFF,0x00,0x00}, //21- 9

{0x20,0x40,0xFF,0x00,0xFF,0x81,0x81,0xFF}, //22- 10

{0xFF,0x81,0x81,0x81,0x81,0x81,0x81,0xFF}, // 23-КВАДРАТ

{0xFF,0x81,0x81,0x99,0x99,0x81,0x81,0xFF}, // 24-КВАДРАТНЫЙ ГЛАЗ

{0xAA,0x80,0x01,0x80,0x01,0x80,0x01,0x55}, // 25 БЕГУЩИЙ ОГОНЯ ПОЛОЖЕНИЕ-1

{0x55,0x01,0x80,0x01,0x80,0x01,0x80,0xAA}, // 26 БЕГУЩИЙ ОГОНЯ ПОЛОЖЕНИЕ-2

{0x18,0x3C,0x3C,0x24,0x24,0x24,0x24,0x18}, // 27 -УЗКИЙ ГЛАЗ ЛЕВО.

{0x18,0x24,0x24,0x3C,0x3C,0x24,0x24,0x18}, // 28 -УЗКИЙ ГЛАЗ.

{0x18,0x24,0x24,0x24,0x24,0x3C,0x3C,0x18}, // 29 -УЗКИЙ ГЛАЗ ПРАВО.

{0x18,0x3C,0x24,0x24,0x24,0x24,0x24,0x18}, // 30 -УЗКИЙ ГЛАЗ ЛЕВО ЛЕВЕЕ.

{0x18,0x24,0x24,0x24,0x24,0x24,0x3C,0x18}, // 31 -УЗКИЙ ГЛАЗПРАВО ПРАВЕЕ.

{0x0A,0x05,0x05,0x07,0x07,0x05,0x05,0x0A}, // 32 -УЗКИЙ ГЛАЗ В НИЗУ.

{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, // 33 - ПУСТОЙ ЭКРАН.

{0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}, // 34 - НИЖНЯЯ ЧАСТЬ МАТРИЦЫ.

{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}, // 35 - ВЕСЬ ЭКРАН.

{0x81,0x42,0x24,0xFF,0xFF,0x24,0x42,0x81}, // 36 БУКВА Ж

{0x81,0x42,0x24,0x18,0x18,0x24,0x42,0x81}, // 37 КРЕСТИК.Г

{0x08,0x08,0x08,0xF8,0x1F,0x10,0x10,0x10}, // 38 КРЕСТИК.В

{0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0,0xF0}, // 39 верхняя ЧАСТЬ МАТРИЦЫ.

{0x3C,0x42,0x8D,0x8D,0x81,0x81,0x42,0x3C}, // 40 - ВНИЗ налево.

{0x38,0x44,0x82,0x41,0x41,0x82,0x44,0x38}, // 41 - сердце (пустое).

{0x38,0x44,0x82,0x59,0x59,0x82,0x44,0x38}, // 42 - сердце (с глазом внутри).

{0x38,0x7C,0xFE,0x7F,0x7F,0xFE,0x7C,0x38}, // 43 - сердце.

{0x18,0x24,0x24,0x18,0x18,0x24,0x24,0x18} // 44 - знак бесконечности

};

void setup() {

Serial.begin(9600);

myservo.attach(Servo_Pin);

myservo.write(angle); // устанавливаем сервопривод в серединное положение

pinMode(ENABL1, OUTPUT); //инициируем как выход

pinMode(ENABL2, OUTPUT); //инициируем как выход

pinMode(INPUT1, OUTPUT);

pinMode(INPUT2, OUTPUT); //инициируем как выход

pinMode(INPUT3, OUTPUT); //инициируем как выход

pinMode(INPUT4, OUTPUT);

pinMode(IR_Pin_Forward_Left, INPUT);

pinMode(IR_Pin_Forward_Right, INPUT);

pinMode(IR_Pin_Backward_Left, INPUT);

pinMode(IR_Pin_Backward_Right, INPUT);

pinMode(Trig, OUTPUT); //инициируем как выход

pinMode(Echo, INPUT); //инициируем как вход

digitalWrite(INPUT1, 0);//начальная позиция - стоим.

digitalWrite(INPUT2, 0);//начальная позицыя - стоим.

digitalWrite(INPUT3, 0);//начальная позицыя - стоим.

digitalWrite(INPUT4, 0);//начальная позицыя - стоим.

digitalWrite(ENABL1, 0);//начальная позицыя - стоим.

digitalWrite(ENABL2, 0);//начальная позицыя - стоим.

pinMode(Din, OUTPUT);

pinMode(Load, OUTPUT);

pinMode(Clk, OUTPUT);

//Устанавливаем Decode Mode, Intensity, Scan Limit, Shutdown и Display Test, но сначала

digitalWrite (Clk, 0); //первый такт устанавливаем в 0, чтобы точно знать, что он в 0

// настраиваем

Data32sent(0x09,0x00,0x09,0x00); //Decode Mode - без декодирования.

Data32sent(0x0A,0x08,0x0A,0x08); //Intensity - среднее свечение 17 из 32 см.таблицу

Data32sent(0x0B,0x07,0x0B,0x07); //Scan Limit - Используем все 8 катодов.

Data32sent(0x0C,0x01,0x0C,0x01); //используем режим Shutdown в Normal Mode.

Data32sent(0x0F,0x00,0x0F,0x00); //Display Test - Normal Mode.

FlashNow_Two(28,28);

}

void loop(){

if(Serial.available()){

bluetooth_simvol = (char)Serial.read();

}//end Serial.available() > 0 )

else{

bluetooth_simvol = last_bluetooth_simvol;

}//end else(Serial.available() <= 0 )

switch(bluetooth_simvol){ // выводим значение счетчика и переводим на новую строку

case 'A':// автоматический режим не падения со стола///////////////////////////////////////

last_bluetooth_simvol = 'A';

if (digitalRead(IR_Pin_Forward_Left)==1 && digitalRead(IR_Pin_Forward_Right)==1){//

STOP();

Napravlenie = 'Forward_Left';

Serial.println("if_IR_Pin_Forward_Left");

Forward_Left__Forward_Right_compensation();

break;

}

else if(digitalRead(IR_Pin_Forward_Right)==1 && digitalRead(IR_Pin_Backward_Right)==1){//

STOP();

Napravlenie = 'Forward_Right';

Serial.println("if_IR_Pin_Forward_Right");

Backward_Right__Forward_Right_compensation();

break;

}

else if(digitalRead(IR_Pin_Backward_Left)==1 && digitalRead(IR_Pin_Forward_Left)==1){//

STOP();

Napravlenie = 'Backward_Left';

Serial.println("if_IR_Pin_Backward_Left");

Backward_Left__Forward_Left_compensation();

break;

}

else if(digitalRead(IR_Pin_Backward_Right)==1 && digitalRead(IR_Pin_Backward_Left)==1){//

STOP();

Serial.println("if_IR_Pin_Backward_Right");

Napravlenie = 'Backward_Right';

Backward_Right__Backward_Left_compensation();

}

else if(digitalRead(IR_Pin_Forward_Left)==1){//

STOP();

Napravlenie = 'Forward_Left';

Serial.println("if_IR_Pin_Forward_Left");

Forward_Left_compensation();

break;

}

else if(digitalRead(IR_Pin_Forward_Right)==1){//

STOP();

Napravlenie = 'Forward_Right';

Serial.println("if_IR_Pin_Forward_Right");

Forward_Right_compensation();

break;

}

else if(digitalRead(IR_Pin_Backward_Left)==1){//

STOP();

Napravlenie = 'Backward_Left';

Serial.println("if_IR_Pin_Backward_Left");

Backward_Left_compensation();

break;

}

else if(digitalRead(IR_Pin_Backward_Right)==1){//

STOP();

Serial.println("if_IR_Pin_Backward_Right");

Napravlenie = 'Backward_Right';

Backward_Right_compensation();

break;

}

else{//

Napravlenie = 'Go';

Go();

}

case 'F'://движемся в перёд ///////////////////////////////////////////////////////////////

if(angle != 98){

angle = 98;

myservo.write(angle);

}

last_bluetooth_simvol = 'F';

Forward_Right(140);

Forward_Left(140);

break;

case 'B'://движение назад ///////////////////////////////////////////////////////////////

last_bluetooth_simvol = 'B';

Backward_Right(140);

Backward_Left(140);

break;

case 'L'://поворачиваем влево///////////////////////////////////////////////////////////

last_bluetooth_simvol = 'L';

Forward_Right(140);

Backward_Left(140);

FlashNow_Two(28,0);

delay(25);

break;

case 'R'://поворачиваем вправо//////////////////////////////////////////////////////////

last_bluetooth_simvol = 'R';

Forward_Left(140);

Backward_Right(140);

FlashNow_Two(0,28);

delay(25);

break;

case'T'://стоим на месте/////////////////////////////////////////////////////////////////

STOP();

Serial.println("T");

last_bluetooth_simvol = 'T';

FlashNow_Two(28,28);

delay(50);

break;

case 'l'://всё время поворачиваем влево (автокручение)///////////////////////////////////

if(angle != 128){

angle = 128;

myservo.write(angle);

}

last_bluetooth_simvol = 'l';

Forward_Right(140);

Backward_Left(140);

break;

case 'e'://режим измерения расстояния ультразвуком и трансляция результата на светодиоды

last_bluetooth_simvol = 'e';

if(angle != 98){

angle = 98;

myservo.write(angle);

}

distance_sm = 0;

impulseTime = 0;

//измеряем расстояние.

digitalWrite(Trig, 1);

// Подаем импульс на вход trig дальномера

delayMicroseconds(10); // равный 10 микросекундам

digitalWrite(Trig, 0); // Отключаем

impulseTime=pulseIn(Echo, 1); // Замеряем длину импульса

distance_sm=impulseTime/75; // Пересчитываем в сантиметры

//сравниваем поподаем ли мы в диапозон от 0 до 99.

//если попали, то:

Convert_sm_to_des_and_ed(distance_sm);

break;

case'n'://режим демонстрации////////////////////////////////////////////////////////////

last_bluetooth_simvol = 'n';

switch(Step_show){

case 1://////////////////////////////////////////////////////////// 1) Включение глаз.

FlashNow_Two(31,31);//первый глаз не светится и второй - не светится.

delay(1000);

FlashNow_Two(31,28);//первый глаз не светится, а второй - прищуривается.

delay(1000);

FlashNow_Two(28,28);//первый глаз прищуривается и второй - прищуривается.

delay(1000);

FlashNow_Two(0,0);//круглые глаза

delay(500);

Step_show++;

break;

case 2:///////////////////////////////////2) Осматриваем глазами без поворота головы.

FlashNow_Two(0,0);//ГЛАЗ центр.

delay(250);

FlashNow_Two(3,3);//ГЛАЗ посередине лево.

delay(20);

FlashNow_Two(4,4);//ВЕРХ левый угол.

delay(180);

FlashNow_Two(8,8);//ВЕРХ средне

delay(20);

FlashNow_Two(7,7);//ВЕРХ направо угол

delay(250);

FlashNow_Two(2,2);//ГЛАЗ посередине право.

delay(250);

FlashNow_Two(6,6);//ВНИЗ направо

delay(250);

FlashNow_Two(5,5);//ВНИЗ средне.

delay(250);

FlashNow_Two(40,40);//ВНИЗ налево.

delay(250);

FlashNow_Two(3,3);//ГЛАЗ посередине лево.

delay(250);

FlashNow_Two(0,0);//ГЛАЗ центр.

delay(250);

FlashNow_Two(0,0);//Двойное моргание.

delay(180);

FlashNow_Two(1,1);

delay(20);

FlashNow_Two(0,0);

delay(180);

FlashNow_Two(1,1);

delay(20);

FlashNow_Two(0,0);

delay(250);

Step_show++;

break;

case 3://///////////////////////////////////3) Осматриваем глазами с поворотом головы.

FlashNow_Two(3,3);

delay(400);

if(angle != 128){

angle = 128;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

FlashNow_Two(0,0);

delay(250);

FlashNow_Two(1,1);

delay(50);

FlashNow_Two(0,0);

delay(180);

FlashNow_Two(1,1);

delay(50);

FlashNow_Two(0,0);

delay(250);

FlashNow_Two(0,0);

delay(500);

FlashNow_Two(2,2);

delay(250);

if(angle != 98){

angle = 98;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

FlashNow_Two(0,0);

delay(1000);

FlashNow_Two(0,1);

delay(250);

FlashNow_Two(0,0);

delay(250);

FlashNow_Two(2,2);

delay(500);

if(angle != 58){

angle = 58;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

FlashNow_Two(3,2);

delay(180);

FlashNow_Two(7,4);

delay(180);

FlashNow_Two(2,3);

delay(180);

FlashNow_Two(6,40);

delay(250);

FlashNow_Two(0,0);

delay(400);

FlashNow_Two(3,3);

delay(400);

if(angle != 98){

angle = 98;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

FlashNow_Two(0,0);

delay(250);

FlashNow_Two(1,1);

delay(50);

FlashNow_Two(0,0);

delay(180);

FlashNow_Two(1,1);

delay(50);

FlashNow_Two(0,0);

delay(250);

Step_show++;

break;

case 4://///////////////////////////////////// 4) Странная походка(поочерёдное движение моторами).

Forward_Left(100);//поворачиваем влево.

delay(300);

STOP();

delay(20);

if(angle != 128){

angle = 128;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Forward_Right(100);//поворачиваем вправо.

delay(600);

STOP();

delay(20);

if(angle != 58){

angle = 58;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Forward_Left(100);//поворачиваем влево.

delay(600);

STOP();

delay(20);

if(angle != 128){

angle = 128;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Forward_Right(100);//поворачиваем вправо.

delay(600);

STOP();

delay(20);

if(angle != 58){

angle = 58;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Backward_Right(100);//поворачиваем вправо.

delay(600);

STOP();

delay(20);

if(angle != 58){

angle = 58;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Backward_Left(100);//поворачиваем влево.

delay(600);

STOP();

delay(20);

if(angle != 128){

angle = 128;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Backward_Right(100);//поворачиваем вправо.

delay(600);

STOP();

delay(20);

if(angle != 58){

angle = 58;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Backward_Left(100);//поворачиваем влево.

delay(300);

STOP();

delay(20);

if(angle != 128){

angle = 128;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

STOP();

delay(200);

FlashNow_Two(0,0);

delay(250);

FlashNow_Two(1,1);

delay(50);

FlashNow_Two(0,0);

delay(180);

FlashNow_Two(1,1);

delay(50);

FlashNow_Two(0,0);

delay(250);

if(angle != 98){

angle = 98;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Forward_Right(100);

Backward_Left(100);

delay(1000);

STOP();

delay(200);

Forward_Right(100);

Forward_Left(100);

delay(600);

STOP();

delay(200);

Backward_Right(100);

Forward_Left(100);

delay(1000);

STOP();

delay(200);

Forward_Right(100);

Forward_Left(100);

delay(600);

STOP();

delay(200);

if(angle != 98){

angle = 98;

myservo.write(angle);

FlashNow_Two(0,0);

delay(250);

}

Step_show = last_Step_show;

break;

}//end switch

break;

case 'a':// режим преследования цели вошедшей в диапазон расстоянимй//////////////////////

last_bluetooth_simvol = 'a';

if(angle != 98){

angle = 98;

myservo.write(angle);

}

//измеряем расстояние.

digitalWrite(Trig, 1);

// Подаем импульс на вход trig дальномера

delayMicroseconds(10); // равный 10 микросекундам

digitalWrite(Trig, 0); // Отключаем

impulseTime=pulseIn(Echo, 1); // Замеряем длину импульса

distance_sm=impulseTime/75; // Пересчитываем в сантиметры

//сравниваем поподаем ли мы в диапозон от 16 до 30.

if ( 16 < distance_sm && distance_sm < 40){

//если попали, то:

if(last_distance_sm == 0)last_distance_sm = distance_sm;

else {

if ( last_distance_sm - 3 <= distance_sm && distance_sm <= last_distance_sm + 3 ){

STOP;

FlashNow_Two(42,42);

delay(30);

FlashNow_Two(43,43);

delay(30);

}

if ( last_distance_sm + 3 < distance_sm && distance_sm <= last_distance_sm + 8){

Forward_Right(80);

Forward_Left(80);

FlashNow_Two(42,42);

delay(10);

}

if ( last_distance_sm + 8 < distance_sm && distance_sm <= last_distance_sm + 12){

Forward_Right(100);

Forward_Left(100);

FlashNow_Two(42,42);

delay(10);

}

if ( last_distance_sm + 12 < distance_sm && distance_sm <= last_distance_sm + 18){

Forward_Right(120);

Forward_Left(120);

FlashNow_Two(42,42);

delay(10);

}

if ( last_distance_sm - 8 < distance_sm && distance_sm <= last_distance_sm - 3){

Backward_Right(80);

Backward_Left(80);

FlashNow_Two(42,42);

delay(10);

}

if ( last_distance_sm - 12 < distance_sm && distance_sm <= last_distance_sm - 8){

Backward_Right(100);

Backward_Left(100);

FlashNow_Two(42,42);

delay(10);

}

if ( last_distance_sm - 18 < distance_sm && distance_sm <= last_distance_sm - 12){

Backward_Right(120);

Backward_Left(120);

FlashNow_Two(42,42);

delay(10);

}

}//else(last_distance_sm == 0)

}//if ( 16 < distance_sm && distance_sm < 40)

else

{

STOP();

last_distance_sm = 0;

FlashNow_Two(3,3);

delay(50);

FlashNow_Two(2,2);

delay(50);//сост. глаза.

}// else ( 16 < distance_sm && distance_sm < 40)

break;

}//end switch

}//end loop

void STOP (){

if(Right_Motor_correction < Left_Motor_correction){

digitalWrite(INPUT1, 1);

digitalWrite(INPUT2, 1);

delay(85);

digitalWrite(INPUT3, 1);

digitalWrite(INPUT4, 1);

analogWrite(ENABL1, 0);

analogWrite(ENABL2, 0);

}

else if(Right_Motor_correction > Left_Motor_correction){

digitalWrite(INPUT3, 1);

digitalWrite(INPUT4, 1);

delay(85);

digitalWrite(INPUT1, 1);

digitalWrite(INPUT2, 1);

analogWrite(ENABL1, 0);

analogWrite(ENABL2, 0);

}

else{

digitalWrite(INPUT1, 1);

digitalWrite(INPUT2, 1);

digitalWrite(INPUT3, 1);

digitalWrite(INPUT4, 1);

analogWrite(ENABL1, 0);

analogWrite(ENABL2, 0);

}

}

void Forward_Left_Start(float strong){

analogWrite(ENABL1, strong+Left_Motor_correction+100);

digitalWrite(INPUT1, 1);

digitalWrite(INPUT2, 0);

}

void Forward_Left(float strong){

analogWrite(ENABL1, strong+Left_Motor_correction);

digitalWrite(INPUT1, 1);

digitalWrite(INPUT2, 0);

}

void Forward_Right_Start(float strong){

analogWrite(ENABL2, strong+Right_Motor_correction+100);

digitalWrite(INPUT3, 1);

digitalWrite(INPUT4, 0);

}

void Forward_Right(float strong){

analogWrite(ENABL2, strong+Right_Motor_correction);

digitalWrite(INPUT3, 1);

digitalWrite(INPUT4, 0);

}

void Backward_Left_Start(short int strong){

analogWrite(ENABL1, strong+Left_Motor_correction+100);

digitalWrite(INPUT1, 0);

digitalWrite(INPUT2, 1);

}

void Backward_Left(short int strong){

analogWrite(ENABL1, strong+Left_Motor_correction);

digitalWrite(INPUT1, 0);

digitalWrite(INPUT2, 1);

}

void Backward_Right_Start(short int strong){

analogWrite(ENABL2, strong+Right_Motor_correction+100);

digitalWrite(INPUT3, 0);

digitalWrite(INPUT4, 1);

}

void Backward_Right(short int strong){

analogWrite(ENABL2, strong+Right_Motor_correction);

digitalWrite(INPUT3, 0);

digitalWrite(INPUT4, 1);

}

void FlashNow_Two(int sost, int sost1){ // Зажигает комбинацию - положение ГЛАЗА

for(int i=0;i<=7;i++){ //заполняет 8 столбцов матрицы

Data32sent(8-i,data[sost][i],8-i,data[sost1][i]); //зажигаем один i-ый солбец-катод

}

}

void Data32sent (char adress, char data, char adress1, char data1){ //формирует 32 бит посылку в MAX7219

digitalWrite(Load, 0);

shiftOut(Din, Clk, MSBFIRST, adress); //первым уходит старший байт

shiftOut(Din, Clk, MSBFIRST, data); //вторым младший

shiftOut(Din, Clk, MSBFIRST, adress1); //первым уходит старший байт

shiftOut(Din, Clk, MSBFIRST, data1); //вторым младший

digitalWrite(Load, 1);

}

void Convert_sm_to_des_and_ed(int distans){

if( 1 < distans && distans < 99 )

{

sost1 = distans / 10;

sost2 = distans % 10;

FlashNow_Two(sost2 + 12, sost1 + 12);

delay(250);

}

else

{

FlashNow_Two(44,44);

delay(250);

}

}

void Forward_Left_compensation(void){

STOP();

if(angle != 128){

angle = 128;

myservo.write(angle);// устанавливаем сервопривод в серединное положение

}

FlashNow_Two(0,0);

delay(40);

Backward_Right_Start(90);

FlashNow_Two(1,1);

delay(10);

Backward_Right(90);

FlashNow_Two(0,0);

delay(120);

FlashNow_Two(1,1);

delay(80);

FlashNow_Two(0,0);

delay(700);

Backward_Right_Start(90);

delay(10);

Backward_Right(90);

delay(100);

}

void Forward_Right_compensation(void){

STOP();

if(angle != 58){

angle = 58;

myservo.write(angle);

}

FlashNow_Two(0,0);

delay(40);

Backward_Left_Start(90);

FlashNow_Two(1,1);

delay(10);

Backward_Left(90);

FlashNow_Two(0,0);

delay(120);

FlashNow_Two(1,1);

delay(80);

FlashNow_Two(0,0);

delay(700);

Backward_Left_Start(90);

delay(10);

Backward_Left(90);

delay(100);

Backward_Left_Start(90);

delay(10);

Backward_Left(90);

delay(100);

}

void Backward_Left_compensation(void){

STOP();

delay(40);

FlashNow_Two(0,0);

delay(40);

Forward_Left_Start(90);

delay(70);

FlashNow_Two(1,1);

delay(70);

Forward_Left(90);

FlashNow_Two(0,0);

delay(120);

FlashNow_Two(1,1);

delay(80);

FlashNow_Two(0,0);

delay(700);

Forward_Right_Start(90);

delay(10);

Forward_Right(90);

delay(100);

myservo.write(90); // устанавливаем сервопривод в серединное положение

}

void Backward_Right_compensation(void){

STOP();

FlashNow_Two(0,0);

delay(20);

FlashNow_Two(23,23);

delay(20);

FlashNow_Two(0,0);

delay(40);

Forward_Right_Start(90);

FlashNow_Two(1,1);

delay(10);

Forward_Right(90);

FlashNow_Two(0,0);

delay(120);

FlashNow_Two(1,1);

delay(80);

FlashNow_Two(0,0);

delay(700);

Forward_Left_Start(90);

delay(10);

Forward_Left(90);

delay(100);

myservo.write(90); // устанавливаем сервопривод в серединное положение

}

void Forward_Left__Forward_Right_compensation(void){

STOP();

delay(40);

Backward_Right_Start(90);

Backward_Left_Start(90);

FlashNow_Two(1,1);

delay(10);

Backward_Right(90);

Backward_Left(90);

FlashNow_Two(0,0);

delay(120);

FlashNow_Two(1,1);

delay(80);

FlashNow_Two(0,0);

delay(700);

Backward_Right_Start(90);

delay(10);

Backward_Right(90);

delay(100);

}

void Backward_Right__Forward_Right_compensation(void){

STOP();

delay(40);

if(angle != 58){

angle = 58;

myservo.write(angle);

}

Backward_Left_Start(90);

FlashNow_Two(6,6);

delay(10);

Forward_Left(90);

FlashNow_Two(6,6);

delay(120);

FlashNow_Two(6,6);

delay(80);

FlashNow_Two(6,6);

delay(700);

Forward_Left_Start(90);

delay(10);

Forward_Left(90);

delay(100);

}

void Backward_Left__Forward_Left_compensation(void){

STOP();

delay(40);

if(angle != 128){

angle = 128;

myservo.write(angle);

}

Backward_Right_Start(90);

FlashNow_Two(40,40);

delay(10);

Forward_Right(90);

FlashNow_Two(40,40);

delay(120);

FlashNow_Two(40,40);

delay(80);

FlashNow_Two(40,40);

delay(700);

Forward_Left_Start(90);

delay(10);

Forward_Left(90);

delay(100);

}

void Backward_Right__Backward_Left_compensation(){

STOP();

delay(40);

Forward_Right_Start(90);

Forward_Left_Start(90);

FlashNow_Two(1,1);

delay(10);

Forward_Right(90);

Forward_Left(90);

FlashNow_Two(0,0);

delay(120);

FlashNow_Two(1,1);

delay(80);

FlashNow_Two(0,0);

delay(700);

Forward_Right_Start(90);

delay(10);

Forward_Right(90);

delay(100);

}

void Go(){

Forward_Right(90);

Forward_Left(90);

if(angle != 98){

angle = 98;

myservo.write(angle);

}

FlashNow_Two(28,28);

delay(20);

Размещено на Allbest.ru

...