Программа

//////////////////////////////////////////////////////////////////////////////////////////////////////////

// Robot Platform RASH 1 © AlSHex

// 1.0

//

// Create: 23/03/2015

// Modification: 11/04/2015

//

// Description: Программа управления платформой RASH 1

//

//////////////////////////////////////////////////////////////////////////////////////////////////////////

//припиновка

const int LEDtech = 13; //вывод технологического сигнала на светодиод

const int LEDdist = 3; //вывод сигнала от сонара на светодиод

const int Sonar_t = 14; //данные на сонар

const int Sonar_r = 15; //данные от сонара

const int Mode = 16; //подключение кнопки установки режима — автономный или ручной: =0 — ручной; 1= автономный режим

const int MB1 = 4; //левый мотор — digital

const int MB2 = 5; //левый мотор — pwm

const int MA1 = 6; //правый мотор — pwm

const int MA2 = 7; //правый мотор — digital

const int HDL = 17; //вывод сигнала на фары

const int SPK = 11; //вывод сигнала на динамик

//const int LGHT = ; //данные от датчика освещенности

//настройка модуля интерфейса

const long UART_Speed = 9600; //Скорость UART

//настройка модуля сонара

const int D1 = 20; //коэффициенты функции ШИМ для вывода данных сонара на светодиод

const int D2 = 110;

const float A = 2.5;

const float B = 305;

//настройка модуля управления

const byte Byte_forward = byte(‘W’); //команда: вперед

const byte Byte_back = byte(‘S’); //команда: назад

const byte Byte_left = byte(‘A’); //команда: влево

const byte Byte_right = byte(‘D’); //команда: вправо

const byte Byte_stop = byte(‘x’); //команда: стоп

const byte Byte_sound = byte(‘C’); //команда: звуковой сигнал/мелодия

const byte Byte_light = byte(‘V’); //команда: фары

const int Dist_min = 40; //минимальная дистанция, при которой робот запустит алгоритм объезда препятствий [см]

const int Cycle_lightoff = 1000; //кол-во циклов повтора программы после которого отключаются включенные фары в автономном режиме

const int Speed_default = 255; //значение по умолчанию, которое будет использоваться после включения пока не придёт команда, скорость сбрасывается в значение по умолчанию при переходе на автономный режим

//общие настройки

const int Delay_prog = 10; //задержка выполнения повтора программы, [мс]

const int M_stop = 0; //константы для модуля мотора (для удобства ипользования)

const int M_forward = 1;

const int M_back = 2;

const int M_left = 3;

const int M_right = 4;

//объявление и описание модулей — функций

//========== Interface ==========

void _UART_Interf(unsigned int RST, unsigned int *Data); //данные от интерфейса: Data[0]=1 — наличие команды; Data[1] — данные команды

//RST — сброс: 0= нормальная работа; 1= сброс

//Data — массив данных от интерфейса: Data[0]=1 — наличие команды; Data[1] — данные команды

//

//Подсоединение к UART происходит с помощью библиотеки Serial

//========== Motor ==========

void _Motor(unsigned int RST, unsigned int Mode, unsigned int Speed);

//RST — сброс: 0= нормальная работа; 1= сброс

//Mode — режим работы: 0= стоп; 1= движение вперед; 2= движение назад; 3= поворот налево; 4= поворот направо

//Speed: скорость вращения двигателей — уровень, фомируемый ШИМ: 0= нулевой уровень; 255= максимальный уровень

//

//MA1, MA2, MB1, MB2 — сигналы припиновки двигателей, MB1 и MA2 — цифровые, MB2 и MA1 — аналоговые (0/255)

//LEDtech — цифровой сигнал припиновки для технологического светодиода, загорается при выполнении команды и гаснет при получении команды «стоп»

//========== Sonar ==========

unsigned int _Sonar(unsigned int RST);

//RST — сброс: 0= нормальная работа; 1= сброс

//

//Sonar_t — цифровой сигнал припиновки для сонара Trig

//Sonar_r — цифровой сигнал припиновки для сонара Echo

//LEDdist — аналоговый (0/255) сигнал припиновки для светодиода, на который будет выводиться ШИМ-сигнал для визуализации дальности — чем ближе объект, тем светодиод будет гореть ярче

//========== Control Motor ==========

void _ControlM(unsigned int RST, unsigned int SCTRL, unsigned int DCTRL, unsigned int Mode);

//RST — сброс: 0= нормальная работа; 1= сброс

//SCTRL — сигнал актуальности данных DCTRL: 0= неактулальны; 1= актуальны

//DCTRL — данные (команда) от модуля интерфейса

//Mode — режим управления: 0= управления по командам; 1= автономный режим

//

//Модуль обращается к модулям Sonar, Motor и Rotate

//LEDtech — цифровой сигнал припиновки для технологического светодиода, загорается на 1 секунду при получении команды установки скорости

//========== Control Light ==========

void _ControlL(unsigned int RST, unsigned int SCTRL, unsigned int DCTRL, unsigned int Mode);

//RST — сброс: 0= нормальная работа; 1= сброс

//SCTRL — сигнал актуальности данных DCTRL: 0= неактулальны; 1= актуальны

//DCTRL — данные (команда) от модуля интерфейса

//Mode — режим управления: 0= управления по командам; 1= автономный режим

//

//HDL — цифровой сигнал припиновки для управления фарами: 0= фары выключены; 1= фары включены

//========== Control Sound ==========

void _ControlS(unsigned int RST, unsigned int SCTRL, unsigned int DCTRL, unsigned int Mode);

//RST — сброс: 0= нормальная работа; 1= сброс

//SCTRL — сигнал актуальности данных DCTRL: 0= неактулальны; 1= актуальны

//DCTRL — данные (команда) от модуля интерфейса

//Mode — режим управления: 0= управления по командам; 1= автономный режим

//

//SPK — аналоговый (0/255) сигнал припиновки для вывода ШИМ сигнала

//========== Rotate ========== — алгоритм вращения, модуль выделенный из состава модуля ControlM в самостоятельный

void _Rotate(unsigned int RST, unsigned int Speed);

//RST — сброс: 0= нормальная работа; 1= сброс

//Speed: скорость вращения двигателей — уровень, фомируемый ШИМ: 0= нулевой уровень; 255= максимальный уровень

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

unsigned int CMD[2] = {0,0}; //данные от интерфейса: CMD[0]=1 — наличие команды; CMD[1] — данные команды

unsigned int CTRL[8] = {0,0,0,0,0,0,0,0}; //массив сигналов управления

//CTRL[0]: 0= нормальная работа; 1= сброс модулей

//CTRL[1]: 0= ручной режим; 1= автономный режим

void setup() {

//припиновка

pinMode(MB1, OUTPUT); digitalWrite(MB1, LOW);

pinMode(MB2, OUTPUT); analogWrite(MB2, 0);

pinMode(MA1, OUTPUT); analogWrite(MA1, 0);

pinMode(MA2, OUTPUT); digitalWrite(MA2, LOW);

pinMode(Sonar_t, OUTPUT); digitalWrite(Sonar_t, LOW);

pinMode(Sonar_r, INPUT); digitalWrite(Sonar_r, LOW);

pinMode(LEDdist, OUTPUT); analogWrite(LEDdist, 0);

pinMode(Mode, INPUT); digitalWrite(Mode, LOW);

pinMode(LEDtech, OUTPUT); digitalWrite(LEDtech, LOW);

pinMode(HDL, OUTPUT); digitalWrite(HDL, LOW);

pinMode(SPK, OUTPUT); analogWrite(SPK, 0);

//начальная инициализация модулей

Serial.begin(UART_Speed);

_UART_Interf(1, CMD);

_UART_Interf(0, CMD);

_Sonar(1);

_Sonar(0);

_Motor(1, 0, 0);

_Motor(0, 0, 0);

_Rotate(1, 0);

_Rotate(0, 0);

_ControlM(1, 0, 0, 0);

_ControlM(0, 0, 0, 0);

_ControlL(1, 0, 0, 0);

_ControlL(0, 0, 0, 0);

_ControlS(1, 0, 0, 0);

_ControlS(0, 0, 0, 0);

}

void loop() {

//опрос кнопки режима и установка режима работы

if (digitalRead(Mode) == LOW) {

if (CTRL[1] == 1) { CTRL[0] = 1; } else { CTRL[0] = 0; } //если было переключении из другого режима, то будет выдан reset на все схемы при проходе программы, на след. проходе reset снимется

CTRL[1] = 0;

} else {

if (CTRL[1] == 0) { CTRL[0] = 1; } else { CTRL[0] = 0; }

CTRL[1] = 1;

}

//опрос интерфейса, результат будет доступен в D_Interf

_UART_Interf(CTRL[0], CMD);

//выполнение схемы управления

//управление моторами

_ControlM(CTRL[0], CMD[0], CMD[1], CTRL[1]);

//управление фарами

_ControlL(CTRL[0], CMD[0], CMD[1], CTRL[1]);

//управление звуком

_ControlS(CTRL[0], CMD[0], CMD[1], CTRL[1]);

delay(Delay_prog);

}

