Движение робота по линии (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: Это лишь пример моей реализации алгоритма.