Часть 1 - Общие элементы аппаратной части.
Часть 2 - Конструкции роботов, алгоритмы работы и настройки
В первой части статьи мы рассмотрели аппаратную часть (плата Arduino и плата расширения), которая является общей для трех роботов (Filippo, BIPE, SPIDER). Вторая часть была посвящена механическим конструкциям роботов, которые также имели общие черты.
Проанализируем теперь общие для трех роботов элементы и функции в программе микроконтроллера. Начнем с глаз робота, – ультразвукового датчика препятствий SRF05. Он установлен на всех роботах и позволяет определять препятствия на пути их движения.
В исходном коде программы микроконтроллера процедура работы с датчиком препятствий идентична для всех роботов. Процедура readDistance() (Листинг 1) предназначена для посылки датчику импульса длительностью 2 мкс, датчик в свою очередь генерирует серию ультразвуковых импульсов в направлении движения робота. Этот же вывод сенсора используется потом в качестве входа для чтения, с помощью функции PulseIn. Во время прохождения ультразвука к препятствию, отражение от него и возвращение к датчику на выходной линии датчика поддерживается высокий логический уровень. И это время пропорционально расстоянию до объекта.
Листинг 1. Процедура работы с датчиком препятствий
long readDistance()
{
long duration, cm;
pinMode(PING_PIN, OUTPUT);
digitalWrite(PING_PIN, LOW);
delayMicroseconds(2);
digitalWrite(PING_PIN, HIGH);
delayMicroseconds(5);
digitalWrite(PING_PIN, LOW);
pinMode(PING_PIN, INPUT);
duration = pulseIn(PING_PIN, HIGH); // ritorna distanza ostacolo in cm
return duration / 29 / 2;
}
Для получения сведений о напряжении батареи питания робота используется функция levelBat() (Листинг 2).
Листинг 2. Работа с АЦП микроконтроллера для получения сведений о напряжении батареи питания.
void levelBat()
{
int VbatRAW = analogRead(A0);
float Vbat = float(VbatRAW)/50;
Serial.print("Vbat= ");
Serial.println(Vbat, DEC);
if (Vbat<6.0)
{
Serial.println("Livello batteria basso!");
inOff();
}
}
Функция достаточно простая, используется канал 0 встроенного в микроконтроллер АЦП для преобразования данных и переменная VbatRAW, в которой хранится результат преобразования. Как упоминалось в первой части, для получения значения напряжения батареи в вольтах достаточно разделить полученный результат преобразования на 50. Так как чрезмерный разряд аккумулятора может вывести его из строя, было решено установить аварийный уровень напряжения питания, при котором робот немедленно прекращает работу и все сервоприводы отключаются. Данное пороговое значение установлено в 6.0 В, это идеальное решение при использовании аккумулятора, состоящего из двух LiPo элементов или из шести NiCd или NiMh элементов. Если используется 8 элементов, составляющих аккумулятор, то пороговое значение можно увеличить до 8.0 В или 1.0 В на каждый элемент аккумулятора.
Следующая общая процедура предназначена для инициализации сервоприводов (Листинг 3).
Листинг 3. Инициализация сервоприводов робота.
void initServo()
{ // posizione iniziale dei servi
servo0.attach(2); // connette servo
delay(300);
........
servo7.attach(9); // connette servo
delay(300);
}
Функция servo.attach() определяет подключение сервопривода к указанному выходу, с которого осуществляется управление, и с этого момента на данный выход поставляется ШИМ сигнал для сервопривода.
Функция inOff() выполняет противоположную функцию, удаляя подключение сервоприводов к выходу Arduino и оставляет их без управления (Листинг 4), робот переводится в выключенный режим.
Листинг 4. Процедура отключения сервоприводов от выходов Arduino.
void inOff()
{
leg[0] = 90;
.....
leg[7] = 90;
setServo();
servo0.detach(); // connette servo
....
servo7.detach(); // connette servo
RobotMode = MODE_OFF;
}
Функция, которая переводит робота в «спящий» режим получила название inStandby() и выполняет действия по переводу всех сервоприводов в среднее положение. В случае роботов Filippo и BIPE сервоприводы позиционируются в своих соответствующих нейтральных точках, значения которых хранятся в массиве leg_ntr[] (Листинг 5).
Листинг 5. Первод робота в "спящий режим".
void inStandby()
{
leg[0] = 90;
....
leg[7] = 90;
setServo();
RobotMode = MODE_STANDBY;
Serial.println("Sono in Standby...");
}
И самая важная функция, которая гарантирует установку сервоприводов по требуемым координатам setServo() (Листинг 6).
Листинг 5. Установка сервоприводов по координатам.
void setServo()
{
for (i=0; i<18; i++)
{
servo0.write(leg_old[0] + i*(leg[0]-leg_old[0])/18);
servo1.write(leg_old[1] + i*(leg[1]-leg_old[1])/18);
servo2.write(leg_old[2] + i*(leg[2]-leg_old[2])/18);
servo3.write(leg_old[3] + i*(leg[3]-leg_old[3])/18);
servo4.write(leg_old[4] + i*(leg[4]-leg_old[4])/18);
servo5.write(leg_old[5] + i*(leg[5]-leg_old[5])/18);
servo6.write(leg_old[6] + i*(leg[6]-leg_old[6])/18);
servo7.write(leg_old[7] + i*(leg[7]-leg_old[7])/18);
delay(TimeOneStep/4/18);
}
leg_old[0]=leg[0];
leg_old[1]=leg[1];
leg_old[2]=leg[2];
leg_old[3]=leg[3];
leg_old[4]=leg[4];
leg_old[5]=leg[5];
leg_old[6]=leg[6];
leg_old[7]=leg[7];
}
Если мы будем непосредственно устанавливать сервопривод, то он будет работать рывками. Однако, в нашем случае необходимо реализовать плавное перемещение, чтобы получить нужное движение по координатам в нужное время. Для этого опеределяется вектор leg[], содержащий необходимые координаты для каждого сервопривода и вектор leg_old[], содержащий фактические координаты. Процедура управляет сервоприводами, отправляя им промежуточные значения скоординированные по целевому времени, определяемого переменной TimeOnStep. Такая реализация позволяет достичь плавных движений.
Для робота SPIDER эта процедура называется setServoWalk() и она немного отличается от setServo() из-за различной скорости движения сервоприводов робота. Во время движения робота три ноги имеют постоянный контакт с поверхностью и движутся медленно, четвертая нога должна подниматься и двигаться быстрее остальных в направлении противоположном движению робота.
Демонстрационное видео
Загрузки
Исходные коды программ микроконтроллера для роботов – скачать
