Управление Parrot Jumping Sumo с помощью Arduino и esp8266

Доброго дня, Уважаемые форумчане.
Сегодня хотелось поделиться с общественностью некоторым proof-of-concept.

В чем этот Proof-of-Concept состоит? Об этом дальше.

Предыстория
В качестве подарка на Новый год, получил дроида ParrotJumpingSumo. Он управляется с телефона по Wi-Fi, через программу, разработанную самой компанией Parrot. Особенность дроида – прыжки на 80 см. в длину или в высоту и достаточно высокая скорость передвижения.



Наигравшись с ним за пару-тройку дней, он занял свое место на полке.
Чуть позже, так же в качестве подарка, мне досталась Arduino Uno в составе набора от Амперки. Пройдя весь конспект хакера, я понял, что зацепило. И в голову закралась идея: нельзя ли собрать для дроида пульт, но на основе Arduino? Начались поиски готовых решений, и ничего. Тогда, начал искать информацию, тогда же узнал про модули ESP8266.

Закупка
Когда концепт сформировался, расчехлил кредитку и полез на Aliexpress. Список покупок в результате, получился следующий:
ESP8266(01)

• DC-DC повышающий конвертер
• FTDI-модуль
• Зарядный модуль для аккумуляторов 18650
• Аккумулятор (18650)
• Преобразователь питания ASM1117 (сразу с обвязкой)
• Джойстик
• Arduino Pro Mini (5В, 16 МГц, Atmega 328)

Резисторы/конденсаторы были в комплектах с Aliexpress или же из набора от Амперки.

Принципиальная схема

На данный момент, Arduino, модуль ESP и джойстик, питаются только от DC-DC преобразователя. Связано с тем, что ESP-шка может потреблять до 300 мА, а это много для Arduino, а также из-за разницы вольтажа: Arduino 5-тивольтовая, а ESP-шка держит только 3.3 вольта и не очень толерантна к превышениям. Именно по этой причине, URXD контакт на ESP запитан через делитель напряжения – пару параллельных резисторов на 100 и 330 Ом. И, конечно, ESP запитана через модуль с ASM1117. Модуль можно увидеть под платформой, на которой стоит ESP-шка.

Вид сверху


Вид сбоку


Закупка-2 и работа руками
Кстати, платформа. ESP, хоть и имеет стандартное расположение контактов: 2 ряда по 4; ее нельзя расположить просто так на макетке. Пришлось крутиться. Как крутиться – видно на фотках и схеме ниже. На том же AliExpress пришлось заказать двусторонние монтажные платы, гребенки и колодки. Сделал заготовки и развел провода. На такой платформе можно спокойно ставить ESP модуль в центральной части макетной платы. Фото результата ниже.




Схема разведенных проводов.


Лирическое отступление
Что же. Схему собрал, запустил, чутка пожег ESP от невнимательности, залил первый тестовый скетч и понял, что прямое соединение RST на Arduino и DTR на FTDI не дает должного результата – автоматической перезагрузки модуля при заливке скетча. Открыл даташит на Arduino, и оказалось, что кнопка на самой Arduino работает через резистор на 10 кОм и конденсатор на 100 нФ. Берем резистор соединяем к VCC и RST, конденсатор соединяем с RST и GND, и наконец, соединяем DTR и RST. Вуаля, оно само перезагружается.

Работа над кодом
Дальше пошла работа над кодом. Для этого пришлось просниффать соединение между управляющим устройством (телефон) и самим дроидом. Прошерстить интернет на предмет SDK, мануалов или адаптаций. Кое-что нашлось. Даже стало понятнее, как оно работает.

1. Рукопожатие
Дроид анонсирует себя по сети по протоколу mDNS, используется порт 53 для анонса. Вроде как по анонсу становится понятно, какой порт должен использоваться дальше для работы над устройством. По умолчанию – порт 44444. На этот порт по TCP отправляется JSON-строка с параметрами рукопожатия. От телефона приходило это:

