iRobot Сreate #3 – управление движением (акселерометр, голосовое) с Android через ROS

ПРОДОЛЖЕНИЕ Написание скетча для ARDUINO — начало здесь далее здесь

Подключение платы Arduino с Bluetooth модулем HC05. Bluetooth-модуль HC05 (подробно) подсоединяем к контактам 2,3 Arduino.
1 (Tx HC05)-> 2 (Arduino)
2 (Rx HC05)-> 3 (Arduino)
13,21,22 GND -> GND Arduino
12 -> 3,3В Arduino
Модуль необходимо перепрограммировать
Для этого подаем 3,3В на 34 вывод HC05
Про программирование можно почитать здесь и здесь
В Arduino загружаем скетч (используем библиотеку SoftwareSerial)
Скорость придется подбирать — разные модули изначально запрограммированы на разную скорость
#include <SoftwareSerial.h>

SoftwareSerial mySerial(2, 3); // указываем пины rx и tx соответственно

void setup()  
{
  pinMode(2,INPUT);
  pinMode(3,OUTPUT);
  Serial.begin(38400);
  mySerial.begin(38400);
  Serial.println("38400");
  Serial.println("ok");
}

void loop()
{
  if (mySerial.available())
  {
    int c = mySerial.read(); // читаем из software-порта
    Serial.write(c); // пишем в hardware-порт
  }
  if (Serial.available())
  {
    int c = Serial.read(); // читаем из hardware-порта
    mySerial.write(c); // пишем в software-порт
  }
}

Открываем serial port и посылаем команды программирования
AT+NAME=любое имя
AT+ROLE:0

ОДИН ОЧЕНЬ ВАЖНЫЙ МОМЕНТ!!!
Чтобы модуль был виден устройствам Android необходимо установить параметр CLASS равным 7936.
AT CLASS:7936

Скорость передачи устанавливаем 38400
Модуль готов.

Теперь, используя библиотеку ros_lib напишем скетч, отправляющий сообщения из Arduino в тему Theme_arduino1. Написание скетча для создание узла publisher, отправляющего сообщениия в ROS из Arduino мы рассматривали здесь.
Но здесь возникла небольшая проблема. Для каждого типа сообщений ROS библиотека ros_lib содержит заголовочные файлы. Для сообщений в теме Theme_arduino1 мы создаем пользовательский тип сообщений Arduino1 (о создании файлов типов сообщений см. Создание ROS msg). Вот содержимое файла Arduino1.msg
int16 var1
int16 var2
int16 var3
int16 var4
int16 var5

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

rosrun rosserial_client make_library.py path_to_libraries your_message_package

В нашем случае:
rosrun rosserial_client make_library.py ~/sketchbook vp_arduino1

В папке ~/sketchbook/ros_lib созданы заголовочные файлы для отправки сообщений типа Arduino1 в ROS. Переносим эти папки в папку /libraries IDE Arduino.
Теперь мы можем создать скетч для отправки сообщений в ROS.
Скетч представлен в листинге:

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

#include <SoftwareSerial.h>
#include <ros.h>
#include <vp_arduino1/Arduino1.h>

SoftwareSerial mySerial(2, 3); // указываем пины rx и tx соответственно
// rx-tx hc05 и tx-rx-rx hc05 соответственно
ros::NodeHandle  nh;
vp_arduino1::Arduino1 message1;
ros::Publisher pub1("Theme_arduino1", &message1);

boolean full=false;
int buf1[5]={0,0,0,0,0};
int offbuf=0;
int mode=0;

void setup()  
{
  pinMode(2,INPUT);
  pinMode(3,OUTPUT);
  //Serial.begin(38400);
  mySerial.begin(38400);
  //Serial.println("38400");
  //Serial.println("ok");
  nh.initNode();
  nh.advertise(pub1);
  pub();
}

void loop()
{
  while(mySerial.available())
  {
    byte c = mySerial.read(); // читаем из software-порта
    buf1[offbuf]=c;offbuf++;
    if(offbuf>2)  
      full=true;  
    //Serial.write(c); // пишем в hardware-порт
  }
//
if(full==true)
  {
  switch(buf1[0])  
    {
    case 127:  mode=buf1[1];
               for(int i=0;i<5;i++)
                  buf1[i]=0;
               full=false;
               offbuf=0;
       break;
    case 128:  if(mode<1) pub();
       break;
    case 136:  if(mode<1) pub();
       break;
    case 139:  if(mode<1) 
                {
                int p4=buf1[3];int p3=buf1[2];
                buf1[4]=p4;buf1[3]=p3;
                if(buf1[1]==10)
                  {buf1[1]=1;buf1[2]=1;} 
                else if(buf1[1]==8)  
                   {buf1[1]=0;buf1[2]=1;} 
                else if(buf1[1]==2)  
                   {buf1[1]=1;buf1[2]=0;} 
                else   
                   {buf1[1]=0;buf1[2]=0;} 
                pub();   
               };
       break;
    case 145:  if(mode<1) 
                {
                int p3=buf1[3]*256 + buf1[4];
                int p2=buf1[1]*256+ buf1[2];
                buf1[3]=p3;buf1[2]=p2;
                buf1[1]=1;buf1[4]=0;
                pub();   
               };
       break;
    case 137:  if(mode<1) 
                {
                int p3=buf1[3]*256 + buf1[4];
                int p2=buf1[1]*256+ buf1[2];
                buf1[3]=p3;buf1[2]=p2;
                buf1[1]=1;buf1[4]=0;
                pub();   
               };
       break;
    default:    for(int i=0;i<5;i++)
                buf1[i]=0;
                full=false;
                offbuf=0;    
       break;
    }
  }

}

void pub()
  {
  message1.var1=buf1[0];
  message1.var2=buf1[1];
  message1.var3=buf1[2];
  message1.var4=buf1[3];
  message1.var5=buf1[4];
  pub1.publish( &message1 );
  nh.spinOnce();
  for(int i=0;i<5;i++)
    buf1[i]=0;
  full=false;
  offbuf=0;    
  }

Скетч немного изменяет последовательность байт для управления iRobot_create, это связано с особенностями пакета irobot_create_2_1.
Теперь проверяем что получилось
Запускаем в ROS
первый терминал
roscore

второй терминал — В ROS создаем узел serial_node.py пакета rosserial_python, который соединяет нашу Arduino плату с остальной частью ROS
rosrun rosserial_python serial_node.py /dev/ttyUSB0

Запускаем программу на Android — соединяемся по Bluetooth c Arduino и отправляем команды. Смотрим в ROS третий терминал
rostopic echo Theme_arduino1


Видим примерно такое

var1: 139
var2: 8
var3: 153
var4: 170
var5: 180

---

var1: 145
var2: 0
var3: 100
var4: 0
var5: 0

---

var1: 145
var2: 0
var3: 50
var4: 255
var5: 156


Далее — написание subscriber для получения сообщений по теме Theme_arduino1 и отправки сообщений сервисам irobot_create_2_1.

Комментарии (0)

RSS свернуть / развернуть

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.