Помогите с декодером Pelco-D команд

Есть ардуина, L298N и поворотная платформа для камеры с вращением в двух плоскостях, двигатели управляются постоянным навряжением 24В. Созрело зелание сделать PTZ камеру. С программированием, к сожалению... на уровне школьного Basic, но стараюсь))) С синтаксисом могут быть проблемы.

Суть задачи следующая: получение команд по протоколу Pelco-D через max485 на Rx UART ардуины, с последующим управление двигателями камеры через драйвер L298N. Все что после ардуины уже запустил, но как правильно получать и определять команды не совсем понимаю.

Допросив гугл понял что Посылка Pelco-d состоит из 7 байт:
1- байт синхронизации
2- адрес (естественно нужен, в случае нескольких камер)
3- байт команд 1 (фокус, диафрагма...-нам не интересен)
4- байт команд 2 (наклон, поворот... - то что нужно)
5- дата 1 (скорость поворота - нужная вещь, но при дальнейшей доработке)
6- дата 2 (скорость наклона - так же при дальнейшей доработке)
7- контрольная сумма (в моем случае мало важна)

Байт 6 "дата 2" состоит из бит:
7- фокус в даль
6- зум широкоугольный
5- зум теле
4- наклон вниз (то что нужно)
3- вверх
2- поворот влево
1- вправо
0- всегда 0

В некоторых источниках указана посылка FF 01 00 00 00 00 - STOP
Отсюда ПЕРВЫЙ ВОПРОС: камера выполняет наклон/поворот пока ей с определенной переодичность шлют команду наклоняться/поворачиваться либо до момента прихода посылки Stop?

В мой реализации достаточно вверх, вниз, влево, вправо без диагональных направлений и скорости.
Пример байта 6 (команда на поворот влево, остальные 0):
0b00000100 или 0x04

Соответственно посылка будет иметь следующие виды (адрес: 1, к.с. не важна, пусть будет 12, а скорость 3F):
FF 01 00 04 3F 00 12 - Влево
FF 01 00 02 3F 00 12 - Вправо
FF 01 00 08 00 3F 12 - Вверх
FF 01 00 10 00 3F 12 - Вниз

Поправьте если я что-то не так понял про реализацию протокола.

И самый главный вопрос как все это считать из UART и определить что делать? Ввести переменную ByteBuf[7], в теле цикла проверять поступление данных в UART командой if (Serial.available()), вносить байты данных в ByteBuf через for (int i=0; i<7; i++) {ByteBuf= Serial.read();} и потом сравнивать адрес c ByteBuf[1] по средствам if (ByteBuf[1] == 0х01) {}, а команды c ByteBuf[3] и включать двигатель в том или ином направлении? Либо как-то через прерывание?

 

А вы каким софтом будете пользоваться? Вот ним то и пробуйте слать команды и смотреть в терминале что и как идет.
А вообще то посмотрите туториалы на https://www.commfront.com/pages/pelco-d-protocol-tutorial
Задачу, кстати, вы себе поставили достаточно сложную как для начинающего. Там ведь равномерное разноскоростное движение плюс обработка исключительных ситуаций (уперлись в какую-то сторону).
И не отбрасывайте контрольную сумму, иначе наловите "не тех" команд - в протоколе маркер начала пакета может встречаться внутри пакета, так что узнать валидность пакета без контрольной суммы будет не возможно.

 

Вот примерно так как вы и описали. Только вначале надо отловить маркер начала пакета FF, а потом принять остальные байты, проверить КС и если она совпадает, отправить пакет в функцию обработки. А по прерываниям от таймера надо перемещать камеру (двигатели шаговые или постоянного тока?)

 

Проект реализовал)))
Спасибо Калинину Эдуарду, многое почерпнул из его уроков на дружественном форуме.

Схема проекта:



Примененное оборудование:
- Arduino Uno
- Драйвер L298N (логика включена - всего два сигнала на канал. Перемычка VC=VS убрана!!! )


- Приёмопередатчик MAX485
- Поворотная платформа (со встроенными оконечниками - самостоятельно отключает питание от двигателя в данном направлении. Двигатели работают на постоянном токе)

Спойлер: Программа

#include <MsTimer2.h>

int en1 = 10; // первый двигатель (отвечает за наклон)
int in1 = 9;

int en2 = 5; // второй двигатель (отвечает за поворот)
int in3 = 6;

