Движение робота по линии (Bricknxc)

 

При случайном стечении обстоятельств, во время искажения пространства мне в руки попал Lego NXT и навязчивая идея сделать робота, ездящего по линии.

Идея заключалась в том, чтобы сделать робота, который смог бы с минимальным количеством датчиков проходить черную линию любой ширины и любой кривости.
Первоначальный вариант:

Все, что нам нужно — два серво-мотора и один датчик света.

У NXT датчика света разрешение в 100 градаций серого, соответственно, можно использовать его так:

Также можно отойти от примитивного использования метода черное-белое, к черное-серое-белое, заставив робота ехать не по линии, а по краю линии. (по «серой» стороне)

Если если машина наезжает датчиком на черную линию, то нужно подкручивать левым мотом, если наезжает на белую, то подкручивать правым мотором.

Программа будет примерно такой:

1
2
3
4
5
6
7
8
9
10
11
12
if (Sensor(IN_2)<=48) {
   OnFwd(OUT_B, 70);
   Off(OUT_C);
}
if (Sensor(IN_2)>=67) {
   OnFwd(OUT_C, 70);
   Off(OUT_B);
}
 
if (Sensor(IN_2)>=55 && Sensor(IN_2)<=59) {
   OnFwd(OUT_BC, 50);
}

Такая программа заработает 100%, если вы подкорректируете значение освещения. Но, однако точность прохождения линии будет невысокой.
По тому же принципу можно разбить всю линию на остальные составляющие:

Чтобы не городить кучу условий, можно изменять мощность моторов прямо-пропорционально  освещенности, использую уравнение. Так как мощность моторов изменяется от 0 до 100, то нужно рассчитать коэффициент преобразования диапазонов. *map()*

K = 100/(Lmax — Lmin)

Мы можем попробовать простой алгоритм для проверки этого коэффициента:

1
2
3
int kp = 100/(WHITE-BLACK);
---------------
OnFwd(OUT_BC, kp*Sensor(IN_2));

Смысл алгоритма — изменение мощности прямо пропорционально освещению.

Также необходимо учитывать, что у нас два мотора и нужно рассчитывать мощность для каждого мотора.

Power = (GRAY — L)*K

Для левого мотора, формула такова:

PowerL = Power + 50

+50 — потому, что если наш робот наехал на черную линию, то нужно подкручивать левым мотором и наоборот. Для правого:

PowerR = 50 — Power

GRAY — серый цвет и определяется так:

GRAY = (Lmax + Lmin)/2

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
int kp = 100/(WHITE-BLACK); //Коэффициент
int gray = (WHITE+BLACK)/2; //Рассчитываем серый цвет
int power = 0;
—————
while(true) {
   int light  = Sensor(IN_2);
   if (light<BLACK) light = BLACK; //Чтобы не было цвета «чернее» черного
   power = (gray - light)*kp; //Рассчитываем общую мощность
   powerL = power + 50; //Мощность левого мотора
   powerR = 50 — power; //Мощность правого мотора
   //Устранение возможных проблем с коэффициентом.
   if (powerR>100) powerR=100;
   if (powerL>100) powerL=100;
   if (powerR<0) powerR=0;
   if (powerL<0) powerL=0;
   //Запускаем моторы
 
   OnFwd(OUT_B, powerL);
   OnFwd(OUT_C, powerR);
}

Дело почти сделано. Вот, собственно говоря, видео:

Как я писал выше, робот должен проходить любые завороты трассы. С такой конструкцией он не сможет пройти остроугольные повороты. Можно изменить конструкцию, поставив колеса ближе, но тогда «дерганность» робота будет высокой.

Единственный правильный способ (сугубо мое мнение) — добавить систему коррекции поворотов. То есть, добавить «спасательный режим» если угол поворота очень резкий.
Ничего не остается как поставить еще два датчика, таким образом: (центральный датчик должен быть немного дальше!)

Принцип таков:

  • Если боковые датчики видят черный, то делаем принудительный поворот направо до тех пор, пока центральный не увидит черную, а затем белую.
  • Если боковые датчики видят белый, то делаем принудительный поворот налево до тех пор, пока центральный не увидит белую, а затем черную.

Фактически, если трасса без крутых поворот, то боковые датчики не нужны вообще.

Вот то, что необходимо добавить:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
int sq1 = 0;
int sq2 = 0;
-----
bgray = (black+gray)/2;
wgray = (white+gray)/2;-———
sq1 = Sensor(IN_1);
sq2 = Sensor(IN_3);
 
if (sq1>=wgray && sq2>=wgray) { //Если оба - белые
 OnRev(OUT_B, 100); OnFwd(OUT_C, 100); //Поворот
 TextOut(0, LCD_LINE5, "Error line! IN==W");
 if (!SoundState()) PlayTone(200, 300);
 while(Sensor(IN_2)>=white-2);
 Wait(300-batlvl); //Задержка с учетом разряда батареи - нужна для того, чтобы моторы успели повернуть.
 while(Sensor(IN_2)<=black+2);
 //Off(OUT_BC);
 pb = -100; pc = 100;
}
 else
if (sq1<=bgray && sq2<=bgray && errcor) {
 OnFwd(OUT_B, 100); OnRev(OUT_C, 100);
 TextOut(0, LCD_LINE5, "Error line! IN==B");
 if (!SoundState()) PlayTone(300, 300);
 while(Sensor(IN_2)>=white-2);
 Wait(400-batlvl);
 while(Sensor(IN_2)<=black+2);
 //Off(OUT_BC);
 pb = 100; pc = -100;
}

Можно заметить, что датчики определяют не белое или черное, а серо-белое и серо-черное, так как у меня датчики немного были подняты и определяли линию неверно.
Можно немного понизить агрессивность системы, поменяв значения 100 на 70.

Все это связано с тем, что скорость и точность моторов не позволяет все точно проходить, поэтому и сделана система корректировки ошибок.
Вся конструкция вживую:


 
Прохождение сложной трассы:

 
Окончательную программу я оснастил небольшим меню:

  • WLineTrace — езда по линии с автокалибровкой цвета, c широкими колесами (нужно поставить на черную линию)
  • LLineTrace — езда по линии с автокалибровкой цвета, c узкими колесами.
  • Callibration — ручная калибровка цвета.
  • Err.corr — коррекция ошибок. Если она выключена, то боковые датчики не нужны.

Порты NXT:

  • OUT_B — левый мотор
  • OUT_C — правый мотор
  • IN_1 — боковой левый сенсор
  • IN_2 — центральный (главный) сенсор
  • IN_3 — боковой правый сенсор

 

Основная программа + файлы, которые нужно загрузить через NXTExplorer.
[Ссылка]
 
PS: Это лишь пример моей реализации алгоритма.
 

0
0

About Кирилл Васин

Прохожий из шапки сайта

8 Comments

  1. круто! когда вы успели уже робота сделать? и то это за устройство на роботе с экраном?

    1. Все будни прошлой недели были заняты роботом. 🙂 Устройство — NXT Mindstorm — микроконтроллер от Лего. (http://www.mindstorms.ru/)
      Блок этот не мой, мне его в школе выдали на пару дней, сказав, чтобы я что-то для обучающихся сделал на базе этой платформы.

  2. Особо точность выполнения не проверял, но повороты в 90 град. может осилить робот с помощью двух датчиков по бокам линии. Если что, программа и т.п. лучше пишите не на почту, а Вконтакте: http://vk.com/panasenkodaniil

    1. Верно, повороты под 90 Град. можно проходить, используя два датчика, однако, точность следования по самой линии будет сильно страдать.

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *