Ориентация в пространстве

Первое, что необходимо знать микроконтроллеру, — местоположение Х-рамы, которая может оказаться в любой позиции после сеанса работы. Магнитные маркеры — это просто метки, наподобие километровых столбов. На них не указано расстояние. Вот почему в переднем правом углу главной рамы находятся два микропереключателя. Когда портальный робот включается, программа перемещает раму до тех пор, пока не сработают оба выключателя.

После этого у микро-Контроллера PIC будет надежная точка отсчета: базовая позиция.
Как только робот получил точку отсчета, он может перемещать X-I раму по рабочей области, подсчитывая маркеры в направлениях X и Y. I Теперь он всегда будет знать, где находится Х-рама.
Команды на лебедки X и Y подаются в виде уровней напряжения на RB4 (Х-лебедка), RB5 (Y-лебедка) и RB6 (направление).
Для каждого направления следует установить высокий уровень на выводе RB4 (Х-лебедка, М2) для движения влево/вправо или на выводе RB5 (Y-лебедка, Ml) для движения назад/вперед. Направление определяет состояние вывода RB6.
Для остановки Х-рамы следует установить низкий уровень на выводе RB4 или RB5.
После вывода Х-рамы в базовую позицию можно приступать к ее перемещению в рабочую область для решения поставленных задач. Эти задачи используют набор подпрограмм для навигации Х-рамы в рабочей области.

Перемещение Х-рамы
Одно из преимуществ портального робота заключается в том, что процессор всегда знает координаты Х-рамы.
В силу этого одной из важных задач программирования является перемещение Х-рамы в необходимую точку. Как уже объяснялось ранее, портальный робот позиционирует Х-раму в квадратной сетке. В опытном образце использовалась сетка 6Х6, что в сумме дает 36 ячеек.
Для перемещения Х-рамы используется четыре основных подпрограммы: по одной для каждого направления.
Подпрограмма для перемещения на один шаг влево.
Перед вызовом этой подпрограммы следует установить или обнулить разряд регистра флагов. Восемь разрядов этого регистра используются для индикации каких-либо событий или уведомления о то-мо, что нечто должно произойти в будущем. В этой программе разряд обнуляется. Это означает, что Х-рама после выполнения одного шага должна остановиться. Если рама должна продолжать свое движение, то этот разряд устанавливается в 1.
Работа подпрограммы начинается с мигания красного светодиода. Это несущественно и может быть опущено, хотя и удобно при тестировании программы. Далее включается двигатель М2 для намотки троса На лебедку и перемещения рамы влево. На этой стадии отрабатывается длинная задержка, поскольку предполагается, что Х-рама остановлена на маркере. Для поиска следующего маркера она должна отодвинуться от текущего.
В зависимости от скорости вращения вала и передаточного коэффициента коробки передач может потребоваться изменить длительность этой задержки.
Единственный практический способ определить ее — метод проб и ошибок.
Далее следует цикл, состоящий из двух командных строк, в котором разряд регистра CM1CON0 опрашивается до тех пор, пока не окажется состоянии лог. 1. Обратите внимание на переключение к банку 2 перед опросом CM1CON0, а также — на обратное переключение к банку 0 после обнаружения маркера.
Подпрограмма завершается остановом лебедки или предоставлением ей возможности работать дальше в зависимости от состояния flags. Х-рама переместилась от одного маркера к следующему в том же ряду.
Подпрограмма right работает аналогу гичным образом, однако двигает раму в противоположном направлении.
При вводе этого листинга с клавиатуры просто скопируйте листинг подпрограммы left и отредактируйте его.

Перемещение Y-рамы
Эта рама также перемещает Х-раму. Для этого используются две подпрограммы: back и forward, — которые перемещают Y-раму, переключая Y-лебедку, Ml. Структура подпрограмм back и forward аналогична структуре подпрограмм left и right.

