КЛУБ: Математическая логика в управлении робота

На заседании клуба от 18.12.17 мы рассмотрели с Вами управление роботом на основе математической логики. Как пример рассматривали робота оснащенного 4-мя датчиками определения препятствий, с которыми он не должен был сталкиваться. Логическую схему робота можно представить в виде черного ящика с четырьмя входами и двумя выходами на моторы (логическая 1 вращение мотора в сторону обеспечивающие движение вперед, логический 0 - движение назад)

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

Количество комбинаций (N) определяется количеством обрабатываемых датчиков (n) по формуле: 

N=2^n

Логические формулы управления строятся на основе построенной таблицы  состояний по строкам выходов с выходным сигналом 1. Для получения этой 1 необходимо логически перемножить состояния входов, если текущее состояние входа 0, то данный вход записываем с использованием инверсии например "А=0", записываем "не-А" или "-А". Для представленной таблицы для М1 получим следующее (& знак логического умножения, может опускаться между переменными логического выражения)

Для выхода М2 формулы и таблицу состояний записать САМОСТОЯТЕЛЬНО в тетрадь

Результирующая формула получается логическим сложением полученных формул для единичных выходов 

где OR или | - логическое ИЛИ

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

Зона Ардуино: Алгоритм робота обходящего препятствия

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

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

Блок движения мы отработали на предыдущих уроках и добились остановки робота перед препятствием, на время пока препятствие не будет устранено.

Логический блок поворота включает алгоритм определения дистанций, блок логики и блок поворота

Блок логики преобразует полученные дистанции в логические сигналы сравнивая с дистанцией остановки (D) и с исправленной (DO) при учете поворота дальномера. Данные дистанции вводится в блок начальных условий скетча. На выходе блока формируются  логические сигналы управления моторами (М1, М2) 

Блок поворота получив управляющие сигналы М1 и М2 запускают двигатели в ту или иную сторону и останавливают их только после того как дистанция будет превышать заданный порок

Работу моторов определяют логические функции со ставленые по логической таблице поведения. Для построения этой таблицы рассмотрим черный ящик(черный ящик это такое устройство о внутренним содержании которого, мы ничего не знаем, но знаем что подается на вход и что снимается с выхода) и зададим в правой части ожидаемые реакции моторов, где 1 вращение по часовой, а 0 вращение против часовой стрелке или вращение моторов в сторону обеспечивающих движение вперед и назад.   

По этой таблице составляются логические формулы управления моторами. Составление логических формул управления рассмотрим на следующем уроке

ЗОНА АРДУИНО: Общие правила создания скэтча в ИС ДРАКОН

Представлены 2 фрагмента дракон схемы по созданию скэтча для АРДУИНО. Красным показаны обязательные схемы скетча. Сииним программируемые элементы (3 квадрат слева на право). В модуле (Схема гном) указывается порядок подключения функций скэтча причем первым подключается void setup, а последним void loop

ЗОНА АРДУИНО: Алгоритм проверки сервомотора

    // ИС Дракон. Маршрутный транслятор.

    // Y:\home\deniska\Документы\др длкр\arduino6.drt
    // 1. Схема - Гном, Изменение 15.12.2017 17:48:51
    // 5. Модуль, Начало
#include <Servo.h>
// для дальнейшей работы назовем 9 пин как servoPin
#define servoPin 9
// 544 это эталонная длина импульса при котором сервопривод должен принять положение 0°
#define servoMinImp 544
// 2400 это эталонная длина импульса при котором сервопривод должен принять положение 180°
#define servoMaxImp 2400 
//  объявляем наш сервопривод
Servo myServo;


    // Y:\home\deniska\Документы\др длкр\arduino6.drt
    // 6. Схема - процедура, Изменение 15.12.2017 17:11:08, Начало
