По просьбам подписчиков начинаем публикацию ответов на "Задачки по робототехнике Карандаша и Самоделкина". Сегодня мы рассмотрим решение задачи №1.
Казалось бы - что тут сложного? Дели расстояние на время и получай скорость. Если Вы попробуете подобрать мощность на моторах таким образом, чтобы робот проезжал заданное расстояние за определенное время то обнаружите, что и робот движется не с постоянной скоростью (ему нужно время на разгон) и поверхность пола/стола не идеальна и от заряда батареек зависит мощность, причем нелинейно.
Мы предлагаем два решения задачи. Первое решение - для робототехников, еще не знакомых с регуляторами. Алгоритм в общем виде, может быть описан на любом языке программирования.
// пусть роботу нужно проехать 2 м за 35 секунд
R = 2000
T = 35
pi = 3.14
// Размер колеса робота, мм
W = 56
// Рассчитываем, сколько градусов всего нужно проехать, с учетом размера колес
S = R * 360 / (W * pi)
// Рассчитываем среднюю скорость в град/сек
С = S / T
// Мощность на моторах, с которой робот проезжает заданное расстояние за +/- приблизительное заданное время
P = 50
// начинаем движение
Моторы(P)
// Коэф-т ускорения
K = 2
ЦИКЛ
{
E1 = считываем показание энкодера моторов
Ждать (1 сек)
E2 = считываем показание энкодера моторов
// смотрим сколько проехал робот за секунду
E = E2 - E1
// корректируем мощность на моторах
ЕСЛИ (E > C) ТОГДА P = P - K
ЕСЛИ (E < C) ТОГДА P = P + K
Моторы(P)
// корректируем среднюю скорость с учетом оставшегося расстояния, осталось проехать:
S = R * 360 / (W * pi) - E2
// обновление средней скорости
C = S / (T - таймер)
}
Второй вариант решения, с использованием ПИД-регулятора:
// пусть роботу нужно проехать 2 м за 35 секунд
R = 2000
T = 35
pi = 3.14
// Размер колеса робота, мм
W = 56
// Рассчитываем, сколько мм он должен проезжать за 0.1 сек
S = R / (T * 10)
// Теперь в градусах оборотов мотора c учетом размера колес
S = ((R / (T * 10)) * 360 / (W * pi)
// Мощность на моторах, с которой робот проезжает заданное расстояние за +/- приблизительное заданное время
P = 50
// Коэфт-ты ПИД регулятора
Kp = 0.1
Ki = 0.0001
Kd = 0.2
// начальное значение ошибки регулятора
E = 0
ЦИКЛ
{
// предыдущее значение ошибки
Eold = E
E1 = считываем показание энкодера моторов
Ждать (0.1 сек)
E2 = считываем показание энкодера моторов
?/ вычисляем ошибку - на сколько градусов
// быстрее или медленнее у робота получилось
E = (E2-E1) - S
// сумма всех ошибок - интеграл
I = I + E
// ПИД-регулятор
V = (Kp * E) + (Ki * I) + (Kd * (E - Eold))
// подаем мощность M на моторы
M = P - V
Моторы(М)
}