//========== Interface module ==========

void _UART_Interf(unsigned int RST, unsigned int *Data) {

unsigned int DUART;

static unsigned int cnt_byte;

if (RST == 0) {

if (Serial.available() != 0) {

DUART = Serial.read();

switch (cnt_byte) { //проверка целостности пакета, если хоть один сбой — сбрасываем приём и начинаем поиск заголовка нового пакета

case 0:

if (DUART == byte(‘t’)) { cnt_byte++; } else { cnt_byte = 0; }

Data[0] = 0; Data[1] = 0;

break;

case 1:

if (DUART == byte(‘x’)) { cnt_byte++; } else { cnt_byte = 0; }

Data[0] = 0; Data[1] = 0;

break;

case 2:

if (DUART == byte(‘_’)) { cnt_byte++; } else { cnt_byte = 0; }

Data[0] = 0; Data[1] = 0;

break;

case 3:

if (DUART == byte(‘c’)) { cnt_byte++; } else { cnt_byte = 0; }

Data[0] = 0; Data[1] = 0;

break;

case 4:

if (DUART == byte(‘o’)) { cnt_byte++; } else { cnt_byte = 0; }

Data[0] = 0; Data[1] = 0;

break;

case 5:

if (DUART == byte(‘m’)) { cnt_byte++; } else { cnt_byte = 0; }

Data[0] = 0; Data[1] = 0;

break;

case 6:

if (DUART == byte(‘=’)) { cnt_byte++; } else { cnt_byte = 0; }

Data[0] = 0; Data[1] = 0;

break;

case 7: //если дошли до конца, то пакет верен и можно выдать команду

cnt_byte = 0;

Data[0] = 1; Data[1] = DUART;

break;

}

} else {

Data[0] = 0; Data[1] = 0;

}

} else {

cnt_byte = 0;

Data[0] = 0; Data[1] = 0;

}

}

//========== Sonar module ==========