void setup()
{



    // ==================== Шампур

    // 7. Заголовок / void setup()

    // 9. Действие / // устанавливаем пин как вывод управления сервоприводом, // а также для работы сервопривода непосредственно в диапазоне углов от 0 до 180° задаем мин и макс значения импульсов. // импульсы с большей или меньшей длиной восприниматься не будут. // для сервоприводов даже одной партии значения длин импульсов могут отличаться, может быть даже и 584-2440. // поэкспериментируйте и найдите идеальные длины импульсов конкретно для вашего сервопривода.
myServo.attach(servoPin, servoMinImp, servoMaxImp);

    // 8. Конец / Конец

    // 6. Схема, Конец
//
}


    // Y:\home\deniska\Документы\др длкр\arduino6.drt
    // 10. Схема - процедура, Изменение 15.12.2017 17:35:21, Начало
void loop() 
{ 
int A;

    // ==================== Шампур

    // 11. Заголовок / start povorot

    // 13. Цикл Ветка / Loop

L23: ;
    // 23. Действие / A=0;
A=0;

L24: ;
    // 24. Цикл Действие / устанавливаем угол A°
myservo.write(A);  
delay(2000);

    // 25. Вопрос / A == 180 == Нет
if (!(A =180)) goto L28; 

    // 21. Вопрос / A == 200 == Да
if (A == _end100) goto L20; 

    // 14. Цикл Адрес -> 13. Ветка / Loop
goto L23; 

    // ==================== Шампур

L28: ;
    // 28. Действие / увеличиваем А на 90
A = A+90;
goto L24; 

    // ==================== Шампур

    // 32. Адрес -> 19. Ветка / exit

    // ==================== Шампур

    // 19. Ветка / exit

    // 20. Конец / Конец

    // 10. Схема, Конец
L_end10: ;
//
}

ЗОНА АРДУИНО: Учим робота сделать поворот

На прошлом уроке мы провели исследования робота в ходе которого мы научили робота останавливаться перед препятствием, которое он обнаруживает ультро-звуковым датчиком прямо по ходу движения. Кроме этого Вы устранили неправильное подключение ультро-звукового датчика, научились писать команды замера дистанции, внесли изменения в нашу ДРАКОН-СХЕМУ, сгенерировали новый скетч №3.

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

Управляющий сигнал представляет из себя импульсы с нужной нам шириной, которые посылаются с определенной частотой. Для рассматриваемых нами сервоприводов частота посылания импульса почти всегда будет около 50 Гц (это примерно 1 раз в 20мс), а ширина импульса будет лежать в пределе от 544мкс до 2400мкс.

Как видно из картинке, импульс шириной в 544мкс выставит выводной вал в положение 0°, 1520мск соответствует углу в 90°, а 2400мкс соответствует 180°. 

Для подключения к контроллеру от сервопривода тянется 3 провода обжатых стандартным 3 пиновым разъемом с шагом 2.54мм . Цвета проводов могут варьироваться. Коричневый или черный - земля (GND), красный - плюс источника питания (VTG), оранжевый или белый - управляющий сигнал (SIG).

Типовой скетч сервопривода

#include <Servo.h>
// для дальнейшей работы назовем 9 пин как servoPin
#define servoPin 9
// 544 это эталонная длина импульса при котором сервопривод должен принять положение 0°
#define servoMinImp 544 
// 2400 это эталонная длина импульса при котором сервопривод должен принять положение 180°
#define servoMaxImp 2400 
//  объявляем наш сервопривод
Servo myServo;
void setup()
{
myServo.attach(servoPin, servoMinImp, servoMaxImp);
// устанавливаем пин как вывод управления сервоприводом,
// а также для работы сервопривода непосредственно в диапазоне углов от 0 до 180° задаем мин и макс значения импульсов.
// импульсы с большей или меньшей длиной восприниматься не будут.
// для сервоприводов даже одной партии значения длин импульсов могут отличаться, может быть даже и 584-2440.
// поэкспериментируйте и найдите идеальные длины импульсов конкретно для вашего сервопривода.

}

void loop() {  
  // устанавливаем угол 0°  
  myservo.write(0);  
  delay(2000);
  // устанавливаем угол 90°  
  myservo.write(90);  
  delay(2000);
  // устанавливаем угол 180°  
  myservo.write(180);  
  delay(2000);

}

Словарик
myservo.read();
Считывает текущий угол поворота сервопривода, возвращает значение типа int — угол от 0 до 180 градусов.

myservo.attached();
Проверяем, привязан ли сервопривод. Возвращает логическое значение bool.

