Общие сведения
В одном из прошлых уроков мы собирали машинку из ВПХ-конструктора и учили её ездить по линии. Сегодня мы усовершенствуем её и оснастим новым модулем для захвата предметов. Так она сможет перевозить грузы.
Видео
Нам понадобится
- 1х Захват для роботов из ПВХ
- 2х Мотор-редуктор с энкодером N20-100rpm-12V с управляющим контроллером
- 1х Бампер с 9 датчиками линий с шагом 14мм или с шагом 7мм
- 1х Piranha Uno R3 (или другой контроллер такого же формата)
- 1х Trema Shield
- 1х Источник питания 5В, 5-12В (Li-ion 14500)
- 1х Аккумулятор 14500 Li-ion, 3.7V, 900mah, с защитой
- 2х i2C Hub (один из них мы используем для подключения питания моторов, чтобы обойтись без пайки)
- 2х Колесо для Micro Metal Gearmotor
- 1x Шаровая опора (12 мм)
- 1х Пластина большая (конструктор ПВХ)
- 1х Крепления Arduino (конструктор ПВХ)
- 1х 4-проводной шлейф «мама-мама» 20 см
- 4х Шлейф питания, МАМА-МАМА
- 2х Стойки М3*12 Nylon-black (4 штуки в комплекте)
- 1х Стойки М3*6 Nylon-black (4 штуки в комплекте)
- 1х Винт М3х12, (10 шт в комплекте)
- 1х Винт М3х6 (10 шт в комплекте)
- 1х Гайка М3 (10 шт в комплекте)
- 4х Шайба М3 Nylon (5 шт в комплекте)
- iarduino_I2C_Motor
- iarduino_I2C_Bumper
При необходимости, ознакомьтесь с нашей инструкцией по установке библиотек в Arduino IDE.
Видео
Редактируется...
Сборка корпуса
Сборка корпуса описана в предыдущей статье о ВПХ-машинке. Всё остаётся без изменений, кроме установки большей площадки под Arduino для возможности крепления захвата.
Сборка захвата
Подробный процесс сборки захвата описан в инструкции (в комплекте) и на странице товара.
Для установки сервопривода на угол 90 градусов, используйте следующий скетч:
#include <Servo.h> // Подключение библиотеки
Servo servo; // Создание объекта сервопривода
void setup(){
servo.attach(5); // Сервопривод подключен к 5 пину
servo.write(90); // Установить вал сервопривода в угол 90 градусов
}
void loop(){
}
Подключение
Скетч проекта
// Подключаем библиотеки: //
#include <Wire.h> // Подключаем библиотеку для работы с аппаратной шиной I2C.
#include <iarduino_I2C_Motor.h> // Подключаем библиотеку для работы с мотором I2C-flash.
#include <iarduino_I2C_Bumper.h> // Подключаем библиотеку для работы с бампером I2C-flash.
#include <Servo.h>
// Создаем объекты
iarduino_I2C_Motor mot[2]={0x0A,0x0B}; // Объявляем массив mot состоящий из двух объектов для работы с моторами I2C-flash, указав адреса модулей моторов на шине I2C (0x0A-левый, 0x0B-правый).
iarduino_I2C_Bumper bum (0x0C); // Объявляем объект bum для работы с бампером I2C-flash, указав адрес модуля на шине I2C.
Servo servo;
const float val_Speed = 0.05f; // Скорость движения машины в м/сек.
const float pid_KP = val_Speed * 0.15f; // Коэффициент пропорциональной обратной связи ПИД регулятора.
const float pid_KI = val_Speed * 0.005f; // Коэффициент интегральной обратной связи ПИД регулятора.
const float pid_KD = val_Speed * 0.15f; // Коэффициент дифференциальной обратной связи ПИД регулятора.
float arr_ERR[10] = {0,0,0,0,0,0,0,0,0,0}; // Массив последних ошибок для формирования интегральной составляющей ПИД регулятора.
uint8_t i, j; // Переменные для работы с массивом последних ошибок.
/////////////////////НАСТРОЙКИ////////////////////////////
uint8_t servoOpen = 40; // Угол сервопривода, при котором захват будет в открытом положении
uint8_t servoClose = 85; // Угол сервопривода, при котором захват будет в закрытом положении
const float val_Radius = 17.0f; // Радиуса колеса в мм.
#define backTime 1500 // Время отъезда назад, мс
//////////////////////////////////////////////////////////
void setup(){
Serial.begin(9600); // Открываем связать по Serial порту на скорости 9600 бод
delay(500); // Ждём завершение переходных процессов связанных с подачей питания.
mot[0].begin(); // Инициируем работу с левым мотором I2C-flash.
mot[1].begin(); // Инициируем работу с правым мотором I2C-flash.
bum.begin(); // Инициируем работу с бампером I2C-flash.
servo.attach(5); // Сервопривод подключен к 5 пину
mot[0].radius = val_Radius; // Указываем радиус колеса левого мотора в мм.
mot[1].radius = val_Radius; // Указываем радиус колеса правого мотора в мм.
mot[0].setDirection(false); // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях).
mot[1].setDirection(true); // Указываем правому мотору, что его вращение должно быть прямым ( по часовой стрелке при положительных скоростях).
mot[0].setStopNeutral(false); // Указываем не освобождать ротор левого мотора при его остановке (если указать true, то ротор остановленного мотора можно вращать).
mot[1].setStopNeutral(false); // Указываем не освобождать ротор правого мотора при его остановке (если указать true, то ротор остановленного мотора можно вращать).
}
void loop(){
servo.write(servoOpen); // Открываем захват
while(bum.getLineDigital(BUM_LINE_ALL)){ // Пока хотя бы один датчик находится над линией
direct(); // Двигаемся вперёд
}
stay();
servo.write(servoClose); // Закрываем захват
delay(500); // Задержка
back(1500); // Отъезжаем назад 1с
rotation(); // Разворот
while(bum.getLineDigital(BUM_LINE_ALL)){ // Пока хотя бы один датчик находится над линией
direct(); // Двигаемся вперёд
}
stay(); // После - останавливаемся
drop(); // Отпускаем груз
back(1500); // Отъезжаем назад 1с
rotation(); // Разворот
}
void stay(){ // Функция остановки
mot[0].setSpeed(0, MOT_M_S); // Выключаем оба двигателя
mot[1].setSpeed(0, MOT_M_S);
}
void rotation(){ // Функция разворота
mot[0].setSpeed(0.07, MOT_M_S); // Задаём скорости моторам
mot[1].setSpeed(-0.07, MOT_M_S);
delay(2000); // Задержка, чтобы машинка успела повернуться
while(!bum.getLineDigital( 5 )){ // Поворот пока под 5 датчиком линии нет линии
mot[0].setSpeed(0.07, MOT_M_S); // Задаём скорости моторам
mot[1].setSpeed(-0.07, MOT_M_S);
}
stay(); // Остановка
}
void drop(){ // Функция плавной постановки груза
for (uint8_t i=0; i<5; i++){ // Повторяем 5 раз
servo.write(servoClose-i); // Открываем захват на угол, с каждым разом всё больший и больший
delay(200); // Задержка
servo.write(servoClose-15); // Приоткрываем захват
delay(5); // Небольшая задержка, чтоб груз успел опуститься вниз
}
servo.write(servoOpen); // Полностью открываем захват
delay(200); // Задержка
}
void back(uint32_t timer){ // Функция отъезда назад
uint32_t timeStart = millis(); // Запоминаем время старта отъезда
while(timeStart + timer > millis()){// Пока от момента старта прошло меньше времени чем timer
mot[0].setSpeed(-0.07, MOT_M_S); // Задаем моторам скорость
mot[1].setSpeed(-0.07, MOT_M_S);
}
stay(); // Остановка
}
void direct(){ // Функция движения вперёд
i++; i%=10; j=10; // Определяем значения переменных работы с массивом ошибок.
// Получаем текущую ошибку центрирования линии: //
arr_ERR[i] = bum.getErrPID(); // Функция getErrPID() возвращает ошибку от 0 до ±4.5, где 0 - линия по центру, ±4.0 - линия на крайнем датчике, ±4.5 - линия потеряна.
// Вычисляем все составляющие ПИД регулятора: //
float pid_P = arr_ERR[i]; // Пропорциональная составляющая «pid_P» представлена величиной текущей ошибки «ARR_ERR[i]».
float pid_I = 0.0f; while(j--){pid_I+=arr_ERR[j-1];} // Интегральная составляющая «pid_I» представлена суммой последних ошибок взятых из массива «arr_ERR».
float pid_D = arr_ERR[i]-arr_ERR[(i+9)%10]; // Дифференциальная составляющая «pid_D» представлена разницей между текущей «ARR_ERR[i]» и предыдущей «arr_ERR[(i+9)%10]» ошибкой.
float PID = pid_P*pid_KP+pid_I*pid_KI+pid_D*pid_KD; // Вычисляем результат ПИД регулирования.
// Устанавливаем скорость вращения колёс: //
mot[0].setSpeed( val_Speed + PID , MOT_M_S ); // Устанавливаем скорость левого мотора в м/сек.
mot[1].setSpeed( val_Speed - PID , MOT_M_S ); // Устанавливаем скорость правого мотора в м/сек.
}
Алгоритм работы
- Открываем захват;
- Едем до конца линии;
- Закрываем захват;
- Отъезжаем назад;
- Разворачиваемся;
- Едем до конца линии;
- Аккуратно ставим предмет;
- Отъезжаем назад;
- Разворачиваемся.
Движение вперед до конца линии производится с помощью постоянного отслеживания сигналов с датчика линии. Как только линия пропадает — прекращаем движение.
Разворот производится тоже благодаря сигналам с датчиков — разворачиваемся до тех пор, пока линия не окажется под центральным.
Движение вперёд происходит по уже рассмотренному нами принципу ПИД-регулирования в статье Установка бампера с 9 датчиками линии на робота "Малыш".
Для того, чтобы аккуратно поставить предмет, воспользуемся возможностью быстрого открытия и закрытия захвата. Будем приоткрывать его с каждым разом на чуть бо́льший угол, таким образом, предмет не сразу упадёт вниз, а более-менее плавно опустится на поверхность.
Ссылки
- Захват для роботов из ПВХ
- Мотор-редуктор с энкодером N20-100rpm-12V с управляющим контроллером
- Бампер с 9 датчиками линий с шагом 14мм.
- Источник питания 5В, 5-12В (Li-ion 14500)
- Калибровка бампера
- Библиотеки:
Обсуждение