Тема из архива сообщества MAL4X: 65 сообщений, 2012–2013. Обсуждение закрыто, ссылки и цены в сообщениях могли устареть.
taran_ob
Чета не могу найти заводской энкодер меньше 50 импульсов по приемлемой цене. Если ориентироваться на 50, то квадратура даст дискретность 200 за оборот. По грубым прикидкам у меня будет около 30 об за 0.5с (60 об/с). Получается 200*30=6000 на весь диапазон перемещения (хотя в простейшем случае достаточно наверно и 512 при диапазоне задания 256) или 12000 отсчетов/с. Чтоб их отследить таймером нужно сканировать с частотой как минимум в два раза больше, лучше 3. Выходит 12000*3=36000Гц.
Собственно, я для себя проблему похоже решил. По быстрому соорудил для экспериментов пару енкодеров на дешевых щелевых оптронах и распечатанным диском на лазерном принтере с 15 штрихами Ф40мм (60 отсчетов за оборот или 1800 на весь диапазон перемещения). Сканировать нужно буде с частотой 10800Гц. Можно было бы сделать и 30 штрихов. Но все таки есть опасение, что при таком шаге, в статике, когда сим не движется, попа будет ощущать колебания стабилизации между двумя метками енкодера (6 градусов отклонения вала двигателя), так как у меня не будет червячного редуктора и мотор может проворачиваться мод весом сима.
В общем купить заводской энкодер или повысить точность позиционирование может оказаться проблемой.
Вот как оно работает. Четкого меандра пока нет, нужно поиграть с компаратором.
Ale
taran_ob писал(а):
Собственно, я для себя проблему похоже решил.
Красивое решение Диск пленочный не коробится? Или ты его на плекс наклеил?
taran_ob
Ale писал(а):
На первый взгляд - вроде все верно. Но я не представляю, как компилятор этот алгоритм распишет в машинных кодах. Т.е действительно ли это более компактная реализация, или просто более компактная запись? Я привык программировать на ассемблере, может быть поэтому выбираю скорее "элементарные" операторы языка С, реализацию которых на ассемблере я хорошо представляю...
Понял, массив в прерывании не самое лучшее решение, спасибо.
Вот еще какой трабл. Если контроллеры весят на 485, какие меры принять для обеспечения синхронизацию расчета регулятора? Или на это можно забить исходя из шустрости обмена? Вижу варианты управления: широковещание - из потока каждый контроллер забирает свою координату программа шлет каждому контроллеру свою координату, затем широковещательная команда на выполнение смены позиции может как то еще?
taran_ob
Ale писал(а):
Диск пленочный не коробится? Или ты его на плекс наклеил?
Распечатал на одной полосе 2 диска, сложил по палам тонер к тонеру совмещая рисунок, после чего загладил место сгиба. Затем склеил двухсторонним скотчем.
taran_ob
вчера менял баллон в освежителе воздуха, колпачок оказался в размер диаметра двигателя, туда он и перекочует, закрывая энкодер
null
taran_ob Пока не сильно понимаю как будет работать сим в целом, но то что сделано - просто супер. Честно говоря настораживает отсутствие редуктора, хватит ли момента удержания этому мотору? .... не исключаю что это мои предрассудки
taran_ob
Для моей конструкции нужен вал с около 500об/мин. Эту частоту будет обеспечивать двигатель 3500 об/мин с редуктор 1/7.5 (есть) на зубчатых шкивах (если бы был трехфазник на 1000об/мин, то соответственно редуктор нужен всего 1/2). Далее вал с 500 об/мин через зубчатый шкив около Ф20мм и 2 натяжных ролика перемещает ремень (не кольцевой) зафиксированный по краям металлической полосы согнутой в дугу которую ремень огибает (зубцами к дуге). Диаметр дуги около Ф1500мм. Получается еще один редуктор 20/1500 или 1/75. Такая конструкция привода позволит перемещать сим по направляющим (рельсам) согнутыми в дугу. Не будет иметь жестких люфтов. И обеспечит смещение оси вращения ближе к вестибулярному аппарату по сравнению с карданом.
taran_ob
null Спасибо за тему
taran_ob
Пока 3d модели нет. Набросок...
taran_ob
Моторы с редуктором, родных енкодеров не будет- мешают.
Ale
taran_ob писал(а):
Или на это можно забить исходя из шустрости обмена? Вижу варианты управления: широковещание - из потока каждый контроллер забирает свою координату
Забить... Именно так
taran_ob писал(а):
Такая конструкция привода позволит перемещать сим по направляющим (рельсам) согнутыми в дугу. Не будет иметь жестких люфтов. И обеспечит смещение оси вращения ближе к вестибулярному аппарату по сравнению с карданом.
Очень интересно. С нетерпением ждем дальнейшего развития событий
taran_ob
Направляющие как и показывал в теме для новичков, только не на одной рамке 4 а по две на раме сима и опоре на полу, а два мотора на одной рамке под 90градусов друг к другу между ними.
null
Интересная конструкция, подключаюсь к Ale в ожидании продолжения Какая будет электронная часть? Судя по предыдущим ссылкам это что-то на базе сервопривода Чена или я ошибаюсь?
taran_ob
От Чена только конструктив - 1 мотор 1 драйвер-мост с мозгом, правда не тини2313 а мега88, как у arisov77 . Предварительную схему выкладывал в теме про новый контроллер. По коду, у Ale позаимствую протокол обмена, процедуру отслеживания посылки, алгоритм выполнения заданий от ПК и возможно сканер энкодера (пока прикручен от Чена). Регулятор, обдумываю два варианта: 1 ПИ, с одной особенностью - частота приема новой координаты >= частота расчета ШИМ. 2 ПИД как у Чена (возможно ошибаюсь)- частота приема новой координаты < частоты расчета ШИМ которая соизмерима с частотой измерения положения энкоднра (сервоцикл), но со стабилизацией скорости от задания к заданию. Управление мостом, больше склоняюсь к режиму 50% ШИМ -двигатель не вращается. В таком режиме нагрев полевиков не вызван нагревом внутренних (технологических) диодов в отличие от режима ШИМ / направление.
Ale
taran_ob писал(а):
Управление мостом, больше склоняюсь к режиму 50% ШИМ -двигатель не вращается. В таком режиме нагрев полевиков не вызван нагревом внутренних (технологических) диодов в отличие от режима ШИМ / направление.
Это как это? Полный мост? И о каком нагреве ты говоришь?
taran_ob
Да, полный. Особенность программного формирования ШИМ для такого режима еще не изучил. В протеусе пока просто вывожу прямой и инверсный шим на два канала. Мне понравился перевод статьи http://roboforum.ru/wiki/H-мост Кстати, Ale, чтоб твой мост заработал как полный, по моему, достаточно ШИМ подавать на DIR, а PWM подтянуть к 1. Только вот с мертвым временем может быть затык наверно?
taran_ob
Ну, общий нагрев может будет и больше, но он не будет вызван диодами нижних ключей.
null
taran_ob писал(а):
От Чена только конструктив - 1 мотор 1 драйвер-мост с мозгом, правда не тини2313 а мега88, как у arisov77 .
Изучал оба варианта, когда искал более разумную схему моста. Но так и не реализовал
taran_ob писал(а):
Управление мостом, больше склоняюсь к режиму 50% ШИМ -двигатель не вращается. В таком режиме нагрев полевиков не вызван нагревом внутренних (технологических) диодов в отличие от режима ШИМ / направление.
Что-то не осилил. А если 25% или 75% ШИМ что должно происходить?
Ale
null писал(а):
Что-то не осилил. А если 25% или 75% ШИМ что должно происходить?
Полный мост имеет управление по обоим плечам, в отличии от полу моста, где одно плечо выключено, пока по второму ШИМ гуляет. Так что если подать 25% на первое плечо, и 75% на второе - на движке будет 50% напряжения (75-25). Если на первое подать 75%, а на второе 25%, то на движке те же 50%, но другой полярности. Ну а если 50% и 50%, то движок стоит, хотя ШИМ молотит оба плеча
Ale
taran_ob писал(а):
Ну, общий нагрев может будет и больше, но он не будет вызван диодами нижних ключей.
Общий это какой? Общие потери мощности во всей схеме? За статью - спасибо, почитаю. Но вот про диоды и нагрев - пока что-то не догоняю я. А вообще, не знаю пока, как в полном мосте. Но в полумосте, когда на обоих плечах ноль, то нижние транзисторы открыты, обмотка движка как бы закорочена через землю. Думаю, именно этот режим, кроме того, что экономит энергию, еще и активно ТОРМОЗИТ движок за счет большого тока, возникающего в результате самоиндукции. Т.е движок превращается в генератор с закороченной обмоткой... Это динамическое торможение.
null
Ale писал(а):
Полный мост имеет управление по обоим плечам, в отличии от полу моста, где одно плечо выключено, пока по второму ШИМ гуляет.
Теперь понял, но в чем выигрыш? Мне кажется только управление таким мостом усложняется .....
Ale
null писал(а):
но в чем выигрыш? Мне кажется только управление таким мостом усложняется
Я тоже так думаю...
taran_ob
Этот в значительной мере популярный метод удаляет практически полностью нагрузку с ограничительных диодов.
Диоды - слабое место в полевике (по току).
В течении периода холостого хода напряжение на обмотках мотора грубо равно Vbat+Vg, что значительно выше, чем в предыдущих методах. Это приводит к значительно более быстрому уменьшению магнитного поля.
Скорость стабилизации частоты вращения будет выше. Минус - конечно же нагрев и уменьшение максимальной скорости вращения.
taran_ob
Ale, а ты не хочешь вникнуть в Ченовские исходники, ты же асм знаешь... Знаю, ты к пиду скептически относишся, но все же... Они являются отрывной точкой для многих проектов. Очень бы хотелось до конца понять алгоритм следящего сервопривода. Возможно и новые идеи появятся.
Ale
taran_ob писал(а):
Ale, а ты не хочешь вникнуть в Ченовские исходники, ты же асм знаешь...
Присылай, посмотрим...
taran_ob писал(а):
Знаю, ты к пиду скептически относишся,
Нет. Совсем не скептически. Вопрос в том, как его реализовывать, и под какие задачи. А то, что это хороший регулятор, я не буду отрицать.
taran_ob
Ale писал(а):
Присылай, посмотрим...
http://elm-chan.org/works/smc/report_e.html Внизу страницы, в архиве исходники. Новый - Firmware and Changes for SMC3/A, полный мост Старый - Firmware for SMC - ШИМ/направление
ПС Подумал вот еще о чем (про станочные сервы), по началу смутило стабилизация скорости, высказывал раннее, думал что в глобальном времени, а потом осенило, это же - мгновенная скорость в нашем случае, то есть, наоборот хорошо. Значит от задания до задания с постоянной скоростью это же нам и надо! И действительно, когда станок рисует окружность с допуском в сотку, попадая в начало, можно себе представить потенциал пида для синхронной стабилизации скорости.
Там на блок схеме пида практически наш сим нарисован
taran_ob
Ale писал(а):
Вопрос в том, как его реализовывать, и под какие задачи.
Так, тема пида становится баяном, тем не менее давайте начнем с чистого листа, потому что было много сумбурных высказываний, недопониманий и подмен понятий.
Уверен, никто не будет спорить с тезисами: 1. Угол поворота вала пропорционален координате. 2. Воздействие (ШИМ) - величина пропорциональная частоте вращения вала двигателя. Если ШИМ - константа, то и частота - константа. 3. Основная задача контроллера - принимать координаты и выдавать такие значения ШИМ, чтоб траектория построенная по показаниям обратной связи была копией траектории построенной по принятым координатам с некоторым смещением во времени. 4. Изменение координат происходит с ускорением.
Главный вопрос - как реализовать 3 пункт! Задача сводится к придумыванию некой функции ШИМ=функция(принятая координата, координата обратной связи). Или иными словами- какой ШИМ приложить к двигателю чтоб из текущей координаты попасть в принятую координату через время t =константа = период приема новых координат. Для начала рассматриваем замкнутую систему - идеальный двигатель с обратной связью, ни какого трения.
1. Что предлагает kuzevan. Предполагается, что за период приема координат мы двигаемся равноускоренныо. Такое движение описывается формулой x=x0+V0*t+a*t*t/2, где t=33мс, и с ней нельзя не согласиться. С другой стороны, изменение ШИМ влечет за собой изменение скорости, то есть ускорение. Подставляя одну формулу в другую получаем искомую функцию ШИМа от координат, начальной скорости и начального ШИМа. Пока вроде все правильно...
2. Как реализовано в существующем контроллере. Предполагается что в течении времени t = период перерасчета ШИМ мы движемся с постоянной скоростью, ввиду малого t. Такое движение описывается формулой V=(x-x0)/t. Так как скорость пропорциональна ШИМу, то получаем выражение ШИМ=некий коэф*(новая координата -текущая координата). Но период перерасчета ШИМ (микрозадание) < периода получения координат (задание), следовательно, с каждым шагом уменьшается разность координат, и соответственно и скорость будет постепенно уменьшаться. То есть от микрозадания до микрозадания мы будем двигаться с ускорением. Траектория - парабола.
3. Предположенный мною ранее подход. Нечто среднее между п1 и п2. Предполагается что в течении времени t = период приема координат мы движемся с постоянной скоростью. Такое движение описывается формулой V=(x-x0)/t. Так как скорость пропорциональна ШИМу, то получаем выражение ШИМ=некий коэф*(новая координата -текущая координата). В отличие от п2 от задания до задания движемся с постоянной скоростью.
В реальной системе, где есть внешние возмущения - трение, нелинейности и тд, которые приводят к тому, что коэффициент в п1,2,3 перестают быть константами, влечет за собой ошибку попадания в заданную координату в нужное время. Как с этим бороться? Самый простой вариант, это вычислять ошибку в каждом шаге, со знаком! (переезд - недоезд) прибавляя ее к прошлой - интегральная составляющая. Затем, пропорционально ее увеличению вносить корректировку воздействия (ШИМ) в следующем шаге. Правда возможна ситуация, что почувствовать влияние такой корректировки мы сможем уже тогда когда она не нужна. Чтобы этого избежать период приема координат должен быть очень маленьким. Тут мы упираемся в скорости UART. И самый главный минус - мы честно не стабилизируем скорость.
4. Новый подход! За основу берем п2 - период перерасчета ШИМ < периода получения координат, то есть внутри цикла приема координат есть несколько циклов перерасчета ШИМ. Но с одной поправкой вытекающей из п3 - скорость, от задания до задания, должна быть константа! Как это реализовать? В качестве целевой координаты микрозадания принимать не принятую, а промежуточную координату, которая лежит на пересечении заданной траектории и вертикальной прямой проходящей через точку времени следующего расчета - сводится к решению прямоугольного треугольника (трапеция разделенная на равные маленькие). Теперь уже можно говорить, что на протяжении периода приема координат интегральная составляющая внесет изменения достаточно быстро (к следующему приему), ну во всяком случае UART нас уже не лимитирует. Имеем полноценный ПИ. Также становится возможным стабилизация скорости между расчетами ШИМ, и как следствие во всем задании. Достаточно ввести дифференциальную составляющую, которая по сути является разностью скоростей текущего и прошлого микрозадания и чем эта разность больше тем больше противоположенный вклад в суммарное значение ШИМ. Имеем полноценный ПИД, при котором не о каких остановках или глобальных замедлениях от задания до задания не может быть и речи!
taran_ob
Собственно, сам алгоритм.
//Цикл приема координат
{
for i {mas[i]=f(x0, x_rx)} //В начале цикла приема координат вычисляем промежуточные точки по x0(текущая координата) и x_rx (принятая координата) занося их в массив mass[]
//цикл расчета ШИМ
{
x=mass[i++]
x0=АЦП или энкодер
error=x-x0
sum_error=sum_error+error
ШИМ=Kp*error+Ki*sum_error+Kd*(error-error0)
error0=error
}
}
Здесь mas[i]=f(x0, x_rx) можно учесть не линейность положения платформы от угла поворота вала двигателя. Можно и не использовать массив - вычислять на лету.
kuzevan
С постоянной скоростью мы двигаемся только если ШИМ=К2. Напрямую скорость от ШИМа не зависит, зависит ускорение.
Ale
taran_ob писал(а):
Уверен, никто не будет спорить с тезисами: 1. Угол поворота вала пропорционален координате. 2. Воздействие (ШИМ) - величина пропорциональная частоте вращения вала двигателя. Если ШИМ - константа, то и частота - константа. 3. Основная задача контроллера - принимать координаты и выдавать такие значения ШИМ, чтоб траектория построенная по показаниям обратной связи была копией траектории построенной по принятым координатам с некоторым смещением во времени. 4. Изменение координат происходит с ускорением.
Пожалуй только с 3-м тезисом я бы не стал спорить... Первый - принимаю, но с натяжкой. Поскольку могу , к примеру, синусоидальную зависимость координаты от угла поворота тоже принять за "пропорциональную".
Для всех остальных нужно обязательно оговаривать условия. Второй тезис можно принять только в случае работы двигателя с НЕИЗМЕННОЙ нагрузкой на валу. Четвертый можно принять только если движку надо ИЗМЕНИТЬ СКОРОСТЬ для достижения новой координаты к заданному времени. Но вполне может быть, что скорость менять не придется, ведь двигатель просто вращаясь БЕЗ УСКОРЕНИЯ изменяет координаты тоже... так что..
taran_ob писал(а):
.. давайте начнем с чистого листа, потому что было много сумбурных высказываний, недопониманий и подмен понятий.
А в общем и целом - твои идеи весьма интересны. И проект ты замутил знатный.
Ale
taran_ob писал(а):
Имеем полноценный ПИ. Также становится возможным стабилизация скорости между расчетами ШИМ, и как следствие во всем задании.
А вот как раз эта задача в предложенном тобой алгоритме совершенно не реализована. За счет чего может происходить СТАБИЛИЗАЦИЯ скорости? Ну да, ошибка (x-x0) фактически пропорциональна необходимой скорости (x-x0)/t, поскольку вычисляется через равные промежутки времени. А вот дальше - классический ПИД, который в принципе заточен на достижение заданной координаты. Или за счет подбора коэффициентов мотор должен ехать с заданной скоростью?
Могу помочь тебе реализовать этот алгоритм в коде.
taran_ob
kuzevan писал(а):
С постоянной скоростью мы двигаемся только если ШИМ=К2.
Поясни, не понял.
kuzevan писал(а):
Напрямую скорость от ШИМа не зависит, зависит ускорение.
Извини, но не могу с тобой согласиться, хотя бы по тому, что напряжение(ШИМ) и ускорение величины разных порядков. Повторюсь, если по твоему, то при ШИМ=константа -> ускорение тоже константа, и как следствие скорость стремится к бесконечности. А такого быть не может. В реалии, при ШИМ=константа -> скорость=константа
taran_ob
Ale писал(а):
Второй тезис можно принять только в случае работы двигателя с НЕИЗМЕННОЙ нагрузкой на валу.Четвертый можно принять только если движку надо ИЗМЕНИТЬ СКОРОСТЬ для достижения новой координаты к заданному времени. Но вполне может быть, что скорость менять не придется, ведь двигатель просто вращаясь БЕЗ УСКОРЕНИЯ изменяет координаты тоже...так что..
По второму, согласен, но давай сначала задачу решим для идеального мотора, у которого нет потерь момента. Четвертый, возможно, но такой режим (с постоянной скоростью) нам вообще не интересен, так как не создает ни какого эффекта. Скорей всего будет только в редких случаях - незаметно вернуться в 0, на пример.
taran_ob
Ale писал(а):
А вот как раз эта задача в предложенном тобой алгоритме совершенно не реализована. За счет чего может происходить СТАБИЛИЗАЦИЯ скорости? Ну да, ошибка (x-x0) фактически пропорциональна необходимой скорости (x-x0)/t, поскольку вычисляется через равные промежутки времени. А вот дальше - классический ПИД, который в принципе заточен на достижение заданной координаты. Или за счет подбора коэффициентов мотор должен ехать с заданной скоростью?
В ПИ стабилизации конечно же нет, а в ПИД есть. error-error0 это есть разность мгновенных скоростей, и если error не равно error0, то в регулировку вносится вклад - диф составляющая. Блин, слов начинает не хватать... По этому алгоритму, в период одного задания и модуль и вектор скорости будут стремиться быть одинаковыми при коэффициентах ПИД = конст. После принятия новой координаты, рассчитаются новые промежуточные точки, и опять, до следующего задания модуль и вектор скорости будут одинаковыми, и тд. Приближаясь к моменту получения новой координаты, вектор скорости не будет стремиться принять горизонтальное направление как в существующем контроллере.
kuzevan
Еще в 1687 году Ньютон сформулировал свой второй закон: F=m*a, Но так, как мы не в безвоздушном пространстве, есть еще сила торможения: F=m*a+F_торможения. В моих терминах F - это ШИМ, m - K1, сила торможения - K2, ШИМ=K1*a+K2, Коэффициенты двигателя можно измерить моей прграммой, а вот откуда взять коэффициенты Kp, Ki, Kd ты не пояснил.
taran_ob
Ale писал(а):
Могу помочь тебе реализовать этот алгоритм в коде.
Большое спасибо, обязательно этим воспользуюсь. Пока формирую тз для себя.
taran_ob
kuzevan писал(а):
Но так, как мы не в безвоздушном пространстве, есть еще сила торможения:F=m*a+F_торможения.В моих терминах F - это ШИМ, m - K1, сила торможения - K2,ШИМ=K1*a+K2,
Не могу понять такую аналогию.
Коэффициенты двигателя можно измерить моей прграммой, а вот откуда взять коэффициенты Kp, Ki, Kd ты не пояснил.
Это конечно очень здорово. Только я бы не выносил в отдельную программу, а проводил процесс калибровки при самом первом включении, как кинект в х-боксе. С помощью визуального контроля реализованного в программе Ale настроить пид, почти уверен, сможет и человек не посвященный в его смысл. Ребенок конечно же не справится. Так что поле для творчества останется и для следующих поколений , методики есть, и во многих заводских приложениях реализованы - адаптивный пид. Меня на его реализацию точно не хватит...
taran_ob
Остается еще один открытый вопрос. Как математически (имперически) рассчитать (прикинуть) частоту приема, частоту расчета, дискретность обратной связи, и дискретность воздействия для обеспечения точности позиционирования, скажем, +-1 из 256 возможных положений.
Ale
taran_ob писал(а):
Как математически (эмпирически) рассчитать (прикинуть)
Прикинуть можно. Я бы остановился на периоде передачи данных из игры порядка 5..10 мс (что дает нам в идеале возможность ощущать пятой точкой почти звуковые колебания до 100 (50) Гц при наезде на поребрики/бордюры). Соответственно внутренний расчет (коррекцию) можно делать в 5..10 раз чаще. Что касается дискретности - то (ИМХО) одного байта (256 возможных значений позиции оси) вполне достаточно.
taran_ob
Согласен, а как с дискретностью обратной связи... не совсем понятно... Для пида частота расчета и скорость изменения регулируемого параметра связаны. А скорость изменения регулируемого параметра зависит, как бы, от его дискретности. Замкнутый круг. Что то нужно зафиксировать. Ок, фиксируем период передачи данных из игры 10мс. Пусть внутри будет 10 микрорасчетов, то есть 1мс. На сколько изменятся показания энкодера или ацп при макс скорости за 1мс? Пусть весь диапазон 1024 максимум за 0,5с (2048 в 1с). Тогда 2048*0,001= всего 2 отсчета, и это на макс скорости. Значит слишком быстро считаем для этого разрешения?, ведь при меньшей скорости изменений вообще не будет. А если считать будем медленней, как это повлияет на точность? А какая мин скорость? пс где можно посмотреть реальную траекторию с игры?
Ale
taran_ob писал(а):
пс где можно посмотреть реальную траекторию с игры?
Не знаю, есть ли такая возможность в буржуйском софте, А в моем - уже практически есть. Вот тут пример http://www.youtube.com/watch?v=FAGIx7RU5pA&list=UUvgWBHjBrYaFVUXDk_8T_Vg&index=8 Правда, график пока малоинформативен, но я могу прикрутить экспорт данных а текстовый файл, что бы его можно было хоть в экселе открыть и обработать. Но, хочу заметить, что как раз к "реальной" траектории данные имеют относительное касательство. Данные для оси делает сам пользователь обрабатывая по определенному закону ускорения и перемещения, поступающие из игры.
taran_ob
С разрешением ЗАДАНИЯ в 256 отсчетов согласен, ну в крайнем случае 512 для сима на 3х ногах. Выше сказанное относится именно к разрешению ОБРАТНОЙ СВЯЗИ.
taran_ob
Ale писал(а):
но я могу прикрутить экспорт данных а текстовый файл,
Хотелось бы посмотреть на такой файлик
А для теста в проге просто синусоида?
Ale
taran_ob писал(а):
А для теста в проге просто синусоида?
Эх, видимо не посмотрел ты видео по ссылке. Почему синусоида? Программа формирует сигнал для двух осей по данным из игры. И показывает эти данные на графике (и их же подает на контроллер). Ты же хотел "посмотреть реальную траекторию". Вот я тебе и предложил эту траекторию... реальнее некуда
taran_ob
Не, видео классное! Я не правильно спросил. Имел ввиду что хотелось в файлике на цифры посмотреть. А синусоиду в теме как регулировать на видео видел. То есть тестовая траектория только синус или может быть произвольная?
taran_ob
Похоже погорячился я с энкодерами. При таких скоростях расчета пид к ак минимум в 10 раз точнее нужны. Но тогда и частота сканирования под 100кГц Не зря интуиция подсказывала что просто не будет. Как бы выкрутиться?
Ale
taran_ob писал(а):
Имел ввиду что хотелось в файлике на цифры посмотреть
Сделаю.
taran_ob писал(а):
То есть тестовая траектория только синус или может быть произвольная?
Генератор может давать синус и меандр. + любую последовательность, которую ты ручками запишешь... в виде колонки цифр...
taran_ob
Ale писал(а):
Я бы остановился на периоде передачи данных из игры порядка 5..10 мс (что дает нам в идеале возможность ощущать пятой точкой почти звуковые колебания до 100 (50) Гц при наезде на поребрики/бордюры).
Вот о чем подумал, как то пытался сделать гравировальную головку для станка из привода позиционирования головок харда. Так вот при 30-50 Гц она уже практически не стучала, при том что веса у нее никакого. То есть, сомневаюсь, что платформа будет эффективно справляться с таким заданием, по крайней мере с резиновоременным приводом. Да и редуктор в вайпере сдохнет быстро. Может лучше в твою программу для компа внедрить алгоритм-фильтр который будет отслеживать гармонические колебания и по их амплитуде (или частоте) слать данные на контроллер вместе с координатой. А контроллер в свою очередь формирует отдельный канал с ШИМ для вибромотора. Если на каждой оси по вибро, то разнеся их справа - слева получим стерео эффект наезда на гравий и тд. или ощущение наезда одной парой колес. В таком случае нет смысла так часто слать координаты в контроллер, 20Гц ИМХО достаточно, 100-200Гц для контура стабилизации скорости тоже, частота сканирования энкодера на 45-50имп 33кГц. Тайменги для ПИДа становятся уже более реальными и без ухищрений.
Ale
taran_ob писал(а):
Вот о чем подумал, как то пытался сделать гравировальную головку для станка из привода позиционирования головок харда. Так вот при 30-50 Гц она уже практически не стучала, при том что веса у нее никакого. То есть, сомневаюсь, что платформа будет эффективно справляться с таким заданием, по крайней мере с резиновоременным приводом. Да и редуктор в вайпере сдохнет быстро.
Думаю, ты прав... Я тут пошел на поводу у Таноса... Хотя давно понятно, что не стоит Он развивает свой продукт как раз в сторону увеличения скорости передачи координат. Я с ним поспорил на буржуйском сайте, а тут чего-то ступил... В общем, давай пересмотрим, ну, скажем на те же 33мс. Уж 15 Гц (небольшой амплитуды) можно попытаться получить от платформы.
taran_ob писал(а):
Может лучше в твою программу для компа внедрить алгоритм-фильтр который будет отслеживать гармонические колебания и по их амплитуде (или частоте) слать данные на контроллер вместе с координатой. А контроллер в свою очередь формирует отдельный канал с ШИМ для вибромотора.
Типа как на руле сделано . Усложним механику, как мне кажется. Хотя с точки зрения программ и контроллеров никаких сложностей нет.
taran_ob
Или может аппаратно решить проблему? Аппаратный подход 1. Модернизировать инкрементный энкодер под интерфейс шаг/направление (заводские такие тоже есть) импульсов на 100-200. Счетный импульс завести на ногу таймера, по переполнению увеличивать множитель, направление энкодера на внешнее прерывание в котором сохранять значение таймера и тд. При этом здорово разгрузить контроллер. Аппаратный подход 2. Энкодер оставить такой как есть 15-30имп, сканировать по таймеру. И завести с него сигнал еще на модуль захвата, по которому организовать контур стабилизации скорости - измерять время между импульсами. Но при этом теряем 16бит таймер - шимить придется 8бит. Аппаратный подход 3 пересесть на камень с аппаратным декодером квадратур. Но хмега слишком монстроподобная, а в пиках я не шарю.
taran_ob
Спасибо!
Ale писал(а):
Генератор может давать синус и меандр. + любую последовательность, которую ты ручками запишешь... в виде колонки цифр...
Ага, буду разбираться...
taran_ob
Ale писал(а):
Он развивает свой продукт как раз в сторону увеличения скорости передачи координат.
Думаю, этим он компенсирует не эффективность регулятора! Так можно и до релейного управления добраться
Ale писал(а):
В общем, давай пересмотрим, ну, скажем на те же 33мс.
Ок, хороший компромисс!
Ale писал(а):
Усложним механику, как мне кажется.
А че, просто жёстко прикрутить пару (один) моторчик к раме(креслу)... В любом случае как опция, по желанию... Я делал вибро, мотор из струйника (по 3$ в мастерской купил 5шт принтеров), толстая шайба эксцентриком. Там, кстати, и енкодеры готовые, только слишком точные...
taran_ob
Ale писал(а):
Типа как на руле сделано
А у меня в руле нет вибро , но мотор обратной связи есть
null
Ale писал(а):
taran_ob писал(а):Может лучше в твою программу для компа внедрить алгоритм-фильтр который будет отслеживать гармонические колебания и по их амплитуде (или частоте) слать данные на контроллер вместе с координатой. А контроллер в свою очередь формирует отдельный канал с ШИМ для вибромотора.
Типа как на руле сделано . Усложним механику, как мне кажется. Хотя с точки зрения программ и контроллеров никаких сложностей нет
Идея сильнейшая! Очень поддерживаю!
PS У нас на форуме такое проскакивало если не ошибаюсь, но вместо вибро мотора должен был быть низкочастотный динамик. Не знаю чем закончилось ....
taran_ob
Я тут было отвлекся на другой проект... постепенно возвращаюсь... Собрал простенький макет для проверки позиционирования, позже фото выложу. Почти написал тестовую прошивку. Протокол, как и в оригинале - 5ти байтные посылки: 1 заголовок (А), 2 идентификатор контроллера(В для широковещания и 1,2,3 и тд для индивидуальных посылок), 3 команда, 4 старший байт данных, 5 младший байт данных. Ответ (если необходим) в том же формате. Сохранил поддержку приема координат сразу для 2х осей, без ответа, для совместимости с софтом от Ale. Для настройки и отображения графика рассогласования попросил коллегу (очень не удобно Ale отвлекать от своего проекта) написать программку - пишет. Имеется команда для приема ШИМ вибромотора. Установка ограничения тока мотора программная-контроллер формирует шим для опоры компаратора тока, думаю тему можно развить для получения небольшого пикового запаса мощности. Поддержки потенциометра не будет, разве что подключить вместо концевиков в качестве задатчика траектории при тестировании (настройке). По поводу регулятора. Пока отошел от ранее предложенного - боюсь не смогу достаточно быстро опрашивать энкодер, да и делать новые, более разрядные, пока лень. Остановился на таком алгоритме: энкодер на 15 импульсов подключен на INT0 INT1 по любому изменению уровня для отслеживания квадратуры (15*4=60 отсчетов за оборот). Там же происходит подсчет 2МГц тактов таймера2 между импульсами квадратур для вычисления скорости вращения двигателя которая стабилизируется по ПИ закону с частотой 100-200Гц. Заданная же, скорость рассчитывается по рассогласованию позиции. В результате, как и раньше говорил, движение от задания к заданию с постоянной скоростью. Надеюсь, скоро будут тесты в железе.
taran_ob
Приступаю к отладке
null
Симпатично! И да, удачи на испытаниях
taran_ob
Мужчины, нужна ваша помощь! Пока занимаюсь отладкой, хочется услышать ваши замечания по поводу функционала который я пытаюсь вложить.
Работа контроллера начинается с цикла калибровки, в котором определяется диапазон позиции и максимальная скорость импульсов с энкодера. Сначала мотор вращается влево с макс. ШИМ/2 до левого концевика. Затем текущая позиция обнуляется и меняется направление. После достижения правого концевика, производится расчет диапазона позиции с учетом программного лимита. Далее направление опять меняется и устанавливается максимальный ШИМ. После достижения точки половины диапазона, определяется максимальная скорость. Контроллер переходит рабочий режим - цикл стабилизации позиции с заданием от ПК. По камманде, см. ниже, контроллер может перейти в режимы, которые могут помочь при настройке: стабилизация скорости и управление ШИМом. Так как программа для конфигурирования еще не написана, то включил дополнительные режимы с управлением от потенциометра и отправкой контролируемых параметров в ASCII коде для приема в любом терминале. Кратковременным нажатием, во время включения, кнопки DEF, осуществляется вход и циклическое переключение дополнительных режимов. Удерживанием в течении 5с - контроллер примет значения констант записанные во флеше. В принципе, во флеш можно записать ранее настроенные константы и закоротить DEF, чтоб не использовать EEPROM, так как часто в сети люди жалуются на её работу в импульсных схемах.
Протокол обмена. Контроллер принимает запрос и отправляет ответ, если предусмотрен, в едином формате - 5 байт: 1 байт - заголовок, по умолчанию - 0х41 2 байт - идентификатор. Запросы с "широковещательным" идентификатором (по умолчанию 0х42), принимают все контроллеры в сети. Запрос с "личным" идентификатор (любой байт заданный в настройках, по умолчанию 0х01 и 0х02 для 1 и 2 оси соответственно) принимает только один, соответствующий ему, контроллер в сети. 3 байт - команда. Перечень ниже. 4 байт - старший байт данных 5 байт - младший байт данных
Команды:
RESET_CONTROLLER Перезагрузить контроллер. Без ответа. Широк. и индив. запрос. SAVE_CONST_TO_EEPROM Записать константы находящиеся в оперативной памяти контроллера в еепром. Контроллер отправляет эхо для подтверждения принятия запроса. LOAD_DEFAULT_CONST Загрузить константы "по умолчанию" в оперативную память. Контроллер отправляет эхо для подтверждения принятия запроса.
Команды ниже относятся к записи и чтению оперативной памяти.
WRITE_NORM_POSITION Записать нормированную (0-256) позицию, без ответа. При широк. запросе координата в мл. байте. При индив. - в мл. байте координата, в ст. байте ШИМ 1125Гц, для коммутации интеллектуального мосфета который может управлять вибромотором (при поддержке управляющего софта) или чего то еще. WRITE_POSITION Записать позицию в размерности квадратур энкодера из диапазона 0-32767. Ответ - текущее значение. WRITE_VELOCITY Записать +-скорость (частоту), следования квадратур для режима настройки в котором осуществляется стабилизация скорости по ПИ закону. Знак определяет направление. С энкодером на N=15 эта частота совпадает с частотой вращения двигателя Fдвиг=Fэнк*4N/60 в об/мин. Ответ - текущая скорость. WRITE_PWM_MOTOR Записать ШИМ из диапазона (задается командами ниже) от -pwm_max до -pwm_min и от +pwm_min до +pwm_max, для режима настройки в котором можно выполнить проверку моста или отладить ограничение тока мотора. Знак определяет направление. Ответ - текущая скорость. WRITE_PWM_VIBRO Записать ШИМ виброматора. Ответ - текущее значение.
WRITE_KOEF_PROP Записать пропорциональны коэффициент контура стабилизации скорости. Ответ - текущее значение. WRITE_KOEF_INT Записать интегральный коэффициент контура стабилизации скорости. WRITE_KOEF_VEL Записать коэффициент пропорциональности ошибки позиции и скорости необходимой для устранения ошибки за фиксированное время (пока задал 10 мс), для контура стабилизации позиции. Ответ - текущее значение. WRITE_PWM_MIN Записать мин шим мотора. Ответ - текущее значение. WRITE_PWM_MAX Записать макс шим мотора. Ответ - текущее значение. WRITE_SOFT_LIMIT Записать размер (число квадратур) мертвой зоны до концевика. Ответ - текущее значение. WRITE_PWM_REF Записать шим для формирования опорного напряжения компаратора обратной связи по току. Пока только для ограничения тока двигателя. Ответ - текущее значение. WRITE_CONFIG Записать режим управления: калибровка положения и максимальной скорости, стабилизация позиции, стабилизация скорости, регулировка ШИМ. Дополнительные режимы с управлением от потенциометра: стабилизация позиции, стабилизация скорости, регулировка ШИ. В дополнительных режимах заданный и контролируемый параметр передается с частотой 10Гц в ASCII коде для приема данных любым терминалом. WRITE_USART_BAUND Записать скорость порта. Ответ - текущее значение. WRITE_ID Записать идентификатор контроллера. Ответ - текущее значение.
READ_KOEF_PROP READ_KOEF_INT READ_KOEF_VEL READ_PWM_MIN READ_PWM_MAX READ_SOFT_LIMIT READ_PWM_REF READ_CONFIG READ_POSITION_RANGE Прочитать диаппазон позиции определенный в цикле калибровки. READ_KOEF_NORM Прочитать коэффициент нормирования позиций 0-255 к диапазону позиций энкодера. Определяется в цикле калибровки. READ_VELOCITY_MAX Прочитать максимальную скорость при заданном макс. ШИМ. Определяется в цикле калибровки.
Как то так
taran_ob
Сейчас код выглядит так
//Файлы/////////////////////////////////////////////////////////////////////////////////
#include <mega88.h>
#include <delay.h>
#include <stdio.h>
//Макросы/////////////////////////////////////////////////////////////////////////////////
#define BYTEH(word) (char)(word>>8)
#define BYTEL(word) (char)(word)
#define WORD(byteh,bytel) ((int)byteh<<8)|bytel
#define BYTE(nibbleh,nibblel) (nibbleh<<4)|nibblel
#define CLRBIT(byte, mask) byte&=~mask
#define SETBIT(byte, mask) byte|=mask
#define ABS(var) ((var) < 0 ? -(var) : (var))
//Команды///////////////////////////////////////////////////////////////////////////////////
#define WRITE_NORM_POSITION 0xFF
#define RESET_CONTROLLER 0x01
#define SAVE_CONST_TO_EEPROM 0x02
#define LOAD_DEFAULT_CONST 0x03
#define WRITE_POSITION 0x11
#define WRITE_VELOCITY 0x12
#define WRITE_PWM_MOTOR 0x13
#define WRITE_PWM_VIBRO 0x14
#define WRITE_KOEF_PROP 0x21
#define WRITE_KOEF_INT 0x22
#define WRITE_KOEF_VEL 0x23
#define WRITE_PWM_MIN 0x24
#define WRITE_PWM_MAX 0x25
#define WRITE_SOFT_LIM 0x26
#define WRITE_PWM_REF 0x27
#define WRITE_CONFIG 0x28
#define WRITE_USART_BAUND 0x31
#define WRITE_ID 0x32
#define READ_POSITION 0x41
#define READ_VELOCITY 0x42
#define READ_PWM_MOTOR 0x43
#define READ_PWM_VIBRO 0x44
#define READ_KOEF_PROP 0x51
#define READ_KOEF_INT 0x52
#define READ_KOEF_VEL 0x53
#define READ_PWM_MIN 0x54
#define READ_PWM_MAX 0x55
#define READ_SOFT_LIM 0x56
#define READ_PWM_REF 0x57
#define READ_CONFIG 0x58
#define READ_POSITION_RANGE 0x61
#define READ_KOEF_NORM 0x62
#define READ_VELOCITY_MAX 0x63
//Регистр UCSR0A////////////////////////////////////////////////////////////////////////////////////
#define PARITY_ERROR 0b00000100
#define DATA_OVERRUN 0b00001000
#define FRAMING_ERROR 0b00010000
#define DATA_REGISTER_EMPTY 0b00100000
#define RX_COMPLETE 0b10000000
//Регистр reg_flag///////////////////////////////////////////////////////////////////////////////////
#define FLAG_CALIB_COMPL 0b00000001
#define FLAG_CALIB_ERROR 0b00000010
#define FLAG_EN_CALC_VEL_GET 0b00000100
#define FLAG_MOTOR_OVL 0b00001000
#define FLAG_REQUEST_NEW 0b01000000
#define FLAG_RX_BUF_OVF 0b10000000
//Регистр reg_ctrl//////////////////////////////////////////////////////////////////////////////////
#define CTRL_CALIB_POS 0b00000000
#define CTRL_USART_POS_VEL_PWM 0b00000001
#define CTRL_USART_VEL_PWM 0b00000010
#define CTRL_USART_PWM 0b00000100
#define CTRL_ADC_POS_VEL_PWM 0b00001000
#define CTRL_ADC_VEL_PWM 0b00010000
#define CTRL_ADC_PWM 0b00100000
//Порты ввода-вывода///////////////////////////////////////////////////////////////////////////////////
#define PORT_PWM PORTB
#define MASK_PWM 0b00000110
#define MASK_PWMA 0b00000010
#define MASK_PWMB 0b00000100
#define PORT_DEF PINC
#define MASK_DEF 0b00001000
#define PORT_LIM PINC
#define MASK_LIM 0b00110000
#define MASK_LIM_L 0b00010000
#define MASK_LIM_R 0b00100000
#define PORT_ENC PIND
#define MASK_ENC 0b00001100
#define PORT_ENC_SHIFT ((PORT_ENC&MASK_ENC)>>2)
#define PORT_TXEN PORTD
#define MASK_TXEN 0b00010000
#define PORT_VIBRO PORTD
#define MASK_VIBRO 0b00100000
#define PORT_REF PORTD
#define MASK_REF 0b01000000
#define PORT_OVL PIND
#define MASK_OVL 0b10000000
#define PIN_POT 0x02
//Операции с регистрами///////////////////////////////////////////////////////////////////////////////
#define DO_CON_OCR1AB TCCR1A|=0b10100000;
#define DO_DISCON_OCR1AB TCCR1A&=0b01011111;
#define DO_CON_OCR1A TCCR1A&=0b11011111; TCCR1A|=0b10000000;
#define DO_CON_OCR1B TCCR1A&=0b01111111; TCCR1A|=0b00100000;
#define DO_CON_OCR0B TCCR0A|=0b00100000;
#define DO_DISCON_OCR0B TCCR0A&=0b11011111;
//Системные константы///////////////////////////////////////////////////////////////////////////////
#define FREQ_CLK 18432000 //Hz
#define FREQ_TIM0 288000 //Hz
#define FREQ_TIM2 2304000 //Hz
#define FREQ_CALC 100 //Hz
#define DIV_CALC 11 //DIV_CALC=FREQ_TIM0/256*FREQ_CALC
#define FREQ_SEND_ADC 10 //Hz
#define DIV_SEND_ADC 10 //DIV_CALC=FREQ_CALC/FREQ_SEND_ADC
#define MESSAGE_SIZE 5
#define RX_BUFFER_SIZE 32
#define TX_BUFFER_SIZE 32
#define HEADER 0x41
#define ID_INDIV_DOF1 0x01
#define ID_INDIV_DOF2 0x02
#define ADC_VREF_TYPE 0x20
//Умолчания///////////////////////////////////////////////////////////////////////////////
#define DEFAULT_K_PROP 50 //кэфф пропорциональный скорости
#define DEFAULT_K_INT 50 //кэфф интегральный скорости
#define DEFAULT_K_VEL 100 //кэфф пропорциональный скорости (FREQ_CALC скорость единичного перемещения при заданной частоте обновления координаты)
#define DEFAULT_PWM_MIN 0 //минимальный шим мотора
#define DEFAULT_PWM_MAX 1000 //максимальный шим мотора
#define DEFAULT_SOFT_LIM 100 //число шагов до концевика мертвой зоны
#define DEFAULT_PWM_REF 0 //0x007F//50% соответствует 2,5V/5=0.5V U=I*R U=10 A*0,1 Omh = 1V
#define DEFAULT_CONFIG 0x0000 //reg_flag reg_ctrl=CTRL_CALIB_POS
#define DEFAULT_BAUND (FREQ_CLK/(16*57600))-1 //57600 скорость СОМ порта
#define DEFAULT_ID 0x4201 //0x42 широковещание, 0x01 индивидуальный
//Константы в EEPROM//////////////////////////////////////////////////////////////////////////
///*
eeprom int eep_k_prop @0x10;
eeprom int eep_k_int @0x12;
eeprom int eep_k_vel @0x14;
eeprom int eep_pwm_min @0x16;
eeprom int eep_pwm_max @0x18;
eeprom int eep_soft_lim @0x1A;
eeprom int eep_pwm_ref @0x1C;
eeprom int eep_config @0x1E;
eeprom int eep_baund @0x20;
eeprom int eep_id @0x22;
//*/
unsigned int const_k_prop;
unsigned int const_k_int;
unsigned int const_k_vel;
unsigned int const_pwm_min;
unsigned int const_pwm_max;
unsigned int const_soft_lim;
unsigned int const_pwm_ref;
unsigned int const_config;
unsigned int const_baund;
unsigned int const_id;
//Константы/////////////////////////////////////////////////////////////////////////////////
unsigned char const_id_bdc;
unsigned char const_id_indiv;
unsigned char reg_flag;
unsigned char reg_ctrl;
unsigned int const_pos_range;
unsigned int const_k_norm;
unsigned int const_vel_max;
unsigned int const_adc_norm_vel;
unsigned int const_adc_norm_pwm;
unsigned char const_adc_pwm_min;
unsigned char const_adc_pwm_max;
//Переменные///////////////////////////////////////////////////////////////////////////////
unsigned char count_tim0_ovf;
unsigned char count_tim2_ovf;
unsigned char rx_wr_index, rx_rd_index, rx_counter;
unsigned char tx_wr_index, tx_rd_index, tx_counter;
char rx_buffer[RX_BUFFER_SIZE];
char tx_buffer[TX_BUFFER_SIZE];
unsigned char rx_id;
unsigned char rx_command;
unsigned char rx_data_h;
unsigned char rx_data_l;
unsigned char var_tim2_ovf;
unsigned char var_tcnt2;
signed int var_pos_set;
signed int var_pos_get;
signed int var_vel_set;
signed int var_vel_get;
signed int var_pwm_motor;
unsigned int var_pwm_vibro;
//Процедуры и функции////////////////////////////////////////////////////////////////////////////
//Инициализировать контроллер
void ini(void)
{
//UBRR0L=BYTEL(const_baund);
//OCR2A=BYTEL(const_pwm_ref);
//UBRR0L=(char)(FREQ_CLK/(16*const_baund*100))-1;
//TCCR0B=PRE_DIV_CLK_TIM0;
//OCR0A=(char)((FREQ_CLK/2*mas_div[PRE_DIV_CLK_TIM0]*FREQ_ENC_SCAN)-1);
// Crystal Oscillator division factor: 1
#pragma optsize-
CLKPR=0x80;
CLKPR=0x00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif
// Input/Output Ports initialization
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=Out Func1=Out Func0=In
// State7=P State6=P State5=P State4=P State3=P State2=0 State1=0 State0=P
PORTB=0xF9;
DDRB=0x06;
// Port C initialization
// Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State6=P State5=P State4=P State3=P State2=T State1=T State0=T
PORTC=0x78;
DDRC=0x00;
// Port D initialization
// Func7=In Func6=Out Func5=Out Func4=Out Func3=In Func2=In Func1=Out Func0=In
// State7=P State6=0 State5=0 State4=0 State3=P State2=P State1=0 State0=P
PORTD=0x8D;
DDRD=0x72;
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: 288,000 kHz
// Mode: Fast PWM top=0xFF
// OC0A output: Non-Inverted PWM
// OC0B output: Non-Inverted PWM
TCCR0A=0xA3;
TCCR0B=0x03;
TCNT0=0x00;
OCR0A=0x00;
OCR0B=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 18432,000 kHz
// Mode: Fast PWM top=0x03FF
// OC1A output: Non-Inv.
// OC1B output: Non-Inv.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0xA3;
TCCR1B=0x09;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: 2304,000 kHz
// Mode: Normal top=0xFF
// OC2A output: Disconnected
// OC2B output: Disconnected
ASSR=0x00;
TCCR2A=0x00;
TCCR2B=0x02;
TCNT2=0x00;
OCR2A=0x00;
OCR2B=0x00;
// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Any change
// INT1: On
// INT1 Mode: Any change
// Interrupt on any change on pins PCINT0-7: Off
// Interrupt on any change on pins PCINT8-14: Off
// Interrupt on any change on pins PCINT16-23: Off
EICRA=0x05;
EIMSK=0x03;
EIFR=0x03;
PCICR=0x00;
// Timer/Counter 0 Interrupt(s) initialization
TIMSK0=0x01;
// Timer/Counter 1 Interrupt(s) initialization
TIMSK1=0x00;
// Timer/Counter 2 Interrupt(s) initialization
TIMSK2=0x01;
// ADC initialization
// ADC Clock frequency: 576,000 kHz
// ADC Voltage Reference: AREF pin
// ADC Auto Trigger Source: ADC Stopped
// Only the 8 most significant bits of
// the AD conversion result are used
// Digital input buffers on ADC0: On, ADC1: On, ADC2: On, ADC3: On
// ADC4: On, ADC5: On
DIDR0=0x00;
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0x85;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART0 Mode: Asynchronous
// USART Baud Rate: 57600
UCSR0A=0x00;
UCSR0B=0xD8;
UCSR0C=0x06;
UBRR0H=0x00;
UBRR0L=0x13;
// Global enable interrupts
#asm("sei")
}
//Конфигурировать контроллер
void config(void)
{
OCR0A=BYTEL(const_pwm_ref);
UBRR0L=BYTEL(const_baund);
reg_ctrl=BYTEL(const_config);
reg_flag=BYTEH(const_config);
const_id_bdc=BYTEH(const_id);
const_id_indiv=BYTEL(const_id);
}
//Сбросить контроллер
void resetchip(void)
{
#asm("cli")
DO_DISCON_OCR1AB;
CLRBIT(PORT_PWM, MASK_PWMA);
CLRBIT(PORT_PWM, MASK_PWMB);
count_tim0_ovf=0;
count_tim2_ovf=0;
rx_wr_index=0;
rx_rd_index=0;
rx_counter=0;
tx_wr_index=0;
tx_rd_index=0;
tx_counter=0;
rx_id=0;
rx_command=0;
rx_data_h=0;
rx_data_l=0;
var_tim2_ovf=0;
var_tcnt2=0;
var_pos_set=0;
var_pos_get=0;
var_vel_set=0;
var_vel_get=0;
var_pwm_motor=0;
var_pwm_vibro=0;
const_pos_range=0;
const_k_norm=0;
const_vel_max=0;
#asm("rjmp 0")
}
//Cохранить константы в EEPROM
void saveeep(void)
{
///*
eep_k_prop=const_k_prop;
eep_k_int=const_k_int;
eep_k_vel=const_k_vel;
eep_pwm_min=const_pwm_min;
eep_pwm_max=const_pwm_max;
eep_soft_lim=const_soft_lim;
eep_pwm_ref=const_pwm_ref;
eep_config=const_config;
eep_baund=const_baund;
eep_id=const_id;
//*/
}
//Прочитать константы по умолчанию
void loaddef(void)
{
const_k_prop=DEFAULT_K_PROP;
const_k_int=DEFAULT_K_INT;
const_k_vel=DEFAULT_K_VEL;
const_pwm_min=DEFAULT_PWM_MIN;
const_pwm_max=DEFAULT_PWM_MAX;
const_soft_lim=DEFAULT_SOFT_LIM;
const_pwm_ref=DEFAULT_PWM_REF;
const_config=DEFAULT_CONFIG;
const_baund=DEFAULT_BAUND;
const_id=DEFAULT_ID;
}
//Загрузить константы из EEPROM
void loadeep(void)
{
///*
const_k_prop=eep_k_prop;
const_k_int=eep_k_int;
const_k_vel=eep_k_vel;
const_pwm_min=eep_pwm_min;
const_pwm_max=eep_pwm_max;
const_soft_lim=eep_soft_lim;
const_pwm_ref=eep_pwm_ref;
const_config=eep_config;
const_baund=eep_baund;
const_id=eep_id;
//*/
}
//Организовать буфер приема
void rxfifo(void)
{
static char status,data;
status=UCSR0A;
data=UDR0;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer[rx_wr_index++]=data;
if (rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
if (++rx_counter == RX_BUFFER_SIZE)
{
rx_counter=0;
SETBIT(reg_flag, FLAG_RX_BUF_OVF);
}
}
}
//Организовать буфер отправки
void txfifo(void)
{
if (tx_counter)
{
--tx_counter;
UDR0=tx_buffer[tx_rd_index++];
if (tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
}
else CLRBIT(PORT_TXEN, MASK_TXEN);
}
/*
//Получить байт из буфера приема USART
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index++];
if (rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}
//Отправить байт в буфер отправки USART
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSR0A & DATA_REGISTER_EMPTY)==0))
{
tx_buffer[tx_wr_index++]=c;
if (tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
++tx_counter;
}
else
{
SETBIT(PORT_TXEN, MASK_TXEN);
UDR0=c;
};
#asm("sei")
}
*/
//Получить результат АЦП
unsigned char read_adc(unsigned char adc_input)
{
ADMUX=adc_input | (ADC_VREF_TYPE & 0xff);
//delay_us(10); // Delay needed for the stabilization of the ADC input voltage
ADCSRA|=0x40; // Start the AD conversion
while ((ADCSRA & 0x10)==0); // Wait for the AD conversion to complete
ADCSRA|=0x10;
return ADCH;
}
//Принять запрос из буфера приема
void request(void) //на выходе rx_id, rx_command, rx_data_h, rx_data_l
{
if (rx_counter>=MESSAGE_SIZE) //если в буфере приема накопилось достаточно данных, то
{
while ((rx_counter>0)&&(getchar()!=HEADER)) {}; //считать байты из буфера пока не найден байт HEADER
if (rx_counter>=MESSAGE_SIZE-1) //если после этого осталось достаточно данных, то
{
rx_id=getchar(); //считать байт идентификатора
rx_command=getchar(); //считать код команды
rx_data_h=getchar(); //считать ст. байт данных
rx_data_l=getchar(); //считать мл. байт данных
if ((rx_id==const_id_bdc)||(rx_id==const_id_indiv)) //если считанный байт идентификатора совпадает с личным или широковещательным идентификатором, то
SETBIT(reg_flag, FLAG_REQUEST_NEW); //установить флаг нового запроса
}
}
}
//Обработать запрос
void task(unsigned char id, unsigned char command, unsigned char data_h, unsigned char data_l)
{
if (id==const_id_bdc)
{
switch (command)
{
case WRITE_NORM_POSITION: //Записать координату для 2х осей
{
switch (const_id_indiv)
{
case ID_INDIV_DOF1:
{
var_pos_set=data_h*const_k_norm;
break;
};
case ID_INDIV_DOF2:
{
var_pos_set=data_l*const_k_norm;
break;
};
};
break;
};
case RESET_CONTROLLER: //Выполнить сброс контроллера
{
resetchip();
break;
};
};
}
if (id==const_id_indiv)
{
switch (command)
{
//////////////////////////////////////////////////////////////////////
case WRITE_NORM_POSITION:
{
var_pos_set=data_l*const_k_norm;
var_pwm_vibro=data_h;
OCR0B=var_pwm_vibro;
break;
};
case WRITE_POSITION:
{
putchar(HEADER); putchar(id); putchar(READ_POSITION); putchar(BYTEH(var_pos_get)); putchar(BYTEL(var_pos_get));
var_pos_set=WORD(data_h,data_l);
break;
};
case WRITE_VELOCITY:
{
putchar(HEADER); putchar(id); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
var_vel_set=WORD(data_h,data_l);
break;
};
case WRITE_PWM_MOTOR:
{
putchar(HEADER); putchar(const_id_indiv); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
var_pwm_motor=WORD(data_h,data_l);
break;
};
case WRITE_PWM_VIBRO:
{
putchar(HEADER); putchar(id); putchar(READ_PWM_VIBRO); putchar(0x00); putchar(var_pwm_vibro);
var_pwm_vibro=data_l; OCR0B=var_pwm_vibro;
break;
};
//////////////////////////////////////////////////////////////////////
case READ_POSITION:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(var_pos_get)); putchar(BYTEL(var_pos_get));
break;
};
case READ_VELOCITY:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
break;
};
case READ_PWM_MOTOR:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(var_pwm_motor)); putchar(BYTEL(var_pwm_motor));
break;
};
case READ_PWM_VIBRO:
{
putchar(HEADER); putchar(id); putchar(command); putchar(0x00); putchar(BYTEL(var_pwm_vibro));
break;
};
//////////////////////////////////////////////////////////////////////
case WRITE_KOEF_PROP:
{
putchar(HEADER); putchar(id); putchar(READ_KOEF_PROP); putchar(BYTEH(const_k_prop)); putchar(BYTEL(const_k_prop));
const_k_prop=WORD(data_h, data_l);
break;
};
case WRITE_KOEF_INT:
{
putchar(HEADER); putchar(id); putchar(READ_KOEF_INT); putchar(BYTEH(const_k_int)); putchar(BYTEL(const_k_int));
const_k_int=WORD(data_h, data_l);
break;
};
case WRITE_KOEF_VEL:
{
putchar(HEADER); putchar(id); putchar(READ_KOEF_VEL); putchar(BYTEH(const_k_vel)); putchar(BYTEL(const_k_vel));
const_k_vel=WORD(data_h, data_l);
break;
};
case WRITE_PWM_MIN:
{
putchar(HEADER); putchar(id); putchar(READ_PWM_MIN); putchar(BYTEH(const_pwm_min)); putchar(BYTEL(const_pwm_min));
const_pwm_min=WORD(data_h, data_l);
break;
};
case WRITE_PWM_MAX:
{
putchar(HEADER); putchar(id); putchar(READ_PWM_MAX); putchar(BYTEH(const_pwm_max)); putchar(BYTEL(const_pwm_max));
const_pwm_max=WORD(data_h, data_l);
break;
};
case WRITE_SOFT_LIM:
{
putchar(HEADER); putchar(id); putchar(READ_SOFT_LIM); putchar(BYTEH(const_soft_lim)); putchar(BYTEL(const_soft_lim));
const_soft_lim=WORD(data_h, data_l);
break;
};
case WRITE_PWM_REF:
{
putchar(HEADER); putchar(id); putchar(READ_PWM_REF); putchar(0x00); putchar(BYTEL(const_pwm_ref));
const_pwm_ref=data_l; OCR0A=const_pwm_ref;
break;
};
case WRITE_CONFIG:
{
putchar(HEADER); putchar(id); putchar(READ_CONFIG); putchar(reg_flag); putchar(reg_ctrl);
const_config=WORD(data_h, data_l);
break;
};
case WRITE_USART_BAUND:
{
putchar(HEADER); putchar(id); putchar(command); putchar(0x00); putchar(UBRR0L);
const_baund=data_l;
break;
};
case WRITE_ID:
{
putchar(HEADER); putchar(id); putchar(command); putchar(const_id_bdc); putchar(const_id_indiv);
const_id=WORD(data_h, data_l);
break;
};
////////////////////////////////////////////////////////////////////
case READ_KOEF_PROP:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_prop)); putchar(BYTEL(const_k_prop));
break;
};
case READ_KOEF_INT:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_int)); putchar(BYTEL(const_k_int));
break;
};
case READ_KOEF_VEL:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_vel)); putchar(BYTEL(const_k_vel));
break;
};
case READ_PWM_MIN:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pwm_min)); putchar(BYTEL(const_pwm_min));
break;
};
case READ_PWM_MAX:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pwm_max)); putchar(BYTEL(const_pwm_max));
break;
};
case READ_SOFT_LIM:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_soft_lim)); putchar(BYTEL(const_soft_lim));
break;
};
case READ_PWM_REF:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pwm_ref)); putchar(BYTEL(const_pwm_ref));
break;
};
case READ_CONFIG:
{
putchar(HEADER); putchar(id); putchar(command); putchar(reg_flag); putchar(reg_ctrl);
break;
};
case READ_POSITION_RANGE:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_pos_range)); putchar(BYTEL(const_pos_range));
break;
};
case READ_KOEF_NORM:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_k_norm)); putchar(BYTEL(const_k_norm));
break;
};
case READ_VELOCITY_MAX:
{
putchar(HEADER); putchar(id); putchar(command); putchar(BYTEH(const_vel_max)); putchar(BYTEL(const_vel_max));
break;
};
//////////////////////////////////////////////////////////////////////
case RESET_CONTROLLER:
{
if (WORD(data_h, data_l)&0xFFFF)
{
putchar(HEADER); putchar(id); putchar(command); putchar(0xFF); putchar(0xFF);
resetchip();
};
break;
};
case SAVE_CONST_TO_EEPROM:
{
if (WORD(data_h, data_l)&0xFFFF)
{
putchar(HEADER); putchar(id); putchar(command); putchar(0xFF); putchar(0xFF);
saveeep();
resetchip();
};
break;
};
case LOAD_DEFAULT_CONST:
{
if (WORD(data_h, data_l)&0xFFFF)
{
putchar(HEADER); putchar(id); putchar(command); putchar(0xFF); putchar(0xFF);
loaddef();
config();
};
break;
};
}
};
}
//Организовать связь по RS485
void simlink485(void)
{
if (tx_counter==0) //если буфер отправки пуст, то
{
request(); //проверить буфер приема на наличие нового запроса
if (reg_flag&FLAG_REQUEST_NEW) //если новый запрос, то
{
task(rx_id, rx_command, rx_data_h, rx_data_l); //выполнить задание
CLRBIT(reg_flag, FLAG_REQUEST_NEW); //сбросить флаг нового запроса
};
};
}
//Сканировать компаратор
void ovl(void)
{
if (PORT_OVL&MASK_OVL) {#asm("cli") OCR1AH=0; OCR1AL=0; OCR1BH=0; OCR1BL=0; #asm("sei")};
}
//Сканировать концевики
void limit(void)
{
if (PORT_LIM&MASK_LIM)
{
DO_DISCON_OCR1AB;//отключить OCR1A и отключить OCR1B
CLRBIT(PORT_PWM, MASK_PWMA);
CLRBIT(PORT_PWM, MASK_PWMB);
while(1){#asm("cli");};
}
}
//Сканировать енкодер
void encoder(void) //на выходе: var_position_get, var_tcnt2, var_count_tim2_ovf
{
static unsigned char enc_last, enc_now, enc_dir_last, enc_dir_now;
enc_now=PORT_ENC_SHIFT; //прочитать состояние энкодера и сдвинуть в начало байта
enc_dir_now=(enc_last&1)^((enc_now&2)>>1); //определить направление вращения
enc_last=enc_now; //текущее состояние энкодера стало прошлым
if (!enc_dir_now) var_pos_get++; //обновить текущую позицию в зависимости от направления вращения
else var_pos_get--;
if ((enc_dir_last==enc_dir_now)&&(reg_flag&FLAG_EN_CALC_VEL_GET)) //если направление вращения не поменялось, то
{
var_tcnt2=TCNT2; //присвоить глобальной переменной значение таймера2
var_tim2_ovf=count_tim2_ovf; //присвоить глобальной переменной число переполнений таймера2
};
enc_dir_last=enc_dir_now; //текущее направление вращения стало прошлим
CLRBIT(reg_flag, FLAG_EN_CALC_VEL_GET);
count_tim2_ovf=0; //сбросить счетчик переполнений таймера2
TCNT2=0; //сбросить таймер2
SETBIT(EIFR, 0b00000011); //сбросить флаги внешних прерываний
SETBIT(TIFR2, 0b00000001); //сбросить флаг переполнения таймера2
}
//Расчитать задание скорости
signed int calcvelset(signed int pos_set, signed int pos_get, unsigned int k_vel, unsigned int vel_max)
{
static unsigned int vel_set;
vel_set=k_vel*(pos_set-pos_get);
if (vel_set<vel_max) return vel_set;
else return vel_max;
}
//Расчитать фактическую скорость
unsigned int calcvelget(unsigned char count, unsigned char count_ovf)
{
return FREQ_TIM2/(count+(long int)count_ovf*255); //VEL=FREQ_CLC/Ncount частота следования квадратур энкодера
}
//Расчитать ШИМ
signed int calcpwm(signed int vel_set, signed int vel_get, unsigned int k_prop, unsigned int k_int, unsigned int pwm_min, unsigned int pwm_max)
{
static signed int pwm;
static signed int vel_error;
static signed int vel_sum_error;
vel_error=ABS(vel_set)-vel_get;
if ((pwm>pwm_min)&&(pwm<pwm_max)) vel_sum_error=vel_sum_error+vel_error;
pwm=(((long int)k_prop*vel_error)+((long int)k_int*vel_sum_error))/1000; //расчет воздействия
if ((pwm>pwm_min)&&(pwm<pwm_max))
{
if (vel_set>0) return pwm;
else return -pwm;
}
else
{
if (pwm>=pwm_max)
{
if (vel_set>0) return pwm_max;
else return -pwm_max;
}
else return 0;
}
}
//Установить ШИМ
void setmotor(signed int pwm)
{
static unsigned int pwm_abs;
pwm_abs=ABS(pwm);
if (pwm>0) {#asm("cli") OCR1BH=0; OCR1BL=0; OCR1AH=BYTEH(pwm_abs); OCR1AL=BYTEL(pwm_abs); #asm("sei")}
else {#asm("cli") OCR1AH=0; OCR1AL=0; OCR1BH=BYTEH(pwm_abs); OCR1BL=BYTEL(pwm_abs); #asm("sei")};
}
//Выполнять калибровку
void calib(void)
{
static unsigned char step;
switch (step)
{
case 0:
{
step=1;
var_pos_get=0;
var_pwm_motor=-(const_pwm_max/2);
setmotor(var_pwm_motor);
break;
};
case 1:
{
if (PORT_LIM&MASK_LIM_L)
{
step=2;
var_pos_get=0;
var_pwm_motor=const_pwm_max/2;
setmotor(var_pwm_motor);
delay_ms(100);
};
break;
};
case 2:
{
if (PORT_LIM&MASK_LIM_R)
{
step=3;
var_pwm_motor=0;
setmotor(var_pwm_motor);
delay_ms(100);
const_pos_range=var_pos_get-2*const_soft_lim;
const_k_norm=const_pos_range/256;
var_pos_get=const_pos_range+const_soft_lim;
};
break;
};
case 3:
{
step=4;
var_pwm_motor=-(const_pwm_max);
setmotor(var_pwm_motor);
break;
};
case 4:
{
if (var_pos_get<(const_pos_range/2))
{
step=0;
var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
if (var_vel_get>const_vel_max) const_vel_max=var_vel_get;
var_pwm_motor=0;
setmotor(var_pwm_motor);
SETBIT(reg_flag, FLAG_CALIB_COMPL); //установить флаг "калибровка завершена"
}
else
{
if (PORT_LIM&MASK_LIM_L)
{
step=0;
var_pwm_motor=0;
setmotor(var_pwm_motor);
};
};
break;
};
};
}
//Задать позицию потенциометром
signed int adcposset(unsigned char pin, unsigned int k_norm)
{
return k_norm*read_adc(pin);
}
//Задать скорость потенциометром
signed int adcvelset(unsigned char pin, unsigned int k_norm_vel)
{
return k_norm_vel*(read_adc(pin)-127);
}
//Задать ШИМ потенциометром
signed int adcpwm(unsigned char pin, unsigned int k_norm_pwm, unsigned char adc_pwm_min, unsigned char adc_pwm_max)
{
static unsigned int adc;
adc=read_adc(pin);
if ((adc>adc_pwm_min)&&(adc<adc_pwm_max)) return 0;
else return k_norm_pwm*(adc-127);
}
//Прерывания///////////////////////////////////////////////////////////////////////////////
//Внешнее прерывание при изменении состояния канала А энкодера
interrupt [EXT_INT0] void ext_int0_isr(void)
{
encoder();
}
//Внешнее прерывание при изменении состояния канала В энкодера
interrupt [EXT_INT1] void ext_int1_isr(void)
{
encoder();
}
//Прерывание по переполнению таймера0 (1125Гц), для задачи частоты расчета ШИМ
interrupt [TIM0_OVF] void timer0_ovf_isr(void)
{
count_tim0_ovf++;
if (!EIFR) SETBIT(reg_flag, FLAG_EN_CALC_VEL_GET); //если внешних прерываний не было, то разрешить захват таймера2 для расчета скорости
}
//Прерывание по переполнению таймера2 (9000Гц), для расчета скорости вращения энкодера
interrupt [TIM2_OVF] void timer2_ovf_isr(void)
{
if (count_tim2_ovf<254) count_tim2_ovf++;
}
//Прерывание от USART по приему байта
interrupt [USART_RXC] void usart_rx_isr(void)
{
rxfifo();
if (!EIFR) SETBIT(reg_flag, FLAG_EN_CALC_VEL_GET); //если внешних прерываний не было, то разрешить захват таймера2 для расчета скорости
}
//Прерывание от USART после передачи байта
interrupt [USART_TXC] void usart_tx_isr(void)
{
txfifo();
if (!EIFR) SETBIT(reg_flag, FLAG_EN_CALC_VEL_GET); //если внешних прерываний не было, то разрешить захват таймера2 для расчета скорости
}
//Программа//////////////////////////////////////////////////////////////////////////////
void main(void)
{
static char count_calc;
static char ctrl_after_calib;
ini(); //Выполнить инициализацию контроллера
loadeep(); //Загрузить константы из EEPROM
config(); //Выполнить конфигурирование
if (!(PORT_DEF&MASK_DEF)) //Если при включении нажата кнопка DEF, то управление от ADC
{
delay_ms(100);
if (!(PORT_DEF&MASK_DEF))
ctrl_after_calib=CTRL_ADC_POS_VEL_PWM;
}
else ctrl_after_calib=CTRL_USART_POS_VEL_PWM; //иначе управление от USART
if (!(PORT_DEF&MASK_DEF)) //Если DEF нажата более 2с, то
{
delay_ms(2000);
if (!(PORT_DEF&MASK_DEF))
{
loaddef(); //использовать константы "по умолчанию"
config(); //выполнить конфигурирование
ctrl_after_calib=CTRL_USART_POS_VEL_PWM; //управление от USART
};
};
while(1) //Бесконечный программный цикл, переключение режимов контроллера через регистр reg_ctrl
{
while (reg_ctrl==CTRL_CALIB_POS) //Цикл калибровки
{
ovl();
calib();
if (reg_flag&FLAG_CALIB_COMPL)
{
var_pos_set=const_pos_range/2;
reg_ctrl=ctrl_after_calib;
//расчет констант для управления от ADC
const_adc_norm_vel=const_vel_max/128;
const_adc_norm_pwm=const_pwm_max/128;
const_adc_pwm_min=127-(const_pwm_min/const_adc_norm_pwm);
const_adc_pwm_max=127+(const_pwm_min/const_adc_norm_pwm);
};
};
while (reg_ctrl==CTRL_USART_POS_VEL_PWM) //Стабилизация принятой по USART позиции по скорости
{
simlink485();
ovl();
limit();
if (count_tim0_ovf==DIV_CALC)
{
count_tim0_ovf=0;
var_vel_set=calcvelset(var_pos_set, var_pos_get, const_k_vel, const_vel_max);
var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
setmotor(var_pwm_motor);
};
};
while (reg_ctrl==CTRL_USART_VEL_PWM) //Стабилизация принятой по USART скорости
{
simlink485();
ovl();
limit();
if (count_tim0_ovf==DIV_CALC)
{
count_tim0_ovf=0;
var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
setmotor(var_pwm_motor);
};
};
while (reg_ctrl==CTRL_USART_PWM) //Стабилизация принятой по USART ШИМ
{
simlink485();
ovl();
limit();
if (count_tim0_ovf==DIV_CALC)
{
count_tim0_ovf=0;
var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
setmotor(var_pwm_motor);
};
};
while (reg_ctrl==CTRL_ADC_POS_VEL_PWM) //Стабилизация позиции заданной через ADC (для выхода из цикла требуется сброс контроллера)
{
ovl();
limit();
if (count_tim0_ovf==DIV_CALC)
{
count_tim0_ovf=0;
var_pos_set=adcposset(PIN_POT, const_k_norm);
var_vel_set=calcvelset(var_pos_set, var_pos_get, const_k_vel, const_vel_max);
var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
setmotor(var_pwm_motor);
if (count_calc++==DIV_SEND_ADC)
{
count_calc=0;
printf("Ps %d",var_pos_set); printf(" Pg %d",var_pos_get); printf("\n\r");
// putchar(HEADER); putchar(const_id_bdc); putchar(WRITE_POSITION); putchar(BYTEH(var_pos_set)); putchar(BYTEL(var_pos_set));
// putchar(HEADER); putchar(const_id_bdc); putchar(READ_POSITION); putchar(BYTEH(var_pos_get)); putchar(BYTEL(var_pos_get));
}
else
{
if (!(PORT_DEF&MASK_DEF))
{
while (!(PORT_DEF&MASK_DEF)){};
delay_ms(100);
reg_ctrl=CTRL_ADC_VEL_PWM;
};
};
};
};
while (reg_ctrl==CTRL_ADC_VEL_PWM) //Стабилизация скорости заданной через ADC (для выхода из цикла требуется сброс контроллера)
{
ovl();
limit();
if (count_tim0_ovf==DIV_CALC)
{
count_tim0_ovf=0;
var_vel_set=adcvelset(PIN_POT, const_adc_norm_vel);
var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
var_pwm_motor=calcpwm(var_vel_set, var_vel_get, const_k_prop, const_k_int, const_pwm_min, const_pwm_max);
setmotor(var_pwm_motor);
if (count_calc++==DIV_SEND_ADC)
{
count_calc=0;
printf("Vs %d",var_vel_set); printf(" Vg %d",var_vel_get); printf("\n\r");
// putchar(HEADER); putchar(const_id_bdc); putchar(WRITE_VELOCITY); putchar(BYTEH(var_vel_set)); putchar(BYTEL(var_vel_set));
// putchar(HEADER); putchar(const_id_bdc); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
}
else
{
if (!(PORT_DEF&MASK_DEF))
{
while (!(PORT_DEF&MASK_DEF)){};
delay_ms(100);
reg_ctrl=CTRL_ADC_PWM;
};
};
};
};
while (reg_ctrl==CTRL_ADC_PWM) //Стабилизация ШИМ заданной через ADC (для выхода из цикла требуется сброс контроллера)
{
ovl();
limit();
if (count_tim0_ovf==DIV_CALC)
{
count_tim0_ovf=0;
var_vel_get=calcvelget(var_tcnt2, var_tim2_ovf);
var_pwm_motor=adcpwm(PIN_POT, const_adc_norm_pwm, const_adc_pwm_min, const_adc_pwm_max);
setmotor(var_pwm_motor);
if (count_calc++==DIV_SEND_ADC)
{
count_calc=0;
printf("PWM %d",var_pwm_motor); printf(" Vg %d",var_vel_get); printf("\n\r");
// putchar(HEADER); putchar(const_id_bdc); putchar(WRITE_PWM_MOTOR); putchar(BYTEH(var_pwm_motor)); putchar(BYTEL(var_pwm_motor));
// putchar(HEADER); putchar(const_id_bdc); putchar(READ_VELOCITY); putchar(BYTEH(var_vel_get)); putchar(BYTEL(var_vel_get));
}
else
{
if (!(PORT_DEF&MASK_DEF))
{
while (!(PORT_DEF&MASK_DEF)){};
delay_ms(100);
reg_ctrl=CTRL_ADC_POS_VEL_PWM;
};
};
};
};
}
}
Ale
taran_ob писал(а):
Мужчины, нужна ваша помощь! Пока занимаюсь отладкой, хочется услышать ваши замечания по поводу функционала который я пытаюсь вложить.
Не сочти за игнор, но дел навалилось столько, что не могу пока ничем помочь. (( Разберусь, обязательно поучаствую...
taran_ob
спасибо
kuzevan
taran_ob писал(а):
Команды:
RESET_CONTROLLER Перезагрузить контроллер. Без ответа. Широк. и индив. запрос. SAVE_CONST_TO_EEPROM Записать константы находящиеся в оперативной памяти контроллера в еепром. Контроллер отправляет эхо для подтверждения принятия запроса. LOAD_DEFAULT_CONST Загрузить константы "по умолчанию" в оперативную память. Контроллер отправляет эхо для подтверждения принятия запроса.
Команды ниже относятся к записи и чтению оперативной памяти.
WRITE_NORM_POSITION Записать нормированную (0-256) позицию, без ответа. При широк. запросе координата в мл. байте. При индив. - в мл. байте координата, в ст. байте ШИМ 1125Гц, для коммутации интеллектуального мосфета который может управлять вибромотором (при поддержке управляющего софта) или чего то еще. WRITE_POSITION Записать позицию в размерности квадратур энкодера из диапазона 0-32767. Ответ - текущее значение. WRITE_VELOCITY Записать +-скорость (частоту), следования квадратур для режима настройки в котором осуществляется стабилизация скорости по ПИ закону. Знак определяет направление. С энкодером на N=15 эта частота совпадает с частотой вращения двигателя Fдвиг=Fэнк*4N/60 в об/мин. Ответ - текущая скорость. WRITE_PWM_MOTOR Записать ШИМ из диапазона (задается командами ниже) от -pwm_max до -pwm_min и от +pwm_min до +pwm_max, для режима настройки в котором можно выполнить проверку моста или отладить ограничение тока мотора. Знак определяет направление. Ответ - текущая скорость. WRITE_PWM_VIBRO Записать ШИМ виброматора. Ответ - текущее значение.
WRITE_KOEF_PROP Записать пропорциональны коэффициент контура стабилизации скорости. Ответ - текущее значение. WRITE_KOEF_INT Записать интегральный коэффициент контура стабилизации скорости. WRITE_KOEF_VEL Записать коэффициент пропорциональности ошибки позиции и скорости необходимой для устранения ошибки за фиксированное время (пока задал 10 мс), для контура стабилизации позиции. Ответ - текущее значение. WRITE_PWM_MIN Записать мин шим мотора. Ответ - текущее значение. WRITE_PWM_MAX Записать макс шим мотора. Ответ - текущее значение. WRITE_SOFT_LIMIT Записать размер (число квадратур) мертвой зоны до концевика. Ответ - текущее значение. WRITE_PWM_REF Записать шим для формирования опорного напряжения компаратора обратной связи по току. Пока только для ограничения тока двигателя. Ответ - текущее значение. WRITE_CONFIG Записать режим управления: калибровка положения и максимальной скорости, стабилизация позиции, стабилизация скорости, регулировка ШИМ. Дополнительные режимы с управлением от потенциометра: стабилизация позиции, стабилизация скорости, регулировка ШИ. В дополнительных режимах заданный и контролируемый параметр передается с частотой 10Гц в ASCII коде для приема данных любым терминалом. WRITE_USART_BAUND Записать скорость порта. Ответ - текущее значение. WRITE_ID Записать идентификатор контроллера. Ответ - текущее значение.
READ_KOEF_PROP READ_KOEF_INT READ_KOEF_VEL READ_PWM_MIN READ_PWM_MAX READ_SOFT_LIMIT READ_PWM_REF READ_CONFIG READ_POSITION_RANGE Прочитать диаппазон позиции определенный в цикле калибровки. READ_KOEF_NORM Прочитать коэффициент нормирования позиций 0-255 к диапазону позиций энкодера. Определяется в цикле калибровки. READ_VELOCITY_MAX Прочитать максимальную скорость при заданном макс. ШИМ. Определяется в цикле калибровки.
Как то так
Интересно, а какая программа будет пересылать эти команды на контроллер? И зачем?
taran_ob
kuzevan писал(а):
Интересно, а какая программа будет пересылать эти команды на контроллер? И зачем?
Как зачем? Чтоб играть! Как минимум xSimCTRLv2.
taran_ob
Что касается остальных команд (будет еще пару), так это минимальный инструментарий, посредством которого пользователь, а может быть и управляющая программа на пк, смог бы провести цикл настройки контроллера - задать траекторию, выполнить измерение, изменить коэффициент и при этом ничего не спалить. От пользователя достаточно было бы знать только максимальный ток двигателя. Далее по току (обратная связь предусмотрена) определяется максимальный ШИМ. По макс. ШИМ - определяется макс. скорость. Уменьшением шим - можно определить соответствующую ему минимальную скорость двигателя. По полученным базовым данным, можно определить и коэффициенты PIV. В принципе, нужно определить всего PI, так как коэффициент V (kp в контроллере ALE), при стабилизированной скорости по PI, является фиксированным временем в течении которого планируется погасить рассогласование позиции - период расчета (период сервоцикла), и не должен зависеть от конструкции и привода сима. Вот так мне все видится....
Чета не могу найти заводской энкодер меньше 50 импульсов по приемлемой цене. Если ориентироваться на 50, то квадратура даст дискретность 200 за оборот. По грубым прикидкам у меня будет около 30 об за 0.5с (60 об/с). Получается 200*30=6000 на весь диапазон перемещения (хотя в простейшем случае достаточно наверно и 512…
Кто участвовал в обсуждении?
В архиве сохранилось 65 сообщений (с 19 декабря 2012 по 5 марта 2013). Больше всего писали: taran_ob, Ale, null. Тема закрыта, новые ответы не принимаются.
Что такое X-Sim и SimTools?
Программы, которые берут телеметрию из игры (ускорения, крен, тангаж, рысканье) и превращают её в команды для приводов платформы движения. X-Sim — более старый и гибкий конструктор, SimTools проще в настройке и поддерживает много игр через плагины.
Сколько степеней свободы нужно для домашнего симулятора?
Для авто- и авиасимов чаще всего хватает 2DOF (крен и тангаж): это два мотора и простая механика. 3DOF добавляет рысканье или вертикаль, 6DOF (платформа Стюарта) даёт полную свободу, но требует шести приводов, жёсткой рамы и точной настройки.