Универсальная мобильная платформа с сетью на базе Xbee

Основной задачей было создание базы (платформы) с возможностью дистанционного управления с компьютера несколькими роботами, обмена данными между компьютером и роботами между собой (mesh networking) вплоть до осуществления коллективных действий группой роботов.

Для решения это задачи нет ничего лучше радиомодулей Xbee, использующих протокол 802.15.4. Софт верхнего уровня может быть разработан в любой среде программирования позволяющей работать с последовательным портом. Мы же сконцентрируемся на создании железок и программировании Arduino и Xbee. В результате я собрал двух роботов для экспериментов, один из которых помимо радиомодуля оснащён инфракрасными датчиками Sharp для реализации объезда препятствий.



Конструкция
Несущая платформа вырезана из листового полистирола толщиной 5мм. Крепления сервомашинок вырезаны из полистирола толщиной 8мм и приклеены дихлорэтаном к платформе, и для дополнительной прочности притянуты к ней саморезами. Платформа подразумевает расположение двух сервомашинок, Arduino и 4-х аккумуляторов размера AA для питания. Точных размеров я здесь не привожу потому, как они могут варьироваться в зависимости от применяемых колес и сервомашинок. Необходимо лишь обратить внимание на центр масс, чтобы конструкция была устойчивой и не опрокидывалась назад при резких движения (особенно актуально при расположении тяжёлых аккумуляторов сзади, что удобно для их замены). В роботах применяются колеса от Polulu и сервомашинки постоянного вращения.





Pазработка
Разработку я начал с алгоритма объезда препятствий. Сзади и впереди установлены инфракрасные датчики Sharp GP2Y0A02, слева и справа GP2Y0A21YK.


Эти датчики отличаются расстоянием определения препятствия. Принцип работы заключается в измерении уровня отраженного от препятствия инфракрасного импульса испущенного светодиодом датчика. Датчик выдаёт аналоговый сигнал, поступающий на вход АЦП Arduino.

Схема проще некуда, единственный момент — помехозащищенность. Для её повышения производитель рекомендует возле каждого датчика ставить конденсатор на 10 мкф. Я не стал пренебрегать этим. Разумеется, это не защитит робота от случайных объектов, попавших в поле зрения датчиков и бессмысленных метаний. Необходима некоторая фильтрация или инерционность. Я использовал идею «модерерирования» новых значений от датчика предыдущими.

Sensor[i] = Sensor[i]* 7 + analogRead(i);  //math smoothing (moderating new values by revious)
Sensor[i] = Sensor[i] / 8;                 //smoothed value

Значение, полученное от датчика, вычисляется в зависимости от предыдущих значений и не может изменятся скачкообразно. Выходная характеристика датчиков нелинейная, но при желании можно привести результаты измерений к удобным человеческим данным (например к см). В моём случае это не понадобилось, и я ограничился обработкой абстрактных значений.

Логика проста: засёк датчиком препятствие — ломись в противоположную сторону. Единственное дополнение это то, что срабатывание переднего и заднего датчиков одновременно приводит к развороту робота.

Организация радиосвязи на базе Xbee более интересная задача. Xbee — это радиомодули, производимые фирмой Digi (MaxStream) и использующие протокол Zigbee и 802.15.4.
ru.wikipedia.org/wiki/Zigbee

Я использовал Series 1 модули, реализация задачи на которых несколько проще Series 2. Xbee достаточно сложное полноценное микропроцессорное устройство, в отличие от простых радиомодулей на 433Mhz, часто используемых в любительской робототехнике. Стоит отметить наличие встроенных входов-выходов, в том числе и аналоговых, возможности энергосбережения и т.д. Это самостоятельное устройство отлично справляющееся с определенным кругом задач и без дополнительного микроконтроллера. Однако в нашем случае Xbee работает модемом для связи с управляющим компьютером.

Существует два режима работы Xbee: прозрачный режим AT mode (Transparent mode) и API mode. В прозрачном режиме Xbee просто транслирует через радиоканал сигналов Tx на все находящиеся в зоне действия сети модули. АPI режим куда интереснее и полностью использует их возможности.

