Робот-паук на сервоприводах + ИК-управление

Четырехногий робот-паук создан для демонстрации работы сервомашинок под управлением контроллера Arduino (для кружка робототехники).

У робота два режима:

• автономный - робот движется вперед, при обнаружении препятствия (используется ультразвуковой датчик) поворачивается и движется дальше;
• внешнее управление с помощью ИК-пульта.

Использовались сервомашинки Turnigy TGY-9025MG металлическим редуктором.

В качестве ног робота использовались заглушки для струйных картриджей, скрепленные с помощью поликапролактона

Корпус был сделан из упаковочного материала для компов. Для сервомашинок требуется отдельное питание. В качестве источника питания используется Li-po батарея Turnigy 2S 1600 mAh. 

Вот вид сверху и снизу робота в процессе сборки.

Для управления сервоприводом в Arduino имеется стандартная библиотека Servo. На платах, отличных от Mega, использование библиотеки отключает возможность использования analogWrite() (PWM) на пинах 9 и 10 (вне зависимости подключены к этим пинам сервы или нет). На платах Mega, до 12 серв могут использоваться без влияния на функциональность PWM, но использование от 12 до 23 сервомашинок отключит PWM на пинах 11 и 12. Cервопривод подключается 3-мя проводами: питание, земля и сигнальный. Питание – красный  провод. Черный(или коричневый) провод – земля подключается к GND выводу Arduino, сигнальный(оранжевый/желтый/белый) провод подключается к цифровому выводу контроллера Arduino. Будем использовать выводы 5,6,7,8 Arduino.

Напряжение выдаваемое батареей 7.4 – 8.4 В. Т.к. для питания сервоприводов необходимо напряжение 4.8 – 6.0 В будем использовать стабилизатор напряжения 5В, собранный на микросхеме L7805. Одна микросхема постоянно перегревалась, проблема решилась установкой параллельно двух микросхем L7805. 

Для обнаружения препятствий будем использовать ультразвуковой датчик HC-SR04, который позволяет определять расстояние до объекта в диапазоне от 2 до 500 см с точностью 0.3 см. Если расстояние до препятствия меньше 10 см, робот делает поворот и движется дальше вперед.  

В качестве пульта используется  пульт lg, приемник ИК-сигналов - TSOP31238(1-GND, 2 - +5V, 3-OUT).

Схема электрическая

И весь робот в сборе (плата Arduino питается от батарейки Крона).

Приступим к написанию скетча

Для управления сервоприводами используется Arduino библиотека Servo. Нам необходимо реализовать совокупность движений сервоприводов для движения робота-паука вперед, назад, поворота по часовой стрелке и поворота против часовой стрелки. Кроме того необходимо реализовать функции остановки робота, а также для экономии электроэнергии предусмотрим режим засыпания (когда сервоприводы находятся в режиме detach) и пробуждения (перевод сервоприводов в режим attach).  Поэтому каждое движение робота состоит из нескольких шагов.

Например движение вперед состоит из следующих шагов:

1. левая передняя нога вперед;
2. правая передняя нога вперед;
3. левая задняя нога вперед;
4. правая задняя нога вперед;
5. четыре ноги вместе назад (что приведет к перетаскиванию тела робота-паука).

Данные для угла поворота каждой сервы на каждом шаге для каждого движения робота-паука хранятся в трехмерном массиве arr_pos.

int arr_pos[4][6][4]={

   { // forward
   {90,90,90,90},{45,90,90,90},{45,135,90,90},
   {45,135,45,90},{45,135,45,135},{135,45,135,45}
   },

   { // back
   {90,90,90,90},{90,90,90,45},{90,90,135,45},
   {90,45,135,45},{135,45,135,45},{45,135,45,135}
   },

   { // circle_left
   {90,90,90,90},{0,90,90,90},{0,0,90,90},
   {0,0,0,90},{0,0,0,0},{180,180,180,180}
   },

   { // circle_right
   {90,90,90,90},{180,90,90,90},{180,180,90,90},
   {180,180,180,90},{180,180,180,180},{0,0,0,0}
   }

 };

int pos_stop[1][4]={{90,90,90,90}};

Процедура course(int variant)реализует перемещения сервоприводов для каждого шага следующих движений робота-паука: вперед, назад, поворота по часовой стрелке и поворота против часовой стрелки.

void course(int variant) 
 {
 int i=0;
 for(i=1;i<6;i++)
   {
   if(arr_pos[variant-1][i][0]!=arr_pos[variant-1][i-1][0])
   {myservo11.write(arr_pos[variant-1][i][0]);}

   if(arr_pos[variant-1][i][1]!=arr_pos[variant-1][i-1][1])
   {myservo12.write(arr_pos[variant-1][i][1]);}

   if(arr_pos[variant-1][i][2]!=arr_pos[variant-1][i-1][2])
   {myservo13.write(arr_pos[variant-1][i][2]);}

   if(arr_pos[variant-1][i][3]!=arr_pos[variant-1][i-1][3])
   {myservo14.write(arr_pos[variant-1][i][3]);}

   delay(200);
   }
 }