Код (CSS):

{"d2c_port":54321,"controller_name":"HTC one dual sim ","controller_type":"HTC","audio_codec":3,"arstream2_client_stream_port":55004,"arstream2_client_control_port":55005}

В ответ, от устройства получал такую JSON-строку:

Код (CSS):

{"status":0, "c2d_port":54321, "arstream_fragment_size":65000, "arstream_fragment_maximum_number":4, "arstream_max_ack_interval":-1, "c2d_update_port":51, "c2d_user_port":21 }

Ключевыми тут являются параметры d2c_port и c2d_port – они указывают порт по которому устройство будет отправлять на дроида команды. На этом порту у дроида открыт UDP-socket, который ждет подключения и команд.

В коде для Arduino получается обойтись прямой отправкой строки на UDP-порт:

Код (CSS):

{"d2c_port":54321,"controller_name":"Arduino","controller_type":"ESP8266"}

В ответ приходит та же строка, что и описана выше. После этого, дроид готов к получению команд.

2. Команды
Согласно протоколу, на дроид может прийти 4 вида команд. Но, нас интересует в первую очередь команда для движения – это команды типа SYNC и они кодируются цифрой 2.
Длина команд типа SYNC всегда одинаковая – 28 шестнадцатиричных значений, или 14 байт. Все значения целочисленные, положительные (пока кажется, что они все положительные).
Пакет имеет структуру

Код (C++):

{SYNC, 0x0a, SEQ, sizeof(command), 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, active, speed, turn}

А тут по частям:

• SYNC – тип пакета, о нем было выше
• 0x0a – разделитель
• SEQ – число – последовательность буферов дроида, которые используются. Для пакетов используются буферы от 10 до 127, то есть при каждой отправке пакета с командой это значение должно увеличиваться, пока не достигнет 127, а потом снова сбрасывается до 10.
• sizeof(command) – размер команды. В нашем случае, это всегда будет 14 байт.
• 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00 – блок байт, который всегда повторяется в пакетах. Пока, сложно сказать, для каких целей этот блок нужен.
• Active – признак того, что есть какое-то действие, то есть speed!=0 или turn!=0.
• Speed – параметр говорит сам за себя, это скорость дроида. В интернете нашел информацию, что диапазон [-127, 127]. Но, кажется, вариант не прокатил. Заработало при [0, 255]. Причем [1, 127] – движение назад, а [128, 255] – движение вперед.
• Turn – поворот налево и направо. Тут интернет тоже дает значения [-64, 64]. И опять же, сработало только при [1, 127].

Вместо заключения
Кроме основных движений дроида (вперед-назад и влево-вправо), есть еще и «прыжки» вперед и вверх, переворот и балансирование. По этим движениям пока никаких исследований не проводилось. Возможно, чуть позже попробую. Кстати, внимательный читатель поинтересуется, зачем в схеме зарядное устройство для 18650? Ответ достаточно простой - есть план по подключению аккумулятора с одновременной работой от аккумулятора и зарядкой аккумулятора от USB.

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

Вот такой вот proof-of-concept.
Если кому-то стало интересно попробовать, то добро пожаловать. Код выкладываю как есть. Можете свободно копировать и модифицировать при условии сохранения авторства (себя можно и даже нужно вписывать).

Fritzing-файл с кодом
https://drive.google.com/file/d/0Bw7vh8A1_frLYk1HdGF6dnVjZW8/view?usp=sharing



P.S.Приветствуются предложения по улучшению качества и стабильности кода, а так же, по расширению его функционала.
Ошибки-опечатки в тексте - в личку.

Ну и конечно, для тех, кто дочитал до конца - видео.

 

Сам код

Спойлер: Код проекта

Код (C++):

/*
    Author: Chih-Pih
    Modified by:
    1. Chih-Pih
    2.
*/