myservo.detach();
Отключает сервопривод от пина.
Предлагаемый алгоритм программы робота

Но для начала необходимо написать скетч проверки и настройки работы сервомотора 

Как правильно оформить ДРАКОН-СХЕМУ

Царская дорога должна быть прямой, и на имение загруженной, все дополнительное вправо.

Лишних лиан не должно быть, чем проще тем лучше.

ЗОНА АРДУИНО: учим робота

    // ИС Дракон. Маршрутный транслятор.

    // Y:\home\deniska\Документы\др длкр\arduino2.drt

    // 13. Схема - Гном, Изменение 03.12.2017 6:34:54

    // 17. Модуль, Начало

 //varitable 

#define Trig 17

#define Echo 18

unsigned int impulseTime=0; 

unsigned int distance_sm=0;

int in1 = 2;

int in2 = 3;

int in3 = 4;

int in4 = 7;

int inA = 5;

int inB = 6;

    // Y:\home\deniska\Документы\др длкр\arduino2.drt

    // 9. Схема - процедура, Изменение 02.12.2017 22:31:29, Начало

void setup()

{

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

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

pinMode(in1, OUTPUT);

pinMode(in2, OUTPUT);

pinMode(in3, OUTPUT);

pinMode(in4, OUTPUT);

pinMode(inA, OUTPUT);

pinMode(inB, OUTPUT);

pinMode(inA, INPUT);

pinMode(inB, INPUT);

    // ==================== Шампур

    // 10. Заголовок / setap

    // 12. Комментарий / настройка портов

    // 11. Конец / Конец

    // 9. Схема, Конец

//

}

    // Y:\home\deniska\Документы\др длкр\arduino2.drt

    // 1. Схема - процедура, Изменение 03.12.2017 7:00:18, Начало

void loop()

{

    // ==================== Шампур

    // 2. Заголовок / void loop

    // 4. Цикл Ветка / lupo()

L30: ;

    // 30. Действие / замер дистанции

digitalWrite(Trig, HIGH); 

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

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

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

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

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

    // 18. Вопрос / distance_sm<30 == Да

if (distance_sm<30) goto L22; 

    // 20. Вопрос / digitalRead(in1) < 3 == Да

if (digitalRead(in1) < 3) goto L29; 

    // 5. Цикл Адрес -> 4. Ветка / lupo()

goto L30; 

    // ==================== Шампур

L29: ;

    // 29. Вывод / на управление моторами / digitalWrite(in1, HIGH); digitalWrite(in3, HIGH);

digitalWrite(in1, HIGH);

digitalWrite(in3, HIGH);

goto L30; 

    // ==================== Шампур

L22: ;

    // 22. Вопрос / digitalRead(in1) < 3 == Нет

if (!(digitalRead(in1) < 3)) goto L27; 

goto L30; 

    // ==================== Шампур

L27: ;

    // 27. Вывод / на управление моторами / digitalWrite(in1, LOW); digitalWrite(in3, LOW);

    // -31. Правый комментарий / digitalWrite() - отправляет на цифровой вывод значение HIGH или LOW. Если функцией pinMode() вывод сконфигурирован как выход (OUTPUT), то при выполнении функции digitalWrite() его напряжение будет изменено на соответствующее значение: 5 В (либо 3.3 В для плат, работающих от 3.3В) при отправке HIGH, 0 В (земля) - при LOW.

digitalWrite(in1, LOW);

digitalWrite(in3, LOW);

goto L30; 

    // 1. Схема, Конец

//

}

    // 17. Модуль, Конец

//

Словарик

digitalRead - Считывает уровень сигнала HIGH или LOW с указанного цифрового вывода.

digitalWrite - Отправляет на цифровой вывод значение HIGH или LOW.

pulseIn - Считывает длительность импульса (любого - HIGH или LOW) на выводе. 

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

goto - оператор безусловного перехода

int - Тип данных знаковое целое число занимает в памяти 2 байта.

unsigned int - Тип данных unsigned int - беззнаковое целое число, также как и тип int (знаковое) занимает в памяти 2 байта. Но в отличие от int, тип unsigned int может хранить только положительные целые числа в диапазоне от 0 до 65535 (2^16)-1).