Приложение

Все обновления прошивок смотрите на сайте ПР

Листинг 1 - Сигнальная лампа на стойке управления

#include <Ethernet.h>       //библиотека для работы с Ethernet-модулем
#include <EthernetUdp.h>    //библиотека для обмена данными по протоколу UDP
#include <EEPROM.h>         //библиотека для работы с энергонезависимой памятью
#include <Wire.h>           //библиотека для работы с интерфейсом I2C

//период отправки текущего состояния светодиодов
uint8_t time_delay =            100;

//локальный и удалённый UDP-порты
unsigned int localPort =        8888;
unsigned int UDPremotePort =    8888;

//скорость обмена данными через последовательный порт
unsigned long baud_serial =     115200;

//IP-адрес контроллера и IP-адрес принимающего устройства
IPAddress MyIP(192, 168, 2, 15);
IPAddress ServerIP(192, 168, 2, 3);

//пины, которыми управляются светодиоды
#define yellow_led 2
#define green_led 3
#define blue_led 4
#define red_led 5

//шаблон буферов для обмена данными
char packetBuffer[UDP_TX_PACKET_MAX_SIZE];  //буфер для хранения входящего UDP-пакета
char ReplyBuffer[50];                       //буфер для формирования ответного сообщения

//объект для обмена данными по протоколу UDP
EthernetUDP Udp;

//MAC-адрес Ethernet-модуля
byte mac[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xAA, 0xDD};

//строки для хранения данных, полученных через последовательный порт
String inputString;
String inputIPString;

//таймеры для периодической отправки данных
unsigned long timer_print_serial;
unsigned long timer_print_udp;
unsigned long timer_print_dxl;

void setup() {
  //инициализируем последовательный порт
  Serial.begin(baud_serial);

  //задаём начальные значения таймеров
  timer_print_serial = millis();
  timer_print_udp = millis();

  //настраиваем пины светодиодов на работу в режиме выхода
  pinMode(red_led, OUTPUT);
  pinMode(blue_led, OUTPUT);
  pinMode(green_led, OUTPUT);
  pinMode(yellow_led, OUTPUT);

  //проверяем работу светодиодов
  checkLeds();

  //формируем MAC-адрес на основе IP-адреса контроллера
  mac[0] = MyIP[0];
  mac[1] = MyIP[1];
  mac[2] = MyIP[2];
  mac[3] = MyIP[3];
  mac[4] = MyIP[2];
  mac[5] = MyIP[3];

  //сообщаем о готовности контроллера к работе
  Serial.println("Ready");

  //запускаем Ethernet-соединение
  Ethernet.begin(mac, MyIP);

  //выводим сетевые настройки в последовательный порт
  Serial.print("My IP: ");
  Serial.println(MyIP);
  Serial.print("Server IP: ");
  Serial.println(ServerIP);

  //запускаем UDP-соединение на заданном локальном порту
  Udp.begin(localPort);
}

void loop() {
  //если в последовательный порт поступили данные,
  //посимвольно считываем их во входную строку
  while (Serial.available()) {
    char data = (char)Serial.read();
    inputString += data;
    delay(10);
  }

  //проверяем наличие входящего UDP-пакета
  int packetSize = Udp.parsePacket();

  if (packetSize) {
    Serial.print("-> Udp: ");

    //считываем полученный UDP-пакет в буфер
    Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);

    //выводим полученные данные в последовательный порт
    Serial.println(packetBuffer);

    //устанавливаем состояния светодиодов
    setValue(packetBuffer);
  }

  //обрабатываем данные, полученные через последовательный порт
  if (inputString.length() > 0) {
    Serial.print("-> ");
    Serial.println(inputString);

    //устанавливаем состояния светодиодов
    setValue(inputString);

    //очищаем строку после обработки команды
    inputString = "";
  }

  //периодически отправляем текущее состояние светодиодов
  if (millis() - timer_print_serial > time_delay) {
    timer_print_serial = millis();

    //выводим состояние светодиодов в последовательный порт
    printSerialValue();

    //отправляем состояние светодиодов по UDP
    printUdpValue();
  }
}