Для остановки, засыпания и пробуждения робота-паука существует процедура go_hor_all()

void go_hor_all()
 {
 myservo11.write(pos_stop[0][0]);
 myservo12.write(pos_stop[0][1]);
 myservo13.write(pos_stop[0][2]);
 myservo14.write(pos_stop[0][3]);
 delay(500);
}

Реализуем простое ИК-управление с пульта. Выбираем 7 клавиш, данные о кодах заносим в скетч в виде констант. И в цикле loop() реализуем логику выбора движений робота-паука при нажатии клавиш ИК-пульта. Программа получения кода get_ir_kod() вызывается по прерыванию CHANGE на входе 2. Используется Arduino библиотека IRremote.

К режиму управления робота с ИК-пульта добавим автономный режим. В автономном режиме робот будет двигаться вперед, при достижении препятствия робот будет делать поворот и опять двигаться вперед. Ультразвуковой датчик HC-SR04 позволяет определять расстояние до объекта в диапазоне от 2 до 500 см с точностью 0.3 см. Сенсор излучает короткий ультразвуковой импульс (в момент времени 0), который отражается от объекта и принимается сенсором. Расстояние рассчитывается исходя из времени до получения эха и скорости звука в воздухе. Если расстояние до препятствия меньше 10 см, робот делает поворот и движется дальше вперед. Переход из режима ИК-управления в автономный режим производим нажатием клавиш "желтая" и "синяя".

Для работы с датчиком HC-SR04 будем использовать Arduino библиотеку Ultrasonic. Конструктор Ultrasonic принимает два параметра — номера пинов к которым подключены выводы Trig и Echo:

#include "Ultrasonic.h"

// trig -12, echo - 13

Ultrasonic ultrasonic(12, 13); 

Получается такой код

// коды клавиш ИК пульта 
// lg 6710v00090d
#define FORWARD 32       //  pr +
#define BACK  33         //  pr -
#define CIRCLE_LEFT 17   //  vol-
#define CIRCLE_RIGHT 16  //   vol+
#define STOP 54          //   зеленая
#define SLEEP 55          //  красная
#define AWAKE 37         //  ок
#define EXT 50          //  желтая
#define AUTO 52          //  синяя

... .... .....
 
void loop() 
  { 
  delay(1000);
  if(ext==0)
   {
   float dist_cm = ultrasonic.Ranging(CM);
   Serial.print("dist_cm=");Serial.println(dist_cm);
   if(dist_cm<10.0)
     ir_kod=CIRCLE_LEFT;
   else
     ir_kod=FORWARD;
   }
  if(ir_kod!=0)
   {
   Serial.print("ir_kod=");Serial.println(ir_kod);  
   switch(ir_kod)
     {
     case FORWARD :     // вперед
        course(1);
        Serial.print("forward\n");
        break;
     case BACK :     // назад
        course(2);
        Serial.print("back\n");
        break;
     case CIRCLE_LEFT:     // вращение влево
        course(3);
        Serial.print("circle_left\n");
        break;
     case CIRCLE_RIGHT :     // вращение вправо
        Serial.print("circle_right\n");
        course(4);
        break;
     case STOP :     // остановка
        ir_kod=0;
        go_hor_all();
        Serial.print("pause\n");
        break;
     case SLEEP :     // засыпание
        ir_kod=0;
        go_hor_all();
        myservo11.detach();myservo12.detach(); 
        myservo13.detach();myservo14.detach();
        digitalWrite(13,LOW);
        Serial.print("sleep\n");
        break;
     case AWAKE :     // пробуждение
        ir_kod=0;
        myservo11.attach(5);myservo12.attach(6); 
        myservo13.attach(7);myservo14.attach(8);
        digitalWrite(13,HIGH);
        go_hor_all();
        Serial.print("awake\n");
        break;
     case AUTO :     // режим автономный
        //ir_kod=FORWARD;
        ext=0;
        myservo11.attach(5);myservo12.attach(6); 
        myservo13.attach(7);myservo14.attach(8);
        Serial.print("auto\n");
        break;
    default:
        break;
     }
   } 
 } 

// получить код переданный с ИК пульта
void get_ir_kod()
 {
 detachInterrupt(0);    // отключить прерывание 0
 if (irrecv.decode(&results))  
  { 
  //Serial.println(results.value);
  if (results.value > 0 && results.value < 0xFFFFFFFF) 
    { 
    ir_dt = results.value;
    if(ir_dt==EXT && ext==0) 
      {ir_kod = SLEEP;ext=1;} 
    else if(ext==1) 
      {
      if(ir_dt==FORWARD || ir_dt==BACK || ir_dt==CIRCLE_LEFT
         || ir_dt==CIRCLE_RIGHT || ir_dt==STOP || ir_dt==SLEEP
         || ir_dt==AWAKE || ir_dt==AUTO )
       ir_kod = ir_dt;
      } 
    else 
      ;
    } 
    irrecv.resume(); 
  } 
  attachInterrupt(0, get_ir_kod, CHANGE); 
 }

Архив со скетчем и библиотеками Ultrasonic и IRremote можно скачать ниже

Теги:

• Сервопривод
• Arduino
• Робот