unsigned int _Sonar(unsigned int RST) {

unsigned int Duration;

if (RST == 0) {

digitalWrite(Sonar_t, HIGH); //инициализация работы УЗ датчика

delayMicroseconds(10);

digitalWrite(Sonar_t, LOW);

Duration = pulseIn(Sonar_r, HIGH); //приём данных с УЗ датчика (ширина импульса в мс)

//вывод дальности на светодиод

if (Duration/58 > D1 && Duration/58 < D2) { analogWrite(LEDdist,int((-A*float(Duration/58)+B))); } else { if (Duration/58 < D1) { analogWrite(LEDdist, HIGH); } else { analogWrite(LEDdist, LOW); } } return Duration/58; //выдача расстояния в сантиметрах } else { digitalWrite(LEDdist, LOW); return 0; } } //========== Control Motor module ========== void _ControlM(unsigned int RST, unsigned int SCTRL, unsigned int DCTRL, unsigned int Mode) { unsigned int Dist; static unsigned int Speed; static unsigned long Time_forward; if (RST == 0) { if (Mode == 0) { //"ручной" режим работы по командам if (SCTRL == 1) { //если получили команду switch (byte(DCTRL)) { //расшифровываем команду case Byte_forward: Dist = _Sonar(0); if (Dist > D1) { _Motor(0, M_forward, Speed); } //если можно ехать вперед — едем

break;

case Byte_back:

_Motor(0, M_back, Speed);

break;

case Byte_left:

_Motor(0, M_left, Speed);

break;

case Byte_right:

_Motor(0, M_right, Speed);

break;

case Byte_stop:

_Motor(0, M_stop, Speed);

break;

default:

break;

}

if (DCTRL > 47 && DCTRL < 58) { //получение скорости Speed = (DCTRL-47)*25+5; digitalWrite(LEDtech, HIGH); delay(1000); digitalWrite(LEDtech, LOW); } } } if (Mode == 1) { //автономный режим Speed = Speed_default; Dist = _Sonar(0); if (Dist > Dist_min) {

if (millis()-Time_forward < 21000) { _Motor(0, M_forward, Speed); } else { //если 20 сек движется только прямо, то нужно сдать назад, т.к. наверное застряли в небольшой комнате 🙂 _Motor(0, M_stop, Speed); delay(300); _Motor(0, M_back, Speed); delay(600); _Motor(0, M_stop, Speed); delay(300); _Rotate(0, Speed); _Motor(0, M_stop, Speed); delay(300); Time_forward = millis()-1; //-1 для гарантии, что millis()-Time_forward будет точно всегда положительное число } } else { _Motor(0, M_stop, Speed); delay(300); _Rotate(0, Speed); delay(300); Time_forward = millis()-1; } } } else { Dist = 0; Speed = Speed_default; Time_forward = 0; _Sonar(1); _Motor(0, 0, 0); _Rotate(1, 0); digitalWrite(LEDtech, LOW); } } //========== Control Light ========== void _ControlL(unsigned int RST, unsigned int SCTRL, unsigned int DCTRL, unsigned int Mode) { static unsigned int Light; // 0= фары выключены; 1= фары включены if (RST == 0) { if (Mode == 0) { //"ручной" режим работы по командам if (SCTRL == 1) { //если получили команду switch (byte(DCTRL)) { //расшифровываем команду case Byte_light: if (Light == 0) { Light = 1; digitalWrite(HDL, HIGH); } else { Light = 0; digitalWrite(HDL, LOW); } break; default: break; } } } if (Mode == 1) { //автономный режим //block operations } } else { Light = 0; digitalWrite(HDL, LOW); } } //========== Control Sound ========== void _ControlS(unsigned int RST, unsigned int SCTRL, unsigned int DCTRL, unsigned int Mode) { if (RST == 0) { if (Mode == 0) { //"ручной" режим работы по командам if (SCTRL == 1) { //если получили команду switch (byte(DCTRL)) { //расшифровываем команду case Byte_sound: //block operations break; default: break; } } } if (Mode == 1) { //автономный режим //block operations } } else { analogWrite(SPK, 0); } } //========== Motor module ========== void _Motor(unsigned int RST, unsigned int Mode, unsigned int Speed) { if (RST == 0) { switch (Mode) { //расшифровка команды управления и выполнение действия case 0: //stop digitalWrite(LEDtech, LOW); digitalWrite(MB1, LOW); analogWrite(MB2, 0); analogWrite(MA1, 0); digitalWrite(MA2, LOW); break; case 1: //forward digitalWrite(LEDtech, HIGH); digitalWrite(MB1, HIGH); analogWrite(MB2, 255-Speed); analogWrite(MA1, Speed); digitalWrite(MA2, LOW); break; case 2: //back digitalWrite(LEDtech, HIGH); digitalWrite(MB1, LOW); analogWrite(MB2, Speed); analogWrite(MA1, 255-Speed); digitalWrite(MA2, HIGH); break; case 3: //left digitalWrite(LEDtech, HIGH); digitalWrite(MB1, LOW); analogWrite(MB2, Speed); analogWrite(MA1, Speed); digitalWrite(MA2, LOW); break; case 4: //right digitalWrite(LEDtech, HIGH); digitalWrite(MB1, HIGH); analogWrite(MB2, 255-Speed); analogWrite(MA1, 255-Speed); digitalWrite(MA2, HIGH); break; default: break; } } else { digitalWrite(LEDtech, LOW); digitalWrite(MB1, LOW); analogWrite(MB2, 0); analogWrite(MA1, 0); digitalWrite(MA2, LOW); } } //========== Rotate ========== - алгоритм поворота void _Rotate(unsigned int RST, unsigned int Speed) { unsigned int Dist; static unsigned int Num; unsigned int cnt; unsigned long Now_time; if (RST == 0) { do { if (Num%2 == 0) { //определение четности if (Num >= 0 && Num < 128) { _Motor(0, M_right, Speed); delay(100); _Motor(0, M_stop, Speed); delay(100); } if (Num >= 128 && Num < 255) { _Motor(0, M_right, Speed); delay(200); _Motor(0, M_stop, Speed); delay(100); } } else { if (Num >= 0 && Num < 128) { _Motor(0, M_left, Speed); delay(250); _Motor(0, M_stop, Speed); delay(100); } if (Num >= 128 && Num < 255) { _Motor(0, M_left, Speed); delay(150); _Motor(0, M_stop, Speed); delay(100); } } cnt++; Dist = _Sonar(0); } while (Dist < Dist_min && cnt <= 3); //выходим если дальность разрешенная или если 3 раза провернулись (на след. цикле программы зайдем сюда опять и будем снова крутиться может быть по-другому из-за другого Num) cnt = 0; Now_time = millis(); //чтение времени while (Now_time > 255) { //ограничение времени

Now_time -= 255;

}

Num += Now_time; //получение нового случайного числа: старое+время

while (Num > 255) { //ограничение нового числа при необходимости

Num -= 255;

}

} else {

Dist = 0;

Num = 0;

cnt = 0;

Now_time = 0;

_Motor(1, 0, 0);

_Sonar(1);

}

}