//функция отправки состояния светодиодов по UDP
void printUdpValue() {
  //преобразуем состояния светодиодов в символьные массивы
  char led_red[2];
  String(digitalRead(red_led)).toCharArray(led_red, 2);

  char led_blue[2];
  String(digitalRead(blue_led)).toCharArray(led_blue, 2);

  char led_green[2];
  String(digitalRead(green_led)).toCharArray(led_green, 2);

  char led_yellow[2];
  String(digitalRead(yellow_led)).toCharArray(led_yellow, 2);

  //начинаем формирование UDP-пакета
  Udp.beginPacket(ServerIP, UDPremotePort);

  //записываем в пакет состояния всех светодиодов
  Udp.write("leds: ");
  Udp.write("r: ");
  Udp.write(led_red);
  Udp.write(" b: ");
  Udp.write(led_blue);
  Udp.write(" g: ");
  Udp.write(led_green);
  Udp.write(" y: ");
  Udp.write(led_yellow);
  Udp.write("\n");

  //завершаем формирование и отправляем UDP-пакет
  Udp.endPacket();
}

//функция установки состояния светодиодов
void setValue(String str) {
  //команда должна содержать не менее четырёх символов:
  //красный, синий, зелёный и жёлтый светодиоды
  if (str.length() >= 4) {
    for (int i = 0; i < str.length(); i++) {
      switch (i) {
        //первый символ управляет красным светодиодом
        case 0:
          digitalWrite(red_led, String(str[i]).toInt());
          break;
        //второй символ управляет синим светодиодом
        case 1:
          digitalWrite(blue_led, String(str[i]).toInt());
          break;

        //третий символ управляет зелёным светодиодом
        case 2:
          digitalWrite(green_led, String(str[i]).toInt());
          break;

        //четвёртый символ управляет жёлтым светодиодом
        case 3:
          digitalWrite(yellow_led, String(str[i]).toInt());
          break;
     }
    }
  }
}

//функция вывода состояния светодиодов в последовательный порт
void printSerialValue() {
  //считываем текущие состояния светодиодов
  uint8_t led_red = digitalRead(red_led);
  uint8_t led_blue = digitalRead(blue_led);
  uint8_t led_green = digitalRead(green_led);
  uint8_t led_yellow = digitalRead(yellow_led);

  //формируем и выводим строку с состояниями светодиодов
  Serial.println(
    "leds: r: " + String(led_red) + " " +
    "b: " + String(led_blue) + " " +
    "g: " + String(led_green) + " " +
    "y: " + String(led_yellow));
}

//функция проверки работоспособности светодиодов
void checkLeds() {
  //задержка между переключениями светодиодов
  uint8_t time_sleep = 100;

  //последовательно включаем все светодиоды
  digitalWrite(red_led, HIGH);
  delay(time_sleep);

  digitalWrite(blue_led, HIGH);
  delay(time_sleep);

  digitalWrite(green_led, HIGH);
  delay(time_sleep);

  digitalWrite(yellow_led, HIGH);
  delay(time_sleep);

  //последовательно выключаем все светодиоды
  digitalWrite(red_led, LOW);
  delay(time_sleep);

  digitalWrite(blue_led, LOW);
  delay(time_sleep);

  digitalWrite(green_led, LOW);
  delay(time_sleep);

  digitalWrite(yellow_led, LOW);
  delay(time_sleep);

  //дважды одновременно включаем и выключаем все светодиоды
  for (int i = 0; i < 2; i++) {
    digitalWrite(red_led, HIGH);
    digitalWrite(blue_led, HIGH);
    digitalWrite(green_led, HIGH);
    digitalWrite(yellow_led, HIGH);
    delay(time_sleep);

    digitalWrite(red_led, LOW);
    digitalWrite(blue_led, LOW);
    digitalWrite(green_led, LOW);
    digitalWrite(yellow_led, LOW);
    delay(time_sleep);
  }
}

Листинг 2 - Сигнальная лампа внутри ячейки

//скорость обмена данными, идентификатор устройства и время ожидания ответа
#define DXL_BAUD 1000000
#define DXL_ID 1
#define RTD_TIME 50

//пины управления направлением передачи данных
#define TX_PIN 42
#define RX_PIN 43

//пины, которыми управляются светодиоды
#define RED_PIN 2
#define GREEN_PIN 4
#define BLUE_PIN 5
#define ORANGE_PIN 3

//адреса регистров управления светодиодами
#define RED_DXL_REGISTER 26
#define GREEN_DXL_REGISTER 28
#define BLUE_DXL_REGISTER 29
#define ORANGE_DXL_REGISTER 27

//буфер для хранения принятого пакета Dynamixel
uint8_t dxl_buffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};

//шаблон корректного ответного пакета Dynamixel
uint8_t dxl_good_response[6] = {0xFF, 0xFF, DXL_ID, 0x02, 0x00, 0x00};

