Четырехногий робот-паук создан для демонстрации работы сервомашинок под управлением контроллера 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). Поэтому каждое движение робота состоит из нескольких шагов.
Например движение вперед состоит из следующих шагов:
- левая передняя нога вперед;
- правая передняя нога вперед;
- левая задняя нога вперед;
- правая задняя нога вперед;
- четыре ноги вместе назад (что приведет к перетаскиванию тела робота-паука).
Данные для угла поворота каждой сервы на каждом шаге для каждого движения робота-паука хранятся в трехмерном массиве 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("forwardn"); break; case BACK : // назад course(2); Serial.print("backn"); break; case CIRCLE_LEFT: // вращение влево course(3); Serial.print("circle_leftn"); break; case CIRCLE_RIGHT : // вращение вправо Serial.print("circle_rightn"); course(4); break; case STOP : // остановка ir_kod=0; go_hor_all(); Serial.print("pausen"); break; case SLEEP : // засыпание ir_kod=0; go_hor_all(); myservo11.detach();myservo12.detach(); myservo13.detach();myservo14.detach(); digitalWrite(13,LOW); Serial.print("sleepn"); 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("awaken"); break; case AUTO : // режим автономный //ir_kod=FORWARD; ext=0; myservo11.attach(5);myservo12.attach(6); myservo13.attach(7);myservo14.attach(8); Serial.print("auton"); 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 можно скачать ниже