Решение задачи №1. Проехать заданное расстояние за определенное время

По просьбам подписчиков начинаем публикацию ответов на "Задачки по робототехнике Карандаша и Самоделкина". Сегодня мы рассмотрим решение задачи №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

  Моторы(М)

  }