//счётчик принятых байтов
uint8_t byte_counter = 0;
void setup() {
  //инициализируем последовательный порт для отладки
  Serial.begin(115200);

  //инициализируем последовательный порт для обмена по протоколу Dynamixel
  Serial1.begin(DXL_BAUD);
  Serial1.setTimeout(RTD_TIME);

  //настраиваем пины управления направлением передачи данных
  pinMode(TX_PIN, OUTPUT);
  pinMode(RX_PIN, OUTPUT);

  //переводим интерфейс в режим приёма данных
  setReadMode();

  //настраиваем пины светодиодов на работу в режиме выхода
  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);
  pinMode(ORANGE_PIN, OUTPUT);
}

void loop() {
  //если в последовательный порт поступили данные,
  //последовательно записываем их в буфер
  while (Serial1.available()) {
    dxl_buffer[byte_counter] = Serial1.read();
    byte_counter++;

    //после получения полного пакета обрабатываем его
    if (byte_counter == 8) {
      processDxl1Protocol();

      //сбрасываем счётчик принятых байтов
      byte_counter = 0;

      //выводим содержимое буфера в последовательный порт
      printBuffer();

      //очищаем буфер после обработки
      clearBuffer();

      //считываем и удаляем оставшиеся данные из последовательного порта
      Serial1.readString();
    }
  }

  //если был получен неполный пакет, очищаем буфер и счётчик
  if (byte_counter != 0) {
    byte_counter = 0;
    printBuffer();
    clearBuffer();
    Serial1.readString();
  }

  //небольшая задержка между циклами обработки
  delay(20);
}

//функция обработки пакета протокола Dynamixel 1.0
void processDxl1Protocol() {
  //проверяем заголовок пакета
  if (!isItDynamixel()) return;

  //проверяем идентификатор устройства
  if (!isItMyDynamixelId()) return;

  //проверяем длину пакета для записи одного байта
  if (!isIt1ByteWrite()) return;

  //проверяем инструкцию записи
  if (!isItWriteInstruction()) return;

  //изменяем состояние соответствующего светодиода
  changeRegister();
}

//функция выбора светодиода по адресу регистра
void changeRegister() {
  //управляем красным светодиодом
  if (dxl_buffer[5] == RED_DXL_REGISTER) {
    useLight(RED_PIN);
  }

  //управляем зелёным светодиодом
  else if (dxl_buffer[5] == GREEN_DXL_REGISTER) {
    useLight(GREEN_PIN);
  }

  //управляем синим светодиодом
  else if (dxl_buffer[5] == BLUE_DXL_REGISTER) {
    useLight(BLUE_PIN);
  }

  //управляем оранжевым светодиодом
  else if (dxl_buffer[5] == ORANGE_DXL_REGISTER) {
    useLight(ORANGE_PIN);
  }
}

//функция включения или выключения выбранного светодиода
void useLight(int pin) {
  //если значение регистра больше или равно единице,
  //включаем светодиод
  if (dxl_buffer[6] >= 1) {
    digitalWrite(pin, HIGH);
  }

  //если значение регистра равно нулю,
  //выключаем светодиод
  else {
    digitalWrite(pin, LOW);
  }
}

//функция проверки инструкции записи
bool isItWriteInstruction() {
  //код инструкции записи в протоколе Dynamixel
  if (dxl_buffer[4] == 0x03) {
    return true;
  }

  //выводим сообщение об ошибке инструкции
  Serial.println("4");
  return false;
}

//функция проверки длины пакета для записи одного байта
bool isIt1ByteWrite() {
  //длина пакета с инструкцией записи одного байта
  if (dxl_buffer[3] == 0x05) {
    return true;
  }

  return false;
}

//функция проверки идентификатора устройства
bool isItMyDynamixelId() {
  //проверяем, что пакет предназначен данному устройству
  if (dxl_buffer[2] == DXL_ID) {
    return true;
  }

  return false;
}

//функция проверки заголовка пакета Dynamixel
bool isItDynamixel() {
  //пакет Dynamixel 1.0 начинается с двух байтов 0xFF
  if (dxl_buffer[0] == 0xFF && dxl_buffer[1] == 0xFF) {
    return true;
  }

  return false;
}

//функция переключения интерфейса в режим чтения
void setReadMode() {
  digitalWrite(TX_PIN, LOW);
  digitalWrite(RX_PIN, HIGH);
}

//функция переключения интерфейса в режим записи
void setWriteMode() {
  digitalWrite(TX_PIN, HIGH);
  digitalWrite(RX_PIN, LOW);
}

