Разработка приложения по управлению мобильным роботом

8.4.1. Цель лабораторной работы

Научиться использовать ОСРВ nxtOSEK для управления автономным движущимся устройством (роботом). Целью работы также является сравнения результов различных математических алгоритмов для одной и той же задачи.

8.4.2. Инструкция по выполнению лабораторной работы

8.4.1. Подготовка

Для выполнения данной лабороторной работы потребуются инструменты, установка которых была проведена в предыдущей лабораторной.

8.4.2. Выполнение работы

Решим следующую задачу: имея автономное движущееся устройство с ультрозвуковым сенсором, написать программу для его движения вдоль стенки. За основу будующей программы возьмем пример, использованный в самостоятельной работе предыдущей лабораторной работы - sonartest.

Откройте c-файл, который находится в ../cygwin/nxtOSEK/samples_C/sonartest. Все изменения необходимо делать в блоке

TASK(Task1)
{
}

Для начала сделаем движение вдоль стены, используя релейный регулятор, псевдокод которого выглядет так:

Если S1 < L // L - заданное расстояние
    // S1 - значение с датчика
{        
  R_MOTOR = 0;
  L_MOTOR = Speed;
}
Иначе 
{
  R_MOTOR = Speed;
  L_MOTOR = 0;
}

Для реализации данной программы потребуются знание только двух функций ecrobot_get_sonar_sensor(номер_порта_датчика) и ecrobot_set_motor_speed(номер_порта_мотора, скорость).

В верхней части программы уже прописано определение порта для датчика (при желании можно заменить на собственный S1-S4).

#define PORT_IN_USE NXT_PORT_S4

Добавим сюда еще определение портов для моторов:

#define L_MOTOR NXT_PORT_A
#define R_MOTOR NXT_PORT_C

Теперь напишем само тело задачи:

if (ecrobot_get_sonar_sensor(PORT_IN_USE) < 50) 
{        
  ecrobot_set_motor_speed(R_MOTOR,0);
ecrobot_set_motor_speed(R_MOTOR,100);
}
else 
{
  ecrobot_set_motor_speed(R_MOTOR,0);
ecrobot_set_motor_speed(R_MOTOR,100);
}

Теперь собираем и заливаем программу на устройство. Датчик на устройстве должен быть подключен к порту 4 и устанавлен на левом боку (причем не под прямым углом, а где-то 45 градусов). Моторы должны быть подключены к портам A и С, левый и правый соответствено. После запуска программы, робот должен двигаться вдоль стены.

Недостатки данного алгоритма заметны уже после первого запуска: робот дергается, а иногда вовсе уходит от стенки.

На замену предложенному выше алгоритму приходит П-регулятор, псевдокод которого:

U = K*(S1 - L) // регулятор
        // L - заданное расстояние
     // S1 - считанное значение с               // датчика расстояния
     // K - пропорциональный коэффициент
R_MOTOR = Speed - U;
L_MOTOR = Speed + U;

Давайте вернемся с небес на землю - датчики не точны. Перед конечным запуском требуется каллибровка. Коэффициент K может принимать значения от 2 до 20 включительно.

Используя предложенный алгоритм, напишите и запустите программу для автономного движущегося устройства.

8.4.3. Задания для самостоятельной работы

Напишите и протестируйте программу, используя ПД регулятор:

U = K1*(S1 - L) + K2*(S1-SensorOld)
// SensorOld - предыдущее значение датчик

Сравните полученные алгоритмы.