Робот-паук на сервоприводах
Проект по созданию простейшего четвероногого робота-паука на сервоприводах. У робота будут два режима:
автономный — робот движется вперед, при обнаружении препятствия (используется ультразвуковой датчик) поворачивается и движется дальше;
внешнее управление с помощью ИК-пульта (вперед, назад, поворот влево, поворот вправо, остановка, засыпание).
Для проекта понадобятся 4 сервопривода.
Для сервоприводов требуется отдельное питание. В качестве источника питания взята литий-полимерная батарея Turnigy 2S 1600 mAh. Напряжение, выдаваемое батареей: 7,4–8,4 В. Поскольку для питания сервоприводов необходимо напряжение 4,8–6,0 В, применим стабилизатор напряжения 5 В, собранный на микросхеме L7805. Как выяснилось, одна микросхема постоянно перегревалась, проблема была решена установкой параллельно двух микросхем L7805.
Для обнаружения препятствий использован ультразвуковой датчик HC-SR04 , который позволяет определять расстояние до объекта в диапазоне от 2 до 500 см с точностью 0,3 см. Датчик излучает короткий ультразвуковой импульс (в момент времени 0), который отражается от объекта и принимается сенсором. Расстояние рассчитывается, исходя из времени до получения эха и скорости звука в воздухе. Если расстояние до препятствия меньше 10 см, робот делает поворот и движется дальше вперед.
В качестве пульта управления применен пульт ИК приемник ИК-сигналов — TSOP31238.
 |
| Схема паука-робота |
Программа для управления роботом-пауком
Для управления сервоприводами используется 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);
}
Реализуем простое ИК-управление с пульта. Выбираем семь клавиш, данные о кодах заносим в скетч в виде констант и в цикле loop() реализуем логику выбора движений робота-паука при нажатии клавиш ИК-пульта. Программа получения кода get_ir_kod() вызывается по прерыванию CHANGE на входе 2. Используется Arduino-библиотека IRremote.
К режиму управления робота с ИК-пульта добавим автономный режим. В автономном режиме робот будет двигаться вперед, при достижении препятствия делать поворот и опять двигаться вперед. Примененный ультразвуковой дальномер HC-SR04, как уже отмечалось, позволяет определять расстояние до объекта в диапазоне от 2 до 500 см с точностью 0,3 см. Короткий ультразвуковой импульс, излученный датчиком в момент времени 0, отражается от объекта и принимается сенсором. Расстояние рассчитывается, исходя из времени до получения отраженного эха и скорости звука в воздухе. Если расстояние до препятствия меньше 10 см, робот делает поворот и продолжает движение. Переход из режима ИК-управления в автономный режим осуществляется нажатием клавиш "желтая" и "синяя" на ИК- пульте.
Для работы с датчиком HC-SR04 используется Arduino-библиотека Ultrasonic
Sketch code
#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))
{
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);
}
