Программирование
Здравствуйте!
Это продолжение серии статей о том, как мы создавали своего первого робота. В данной части мы раскроем подходы к управлению роботом для задачи слежения за линией при использовании различных алгоритмов, и другие вопросы программирования.
Часть 1 — О проекте
Часть 2 — Описание программной части робота
Релейный алгоритм
Релейный алгоритм заключается в следующем: пока датчик на белом, робот будет двигаться в сторону черного, а когда датчик на черном, робот будет двигаться в сторону белого. Благодаря тому, что поворот осуществляется по дуге с небольшим радиусом происходит движение вперед. Различные комбинации положения датчиков относительно линии представлены на рисунке. Это самый простой и действенный алгоритм. Ниже приведён код.
qtra— это класс для работы с датчиками. Позже я отказался от него, потому что понял как правильно получать данные с датчиков.
sensorValuesAVG — это средние значения, показываемые датчиками, замеренные перед началом цикла.
motor_speed2 — процедура, которая включает заданный мотор с заданной скоростью.
int sensorValuesAVG1 = 0;
int sensorValuesAVG2 = 0;
void setup() {
. . .
//Формируем средние значения чёрной линии
for (int i= 0; i<10; i++) {
qtra.read(sensorValues);
sensorValuesAVG1 = sensorValues[0] + sensorValuesAVG1;
sensorValuesAVG2 = sensorValues[1] + sensorValuesAVG2;
}
sensorValuesAVG1sr = sensorValuesAVG1 / 10;
sensorValuesAVG2sr = sensorValuesAVG2 / 10;
. . .
}
Алгоритм: если значение левого датчика показывает нахождение на чёрном цвете - двигаемся влево, если правый - вправо. Это пример, поэтому не пытайтесь его скопировать один в один. Вам ещё надо будет настраивать значение PV.
float PV = 0 ;
void loop() {
// считываем значения сенсоров
qtra.read(sensorValues);
// если значение сенсора больше среднего, значит там черное - надо двигаться в обратную сторону
if(sensorValues[0] > sensorValuesAVG)
{
PV = +10;
}
else if (sensorValues[1] > sensorValuesAVG)
{
PV = -10;
}
motorASpeed = 50 + PV;
motorBSpeed = 50 - PV;
motor_speed2(motor_A, motorASpeed); // левое колесо
motor_speed2(motor_B, motorBSpeed); // правое колесо
}
В примере qtra.read(sensorValues) можно смело заменить на sensorValue = analogRead( <номер пина> ).
ПИД-регулятор
При использовании ПИД-регулятора вся сложность проявляется в выборе его коэффициентов. Существует два похода к настройке. Первый – синтез регулятора, то есть вычисление параметров регулятора на основании математической модели системы. Данный метод позволяет точно рассчитать параметры регулятора, но он требует знание точной математической модели робота.
Второй метод – эвристический подбор параметров. Это метод проб и ошибок. Берем готовую систему, меняем один или сразу несколько коэффициентов, включаем регулятор и смотрим за работой системы. В зависимости от того, как ведет себя система с выбранными коэффициентами меняем коэффициенты и повторяем эксперимент.
Известен метод Зиглера-Никольсона для упрощения поиска параметров регулятора. Не буду объяснять, как он работает, описание есть в интернете. Наше желание использовать его не оправдалось. На одном из шагов не получилось выполнить условия, поэтому продолжать далее было безсмысленно. Хотя это позволило нам всё таки найти параметры, благодаря которым ПИД-регулятор был реализован.
void setup() {
. . .
//Коэффициенты ПИД-регулятора
kp=30;
ki=5;
kd=1;
. . .
}
void loop() {
qtra.read(sensorValues);
// вычисляем ошибку, отклонение от заданного курса
if(sensorValues[0] > sensorValuesAVG1 )
{
error = 0.5;
}
else if (sensorValues[1] > sensorValuesAVG2 )
{
error = -0.5;
}
else error = 0;
// формируем управляющий сигнал, реализуем ПИД-закон
PV = kp * error + ki * errorSum + kd * (error - lastError);
lastError = error;
errorSum = errorSum + error;
if (errorSum > MAX_FLOAT-1)
errorSum = MAX_FLOAT;
// ограничиваем уровень управления.
if (PV > 40)
{
PV = 40;
}
if (PV < -40)
{
PV = -40;
}
// ставим управление
motorASpeed = 50 + PV;
motorBSpeed = 50 - PV;
// запускаем моторы
motor_speed2(motor_A, motorASpeed);
motor_speed2(motor_B, motorBSpeed);
}
При вычислении error мы задаём конкретное значение ошибки. Если бы было больше датчиков, тогда можно было бы вычислить ошибку более точно. Здесь величина 0.5 задана интуитивно, и если бы это было 1, 2, 2.5 — никакой роли в данном случае не сыграло бы, т. к. дальше от изменения параметров ПИД-регулятора значение управляющего сигнала изменилось бы.
В коде есть строки, когда мы ограничиваем значение PV. Это надо для того, чтобы скорость не была слишком большой, т. к. может возникнуть проскальзывание, либо робот будет пропускать чёрную линию на поворотах. Ещё, когда PV будет слишком большим, может произойти переполнение и PV будет 254, 255, 0, 1, и т.д. У нас такая ошибка произошла с суммой. Выражено это было в том, что начиная с определённого момент колёса робота начинали крутиться в обратную сторону. Т.е. значения errorSum становились такими (пример): 65 534, 65535, 0, - 65535, - 65534 и т. д.
Драйвер двигателей
Под наши двигатели нам не подошёл популярный L293 – ток маловат. Мы использовали TB6612FNG. Он отличается большим рабочим током, алгоритмом работы соответственно. Даташит написан достаточно подробно и ясно, поэтому мы очень рады были когда всё заработало с первого раза. На русском языке я не встречал описания работы с данным драйвером, думаю что есть необходимость описать здесь.
Драйвер имеет возможность управления двумя двигателями. Каждый двигатель управляется 3-мя входами, и 1 для перевода схемы в режим низкого потребления питания.
Два входа IN1, IN2 задают направление вращения вала и режимы остановки и short brake, когда возможно свободное вращение. Если входы N1, IN2 имеют разные уровни — вращаемся, иначе другие режимы. Скорость вращения задаётся подачей ШИМ сигнала на PWM выход. Чем чаще появляется высокий уровень сигнала — тем больше скорость вращения.
Таким образом у нас выделены режимы работы.
Режим энергосбережения. Подаём низкий сигнал на пин — отключаем схему, иначе включаем. Этот пин схемы инверсный. Т.е. читая строку standbyMotors(false) — читаем как «мотор в режим энергосбережния не вводить».
void standbyMotors (boolean state) {
if (state == true)
digitalWrite(out_STBY, LOW);
else
digitalWrite(out_STBY, HIGH);
}
Режим остановки мотора. По имени motorName мотора получаем номера пинов. А затем используем их.
void brakeMotor (byte motorName) {
byte out_IN1;
byte out_IN2;
byte out_PWM;
getMotorOuts(motorName, out_IN1, out_IN2, out_PWM);
digitalWrite(out_IN1, HIGH);
digitalWrite(out_IN2, HIGH);
}
Включаем моторы для движения. В функцию получаем имя мотора и скорость движения. Вызывая getMotorOuts получаем пины, которые используем для управления схемой драйвера двигателей. Значение speed разбивается на PwmValue и направление directMove. speed переводиться из диапазона 0-100 в 0-255 PwmValue для ШИМ вывода. Моторы должны крутится в разных направлениях.
void runMotor (byte motorName, char speed)
{
byte out_IN1;
byte out_IN2;
byte out_PWM;
getMotorOuts(motorName, out_IN1, out_IN2, out_PWM);
boolean directMove = !(speed > 0);
byte PwmValue = 0;
PwmValue = map(abs(speed), 0, 100, 0, 255);
digitalWrite(out_IN1, directMove);
digitalWrite(out_IN2, !directMove);
analogWrite(out_PWM, PwmValue);
}
Вспомогательная функция выбора мотора. Входные: имя мотора, номера пинов управляющих входов и ШИМ. Последние три изменяются в процессе работы функции. По имени изменяем значения переменных.
void getMotorOuts (byte motorName, byte & out_IN1, byte & out_IN2, byte & out_PWM)
switch (motorName) {
case motor_A:
out_IN1 = out_A_IN1;
out_IN2 = out_A_IN2;
out_PWM = out_A_PWM;
break;
case motor_B:
out_IN1 = out_B_IN1;
out_IN2 = out_B_IN2;
out_PWM = out_B_PWM;
break;
case motor_C:
out_IN1 = out_C_IN1;
out_IN2 = out_C_IN2;
out_PWM = out_C_PWM;
break;
default :
out_IN1 = 0;
out_IN2 = 0;
out_PWM = 0;
return; // an error in occurs
}
return;
}
Считывание с датчиков линии. Изначально использовали QTRSensors — класс из библиотеки Arduino для использования Pololu QTR сенсоров. Класс предназначен для пользования датчиками от QTR-1А до QTR-8А, т. е. когда на схеме 1 датчик, 2 датчика и до 8-ми. Но потом уже не побоявшись analogRead использовали его. Приведу ниже оба способа.
Использование класса QTRSensors.
// количество используемых сенсоров
#define NUM_SENSORS 2
// сколько раз надо считывать данные и усреднять
#define NUM_SAMPLES_PER_SENSOR 4
// есть ли в схеме световой эмиттер, используем его?
#define EMITTER_PIN QTR_NO_EMITTER_PIN
// Инициализация класса.
// Сенсоры на пинах 0 и 1 в количестве NUM_SENSORS штук
// по NUM_SAMPLES_PER_SENSOR раза считываемые без эмиттера.
QTRSensorsAnalog qtra(
(unsigned char[]) {0,1},
NUM_SENSORS,
NUM_SAMPLES_PER_SENSOR,
EMITTER_PIN
);
unsigned int sensorValues[NUM_SENSORS]; // массив для хранения данных сенсора
. . .
qtra.read(sensorValues); // считывание с датчиков
. . .
В нашем случае в sensorValues[0] хранится данные с левого датчика, в sensorValues[1] с правого.
Стандартный способ считывания.
qtrSensorPin – номер пина, sensorValue — значение с датчика.
sensorValue = analogRead(qtrSensorPin);
Scratch
По условиям конкурса ИТ-планета наш робот должен быть программно совместим со средой разработки Scratch. Т.е. так чтобы можно было писать программы на Scratch, а робот их должен отрабатывать. Приведу пример нашей переделки кода для ScratchDuino.
Вся суть сводиться к понимаю протокола обмена, а остальное дело техники.
Итак, считываем данные от Scratch.
if (Serial.available() > 0) {
inByte = Serial.read();
Serial.flush();
if (inByte >= req_scratchboard) { // если данные для нас — трудимся
sendValue(ch_firmware, FIRMWAEW_ID);
motorDirection = (inByte >> 5) & B11;
isMotorOn = inByte >> 7;
. . .
}
}
Мне не удалось найти описание этого протокола, времени разбираться и смотреть его в терминале тоже, поэтому пришлось оставь пока так как есть. Используя значения переменных motorDirection и isMotorOn можем двигаться на все четыре стороны. Особенность заключается только в небольшом искажении как со стороны называния переменных так и среды Scratch. И там и там блок (переменная) в своём названии имеют слово «мотор». На самом деле там должно быть слово «робот». Понимая это становиться ясным алгоритм движения.
Разбор сообщения. Напомню, буква «В» перед числом означает байтовый тип. Не пытайтесь искать B11, B01, B10, B00 — это не переменные. В зависимости от того чему равна motorDirection робот будет двигаться вперёд, назад, по часовой стрелки, против часовой стрелки. Последнее соответственно движения «вправо», «влево».
switch (motorDirection) {
case B11:
runMotor(motor_A, motorPower * isMotorOn);
runMotor(motor_B, motorPower * isMotorOn);
break;
case B01:
runMotor(motor_A, -motorPower * isMotorOn);
runMotor(motor_B, motorPower * isMotorOn);
break;
case B10:
runMotor(motor_A, motorPower * isMotorOn);
runMotor(motor_B, -motorPower * isMotorOn);
break;
case B00:
runMotor(motor_A, -motorPower * isMotorOn);
runMotor(motor_B, -motorPower * isMotorOn);
}
Например составим в Scratch такую программу. Программа делает так, что по нажатию на клавишу "Стрелка влево" робот должен двигаться влевую сторону.
Тогда будет выполняться эта ветка кода.
case B10: runMotor(motor_A, motorPower * isMotorOn); runMotor(motor_B, -motorPower * isMotorOn); break;
Считываем данные с датчиков и отсылаем их Scratch-у
// левый датчик sensorValue = analogRead(qtrSensor1); sendValue(ch_analog0, sensorValue); // правый датчик sensorValue = analogRead(qtrSensor2); sendValue(ch_analog1, sensorValue);
Функция отсылки совершает предварительную упаковку данных и пишет их в порт.
void sendValue(byte channel, int value) {
byte high = 0; // high byte to send
byte low = 0; // low byte to send
high = (1 << 7) | (channel << 3) | (value >> 7);
low = (0xff >> 1) & value;
Serial.write(high);
Serial.write(low);
}
Пожалуй все моменты рассмотрены. Если будут вопросы, отвечу на них, если надо рассмотреть какой-то вопрос подробнее — накопим материал и сделаем ещё одну статью.
Друзья! Присоединяйтесь к нашему сообществу разработчиков, вместе мы сила!