Четырехногий робот-паук создан для демонстрации работы сервомашинок под управлением контроллера 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. Одна микросхема постоянно перегревалась, проблема решилась установкой параллельно 2-х микросхем L7805.
Для обнаружения препятствий будем использовать ультразвуковой датчик HC-SR04, который позволяет определять расстояние до объекта в диапазоне от 2 до 500 см с точностью 0.3 см. Если расстояние до препятствия меньше 10 см, робот делает поворот и движется дальше вперед.
В качестве пульта используется пульт lg, приемник ИК-сигналов — TSOP31238(1-GND, 2 — +5V, 3-OUT).
Схема электрическая
И весь робот в сборе (плата Arduino питается от батарейки Крона).
Приступим к написанию скетча
Для управления сервоприводами используется Arduino библиотека Servo. Нам необходимо реализовать совокупность движений сервоприводов для движения робота-паука вперед, назад, поворота по часовой стрелке и поворота против часовой стрелки. Кроме того необходимо реализовать функции остановки робота, а также для экономии электроэнергии предусмотрим режим засыпания (когда сервоприводы находятся в режиме detach) и пробуждения (перевод сервоприводов в режим attach). Потому каждое движение робота состоит из нескольких шагов.
К примеру движение вперед состоит из следующих шагов:
Данные для угла поворота каждой сервы на каждом шаге для каждого движения робота-паука хранятся в 3-хмерном массиве 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 можно скачать ниже
Прикрепленные файлы:
- robot-krab.rar (21 Кб)