• Адресация пакетов данных (пересылка данных на конкретный Xbee в сети).
• Подтверждение и повтор посылки. При посылке пакета, передатчик получает сигнал подтверждение (ACK), индицирующий то, что пакет был успешно доставлен. В другом случае передатчик еще раз (по умолчанию до 3х) пошлёт пакет.
• Простая пересылка данных на конкретный модуль и широковещательные пакеты (на все модули одновременно).
• Пакеты включают в себя checksum для проверки целостности данных.



Система радиоуправления состоит из Xbee координатора (Coordinator) и двух конечных устройств (End device). Конфигурирование модулей может быть осуществлено с помощью AT команд из терминала или с помощью удобной утилиты X-CTU (только для Windows).
www.digi.com/support/productdetail?pid=3352

Настройки для координатора:
RE //Сброс на заводские настройки
AP=2 //Переводим Xbee в API режим
CE=1 //Назначаем модуль координатором
MY=1000 //Устанавливаем адрес устройства
ID=1110 //Устанавливаем PAN ID.
	//Каждый Xbee в пределах одной сети должен иметь одинаковый идентификатор сети.
	//Сетей может быть несколько, но у нас очевидно одна.   
CH=0C //Все модули должны иметь один и тот же канал
WR //Сохраняем
FR //Перегружаемся

Настройки для конечного устройства.
Настройки для конечного устройства такие же, за исключением:

CE=0 //Назначаем модуль конечным устройством
//Устанавливаем уникальный адрес для каждого устройства. Например, в моём случае:
MY=1111  //Платформа 1
MY=2222  //Платформа 2


Настроить Xbee несложно, но для этого необходимо подключить его к компьютеру. Я слышал о способе сконфигурировать Xbee используя Arduino и посылая AT команды с него, но мне показалось это не слишком удобным. Я использовал утилиту X-CTU и самодельный COM кабель на микросхеме MAX232. Подойдёт и любой другой преобразователь с TTL уровнями. Например, рекомендуемая для работы с Xbee и PC плата Xbee Explorer USB — не что иное как микросхема FT232 и панелька для модулей. Необходимо помнить о том, что Xbee имеет 3,3B питание и соответствующие уровни. Для удобной работы с Xbee и Arduino я использовал Xbee Shield от Sparkfun, имеющий в своём составе преобразователь уровней и стабилизатор на 3,3В.

Но ведь в Arduino Duemilanova и аналогах уже распаяна микросхема FT232, так почему бы не использовать её? Taким образом радиомодули можно параметрировать, вытащив из Arduino атмегу и соединив соответствующие выводы rx/tx FT232 и Xbee. Позже я распаял перемычки, позволяющие делать это без извлечения микроконтроллера из Duemilanova.
У меня Xbee координатор установлен в shield и работает в связке с Arduino. Здесь есть некоторая избыточность, потому что можно обойтись любим из вышеперечисленных способов связать управляющий компьютер с Xbee, но у меня под рукой был только свободный Arduino и Xbee shield, так что их я и использовал, попутно реализовав дополнительный функционал по контролю ошибок по светодиодам. Я использовал два двухцветных светодиода по одному на каждого робота. Зеленый показывает что робот «в сети». Красный — ошибку связи.

В итоге для работы необходимо 2 последовательных порта. Один держит связь с PC, Arduino занимается предобработкой и отправляет/принимает данные через второй порт в Xbee. Duemilanova имеет только один, так что второй я реализовал программно используя библиотеку NewSoftSerial arduiniana.org/libraries/newsoftserial/. В процессе экспериментов выяснилось что программный порт плохо работает с Xbee в API режиме, особенно на скоростях выше 9600. Это существенный недостаток, поэтому программный порт используется для связи с PC, с чем он справляется великолепно, а железный порт (пины 0,1) работает с Xbee. Для этого я немного модифицировал Arduino разорвав цепи RX/TX между Atmega 328 и FT232 и припаяв два провода с PLS контактами к ногам RX/TX FT232. Теперь у меня появилась возможность программировать Xbee c PC (о чем я писал выше) и использовать любые пины Arduino для связи с PC через Software Serial, что также полезно для дебага.



Для работы с модулями Xbee я использовал прекрасную библиотеку xbee-arduino.
code.google.com/p/xbee-arduino/
Обработчик терминальных команд основан на еще одной отличной библиотеке
husks.wordpress.com/2011/05/23/a-minimal-arduino-library-for-processing-serial-commands/ немного подправленной для работы с NewSoftSerial.