//функция вывода содержимого буфера в последовательный порт
void printBuffer() {
  for (uint8_t i = 0; i < 8; i++) {
    Serial.print(String(dxl_buffer[i]) + " ");
  }

  Serial.println();
}

//функция очистки буфера принятого пакета
void clearBuffer() {
  for (uint8_t i = 0; i < 8; i++) {
    dxl_buffer[i] = 0;
  }
}

Листинг 3 - Управление конвейером через DXL-IOT

#include "DxlMaster2.h"  //библиотека для управления сервоприводами Dynamixel

//объект сервопривода Dynamixel с идентификатором 5
DynamixelMotor motor2(5);

//объект дополнительного сервопривода Dynamixel с идентификатором 3
//DynamixelMotor motor3(3);

void setup() {
  //инициализируем интерфейс Dynamixel со скоростью 1 Мбит/с
  DxlMaster.begin(1000000);

  //инициализируем последовательный порт для вывода отладочной информации
  Serial.begin(115200);

  //инициализируем сервопривод с идентификатором 5
  motor2.init();

  //устанавливаем первую версию протокола Dynamixel
  motor2.protocolVersion(1);

  //переводим сервопривод в режим непрерывного вращения
  motor2.wheelMode();

  //инициализируем дополнительный сервопривод
  //motor3.init();

  //переводим дополнительный сервопривод в режим непрерывного вращения
  //motor3.wheelMode();

  //выводим сообщение об успешном запуске программы
  Serial.println("started");
}

void loop() {
  //задаём скорость и направление вращения сервопривода
  motor2.speed(1300);

  //задаём скорость дополнительного сервопривода
  //motor3.speed(300);
}

Листинг 4 - Основной код для управления всеми DXL - совместимыми периферийными устройствами через DXL-IOT (конвейер, сигнальная лампа внутри ячейки, SVcam, TrackingCam3)

#include "DxlMaster2.h"
#include "TrackingCamDxl.h"

// === Скорости и ID ===
const unsigned long serial_baudrate = 115200;
const unsigned long main_baudrate = 1000000;
const unsigned long svcam_baudrate = 115200;

const uint8_t lamp_id = 1;
const uint8_t cam_id = 2;
const uint8_t motor_id = 5;
const uint8_t svcam_id = 3;

// === Регистры лампы ===
const uint8_t red_dxl_register = 26;
const uint8_t green_dxl_register = 28;
const uint8_t blue_dxl_register = 29;
const uint8_t orange_dxl_register = 27;

const int16_t conveyor_speed = 1300;

// === Регистры SVCAM ===
#define DIST_ADDR   24
#define U_ADDR      25
#define V_ADDR      26
#define X_ADDR      28
#define Y_ADDR      29
#define Z_ADDR      30
#define COLOR_ADDR  31

// === Объекты устройств ===
DynamixelDevice lampa(lamp_id);
DynamixelMotor motor2(motor_id);
TrackingCamDxl trackingCam(cam_id);
DynamixelDevice svcam(svcam_id);

// === Таймеры ===
unsigned long previousCamMillis = 0;
unsigned long previousSvcamMillis = 0;
unsigned long lastCommandTime = 0;

// === Переменные SVCAM ===
uint32_t dist_mm = 0, u_pix = 0, v_pix = 0;
uint32_t x_raw = 0, y_raw = 0, z_raw = 0;
uint32_t color_id = 0;

void setup() {
  Serial.begin(serial_baudrate);
  DxlMaster.begin(main_baudrate);

  motor2.init();
  motor2.wheelMode();

  delay(5000);
  Serial.println("Ready. Send command: 0, 1, 2, 3, or 4.");
}