#include <WiFiEsp.h>
#include <WiFiEspUdp.h>
#include <SoftwareSerial.h>
#define RX 10
#define TX 11
#define Xaxis A0
#define Yaxis A1
#define SYNC 2

int xPos = 0;
int yPos = 0;
unsigned int seq  = 10;

/*char ssid[] = "SSID";                   // test connection credentials
char pass[] = "P@SSW0RD";
char remote_IP[] = "192.168.1.8";
*/

  char ssid[] = "PI314"; //droid SSID
  char pass[] = ""; // password for droid SSID, empty
  char remote_IP[] = "192.168.2.1"; // droid IP. default

unsigned int TCP_port = 44444; //remote TCP port, default
unsigned int UDP_Port = 54321; //remote UDP port
boolean hndshk = 0;
String data = "";

SoftwareSerial esp8266serial(RX, TX);
int status = WL_IDLE_STATUS;
WiFiEspClient esp8266;
WiFiEspUDP udp8266;

void setup() {
  Serial.begin(38400);
  pinMode(Xaxis, INPUT);
  pinMode(Yaxis, INPUT);
  esp8266serial.begin(38400);
  WiFi.init(&esp8266serial);
  while ( status != WL_CONNECTED) {
    status = WiFi.begin(ssid, pass);
  }
  esp8266.connect(remote_IP, TCP_port);
  udp8266.begin(UDP_port);
}

void handshake() { //handhshake class. Need for start interaction with JumpingSumo
  Serial.println("handshake");
  esp8266.write("{\"d2c_port\":54321,\"controller_name\":\"Arduino\",\"controller_type\":\"ESP8266\"}"); //send query
  delay(100);
  while (1 == 1) { //wait for answer
    data = esp8266.readString();
    if (data.length() > 0) {
      Serial.println(data); //print answer
      data = ""; //flushanswer for memory economy
    }
  }
}

int filter(int pos, int bz, int center, int i) { // filter function. Inputs: position of joystic, range of blind  zone, center of joystic, x (=0)or y(=1)
  int diap[2][4] = {{1, 92, 128, 192}, {1, 31, 64, 92}}; //range of  values {x, y}, x ={back_min, back max, front_min_front_max}, y - the same
  if (((center - bz) <= pos) && (pos <= (center + bz))) { // set blind zone
    return 0;
  }
  else if  (pos < (center - bz)) { //move back
    return map(pos, (center + bz), 0, diap[i][0], diap[i][1]);
  }
  else {//move forward
    return map(pos, (center + bz), 1023, diap[i][2], diap[i][3]);
  }
}

void controller(int xPos, int yPos, unsigned int seq) { // main, controller class.
  boolean active = 0; //set active status
  int xpos = filter(xPos, 50, 560, 0); //filter X position of joystic
  int ypos = filter(yPos, 50, 560, 1); // filter y position of joystic
  if ((xpos > 0) || (ypos > 0)) { //define of active status
    active = 1;
  }
  byte command[14] = {byte(SYNC), 0x0a, byte(seq), byte(sizeof(command)), 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, byte(active), byte(xpos), byte(ypos)}; //format of  command packet. Create packet with values
  udp8266.beginPacket(remote_IP, UDP_port);  // create connection with Jumping Sumo
  udp8266.write(command, sizeof(command)); //send packet  for Jumping Sumo
  udp8266.endPacket(); //stop connection.
}

void loop() {
  if (!hndshk) { //iff irst time - start handhake
    handshake();
    hndshk = 1; //handshake OK
  }
  xPos = analogRead(Xaxis); //read x position of joystic
  yPos = analogRead(Yaxis); //read y position of joystic
  controller(xPos, yPos, seq); //make control
  delay(500); //delay
  if (seq < 127) seq += 1; //define sequence. Must be between 10 and 127
  else seq = 10; //if 127 --> 10
}

 

а видео результатов трудов))))

 

Да, конечно! Навоевался с движком форума и совсем забыл. Добавил в основной пост.

 

шустрый)))