Итоговый код для Arduino с комментариями
/*
Program for 2 servo bots controlled from PC via terminal commands and Xbee in API mode.
Xbee on hardware serial (pin 0,1)
PC on software serial (pin 2, 3)
This requires modificated Arduino board or TTL/RS232/USB converter connected to NewSoftSerial pins

Command format: vel first_bot_left_servo_speed first_bot_right_servo_speed second_bot_left_servo_speed second_bot_right_servo_speed 
Example: vel 180 0 180 0

LEDs on pins 9,10 and 11,12 indicates status and errors due Xbee
Led's are off - no online robots
Green - Robot online and ready
Red  -  Communication error
*/
#include <XBee.h>
#include <NewSoftSerial.h>
#include <NSSerialCommand.h>
NewSoftSerial PCSerial(2, 3); // 2-Rx, 3-Tx
NSSerialCommand SCmd;   // The PC SerialCommand object
XBee xbee = XBee();
uint8_t payload_1[] = {
  0,0}; //data to send to first bot
uint8_t payload_2[] = {
  0,0}; //data to send to second bot
Tx16Request tx_1 = Tx16Request(0x1111, payload_1, sizeof(payload_1)); 
// 16-bit addressing: 1111 - address of remote XBee - first bot
TxStatusResponse txStatus_1 = TxStatusResponse(); // response of first bot
Tx16Request tx_2 = Tx16Request(0x2222, payload_2, sizeof(payload_2)); 
// 16-bit addressing: 2222 - address of remote XBee - second bot
TxStatusResponse txStatus_2 = TxStatusResponse(); // response of first bot
uint8_t LeftSpeed_1 = 90;  //Speed of the left servo (first bot)
uint8_t RightSpeed_1 = 90; //Speed of the right servo (first bot)
uint8_t LeftSpeed_2 = 90;  //Speed of the left servo (second bot)
uint8_t RightSpeed_2 = 90; //Speed of the right servo (second bot)
void setup() {
  PCSerial.begin(57600);//Start talking with PC 
  xbee.begin(57600);    //Start talking to Xbee
  pinMode(11, OUTPUT);  //Status led
  pinMode(12, OUTPUT);  //ERROR  led
  pinMode(9, OUTPUT);   //Status led
  pinMode(10, OUTPUT);  //ERROR  led
  // Setup callbacks for SerialCommand commands
  SCmd.addCommand("vel",process_velocity);  // Set velocity for robot from PC
  SCmd.addDefaultHandler(unrecognized);     // Handler for command that isn't matched  
  PCSerial.println("Ready"); 
}

