При случайном стечении обстоятельств, во время искажения пространства мне в руки попал 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: Это лишь пример моей реализации алгоритма.
круто! когда вы успели уже робота сделать? и то это за устройство на роботе с экраном?
Все будни прошлой недели были заняты роботом. 🙂 Устройство — NXT Mindstorm — микроконтроллер от Лего. (http://www.mindstorms.ru/)
Блок этот не мой, мне его в школе выдали на пару дней, сказав, чтобы я что-то для обучающихся сделал на базе этой платформы.
молдец!
Особо точность выполнения не проверял, но повороты в 90 град. может осилить робот с помощью двух датчиков по бокам линии. Если что, программа и т.п. лучше пишите не на почту, а Вконтакте: http://vk.com/panasenkodaniil
Верно, повороты под 90 Град. можно проходить, используя два датчика, однако, точность следования по самой линии будет сильно страдать.
а как загрузить эту программу в контроллер??
Я приложил все файлы в конце статьи. Среда разработки BricxCC.