Выход в требуемую точку
Четыре одношаговых подпрограммы выводят Х-раму в любую точку рабочей области. Следующий фрагмент кода выводит ее в точку (0, 0), расположенной в переднем правом углу робота.
Начиная с базовой позиции, в которой рама была оставлена после выполнения подпрограммы, она должна пройти один шаг влево и один шаг назад. Когда для смещения рамы на один шаг используются подпрограммы перемещения, перед их вызовом необходимо установить или обнулить только разряд регистра flags. Так, для перехода из базовой позиции в точку (0, 0), обнулите этот разряд, а затем вызовите подпрограммы left и back.
Подводя общий итог, отметим, что главная программа содержит:
• директивы, включая объявление меток регистров для хранения данных;
• раздел инициализации;
• подпрограмму перемещения Х-рамы в базовую позицию;
• подпрограмму перемещения Y-рамы в точку (0, 0);
• подпрограммы left, right, back, forward, delay и longdelay;
• директиву end.
Полезным добавлением к вышеперечисленному будет подпрограмма flash, которая обеспечивает неопределенно долгое мигание зеленого светодиода. Она расположена в конце главной программы, перед подпрофаммами.
Программа переходит прямо к подпрограмме flash после перемещения Х-рамы в точку (0, 0). По мере поэтапного ввода и тестирования программы подпрограмма flash всегда остается в ее конце. Мигание светодиодов не является обязательным, однако всегда хорошо иметь визуальное подтверждение того, что микроконтроллер завершил выполнение задачи. Если светодиоды мигают, однако микроконтроллер не делает то, что ожидается, значит в программе присутствует ошибка, которую необходимо откорректировать.

Перемещение из точки А в точку В
Простейший примером перемещения из точки А в точку В — подпрограмма перемещения из исходного положения в точку (0, 0). Для этого требуется всего лишь один шаг влево с последующим одним шагом назад. Теперь предположим, что Х-рама уже находится в точке (0, 0), готовая к некоторому действию, предполагающему перемещение к центру рабочей области. Для этого ей, возможно, потребуется отработать два или большее число шагов влево и назад. Соответствующие подпрограммы должны вызываться для каждого шага.
Данная подпрограмма использует для подсчета шагов два регистра: xpos и ypos. Для повторения вызовов по три раза в эти регистры записывается значение 3, а затем выполняется обратный отсчет до нуля.

Перемещение из точки С в точку D
Программа AtoB полезна при позиционировании Х-рамы в начале сеанса. Она предполагает, что рама находится в позиции (0, 0), и переводит ее в другую требуемую для работы точку. Однако, предположим что в какой-либо момент времени раме необходимо изменить координату.
Один из способов реализовать это — вызвать подпрограмму возврата рамы в базовую позицию, затем перевести ее в точку (0, 0), и только после этого — в требуемое положение. Но это займет много времени. Гораздо быстрее перейти из одной точки непосредственно к другой без вывода рамы в базовую позиции и в точку (0, 0). В этом и заключается суть программы CtoD: перемещение из текущей точки С в точку D.
Данная подпрограмма требует шести регистров: xpos, ypos, хс, ус, хс и yd. Регистры xpos и ypos нам уже знакомы.
Новые для нас — регистр текущей координаты X (хс), регистр текущей координаты Y (ус), регистр координаты X точки назначения (xd) и регистр координаты Y точки назначения (yd). Требуемые значения загружаются в них перед вызовом подпрограммы. В данном случае мы предполагаем, что Х-рама находится в позиции (5, 4) и должна переместится в точку (3, 1). Соответствующий фрагмент кода имеет вид:
Блок-схема подпрограммы xctod (показывает расчет расстояния и направления по стартовой и конечной позициям.
Вычитание хс из xd дает расстояние, на которое необходимо переместить раму. Если результат положителен, то Х-рама перемещается влево через вызов подпрограммы left.
Ситуация немного усложнится, если хс находится справа от xd, т.е. значение xpos будет отрицательным.
Подпрограммы left и right работают только с положительными значениями, поэтому перед вызовом подпрограммы right значение xpos необходимо сделать положительным. Отрицательный результат вычитания приводит к установке флага переноса (STATUS ).
Преобразование отрицательного числа в положительное происходит в два этапа. Вначале из отрицательного значения вычитается 1 (например, -4 станет -5), после чего байт инвертируется (т.е. нули заменяются единицами, а единицы — нулями). Это выполняется командой comf. Результат — положительное значение. Продолжая наш пример, -5 в шестнадцатеричном виде — FAh или 11111011b двоичном. Инверсией этого числа будет 00000100, что соответствует десятичному 4. Далее вызывается подпрограмма right с этим значением в регистре xpos. Исходный код подпрограммы xctod.
Для перемещения в направлении Y используется аналогичная подпрограмма yctod. Ее блок-схема и листинг отличаются от xctod заменой х на у во всех командах и метках. Кроме того, вместо подпрограмм left и right вызываются подпрограммы back и forward.
Рассмотренные подпрограммы (одна либо обе) могут быть включены в основную программу, если они не часто используются. При интенсивном использовании их лучше оформить в виде подпрограмм. Перед их вызовом загружайте требуемые значения в хс, ус, xd и yd.

Источник: qwedr.com