void loop() {
  SCmd.readNSSerial();     //  process serial commands
  //////////////////////////////////////////////////////////////////////////////////////////
  // now we are going to send speed of servo to the first bot
  //////////////////////////////////////////////////////////////////////////////////////////
  payload_1[0] = LeftSpeed_1;
  payload_1[1] = RightSpeed_1;
  xbee.send(tx_1);
  // after sending a tx request, we expect a status response
  // wait up to 500 msec for the status response
  if (xbee.readPacket(500)) {
    // got a response!
    // should be a znet tx status            	
    if (xbee.getResponse().getApiId() == TX_STATUS_RESPONSE) {
      xbee.getResponse().getZBTxStatusResponse(txStatus_1);
      // get the delivery status, the fifth byte
      if (txStatus_1.getStatus() == SUCCESS) {
        // successfull
        digitalWrite(11, HIGH); // turn on green status led
        digitalWrite(12, LOW);  // turn off red ERROR led
      } 
      else {
        // the remote XBee did not receive packet.
        digitalWrite(11, LOW); // turn off green status led
      }
    }
  } 
  else {
    // local XBee did not provide a timely TX Status Response 
    digitalWrite(11, LOW);  // turn off green status led
    digitalWrite(12, HIGH); // turn on red ERROR led
  }
  //////////////////////////////////////////////////////////////////////////////////////////
  // and now we are going to send speed of servo to the second bot 
  //////////////////////////////////////////////////////////////////////////////////////////
  payload_2[0] = LeftSpeed_2;
  payload_2[1] = RightSpeed_2;
  xbee.send(tx_2);
  // after sending a tx request, we expect a status response
  // wait up to 500 msec for the status response
  if (xbee.readPacket(500)) {
    // got a response!
    // should be a znet tx status            	
    if (xbee.getResponse().getApiId() == TX_STATUS_RESPONSE) {
      xbee.getResponse().getZBTxStatusResponse(txStatus_2);
      // get the delivery status, the fifth byte
      if (txStatus_2.getStatus() == SUCCESS) {
        // successfull
        digitalWrite(9, HIGH); // turn on green status led
        digitalWrite(10, LOW); // turn off red ERROR led
      } 
      else {
        // the remote XBee did not receive packet.
        digitalWrite(9, LOW); // turn off green status led
      }
    }
  } 
  else {
    // local XBee did not provide a timely TX Status Response 
    digitalWrite(9, LOW);   // turn off green status led
    digitalWrite(10, HIGH); // turn on red ERROR led
  }
}
//////////////////////////////////////////////////////////////////////////////////////////
//              function for processing serial input from pc
//////////////////////////////////////////////////////////////////////////////////////////
void process_velocity()    
{
  int aNumber;  
  char *arg; 

  PCSerial.println("We're in process_velocity"); 
  arg = SCmd.next(); 
  if (arg != NULL) 
  {
    aNumber=atoi(arg);    // Converts a char string to an integer
    LeftSpeed_1=aNumber;
    PCSerial.print("Speed of 1st bot left servo was set to: "); 
    PCSerial.println(aNumber); 
   } 
  else {
    PCSerial.println("No set speed"); 
  }

  arg = SCmd.next(); 
  if (arg != NULL) 
  {
    aNumber=atol(arg); 
    RightSpeed_1=aNumber;
    PCSerial.print("Speed of 1st bot right was servo set to: "); 
    PCSerial.println(aNumber); 
    } 
  else {
    PCSerial.println("No set speed"); 
  }
  arg = SCmd.next(); 
  if (arg != NULL) 
  {
    aNumber=atoi(arg);    // Converts a char string to an integer
    LeftSpeed_2=aNumber;
    PCSerial.print("Speed of 2nd bot left servo was set to: "); 
    PCSerial.println(aNumber); 
  } 
  else {
    PCSerial.println("No set speed"); 
  }

  arg = SCmd.next(); 
  if (arg != NULL) 
  {
    aNumber=atol(arg); 
    RightSpeed_2=aNumber;
    PCSerial.print("Speed of 2nd bot right was servo set to: "); 
    PCSerial.println(aNumber); 
  } 
  else {
    PCSerial.println("No set speed"); 
  }

}

//////////////////////////////////////////////////////////////////////////////////////////
// This gets set as the default handler, and gets called when no other command matches. 
//////////////////////////////////////////////////////////////////////////////////////////
void unrecognized()
{
  PCSerial.println("Wrong command"); 
}


Для удобной отладки и испытаний я использовал 4 кнопки, подключенные к цифровым входам Arduino: вперед, назад, влево, вправо. Кнопки просто присваивают соответствующим переменным фиксированные значение скоростей, которые потом передаются через Xbee роботами.
Платформы принимает посылку, и выставляют заданное значение скоростей для сервомашинок. Здесь есть один момент, о котором стоит подумать заранее. Если платформа успешно получает задание и отсылает подтверждение, а потом по какой то причине связь между координатором и роботом теряется, то полученное задание не обновляется и если это было движение, то оно будет продолжаться. Чтобы этого не произошло, я использовал таймер на 200 мс, по истечении которого если новая посылка не получена, то задание на сервопривода сбрасывается. Это реализовано на таймере 2 входящем в состав Atmega, тогда как таймер 1 занимается работой с сервами. Используется библиотека MsTimer2.
arduino.cc/playground/Main/MsTimer2