const byte Up = 0x08; // команда "вверх"
const byte Down = 0x10; // команда "вниз"
const byte Right = 0x02; // команда "вправо"
const byte Left = 0x04; // команда "влево"
const byte UpLeft = 0x0C; // команда "вверх-влево"
const byte UpRight = 0x0A; // команда "вверх-вправо"
const byte DownRight = 0x12; // команда "вниз-вправо"
const byte DownLeft = 0x14; // команда "вниз-влево"
const byte Stop = 0x00; // команда "стоп"
const byte CamAdress = 0x01; // адрес камеры
int MotorSpeed = 255; // сигнал ШИМ выхода на драйвер L298N от 0 до 255 (0-стоп двигатель)
int DeadZone=0x10; // мертвая зона скорости

/*
Мертвая зона введена по причине использования, в данном проекте, только одной скорости поворотной/наклона платформы (максимальной).
Если вы наклоните джойстик PTZ-пульта вправо на (ось Х), то пульт даст однозначную команду вправо на той или иной скорости и
контроллер включит двигатель вверх на нашей(максимальной) скорости,
но если при этом еще совсем чуть-чуть сместите джойстик вверх (ось Y), то пуль выдаст команду вправо-вверх, в право на той или
иной скорости и вверх на минимальной скорости, а контроллер включит двигатели вправо-вверх на максимальных скоростях.
Для устранения этой "погрешности" введена мертвая зона.
*/
byte n; // число принятых байтов в посылке
byte timeOutCount; // счетчик времени тайм-аута обмена

void setup() {
MsTimer2::set(1, timerInterrupt); // прерывания по таймеру 1 мс
MsTimer2::start(); // разрешаем прерывание по таймеру
Serial.begin(9600); // включаем UART на скорости 9600
pinMode(en1, OUTPUT); // ШИМ выход на двигатель 1
pinMode(en2, OUTPUT); // ШИМ выход на двигатель 2
pinMode(in1, OUTPUT); // цифровой выход на двигатель 1 (LOW/HIGH меняют полярность выходного напряжения с L298N на двигатель 1, при EN1>0)
pinMode(in3, OUTPUT); // цифровой выход на двигатель 2 (LOW/HIGH меняют полярность выходного напряжения с L298N на двигатель 2, при EN2>0)
}

void loop() {

}