void applyCommand(char command) {
  lastCommandTime = millis();

  switch (command) {
    case '0':
     lampa.write(blue_dxl_register, 1); delay(100);
     lampa.write(green_dxl_register, 0); delay(100);
     lampa.write(red_dxl_register, 0); delay(100);
     lampa.write(orange_dxl_register, 0); delay(100);
     motor2.speed(0); delay(100);
     Serial.println("0: blue, conveyor stopped");
     break;

    case '1':
     lampa.write(green_dxl_register, 1); delay(100);
     lampa.write(red_dxl_register, 0); delay(100);
     lampa.write(orange_dxl_register, 0); delay(100);
     lampa.write(blue_dxl_register, 0); delay(100);
     motor2.speed(0); delay(100);
     Serial.println("1: green, conveyor stopped");
     break;

    case '2':
     lampa.write(green_dxl_register, 1); delay(100);
     lampa.write(red_dxl_register, 0); delay(100);
     lampa.write(orange_dxl_register, 0); delay(100);
     lampa.write(blue_dxl_register, 0); delay(100);
     motor2.speed(conveyor_speed); delay(100);
     Serial.println("2: green, conveyor is working");
     break;

    case '3':
     lampa.write(orange_dxl_register, 1); delay(100);
     lampa.write(red_dxl_register, 0); delay(100);
     lampa.write(green_dxl_register, 0); delay(100);
     lampa.write(blue_dxl_register, 0); delay(100);
     motor2.speed(0); delay(100);
     Serial.println("3: orange, conveyor stopped");
     break;

    case '4':
     lampa.write(red_dxl_register, 1); delay(100);
     lampa.write(green_dxl_register, 0); delay(100);
     lampa.write(orange_dxl_register, 0); delay(100);
     lampa.write(blue_dxl_register, 0); delay(100);
     motor2.speed(0); delay(100);
     Serial.println("4: red, conveyor emergency stop");
     break;

    default:
     Serial.println("Unknown command. Use 0, 1, 2, 3, or 4.");
     break;
  }
}

const char* blobColorName(uint8_t type) {
  if (type == 0) return "Red";
  if (type == 1) return "Blue";
  if (type == 2) return "Green";
  return nullptr;
}

void readCamera() {
  uint8_t rawCount = trackingCam.readBlobs(5); // read data about first 5 blobs

  Serial.println("=== TrackingCam ===");
  Serial.println("All blobs");
  Serial.println(rawCount); // print numbers of blobs

  for(int i = 0; i < rawCount; i++) // print information about all blobs
  {
    const char* color = blobColorName(trackingCam.blob[i].type);
    if (color == nullptr) continue;
    Serial.println(color);
    Serial.print(trackingCam.blob[i].cx, DEC);
    Serial.print(" ");
    Serial.print(trackingCam.blob[i].cy, DEC);
    Serial.println(" ");
    Serial.println(trackingCam.blob[i].area, DEC);
    Serial.print(" ");
  }

    // wait for the next frame
  long previousMillis = millis();
  while(millis() - previousMillis < 33){
    delay(1);
  }

}

void readSvcam() {
  // Переключаем шину на скорость SVCAM
  DxlMaster.begin(svcam_baudrate);
  delay(20);

  // Читаем все регистры
  svcam.read(DIST_ADDR, dist_mm);
  svcam.read(U_ADDR, u_pix);
  svcam.read(V_ADDR, v_pix);
  svcam.read(X_ADDR, x_raw);
  svcam.read(Y_ADDR, y_raw);
  svcam.read(Z_ADDR, z_raw);
  svcam.read(COLOR_ADDR, color_id);

  // Возвращаем основную скорость
  DxlMaster.begin(main_baudrate);
  delay(50);

  Serial.println("=== SVCAM ===");

    if (dist_mm > 0) {
    // Определяем цвет
    String color_name = "NONE";
    if (color_id == 1) color_name = "RED";
    else if (color_id == 2) color_name = "GREEN";
    else if (color_id == 3) color_name = "BLUE";

    // Преобразуем X, Y, Z к знаковым
    int32_t x_mm = (int32_t)x_raw;
    int32_t y_mm = (int32_t)y_raw;
    int32_t z_mm = (int32_t)z_raw;

    // Отправляем данные в формате:
    // 1
    // COLOR U V DIST X Y Z
    Serial.println("1");
    Serial.print(color_name);
    Serial.print(" ");
    Serial.print(u_pix);
    Serial.print(" ");
    Serial.print(v_pix);
    Serial.print(" ");
    Serial.print(dist_mm);
    Serial.print(" ");
    Serial.print(x_mm);
    Serial.print(" ");
    Serial.print(y_mm);
    Serial.print(" ");
    Serial.println(z_mm);
  } else {
    // Если объект не найден
    Serial.println("0");
  }
  delay(40);  // ~25 Hz
}

void loop() {
  if (Serial.available()) {
    char command = (char)Serial.read();
    if (command != '\n' && command != '\r' && command != ' ') {
      applyCommand(command);
    }
    while (Serial.available()) Serial.read();
    return;
  }
  unsigned long now = millis();

  // Опрос TrackingCam каждые 500мс
  if (now - previousCamMillis >= 500 && now - lastCommandTime >= 300) {
    readCamera();
    previousCamMillis = now;
  }

  // Опрос SVCAM каждые 500мс
  if (now - previousSvcamMillis >= 500) {
    readSvcam();
    previousSvcamMillis = now;
  }
}