//Robot with obstacle avoiding routine and PC RC Control mode (selected by switch)
#include <Servo.h>
#include <XBee.h>
#include <MsTimer2.h>
XBee xbee = XBee();
XBeeResponse response = XBeeResponse();
// create reusable response objects for responses we expect to handle 
Rx16Response rx16 = Rx16Response();
Servo ServoLeft;
Servo ServoRight;
uint8_t LeftSpeed = 0;     //Speed of the left servo
uint8_t RightSpeed = 0;    //Speed of the right servo
int Sensor[6];             // array of sensors values
int i=0;                   // loop index
const int mode=11;         // Mode selector pin
void setup() {
  pinMode(mode, INPUT);    // initialize mode pin as an input
  digitalWrite(mode,HIGH); // enable pull up resistor
  ServoLeft.attach(9);     //Left servo connected to pin 9
  ServoRight.attach(10);   //Right servo connected to pin 10
  ServoLeft.write(90);     //Start without movement
  ServoRight.write(90);    //Start without movement
  pinMode(13, OUTPUT);     //ERROR LED
  xbee.begin(57600);
  MsTimer2::set(200, Stop); // ms period of received movement command
  for (i=0; i<6; i++)
  {
    Sensor[i]=0;            //initialize sensor values to 0
  }
}
void loop() {
  if (digitalRead(mode)==0){                  // if mode=0 -obstacle avoidance ON
    for (i=0; i<6; i++)                      //organize a loop to collect values from all sensors
    {
      Sensor[i] = Sensor[i]* 7 + analogRead(i);  //math smoothing (moderating new values by previous)
      Sensor[i] = Sensor[i] / 8;                 //smoothed value
    }
    if (Sensor[1]>250 && Sensor[5]>250){
      ReverseRight();
      goto exit;
    }
    if (Sensor[0]>250 && Sensor[2]>250){
      FullBackward();
      goto exit;
    }
    if (Sensor[3]>250 && Sensor[4]>250){
      FullForward();
      goto exit;
    }
    else if(Sensor[0] > 250) {
      ReverseRight();
      goto exit;
    }
    else if(Sensor[1] > 250) {
      FullBackward();
      goto exit;
    }
    else if(Sensor[2] > 250) {
      ReverseLeft();
      goto exit;
    }
    else if(Sensor[3] > 250) {
      ForwardRight();
      goto exit;
    }
    else if(Sensor[4] > 250) {
      ForwardLeft();
      goto exit;
    }
    else if(Sensor[5] > 250) {
      FullForward() ;
      goto exit;
    } 
    else {
      Stop();
    }
  }
exit:
  if (digitalRead(mode)==1){                  // if mode=1 -obstacle avoidance ON   
    xbee.readPacket();

    if (xbee.getResponse().isAvailable()) {
      // got something

      if (xbee.getResponse().getApiId() == RX_16_RESPONSE ) {
        // got a rx packet

        if (xbee.getResponse().getApiId() == RX_16_RESPONSE) {
          xbee.getResponse().getRx16Response(rx16);
          MsTimer2::stop(); // stop safety timer
          LeftSpeed = rx16.getData(0);
          ServoLeft.write(LeftSpeed);
          RightSpeed = rx16.getData(1);
          ServoRight.write(RightSpeed);
          digitalWrite(13, LOW);// turn off ERROR led
          MsTimer2::start();    // start safety timer again (counting till next command)  
        }         
      } 
      else {
        // not something we were expecting
        digitalWrite(13, HIGH);//  turn on ERROR led
      }
    }
  }
}
// Functions of Movements
void FullForward()
{
  ServoLeft.write(180);
  ServoRight.write(0); 
}
void FullBackward() 
{
  ServoLeft.write(0);
  ServoRight.write(180); 
}
void ReverseRight()
{
  ServoLeft.write(180);
  ServoRight.write(180); 
}
void ReverseLeft()
{
  ServoLeft.write(0);
  ServoRight.write(0); 
}
void ForwardRight()
{
  ServoLeft.write(180);
  ServoRight.write(75); 
}
void ForwardLeft()
{
  ServoLeft.write(105);
  ServoRight.write(0);
}
void BackwardRight()
{
  ServoLeft.write(0);
  ServoRight.write(105);
}
void BackwardLeft()
{
  ServoLeft.write(75);
  ServoRight.write(180);
}
void Stop() 
{
  ServoLeft.write(90);
  ServoRight.write(90);  
  MsTimer2::stop();                     //Stop safety timer to not interrupt obstacle avoiding routine
}


Результат
В итоге, для отладки я сделал двух роботов.

  • +3
  • 13 мая 2012, 18:56
  • 2nz

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

RSS свернуть / развернуть
+
+1
Отличная работа, шикарное видео!
avatar

Zoltberg

  • 18 мая 2012, 21:36
+
0
Спасибо.
avatar

2nz

  • 18 мая 2012, 21:39

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