//-------------------------------------- обработчик прерывания 1 мс
void timerInterrupt() {

timeOutCount++;

n= Serial.available(); // число принятых байтов

if (n == 0) timeOutCount= 0; // данных нет, сброс таймера

else if (n == 7) { //если принята посылка из 7 байт

byte buf[7]; // чтение команды в буфер
buf[0]= Serial.read(); // байт синхроимпульса
buf[1]= Serial.read(); // байт адреса камеры
buf[2]= Serial.read(); // байт команды 1 - нам не интересен (в этом проекте не реализован)
buf[3]= Serial.read(); // байт команды 2 - информация о наклоне/повороте/зуме
buf[4]= Serial.read(); // байт скорости поворота от 0х00(стоп) до 0x3F(максимум), иногда 0xFF - турбоскорость
buf[5]= Serial.read(); // байт скорости наклона от 0х00(стоп) до 0x3F(максимум), иногда 0xFF - турбоскорость
buf[6]= Serial.read(); // байт контрольной суммы (в этом проекте не реализован)




if(buf[0] == 0xFF) // если получен байт синхронизации 0xFF
{

if (buf[1] == CamAdress) { // если адрес камеры совпадает


if ((buf[3] == Up)&&(buf[5] > DeadZone)) { // если принята команда Up и скорость наклона >DeadZone
digitalWrite(in1, LOW);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вверх"
}

if ((buf[3] == Down)&&(buf[5] > DeadZone)) { // если принята команда Down и скорость наклона >DeadZone
digitalWrite(in1, HIGH);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вниз"
}

if ((buf[3] == Right)&&(buf[4] > DeadZone)) { // если принята команда Right и скорость поворота >0
digitalWrite(in3, LOW);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "вправо"
}

if ((buf[3] == Left)&&(buf[4] > DeadZone)) { // если принята команда Left и скорость поворота >0
digitalWrite(in3, HIGH);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "влево
}

if ((buf[3] == UpRight)&&(buf[5] > DeadZone)&&(buf[4] > DeadZone)) {
// если принята команда UpRight и скорости наклона и поворота >DeadZone
digitalWrite(in1, LOW);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вверх"
digitalWrite(in3, LOW);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "вправо"
}

if ((buf[3] == UpLeft)&&(buf[5] > DeadZone)&&(buf[4] > DeadZone)) {
// если принята команда UpLeft и скорости наклона и поворота >DeadZone
digitalWrite(in1, LOW);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вверх"
digitalWrite(in3, HIGH);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "влево"
}

if ((buf[3] == DownRight)&&(buf[5] > DeadZone)&&(buf[4] > DeadZone)) {
// если принята команда DownRight и скорости наклона и поворота >DeadZone
digitalWrite(in1, HIGH);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вниз"
digitalWrite(in3, LOW);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "вправо"
}

if ((buf[3] == DownLeft)&&(buf[5] > DeadZone)&&(buf[4] > DeadZone)) {
// если принята команда DownLeft и скорости наклона и поворота >DeadZone
digitalWrite(in1, HIGH);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вниз"
digitalWrite(in3, HIGH);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "влево"
}

// устраняем "погрешность":
if ((buf[3] == UpRight)&&(buf[5] > DeadZone)&&(buf[4] < DeadZone)) {
// если принята команда UpRight и скорость наклона>DeadZone,а поворота <DeadZone
digitalWrite(in1, LOW);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вверх"
}

if ((buf[3] == UpLeft)&&(buf[5] > DeadZone)&&(buf[4] < DeadZone)) {
// если принята команда UpLeft и сскорость наклона>DeadZone,а поворота <DeadZone
digitalWrite(in1, LOW);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вверх"
}

if ((buf[3] == DownRight)&&(buf[5] > DeadZone)&&(buf[4] < DeadZone)) {
// если принята команда DownRight и скорость наклона>DeadZone,а поворота <DeadZone
digitalWrite(in1, HIGH);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вниз"
}

if ((buf[3] == DownLeft)&&(buf[5] > DeadZone)&&(buf[4] < DeadZone)) {
// если принята команда DownLeft и скорость ннаклона>DeadZone,а поворота <DeadZone
digitalWrite(in1, HIGH);
analogWrite(en1, MotorSpeed); // пуск двигателя 1 "вниз"
}

if ((buf[3] == UpRight)&&(buf[5] < DeadZone)&&(buf[4] > DeadZone)) {
// если принята команда UpRight и скорость наклона<DeadZone,а поворота >DeadZone
digitalWrite(in3, LOW);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "вправо"
}

if ((buf[3] == DownRight)&&(buf[5] < DeadZone)&&(buf[4] > DeadZone)) {
// если принята команда DownRight и скорость наклона<DeadZone,а поворота >DeadZone
digitalWrite(in3, LOW);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "вправо"
}

if ((buf[3] == UpLeft)&&(buf[5] < DeadZone)&&(buf[4] > DeadZone)) { /
/ если принята команда UpLeft и сскорость наклона<DeadZone,а поворота >DeadZone
digitalWrite(in3, HIGH);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "влево"
}

if ((buf[3] == DownLeft)&&(buf[5] < DeadZone)&&(buf[4] > DeadZone)) {
// если принята команда DownLeft и скорость ннаклона<DeadZone,а поворота >DeadZone
digitalWrite(in3, HIGH);
analogWrite(en2, MotorSpeed); // пуск двигателя 2 "влево"
}

if (buf[4] == Stop) { // если принята команда со скоростью поворота = 0
digitalWrite(in3, LOW);
analogWrite(en2, 0); // остановка двигателя 2
}

if (buf[5] == Stop) { // если принята команда со скоростью наклона = 0
digitalWrite(in1, LOW);
analogWrite(en1, 0); // остановка двигателя 1
}
}
}
}
}

В данной проекте реализовано включение двигателей наклона и поворота при поступлении соответствующих команд.
Направления движения: вверх, вниз, влево, вправо, 4 диагональных направления.
Скорость постоянная, для реализации разноскоростных движений нужно дорабатывать программу. По этому введены "мертвые зоны скорости".
Нужно учесть что байт команд 2 содержит информацию не только о наклоне и повороте, но и о зуме и фокусе, сейчас это не реализовано и при поступлении байта комадн 2 с поворотом/наклоном и фокусом/зумом (наклоне PTZ-джойстика и нажатии кнопки зум/фокус) контроллер полностью проигнорирует эту посылку. Для доработки нужно рассматривать не байт команды, а находящиеся в нем биты (см протокол Pelco-D).
Контроллер при поступлении команды вверх/вниз включает двигатели, остановка происходит после поступления команды Stop (0хFF 0х01 0х00 0х00 0х00 0х00 0х01), а именно когда скорость = 0х00