Робот-картограф
- Войдите на сайт для отправки комментариев
Ср, 22/07/2015 - 22:19
Часть 0.
Вместо вступления
Почему-то пространные инструкции, как нужно что-либо делать, обычно пишут новички. Пришел, увидел, попытался что-то сделать, получилось - и есть повод поделиться своей радостью со всем миром. Хотя, возможно, и сам еще толком не понял, как оно работает и, главное - почему. Или понял, но неправильно.
Другое дело - специалист, кроме массы собак съевший в придачу целый зоопарк. Нет, первое время специалист активно помогает новичкам, но вскоре обнаруживает, что вопросы новичков остаются одними и теми же. Он уже ответил на какой-то вопрос раз десять, а его не перестают задавать. Хотя подробный ответ уже написан. Десять раз. На этом же форуме.
И тогда специалист постепенно переходит от подробных ответов к коротким намекам и сарказму. Нет, намеки абсолютно правильные и, если новичок как следует задумается, этот намек поможет ему больше, чем пространное изложение. Но думать новичок не хочет, и тогда специалист начинает его троллить. На что, в общем-то, имеет право. А заодно троллить и всех остальных новичков.
А к какой категории отношусь я? С одной стороны, есть немалый опыта программирования и схемотехники. Но о существовании Ардуино узнал чуть больше месяца назад, а с тех пор, как первый экземпляр попал в руки, не прошло еще и четырех недель. По всем критериям - новичок. По крайней мере, желание поделиться с окружающими нажитой непосильным трудом информацией еще не иссякло. Так что не будем ломать традиции.
Предыстория
Почитал об Ардуино. Возникло вполне естественное желание что-то на нем соорудить. Для души. Что именно?
- Конечно же, робота! Разве могут быть какие-то сомнения?
Итак, решено!
Но что должен уметь этот робот? Выполнять команды с ПДУ, объезжать препятствия и следовать начертанной на полу линии - неинтересно. Пусть это будет робот-картограф. Чтобы мог объехать квартиру и нарисовать ее план (то есть сразу ограничиваем размер исследуемого пространства десятком-другим метров, достижимое разрешение оцениваем в 1-5 см). А для этого он должен уметь ориентироваться в пространстве. Нужны: валкодеры, компас, акселерометр и гироскоп. Может, и не все это необходимо одновременно, но зато можно будет сравнить результаты расчетов по данным с разных датчиков, проанализировать погрешности, выбрать оптимальное решение, что само по себе интересно. GPS-датчик ввиду его высокой по меркам данного проекта погрешности не рассматриваем.
Приценился в наших Интернет-магазинах (ИМ) и немножко призадумался...
В общем, заказал в китайском ИМ стандартного робота - набор DIY (тележка о двух моторах, сервой с дальномером и датчиками линии под пузом),
а также необходимый набор периферии. Правда, как оказалось, не все друг другу подошло. В частности не подошли валкодеры, т.к. у данного робота есть зубчатые колеса, но нет и некуда прицепить диск с прорезями. Пришлось докупать недостающее уже в местных ИМ.
Итак, переходим к главному.
Часть 1.
Что такое валкодер, и с чем его едят
Применительно к Ардуино, почему-то, данное устройство принято называть энкодером, но этот термин слишком многозначен, поэтому я буду пользоваться более конкретным - "валкодер".
По классической схеме валкодер состоит из диска с прорезями, закрепленного на одной оси с колесом и "оптотройки", которая в отличие от оптопары имеет не 2, а 3 основные детали: излучатель (инфракрасный диод) расположен с одной стороны диска, а два фотодатчика - с другой.
Свет, проходящий через щели диска при вращении последнего, формирует на выходе оптических датчиков импульсы. Причем импульсы датчика сдвинуты по фазе. В идеале - на 90°. Здесь и далее под 360° мы будем понимать не полный оборот колеса, а период следования импульсов. Если бы в валкодере было единственное отверстие, и он выдавал один импульс за оборот, то 360° - был бы как раз один оборот колеса, но т.к. отверстий несколько, например, на рисунке - 4, то оборот колеса будет равен 360°*4 = 1440° валкодера. Это чтобы было понятно, в каких градусах мы меряем.
Так вот, имея один датчик на колесо, можно оценить скорость движения (при наличии часов) и пройденный путь, а когда датчиков два, можно определить также направление вращения.
Но не всегда есть куда насадить диск с прорезями. Мне, например, достался робот, у которого место для валкодера классической конструкции не предусмотрено (тяжелое наследие доардуинных электромеханических самоделок). Зато робот собран на стандартных колесах, которые применяются в детских развивающих конструкторах, а эти колеса имеют внутри зубцы (так как колеса универсальные - на них вместо шин можно насадить гусеницы). И конструкторы нашли выход, несколько изменив конструкцию валкодера - две оптопары расположены в одной плоскости, светодиоды светят в ту же сторону, куда смотрят датчики, и последние принимают не прямой свет от диодов, а отраженный от зубцов. Поэтому каждый зубец колеса формирует импульс в валкодере.
Схематически на фоне колеса показан рефлективный валкодер в разрезе: коричневым - плата валкодера, черным - оптопары, где красным и зеленым - светодиод и оптодатчик. Свет отражается от зубцов и поглощается в промежутке между ними.
Настройка валкодера
Но если в классическом варианте сдвиг фазы между датчиками в 90° формирует одно и то же отверстие, в случае рефлективных валкодеров расстояние между оптопарами составляет 9 мм, а шаг зубцов колеса - примерно 5.2 мм. В результате сдвиг фазы между датчиками составляет не 90°, а 630°, т.е. в 7 раз больше. Понятно, что для периодического сигнала 630° - это то же самое, что -90°. Теоретически. На практике из-за того, что зубцы движутся по окружности, а не по прямой, плоскость зубца не перпендикулярна направлению света, сам свет отражается от разных зубцов, имеющих к тому же дефекты изготовления, возникает погрешность. И всего 15% погрешности от имеющихся 630° составят более 90° и приведут к полному дисбалансу фаз. Что у меня и произошло.
Мало того, что сдвиг фаз вместо 90° оказался близок к 0, так часть импульсов валкодер вообще пропускал.
С последним бороться сравнительно просто - на плате валкодера имеются подстроечники, а вот что делать со сдвигом фазы - пришлось покумекать. Предпринял поиск в И-нете. Самое интересное, что помогли картинки. Стандартно выпускаемых для DIY рефлективных валкодеров существует 3 поколения. На первом номер версии не указывался, на втором стоит "v1.1", а на попавшем ко мне красуется "v1.2". Я обратил внимание на то, что мой валкодер выполнен на тонкой плате - толщиной всего 0.65 мм, а первые версии, если судить по фото, делались на обычном 1.5 мм текстолите. Значит, место для того, чтобы приблизить оптопары к колесу и тем самым уменьшить погрешность, имеется.
Так я и сделал. Купил торт в пластиковой коробке.
Торт пришлось утилизировать :), а из верхней прозрачной крышки, толщина которой оказалась равной 0.40-0.45 мм, вырезал 4 прокладки 25х13 мм. Притянул их струбцинкой к диску-основанию робота в месте крепления моторов и, пользуясь основанием как шаблоном, просверлил по 2 отверстия. Прокладки разместил между мотором и платой валкодера, "пирог" получился такой: крепление мотора, 2 прокладки, плата валкодера, основание робота. В результате датчики валкодеров оказались расположены так, как если бы были распаяны на плату стандартной 1.5 мм толщины.
Но этого мало, нужно еще все отрегулировать и проверить.
Пришлось написать две программы: Одну для Ардуино, который запускал двигатель и снимал показания с валкодеров каждые 0.5 мс, после чего упаковывал их и отправлял по последовательному порту наружу. А другую - для PC, который должен был обработать данные, построить графики и просчитать статистику. В качестве инструмента для последнего был выбран Processing, - до сего момента я его в глаза не видел, но раз уж браться за новое и неосвоенное, то ни в чем себе не отказывать!
Тексты программы Processing не привожу. Имея опыт использования этого инструмента ровно 0 дней, трудно написать программу, которая не показалась бы смешной опытному программисту. Кроме того, программа написана для конкретного случая, адаптация ее для чего-либо другого потребует внесения правок, а 500 строк без русскоязычных комментариев (IDE Processing'а почему-то не поддерживает кириллицу), мне кажется, новичку не осилить.
Программа для Ардуино здесь:
// осциллограмма состояний валкодеров - работает в связке с Processing #define MotorLeft_1 4 #define MotorLeft_2 5 #define MotorRight_1 6 #define MotorRight_2 7 #define EncoderLeft_A 8 #define EncoderLeft_B 9 #define EncoderRight_A 10 #define EncoderRight_B 11 #define LenArray 112 // максимальная длина массива byte State = 0; // 0- начало, 1 - измерение, 2 - ничего не делать long StartTime = 0; // время начала измерения long EndTime = 0; // время конца измерения int NumMeasurements = 0; // количество (переломных) точек измерения unsigned int Counter = 0; // счетчик циклов typedef struct { int time; byte data; } tMeasure; tMeasure Mes[LenArray]; void setup() { Serial.begin(9600); pinMode(MotorLeft_1, OUTPUT); pinMode(MotorLeft_2, OUTPUT); pinMode(MotorRight_1, OUTPUT); pinMode(MotorRight_2, OUTPUT); pinMode(EncoderLeft_A, INPUT); pinMode(EncoderLeft_B, INPUT); pinMode(EncoderRight_A, INPUT); pinMode(EncoderRight_B, INPUT); State = 0; } void loop() { if (State == 0){ digitalWrite(MotorLeft_1, 0); digitalWrite(MotorLeft_2, 1); digitalWrite(MotorRight_1, 0); digitalWrite(MotorRight_2, 1); delay(200); StartTime = millis(); DoMeasurement(); EndTime = millis(); digitalWrite(MotorLeft_2, 0); digitalWrite(MotorRight_2, 0); // Text2Serial(); Bin2Serial(); State = 2; } } void DoMeasurement() { NumMeasurements = 0; // считаем только количество моментов, в которые произошли изменения Counter = 0; // счетчик циклов (по 0.5 мс) - считаем все byte PrevByte = 0; byte CurrByte = 0; unsigned int PrevCounter = Counter; while (NumMeasurements < LenArray) // цикл длиной около 19 мкс (без delay) * LenArray { byte CurrByte = 'A' & 0x70; byte j = digitalRead(EncoderLeft_A); CurrByte = CurrByte | (j); j = digitalRead(EncoderLeft_B); CurrByte = CurrByte | (j << 1); j = digitalRead(EncoderRight_A); CurrByte = CurrByte | (j << 2); j = digitalRead(EncoderRight_B); CurrByte = CurrByte | (j << 3); delayMicroseconds(481); // теперь все измерение 0.5 мс if (CurrByte != PrevByte) { Mes[NumMeasurements].time = Counter; Mes[NumMeasurements].data = CurrByte; NumMeasurements++; } PrevByte = CurrByte; Counter++; } } void Text2Serial() { Serial.print("Start measurement (ms): "); Serial.println(StartTime); Serial.print("End measurement (ms): "); Serial.println(EndTime); Serial.print("Number of Cycles: "); Serial.println(Counter); Serial.print("Length of Data: "); Serial.println(NumMeasurements); Serial.println("Time\tWheelL-A\tWheelL-B\tWheelR-A\tWheelR-B"); for (int i = 0; i < NumMeasurements; i++) { byte data = Mes[i].data; Serial.print(((float)(EndTime-StartTime))*i/(LenArray-1)); Serial.print('\t'); Serial.print(data & 1); Serial.print('\t'); Serial.print(((data >> 1) & 1) + 2); Serial.print('\t'); Serial.print(((data >> 2) & 1) + 4); Serial.print('\t'); Serial.println(((data >> 3) & 1) + 6); } } void Bin2Serial(){ Serial.write(0x55); // ракорд Serial.write(0xAA); Serial.write(0x55); Serial.write(0xAA); Serial.write(0x00); // lowByte(StartTime) Serial.write(0x00); // highByte(StartTime); Serial.write(lowByte((int)(EndTime - StartTime))); // в 0.5-мс отсчетах Serial.write(highByte((int)(EndTime - StartTime))); Serial.write(lowByte(Counter)); // в 0.5-мс отсчетах Serial.write(highByte(Counter)); Serial.write(lowByte(NumMeasurements)); // в 0.5-мс отсчетах Serial.write(highByte(NumMeasurements)); for (int i = 0; i < NumMeasurements; i++) { Serial.write(lowByte(Mes[i].time)); Serial.write(highByte(Mes[i].time)); Serial.write(Mes[i].data); } Serial.write(0xF0); // ракорд Serial.write(0x0F); Serial.write(0xF0); Serial.write(0x0F); }Можно заменить вывод с бинарного на текстовый, перенести его из консоли Arduino IDE в Excell и построить графики.
На собранном роботе добраться до подстроечников невозможно. Работать с разобранным оказалось тоже практически нереально: моторы надо держать в руках, чтобы не убежали, да и масса проводов норовит где-нибудь оборваться или отсоединиться. Поэтому вместо робота был собран стенд:
на котором две пластины основания соединялись не 4, а лишь 2 стойками. При этом верхняя была сдвинута относительно нижней, чтобы дать доступ сверху к подстроечникам. А неиспользуемые стойки были развернуты, чтобы упираться в поверхность стола и не подпускать колеса к чему-нибудь, на что можно опереться, чтобы убежать.
Скрин до переделки и настройки показан ниже:
Можно видеть, что передние фронты двух каналов валкодеров в начале оборота колеса приходят "сначала А потом В", а в середине последовательность меняется на противоположную. И цифирь это подтверждает. Естественно, с таким раскладом правильно определить направление вращения колеса невозможно.
А вот после переделки и настройки:
При длине периода 95 единиц (47.5 мс) сдвиг фазы на одном колесе колеблется от 18 до 25 единиц, среднее значение 21 (80°), а на другом - от 12 до 22, среднее 17 (64°), что вполне приемлемо. То есть, если мы даже будем опрашивать валкодер без прерываний, то при периодичности опроса менее 6 мс мы гарантированно не пропустим ни одного импульса.
Выводы
Если бы мне пришлось заново выбирать элементную базу для робота, я бы предпочел набор либо с готовыми щелевыми валкодерами, либо с мотором, имеющим выход вала с двух сторон, чтобы с одной стороны насадить колесо, а с другой - щелевой валкодер. Думаю, возни было бы меньше. Хотя, кто знаем, может, там возникли бы другие проблемы.
Но если уж от рефлективного валкодера никуда не деться, следует расположить оптопары по возможности ближе к внутренней поверхности колеса с помощью прокладок, чтобы уменьшить погрешность фазы.
Ну и после этого любым доступным способом добиться примерного равенства длительностей положительного и отрицательного импульсов с каждого датчика. Тут может помочь программа, вычисляющая количества "0" и "1" на выходе валкодера за достаточный интервал времени (скажем, 1-2 секунды), и выводящая их отношение в последовательный порт.
Ардуино - вещь универсальная, она может не только совершать какую-то работу, но при этом сама же наблюдать за качеством выполнения этой работы и результаты наблюдений докладывать хозяину :).
Часть 2
Аппаратная поддеожка валкодера
Стенд тестирования счетчика K555ИЕ7
Зачем это нужно
В доступных в И-нете статьях, да и на официальном сайте Ардуино встречаются настоятельные рекомендации для обработки сигналов валкодеров использовать аппаратные прерывания. Утверждается, что в противном случае неизбежно будет потеряна часть импульсов, и, следовательно, в результатах обработки окажутся ошибки: неверно будет вычислен пройденный путь, скорость, направление движения или все это одновременно. Попытаемся выяснить, насколько это соответствует действительности.
Итак, предполагаем, что у нас есть постоянно выполняющийся в цикле код, запрашивающий состояния валкодеров, производящий обработку результатов, и вычисляющий пройденные каждым колесом расстояния и скорости их вращения. Как было показано в предыдущем разделе, интервал между чтениями сигналов валкодеров не должен превышать 6 мс. Кстати, были проведены и дополнительные измерения, в которых двигатели гонялись продолжительное время на разных скоростях и при этом определялись минимальный и максимальный интервалы между двумя последовательными моментами изменения состояния валкодеров. Естественно, это отдельный скетч, но здесь я его не привожу. Как программист я привык, что на одну строку кода основной программы (которая, собственно, и является выходным результатом программиста) приходится не менее десятка строк всевозможных дополнительных программ, предназначенных для отладки и тестирования кода, обработки входных и выходных данных, получения необходимой для работы основной программы статистики и пр. Так что приводить в дополнение к основным файлам еще и десяток-другой используемых для отладки и проверки скетчей не вижу смысла.
В общем, цикл измерения показал, что при некотором повышении напряжения питания минимальный интервал между импульсами снижается до 5 мс, а максимальный интервал всегда менее 400 мс - при меньшей скорости вращения колесо просто останавливается.
Но раз минимальный измеренный интервал 5 мс, а нужно иметь хоть небольшой запас, установим, что период между опросами ни при каких обстоятельствах не должен превышать 4 мс.
Много это или мало?
Опыт подсказывал, что время выполнения кода цикла опроса и анализа валкодеров не должно превышать нескольких сотен мкс. Пусть, максимум, - 1 мс. Но кроме этого есть дополнительные задачи, среди которых:
- управление моторами,
- опрос датчика линии (для данной конструкции робота не нужно, но - вдруг?),
- измерение расстояний при помощи ультразвукового дальномера,
- измерение расстояний при помощи инфракрасного дальномера,
- опрос датчиков положения: акселерометра, гироскопа, компаса...,
- анализ данных и принятие решения на изменение направления движения,
- вычисление местоположения и построение фрагмента карты,
- прием команд от ПДУ через ИК-датчик.
Датчик линии сильно тормозить не будет. Моторы - тоже. ИК-дальномер - чуть хуже: измерение напряжения на аналоговом входе само по себе занимает около 110 мкс, но тоже не критично. Самое неприятное - ИК-датчик, тем более, что по прерыванию это может случиться когда угодно, - читай "в самый неподходящий момент". Но, думается, многобайтовые данные передавать не следует, а прием 4 байтов команды не приведет к катастрофическим последствиям.
Опрос датчиков положения по I2C, как оказалось, процесс не быстрый. Но также должен уложиться в 1 мс. Картография, включая как расчет (с плавающей точкой, что очень медленно), так и сохранение данных в I2C EEPROM имеет смысл делать только в узловых точках, там, где робот меняет направление своего движения. Т.е. это сопряжено с остановкой, сканированием окружающего пространства посредством сервы и дальномера. Ну а если мы стоим, колеса не крутятся, валкодеры не генерируют импульсы, то время на выполнение операций ограничено исключительно емкостью батарей питания.
На самом деле, это может длиться десятки и даже сотни миллисекунд, но в покое это заведомо не приведет к ошибкам валкодеров.
Итак, остается только ультразвуковой дальномер. Для определенности будем считать, что за один проход цикла мы используем не более одного периферийного устройства: т.е. если запросили данные с акселерометра, то компас будем опрашивать только при следующем проходе цикла, подачу очередной команды на моторы - через один, а ИК-дальномер - через два... Датчик ПДУ, однако, в эту картину не вписывается, т.к. его нужно обрабатывать немедленно, когда бы пользователю, нажавшему на кнопку пульта, это ни вздумалось. Примерно оценим, что время стандартного прохода цикла + время обработки ИК-команды с датчика не будет превосходить 1.0-1.5 мс.
С учетом того, что у нас всего есть не более 4 мс, на ИК-дальномер приходится, максимум, 2.5 мс.
Скорость звука примем равной 340 м/с. При дальности действия дальномера 2.6 м звук должен пройти туда и обратно 5.2 м, что он может сделать за 16 мс. Для нас это неприемлемо много. Пусть мы принудительно ограничили таймаут 2.5 мс. За 2.5 мс звук пройдет 85 см. Т.е. максимальное расстояние, которое мы можем измерить, составит не более 42 см. Вряд ли это можно считать приемлемым.
Другими словами, на ходу пользоваться эхолотом нельзя. Вариант без прерываний, действительно, не подходит.
Валкодеры требуют два прерывания - по одному на каждое из колес. Еще одно прерывание нужно для ИК-датчика ПДУ. В Arduino UNO есть только 2 стандартных. Не хватает. Есть, правда, еще PCINT, но как-то в первом проекте, без опыта сразу переходить к нестандартным возможностям не хочется. Гораздо логичнее, как мне кажется, обеспечить аппаратную поддержку валкодеров.
Эксперименты с СИС
обычный режим счета, сигнал на входе А (на В - логическая 1)
обычный режим счета, сигнал на входе B (на А - логическая 1)
Начинал возиться с электроникой я еще в школе - лет 40 назад. Потом, правда, лет на 20 забросил. По тем временам схема на сотне корпусов СИС (микросхем средней степени интеграции) была существенно доступнее самой простенькой ЭВМ (которая стоила заведомо дороже автомобиля). Поэтому первое пришедшее на ум решение - по одному реверсивному счетчику на колесо. Плюс входные сдвиговые регистры - еще два с небольшим запасом. С учетом того, что к ним же можно подключить датчики линии. Итого четыре микросхемы.
Каждый из каналов валкодера выдает по 12 импульсов за оборот, примерно 22 с-1 при максимальной скорости. Наиболее доступны 4-разрядные счетчики. Делят на 16. Т.е. переполнение наступит только через 0.7 секунды. Правда, распознать после этого в какую сторону вращалось колесо, будет проблематично. Но при периоде опроса до 0.3 с никакой неопределенности не возникает. Значит, 16-разрядного счетчика вполне достаточно, чтобы суметь восстановить пройденный колесом путь. Младшие 5 разрядов длины пути при этом в точности будут повторять показания счетчика (4 - с выходов счетчика и 1 - с выхода_валкодера-входа_счетчика).
Для данной конструкции наиболее подходят счетчики К561ИЕ11/MC14516A, но у меня в хозяйстве таковых не оказалось. Нашелся К555ИЕ7 (фото в начале статьи). Почитав документацию, я решил использовать его в недокументированном режиме, посчитав, что если указано, что в момент прихода положительного фронта на "+1" вход "-1" должен быть установлен в логическую единицу и наоборот, то при наличии логического нуля счет просто не будет производиться. Но не тут то было! Оказалось, что если на противоположном входе вместо логической 1 установлен 0, то счетчик... перестает быть счетчиком, т.е. не "замораживает" выходы (чего бы я хотел), а переключает их по одному ему известной логике, не имеющей ничего общего с логикой счетчика.
На рисунках показаны эпюры напряжения на выходах валкодера (каналы А и В) и на выходах счетчика (Q0, Q1, Q2, Q3). В процессе измерений валкодеры эмулировал сам Ардуино. Он же анализировал выход счетчика и передавал его по последовательному порту на PC. Этот скетч также не приводится.
Ну да ладно, не получилось на ИЕ7, должно получиться на другой микросхеме. Но тут возникла следующая проблема - во сколько это обойдется... Советские счетчики стоят копейки - по 3-5 рублей за штуку. Если ехать за ними в Москву (а их еще нужно заранее заказать, т.к. они не относятся к широко распространенным товарам), то это потраченный день и ~500 рублей на дорогу. Заказывать через почту - еще минимум 300 рублей к стоимости заказа. Анализ предложения в И-нет магазинах Китая показал, что по одной штуке они продаются очень дорого - порядка тех же самых 500 рублей (с доставкой), правда, при партии в 20 штук цена на каждую падает более чем на порядок, но заказ все равно обойдется рублей в 800.
Т.е. либо плати 500 рублей и получай пару, либо - 800, и еще 18 штук останется в запасе. Но ведь я не собираюсь выпускать роботов серийно!
Не то чтобы 500-800 рублей было неподъемной суммой, но жаба сидит в каждом из нас. В общем, масштаб цен на неходовую элементную базу заставил меня искать альтернативу...
Контроллер тележки на Arduino
...и она быстро нашлась - Arduino Pro Mini за 104 рубля с доставкой.
Пришелец из Китая
В конце концов, пусть за "тележкой" следит один контроллер, который и будет выполнять всю "грязную" работу, а основной будет наблюдать за окружающей обстановкой, получать у "извозчика" показания счетчиков валкодеров и отдавать ему же приказы на управление моторами.
Т.е. кроме счетчиков валкодеров (кстати, полноразрядных - long, вместо жалких аппаратных 4-5 разрядов) контроллер тележки будет управлять двигателями. А заодно (чтобы не простаивал) измерять значение потребляемого роботом тока, напряжение на батареях питания (чтобы вовремя подать сигнал о необходимости их зарядки), считывать значение датчика линии, а также управлять индикаторными светодиодами и пьезоизлучателем.
Для измерения тока служит блок на LSC712, для измерения напряжения - делитель на резисторах. В обоих случаях измерительный вход шунтируется конденсатором для уменьшения шумов.
Оценки показывают, что все это должно укладываться в 1.0-1.5 мс, а потому данные с валкодеров будут считываться в цикле. Безо всяких прерываний. Две критичных вещи - ультразвуковой датчик и датчик ПДУ контроллером "тележки" опрашиваться не будут. Не будут им опрашиваться и датчики положения, т.к. все они работают по I2C, по этому же протоколу будут обмениваться между собой основной контроллер Ардуино с Ардуино-"извозчиком", а, как известно, в системе должен быть только один Мастер, каковым и будет основной Ардуино. "Извозчик" же будет простым Слейвом, и опрашивать другие слейв-устройства не сможет.
Принципиальная схема
На рисунке показана принципиальная схема, которая была использована при отладке.
В итоговом устройстве, вероятно, в качестве основного контроллера будет использован Ferduino Uno, а для тележки - Arduino Pro Mini, но при отладке (пока не прибыла посылка с Pro Mini) роль основного выполнял Arduino Uno R3, а контроллера тележки - Ferduino Uno.
Особенность Mini - слабомощный стабилизатор питания. Если при отладке вся периферия запитывалась с платы Ferduino (а по сути - от USB), то в рабочем образце нужно будет ставить дополнительный стабилизатор. Я еще не решил, каким он будет, линейным или импульсным, но от него будут запитана вся периферия тележки (валкодеры, логическая часть драйвера мотора, схемы амперметра, датчики линии и светодиоды), а также сервопривод. Именно сервопривод, потребляющий основной ток и работающий в импульсном режиме, и заставляет склоняться в пользу линейного стабилизатора, не потребляющего тока в режиме без нагрузки.
Вся I2C периферия (акселерометр, гироскоп, компас, EEPROM, OLED дисплей), а также датчик ПДУ и дальномеры будут запитаны от стабилизатора основного контроллера.
Датчики тока, напряжения и буззер
Набор функций тележки
Итак, контроллер тележки должен управлять двигателями. Из одной темы настоящего форума я почерпнул информацию о том, что наибольшая погрешность положения (а заодно неконтролируемый разворот вокруг оси) возникает в моменты резкого старта и торможения, когда колеса проскальзывают. Ну и потребление тока в эти моменты возрастает более чем на порядок. А при резкой переполюсовке (смены направления движения с "вперед" на "назад" или наоборот) сила тока возрастает еще вдвое по сравнению со стартовым током.
Чтобы убить сразу двух зайцев, я решил сделать в качестве основных режимов плавный разгон и плавную остановку. Точнее, ступенчатую: величина напряжения (ШИМ), подаваемая на каждый из двигателей, меняется каждые 15 мс ступеньками не более 32 единиц в нужную сторону, пока не достигнет требуемого значения. Т.е. "переполюсовка" выглядит так:
было : 255
0 мс : 223
15 мс : 191
30 мс : 159
45 мс : 127
60 мс : 95
75 мс : 63
90 мс : 31
105 мс : -1
120 мс : -33
135 мс : -65
150 мс : -97
165 мс : -129
180 мс : -161
195 мс : -193
210 мс : -225
225 мс : -255
дальше: -255
Т.к. точного сдвига фаз между датчиками валкодеров добиться не удалось, погрешность положения фронтов оказалась довольно высокой. Собственно, валкодер в каждом канале формирует по 12 импульсов на оборот, т.е. 12 передних и 12 задних фронтов - всего по 24 изменения. Оба вместе - 48, но они за счет погрешности положения фронтов расположены не совсем равномерно. Да и эксцентриситет колес вносит свою лепту в погрешность. В общем, суммарную погрешность я оцениваю порядка 90 градусов валкодера. В идеале погрешность не превышает половину единицы младшего разряда (ошибка дискретизации), в среднем - четверть. У нас - порядка единицы младшего разряда. Тоже не так плохо. Для оценки расстояния. А для оценки скорости - никуда не годится. По сути, погрешность скорости получается порядка величины самой скорости. Чтобы существенно уменьшить погрешность, скорость вычисляется по сумме 4-х последних изменений состояний валкодеров, т.е. по 360 градусов их цикла. Таким образом, мы убираем погрешность от сдвига фаз между валкодерами не равного 90 градусов и от того, что скважность импульсов отличается от 2. Остаются лишь погрешности, связанные с полным оборотом, в частности, из-за эксцентриситета колеса и неточности изготовления отдельных зубцов. В принципе, можно было бы усреднить и по 48 отсчетам, но тогда получилась бы слишком большая задержка в определении скорости - в среднем на полоборота колеса, т.е. на 66 мм пути. Тогда как сам путь измеряется в единицах равных 2.75 мм.
Измерение любого аналогового сигнала - длительная операция (что связано со спецификой работы АЦП), кроме того, сопряженная со значительным уровнем шумов. Поэтому предусмотрены режимы работы как с проведением измерений, так и без них. Соответственно - команды включения и выключения режима измерения каждой величины по отдельности. Для уменьшения уровня шума применены как аппаратные средства (шунтирующий конденсатор), так и программные - усредняется значение 16 измерений. Одновременно такое усреднение позволяет увеличить чувствительность датчиков, т.к. по сути, производится не усреднение, а простое суммирование, т.е. выходная величина лежит уже не в диапазоне от 0 до 1023, а в диапазоне от 0 до 1023 * 16 = 16368. Т.е. результат получается не 10-, а 14-разрядным.
Если вдруг случайно будет пропущен один фронт одного из импульсов валкодеров, в некоторых случаях это можно детектировать (собственно, во всех случаях, когда пропущена ровно один фронт). Количество таких пропусков также подсчитывается для каждого из колес. Но в режиме нормальной работы пока ни одной ошибки зафиксировано не было. Но проверка осталась.
Если колеса не движутся, импульсы с валкодеров не поступают, состояния не меняются. А скорость вычисляется по 4-м последним интервалам смены состояний. Чтобы избежать ошибок в вычислении скорости (например, после часа стоянки будет возвращена скорость непосредственно перед остановкой), введены таймауты. Величина таймаута выбрана равной 400 мс, т.к. в процессе измерений выяснилось, что колесо скорее остановится совсем, чем интервал между сменами состояния достигнет такой величины. Количество таймаутов также подсчитывается. Кстати, можно вычислить _время_простоя_ = _количество_таймаутов_ * _длительность_таймаута_.
Команды передаются по протоколу I2C. Каждая команда - один байт. Если это команда управления, за ней может следовать несколько байтов данных. Команды запроса данных не передают байты данных от Мастера Слейву - только код команды. Но после этого Мастер должен прочитать какое-то количество байтов от Слейва. Во всех командах, где присутствуют данные, связанные с колесами, первыми следуют данные, относящиеся к левому колесу.
В процессе того, как "крутится" цикл, подводится статистика: распределение длительности выполнения цикла в 20 мкс единицах. Средняя длительность цикла без измерения аналоговых величин составляет около 300 мкс, если при каждом проходе цикла измерять обе величины, время выполнения цикла примерно удваивается. В случае, если Мастер запрашивает у Слейва данные, последний отдает их по прерываниям, т.е. этот процесс приостанавливает выполнение цикла. Верхняя оценка для времени приостановки цикла - 650 мкс. В случае, если Мастер запрашивает у Слейва данные, последний отдает их по прерываниям, т.е. этот процесс приостанавливает выполнение цикла. Верхняя оценка для времени приостановки цикла - 650 мкс. Но она разбивается на два фрагмента, каждый из которых вызывается по прерыванию: одно - для чтения кода команды, а другое - для передачи данных.
Тележка поддерживает следующие команды (в скобках - количество байтов данных, передаваемых от Мастера Слейву):
0x1a - плавно установить скорость левого мотора, (int - 2 байта),
0x2e - плавно установить скорость правого мотора, (int - 2 байта),
0x3a - мгновенно установить скорость левого мотора, (int - 2 байта),
0x4e - мгновенно установить скорость правого мотора, (int - 2 байта),
0x5b - плавно установить скорость двух моторов, (2 int - 4 байта),
0x6b - мгновенно установить скорость двух моторов (2 int - 4 байта),
0xc8 - разрешить измерение силы тока, (0 байтов),
0xc9 - запретить измерение силы тока, (0 байтов),
0xd8 - разрешить измерение напряжения, (0 байтов),
0xd9 - запретить измерение напряжения, (0 байтов),
0xe9 - установить режим работы пьезоизлучателя, (1 байт),
0xf9 - установить режим работы одного из светодиодов, (1 байт),
Команды запроса состояния (в скобках - количество байтов данных, передаваемых от Слейва Мастеру):
0x10 - путь обоих колес с учетом направления (5м и -3м = 2м), (2 long - 8 байтов),
0x30 - модуль пути обоих колес (5м и -3м = 8м), (2 long - 8 байтов),
0x43 - скорость обоих моторов, (2 int - 4 байта),
0x80 - количество ошибок обоих валкодеров, (2 long - 8 байтов),
0x81 - мин. и макс. интервалы между фронтами для обоих валкодеров, мс (4 int - 8 байтов),
0x82 - количество таймаутов для обоих колес, (2 long - 8 байтов),
0x90-0x9f - распределение длительности основного цикла в 20 мкс единицах, выдается порциями по 32 байта,
0xc2 - запрос силы тока (если измерение выключено, возвращает -1), (int - 2 байта),
0xd2 - запрос напряжения батареи (если выключено, возвращает -1), (int - 2 байта),
0xf0 - запрос состояния датчиков линии (1 байт, из которого используются 3 младшие бита).
Исходники
Приведены скетчи и библиотечные функции отладочного варианта.
Motor_Dolly_Control_03.ino - скетч основного контроллера (Мастер).
// Master - управление тележкой (dolly) // на время отладки программируется UNO R3 через COM3 - master #include <Wire.h> #define I2C_WR_Delay_mcs 200 // задержка между записью команды на чтение и самим чтением (>=175) #define I2C_Ask_Interval 300 // задержка между отдельными запросами к Слейву #define MKSinterval 20 // интервал измерений DOLLY в микросекундах long PrevState = 0; // для отслеживания перехода в новое состояние bool RegularPrint = true; void setup() { Wire.begin(); Serial.begin(115200); Serial.println("Start Master"); TWBR = 12; // set 400MHz I2C } void loop() { char ch, ch2; int v; if (Serial.available() > 0) { ch = Serial.read(); switch (ch){ case 'l': v = Serial.parseInt(); SetMotorLeft(v, true); break; case 'L': v = Serial.parseInt(); SetMotorLeft(v, false); break; case 'r': v = Serial.parseInt(); SetMotorRight(v, true); break; case 'R': v = Serial.parseInt(); SetMotorRight(v, false); break; case 'b': v = Serial.parseInt(); SetMotors(v, v, true); break; case 'B': v = Serial.parseInt(); SetMotors(v, v, false); break; case 'p': // long statistic case 'P': PrintStat(); break; case 's': // toggle Regular Print case 'S': RegularPrint = !RegularPrint; break; case 'e': // LED control case 'E': v = Serial.parseInt(); SetLed(v); break; case 'z': // Buzzer control case 'Z': v = Serial.parseInt(); SetBuzzer(v); break; case 'i': // измерение тока case 'I': // Serial.print("Available: "); // Serial.println(Serial.available()); delayMicroseconds(80); // !!! при 70 зафиксирован 1 отказ на 10 запросов ch2 = Serial.read(); Serial.print(" input "); Serial.print(ch); Serial.print(" and "); Serial.println(ch2); if((ch2 == 'g') or (ch2 == 'G')) { v = Get1int(0xc2); Serial.println(v); Serial.print("Electric Current "); Serial.print(((long)(v - 511*16))*237 / 100); Serial.println(" mA"); } else if((ch2 == 'p') or (ch2 == 'P')) SendCommand(0xc8); else if((ch2 == 'f') or (ch2 == 'F')) SendCommand(0xc9); break; case 'v': // измерение напряжения case 'V': delayMicroseconds(80); ch2 = Serial.read(); if((ch2 == 'g') or (ch2 == 'G')) { v = Get1int(0xd2); Serial.println(v); Serial.print("Voltage "); Serial.print(((long)v) *1000 / 3274); Serial.print(" mV "); Serial.println(((long)v) *1715 / (3274)); } else if((ch2 == 'p') or (ch2 == 'P')) SendCommand(0xd8); else if((ch2 == 'f') or (ch2 == 'F')) SendCommand(0xd9); break; // case 't': // case 'T': // dolly.PrintDT(); // break; } } long State = millis()/2000; if((PrevState != State) && RegularPrint) { long Start_ms = micros(); // запрос энкодеров (8 байтов) long LeftCounter, RightCounter; Get2long(0x10, LeftCounter, RightCounter); long ms_0 = micros() - Start_ms; Serial.print("LC: "); Serial.print(LeftCounter); Serial.print(", RC: "); Serial.print(RightCounter); long ms_1 = micros() - Start_ms; // запрос ABS энкодеров (8 байтов) delayMicroseconds(I2C_Ask_Interval); long ms_2 = micros() - Start_ms; long LeftAbs, RightAbs; Get2long(0x30, LeftAbs, RightAbs); long ms_3 = micros() - Start_ms; Serial.print(", LA: "); Serial.print(LeftAbs); Serial.print(", RA: "); Serial.print(RightAbs); long ms_4 = micros() - Start_ms; // запрос скоростей (4 байта) delayMicroseconds(I2C_Ask_Interval); long ms_5 = micros() - Start_ms; int LeftVelocity, RightVelocity; Get2int(0x43, LeftVelocity, RightVelocity); long ms_6 = micros() - Start_ms; Serial.print(", LV: "); Serial.print(LeftVelocity); Serial.print(", RV: "); Serial.print(RightVelocity); long ms_7 = micros() - Start_ms; // запрос количества ошибок (8 байтов) delayMicroseconds(I2C_Ask_Interval); long ms_8 = micros() - Start_ms; long LeftErrors, RightErrors; Get2long(0x80, LeftErrors, RightErrors); long ms_9 = micros() - Start_ms; Serial.print(", LE: "); Serial.print(LeftErrors); Serial.print(", RE: "); Serial.print(RightErrors); long ms_a = micros() - Start_ms; // запрос количества таймаутов (8 байтов) delayMicroseconds(I2C_Ask_Interval); long ms_b = micros() - Start_ms; long LeftTimeOut, RightTimeOut; Get2long(0x82, LeftTimeOut, RightTimeOut); long ms_c = micros() - Start_ms; Serial.print(", LT: "); Serial.print(LeftTimeOut); Serial.print(", RT: "); Serial.print(RightTimeOut); long ms_d = micros() - Start_ms; // запрос состояния датчиков линии (1 байт) delayMicroseconds(I2C_Ask_Interval); long ms_e = micros() - Start_ms; byte line; Get1byte(0xf0, line); long ms_f = micros() - Start_ms; Serial.print(", Line: "); for(int i = 0; i <8; i++) Serial.print((line >> i) & 1); long ms_g = micros() - Start_ms; Start_ms = micros() - Start_ms; Serial.print(", Time: "); Serial.print(Start_ms); Serial.print(' '); Serial.print(ms_0); Serial.print(' '); Serial.print(ms_1 - ms_0); Serial.print(' '); Serial.print(ms_2 - ms_1); Serial.print(' '); Serial.print(ms_3 - ms_2); Serial.print(' '); Serial.print(ms_4 - ms_3); Serial.print(' '); Serial.print(ms_5 - ms_4); Serial.print(' '); Serial.print(ms_6 - ms_5); Serial.print(' '); Serial.print(ms_7 - ms_6); Serial.print(' '); Serial.print(ms_8 - ms_7); Serial.print(' '); Serial.print(ms_9 - ms_8); Serial.print(' '); Serial.print(ms_a - ms_9); Serial.print(' '); Serial.print(ms_b - ms_a); Serial.print(' '); Serial.print(ms_c - ms_b); Serial.print(' '); Serial.print(ms_d - ms_c); Serial.print(' '); Serial.print(ms_e - ms_d); Serial.print(' '); Serial.print(ms_f - ms_e); Serial.print(' '); Serial.println(ms_g - ms_f); PrevState = State; } } void PrintStat() // статистика { union { long buffLong[8]; byte buffByte[32]; }; for(byte i = 0; i < 8; i++) { Get32bytes(0x90 + i, buffByte); for(int j = 0; j < 8; j++) { Serial.print("Range to \t"); Serial.print(MKSinterval*(i*8+j+1)); Serial.print("\tCount:\t"); Serial.println(buffLong[j]); } } int MinDleft, MaxDleft, MinDright, MaxDright; Get4int(0x81, MinDleft, MaxDleft, MinDright, MaxDright); Serial.print(" LD: "); Serial.print(MinDleft); Serial.print(" - "); Serial.print(MaxDleft); Serial.print(", RD: "); Serial.print(MinDright); Serial.print(" - "); Serial.println(MaxDright); } void SendCommand(byte Command) { Wire.beginTransmission(4); // transmit to device #4 Wire.write(Command); // sends command Wire.endTransmission(); // stop transmitting Serial.print("Command: "); Serial.println(Command, HEX); } void Get2long(byte Command, long& Val_L, long& Val_R) { Wire.beginTransmission(4); // transmit to device #4 Wire.write(Command); // sends command Wire.endTransmission(); // stop transmitting delayMicroseconds(I2C_WR_Delay_mcs); Wire.requestFrom(4, 8); // request 8 bytes from slave device #4 Val_L = Wire.read() + ((long)Wire.read() << 8) + ((long)Wire.read() << 16) + ((long)Wire.read() << 24); Val_R = Wire.read() + ((long)Wire.read() << 8) + ((long)Wire.read() << 16) + ((long)Wire.read() << 24); } int Get1int(byte Command) { Wire.beginTransmission(4); // transmit to device #4 Wire.write(Command); // sends command Wire.endTransmission(); // stop transmitting delayMicroseconds(I2C_WR_Delay_mcs); Wire.requestFrom(4, 2); // request 4 bytes from slave device #4 return Wire.read() + ((int)Wire.read() << 8); } void Get2int(byte Command, int& Val_L, int& Val_R) { Wire.beginTransmission(4); // transmit to device #4 Wire.write(Command); // sends command Wire.endTransmission(); // stop transmitting delayMicroseconds(I2C_WR_Delay_mcs); Wire.requestFrom(4, 4); // request 4 bytes from slave device #4 Val_L = Wire.read() + ((int)Wire.read() << 8); Val_R = Wire.read() + ((int)Wire.read() << 8); } void Get4int(byte Command, int& Val_0, int& Val_1, int& Val_2, int& Val_3) { Wire.beginTransmission(4); // transmit to device #4 Wire.write(Command); // sends command Wire.endTransmission(); // stop transmitting delayMicroseconds(I2C_WR_Delay_mcs); Wire.requestFrom(4, 8); // request 8 bytes from slave device #4 Val_0 = Wire.read() + ((int)Wire.read() << 8); Val_1 = Wire.read() + ((int)Wire.read() << 8); Val_2 = Wire.read() + ((int)Wire.read() << 8); Val_3 = Wire.read() + ((int)Wire.read() << 8); } void Get32bytes(byte Command, byte buff[32]) { Wire.beginTransmission(4); // transmit to device #4 Wire.write(Command); // sends command Wire.endTransmission(); // stop transmitting delayMicroseconds(I2C_WR_Delay_mcs); Wire.requestFrom(4, 32); // request 32 bytes from slave device #4 for(int i = 0; i < 32; i++) buff[i] = Wire.read(); } void Get1byte(byte Command, byte& buff) { Wire.beginTransmission(4); // transmit to device #4 Wire.write(Command); // sends command Wire.endTransmission(); // stop transmitting delayMicroseconds(I2C_WR_Delay_mcs); Wire.requestFrom(4, 1); // request 32 bytes from slave device #4 buff = Wire.read(); } ///////////////////////////////////////////////////// // команды чтения - команды запроса информации ///////////////////////////////////////////////////// // команды записи - команды управления моторами void SetMotors(int Left, int Right, bool Slowly) { Wire.beginTransmission(4); // transmit to device #4 Serial.print("Motors: "); Serial.print(Left); Serial.print(", "); Serial.println(Right); if (Slowly) Wire.write(0x5b); // sends five bytes else Wire.write(0x6b); // sends five bytes Wire.write(Left >> 8); Wire.write(Left & 0xff); Wire.write(Right >> 8); Wire.write(Right & 0xff); Wire.endTransmission(); // stop transmitting Serial.println("Motors OK!"); } void SetMotorLeft(int Left, bool Slowly) { Wire.beginTransmission(4); // transmit to device #4 Serial.print("Left: "); Serial.println(Left); if (Slowly) Wire.write(0x1a); // sends five bytes else Wire.write(0x3a); // sends five bytes Wire.write(Left >> 8); Wire.write(Left & 0xff); Wire.endTransmission(); // stop transmitting Serial.println("Left OK!"); } void SetMotorRight(int Right, bool Slowly) { Wire.beginTransmission(4); // transmit to device #4 Serial.print("Right: "); Serial.println(Right); if (Slowly) Wire.write(0x2e); // sends five bytes else Wire.write(0x4e); // sends five bytes Wire.write(Right >> 8); Wire.write(Right & 0xff); Wire.endTransmission(); // stop transmitting Serial.println("Right OK!"); } void SetLed(int v) // { byte LED = v / 10; byte mode = v % 10; Wire.beginTransmission(4); // transmit to device #4 Serial.print("LED: "); Serial.print(LED); Serial.print(", mode: "); Serial.println(mode); Wire.write(0xf9); // sends 1 byte Wire.write((constrain(LED, 0, 3) << 6) + mode); Wire.endTransmission(); // stop transmitting Serial.println("LED OK!"); } void SetBuzzer(int v) // { byte buzzer = v / 10; byte mode = v % 10; Wire.beginTransmission(4); // transmit to device #4 Serial.print("Buzzer freq : "); Serial.print(buzzer); Serial.print(", mode: "); Serial.println(mode); Wire.write(0xe9); // sends 1 byte Wire.write((constrain(buzzer, 0, 3) << 6) + mode); Wire.endTransmission(); // stop transmitting Serial.println("Buzzer OK!"); }Motor_Dolly_03.ino - скетч контроллера тележки (Слейв). В начале работы самостоятельно некоторое время крутит колеса в разные стороны, после отладки это нужно убрать.
// Slave - контроллер тележки (dolly) // на время отладки программируется FERDUINO через COM4 - Slave (в дальнейшем Pro Micro) #include "Dolly.h" #include <Wire.h> #define MotorLeft_0 4 #define MotorLeft_1 5 #define MotorRight_0 7 #define MotorRight_1 6 #define ValcoderLeft_0 11 #define ValcoderLeft_1 10 #define ValcoderRight_0 9 #define ValcoderRight_1 8 #define LineLeft 3 #define LineMiddle 2 #define LineRight 12 #define BuzzerPin A3 #define LEDpin_0 13 #define LEDpin_1 A2 // #define LEDpin_2 A6 // #define LEDpin_3 A7 #define ElCurrentPin A0 #define VoltagePin A1 int OldState = -1; long Counter = 0; bool DoPrintShortStat = false; void setup() { Wire.begin(4); // join i2c bus with address #4 Wire.onReceive(receiveEvent_2); // register event Wire.onRequest(requestEvent_2); // register event Serial.begin(115200); dolly.MotorsInit(MotorLeft_0, MotorLeft_1, MotorRight_0, MotorRight_1); dolly.ValcodersInit(ValcoderLeft_0, ValcoderLeft_1, ValcoderRight_0, ValcoderRight_1); dolly.LineSensorInit(LineLeft, LineMiddle, LineRight); // line sensor pins Serial.setTimeout(1); dolly.BlinkInit(0, LEDpin_0); dolly.BlinkInit(1, LEDpin_1); dolly.BuzzerInit(BuzzerPin); dolly.ElCurrentInit(ElCurrentPin, false); // dolly.VoltageInit(VoltagePin, false); // } void receiveEvent_2(int howMany) { dolly.receiveEvent(howMany); }; // процедура приема данных от мастера по I2C void requestEvent_2() { dolly.requestEvent(); }; // процедура передачи данных мастеру по I2C void loop() { int State = millis()/1000; if(State != OldState) { OldState = State; if (State == 0){ dolly.Motors(255,255); // L+ R+ } else if (State == 1){ dolly.Motors(0,0); // } else if (State == 2){ dolly.Motors(-255,-255); // L- R- } else if (State == 3){ dolly.Motors(0,0); // } else if (State == 4){ dolly.Motors(255,-255); // L+ R- } else if (State == 5){ dolly.Motors(0,0); // } else if (State == 6){ dolly.Motors(-255,255); // L- R+ } else if (State == 7){ dolly.Motors(0,0); // } else if (State == 8){ dolly.PrintDT(); } else if (State > 8) { char ch; int v; if (Serial.available() > 0) { ch = Serial.read(); switch (ch){ case 'l': case 'L': v = Serial.parseInt(); dolly.Motors(v, 0); break; case 'r': case 'R': v = Serial.parseInt(); dolly.Motors(0, v); break; case 'b': case 'B': v = Serial.parseInt(); dolly.Motors(v, v); break; case 'p': case 'P': PrintShortStat(); break; case 's': case 'S': DoPrintShortStat = !DoPrintShortStat; break; case 't': case 'T': dolly.PrintDT(); break; } } if(DoPrintShortStat){ PrintIntervals(); } } } dolly.Run(); // if(((Counter % 2000) == 0) && (State < 8)){ // PrintShortStat(); // } Counter++; } void PrintShortStat() { Serial.print(Counter); Serial.print("\tLC\t"); Serial.print(dolly.GetCounterLeft()); Serial.print("\tRC\t"); Serial.print(dolly.GetCounterRight()); Serial.print("\tLV\t"); Serial.print(dolly.GetVelocityLeft()); Serial.print("\tRV\t"); Serial.print(dolly.GetVelocityRight()); Serial.print("\tLE\t"); Serial.print(dolly.GetErrorsLeft()); Serial.print("\tRE\t"); Serial.println(dolly.GetErrorsRight()); } void PrintIntervals() { Serial.print("LV\t"); Serial.print(dolly.GetVelocityLeft()); Serial.print("\tRV\t"); Serial.print(dolly.GetVelocityRight()); int data[4] = {0}; dolly.GetIntervalsLeft(data); for(int i = 0; i < 4; i++) { Serial.print("\t"); Serial.print(data[i]); } Serial.print("\t"); Serial.print(data[0] + data[1] + data[2] + data[3]); dolly.GetIntervalsRight(data); for(int i = 0; i < 4; i++) { Serial.print("\t"); Serial.print(data[i]); } Serial.print("\t"); Serial.print(data[0] + data[1] + data[2] + data[3]); Serial.println(); }dolly.h - заголовочный файл библиотеки контроллера тележки.
#include <Arduino.h> #include <Wire.h> #define MaxValcoderTimeInterval 400 // если интервал между отсчетами валкодера больще, скорость считается = 0 (макс зафиксированое знечение 391) class Dolly { // все, касающееся управлением тележкой public: void MotorsInit(byte Left_0, byte Left_1, byte Right_0, byte Right_1); // pin numbers, Left_1 & Right_1 must be PWM void ValcodersInit(byte Left_0, byte Left_1, byte Right_0, byte Right_1); void Motors(int Left, int Right, bool Slowly = true); // set motors Velocity: Left, Right from -255 to 255 void MotorLeft(int Left, bool Slowly = true); // set motors Velocity: from -255 to 255 void MotorRight(int Right, bool Slowly = true); // set motors Velocity: from -255 to 255 void Run(); // each cycle routine for Motors & Valcoders void PrintDT(); // вывод статиcтики в Serial int GetVelocityLeft() {return 10996*DirectionLeft/(ValIntLeft[0] + ValIntLeft[1] + ValIntLeft[2] + ValIntLeft[3]); }; // скорость усредненная по 4 последним отсчетам int GetVelocityRight() {return 10996*DirectionRight/(ValIntRight[0] + ValIntRight[1] + ValIntRight[2] + ValIntRight[3]); }; // long GetCounterLeft() {return ValcoderCounterLeft; }; // пройденный путь в отсчетах валкодера ~2.75 мм long GetCounterRight() {return ValcoderCounterRight; }; // (со знаком: 100 вперед + 100 назад = 0) long GetCounterAbsLeft() {return ValcoderCounterAbsLeft; }; // пройденный путь в отсчетах валкодера ~2.75 мм long GetCounterAbsRight() {return ValcoderCounterAbsRight; }; // (абсолютный: 100 вперед + 100 назад = 200) long GetErrorsLeft() {return ValcoderErrorsLeft; }; // происходят из-за пропуска переходов long GetErrorsRight() {return ValcoderErrorsRight; }; // void GetIntervalsLeft(int data[4]) { for(int i; i < 4; i++) data[i] = ValIntLeft[i]; }; // интервалы в мс между изменением состояния void GetIntervalsRight(int data[4]) { for(int i; i < 4; i++) data[i] = ValIntRight[i]; }; // 4 последних void GetValStates(byte states[16], long times[16]) { for(int i; i < 4; i++) {states[i] = ValState[i]; times[i] = ValTime[i];} }; // запрос последних 16 состояний и времен их смены void LineSensorInit(byte Left, byte Middle, byte Right); // line sensor pins (Black - 1, White - 0) byte GetLineSensor() // D0 - Left, D1 - Middle, D2 - Right { return digitalRead(LineSensorLeft) | (digitalRead(LineSensorMiddle) << 1) | (digitalRead(LineSensorRight) << 2);}; void receiveEvent(int howMany); // процедура приема данных от мастера по I2C void requestEvent(); // процедура передачи данных мастеру по I2C void BlinkInit(byte LED, byte pin); // инициализация светодиодов (от 0 до 3) void SetBlinkMode(byte LED, byte mode); // настройка мигания светодиодов void BuzzerInit(byte pin); // инициализация буззера void SetBuzzerMode(byte mode); // гастройка режима буззера: 0 - нет звука void ElCurrentInit(byte pin, bool permit); // void PermitElCurrent() { DoElCurrentMeasure = true; }; // разрешить измерение эл. тока void ForbidElCurrent() { DoElCurrentMeasure = false; }; // запретить измерение эл. тока int GetElCurrent(); // возвращает силу тока в 14-разрядной величине (от 0 до 1023*16, середина 511*16) void VoltageInit(byte pin, bool permit); // void PermitVoltage() { DoVoltageMeasure = true; }; // разрешить измерение напряжения void ForbidVoltage() { DoVoltageMeasure = false; }; // запретить измерение напряжения int GetVoltage(); // возвращает напряжение в 14-разрядной величине (от 0 до 1023*16, середина 511*16) private: void request90(); // фрагмент ответа на запрос статистики (команды 0x90-0x97) bool ValcodersInited = false; void SetMotors(byte left_0, byte left_1, byte right_0, byte right_1); // низкоуровневая установка моторов void SetMotors_0(int Left, int Right, bool Slowly); // общее для команд правого, левого и двух моторов byte MotorLeftA, MotorLeftB, MotorRightA, MotorRightB; // пины управления моторами long PrevMotorTime = 0; // время в мс, когда произошло последнее изменение состояния моторов int PrevMotorLeft = 0, PrevMotorRight = 0; // [-255...255] скорости моторов на момент PrevTime int TargetLeft = 0, TargetRight = 0; // к какому состоянию должны стремиться моторы byte ValcoderLeftA, ValcoderLeftB, ValcoderRightA, ValcoderRightB; // пины управления валкодерами long PrevValcoderTimeLeft = 0, PrevValcoderTimeRight = 0; // время в мс, когда произошло последнее изменение состояния byte PrevValcoderLeft = 0, PrevValcoderRight = 0; // bit 0 - A, bit 1 - B long ValcoderCounterLeft = 0, ValcoderCounterRight = 0; // путь, пройденный валкодерами (со знаком) long ValcoderErrorsLeft = 0, ValcoderErrorsRight = 0; int ValIntLeft[4] = {MaxValcoderTimeInterval}, ValIntRight[4] = {MaxValcoderTimeInterval}; // последние 4 интервала в мс long ValcoderCounterAbsLeft = 0, ValcoderCounterAbsRight = 0; // путь, пройденный валкодерами (абсолютное значение) long ValcoderTimeOutLeft =0, ValcoderTimeOutRight = 0; // количество таймаутов = 0.4сек*_время_остановки_ byte ValState[16] = {0}; // последние 8 состояний валкодера long ValTime[16] = {0}; // моменты времени в мс на последние 8 смен состояний валкодера long ChangeStateCounter = 0; // счетчик смен состояний валкодеров (должен быть равен ValcoderCounterAbsLeft + ValcoderCounterAbsRight) int DirectionLeft = 0, DirectionRight = 0; // [-1,0,1] - направление движения byte LastCommand = 0; // сюда (receiveEvent) записывается команда чтения, используется и обнуляется (requestEvent) // для статистики long StatDT[64] = {0}; long PrevCallTime; // для статистики long MaxDeltaTime = 0; // !!! пока не используется int MinDLeft = 32000, MinDRight = 32000; int MaxDLeft = 0, MaxDRight = 0; long Counter = 0; // считает количество вызовов RUN // индикация (звук и свет) byte LineSensorLeft, LineSensorMiddle, LineSensorRight; byte LEDmode[4] = {0}; // режим мигания светодиодов: byte LEDpin[4] = {127}; // пины контроллера, к которым подключены светодиоды byte BuzzerMode = 0; // режим буззера byte BuzzerShift = 0; // на сколько бит сдdгать частоту RUN для получения звуковой частоты byte BuzzerPin = 127; // измерение аналоговых величин byte ElCurrentPin = 127; byte VoltagePin = 127; int ElCurrent[16] = {0}; // значения тока в 16 последовательных измерениях int Voltage[16] = {0}; // значения напряжения в 16 последовательных измерениях bool DoElCurrentMeasure = false; // следуеит ли измерять ток bool DoVoltageMeasure = false; // следует ли измерять напряжение }; extern Dolly dolly;dolly.cpp - исполняемый код библиотеки контроллера тележки.
// на время отладки программируется UNO R3 через COM3 - master // #define DOLLY_DEBUG_COMMAND // #define DOLLY_DEBUG_MOTORS // #define DOLLY_DEBUG_INIT #include "dolly.h" #define TimeConstant 15 // 15 // шаг по времени по которому осуществляется изменение моторов в режиме Slowly #define VelocityStep 32 // максимальное приращение скорости моторов за шаг #define MKSinterval 20 // интервал измерений в микросекундах #define MaxOUTmodes 9 // максимальное количество режимов мигания светодиодов byte OUTmodes[] = {0, 0xAA, 0b11001100, 0b11110000, 0b10011100, 0b10101000, 0b10100000, 1, 127, 255}; #define InitialShift 1 // делитель для получения из частоты вызова RUN высшей частотыи буззера //class Dolly { // все, касающееся управлением тележкой //public: void Dolly::MotorsInit(byte Left_0, byte Left_1, byte Right_0, byte Right_1) // pin numbers, Left_1 & Right_1 must be PWM { MotorLeftA = Left_0; MotorLeftB = Left_1; MotorRightA = Right_0; MotorRightB = Right_1; pinMode(MotorLeftA, OUTPUT); pinMode(MotorLeftB, OUTPUT); pinMode(MotorRightA, OUTPUT); pinMode(MotorRightB, OUTPUT); digitalWrite(MotorLeftA, 0); analogWrite(MotorLeftB, 0); digitalWrite(MotorRightA, 0); analogWrite(MotorRightB, 0); PrevMotorTime = -TimeConstant; TWBR = 12; // set 400MHz I2C #ifdef DOLLY_DEBUG_INIT Serial.print("Motors Init\t"); Serial.print(MotorLeftA); Serial.print("\t"); Serial.print(MotorLeftB); Serial.print("\t"); Serial.print(MotorRightA); Serial.print("\t"); Serial.println(MotorRightB); #endif } void Dolly::ValcodersInit(byte Left_0, byte Left_1, byte Right_0, byte Right_1) { ValcoderLeftA = Left_0; ValcoderLeftB = Left_1; ValcoderRightA = Right_0; ValcoderRightB = Right_1; PrevValcoderLeft = (digitalRead(ValcoderLeftB) << 1) | digitalRead(ValcoderLeftA); PrevValcoderRight = (digitalRead(ValcoderRightB) << 1) | digitalRead(ValcoderRightA); // bit 0 - A, bit 1 - B long t = millis(); PrevValcoderTimeLeft = t; PrevValcoderTimeRight = t; ValcodersInited = true; #ifdef DOLLY_DEBUG_INIT Serial.print("Valcoders Init\t"); Serial.print(ValcoderLeftA); Serial.print("\t"); Serial.print(ValcoderLeftB); Serial.print("\t"); Serial.print(ValcoderRightA); Serial.print("\t"); Serial.println(ValcoderRightB); Serial.print("Valcoder Left\t"); Serial.print(PrevValcoderLeft, BIN); Serial.print("\tRight\t"); Serial.println(PrevValcoderRight, BIN); #endif } void ConvertVelocity2Controls(int Velocity, byte &out_0, byte &out_1) // преобразует вхолдную величину в управляющие команды моторов { if(Velocity < 0) { out_1 = - Velocity; // 255 + Velocity; out_0 = 0; // 1; } else if(Velocity == 0) { out_1 = 0; out_0 = 0; } else // Velocity > 0 { out_1 = 255 - Velocity; // Velocity; out_0 = 1; // 0; } } void PrintLLRR(byte left_0, byte left_1, byte right_0, byte right_1, long Prev) { Serial.print("Motors\t"); Serial.print(left_0); Serial.print("\t"); Serial.print(left_1); Serial.print("\t"); Serial.print(right_0); Serial.print("\t"); Serial.print(right_1); Serial.print("\t"); Serial.print(millis()); Serial.print("\tPrev:\t"); Serial.println(Prev); } void PrintLR(int left, int right, long Prev) { Serial.print("Motors\t"); Serial.print(left); Serial.print("\t"); Serial.print(right); Serial.print("\t\t\t"); Serial.print(millis()); Serial.print("\tPrev:\t"); Serial.println(Prev); } void Dolly::SetMotors(byte left_0, byte left_1, byte right_0, byte right_1) { #ifdef DOLLY_DEBUG_MOTORS Serial.print("SetMotors\t"); Serial.print(left_0); Serial.print("\t"); Serial.print(left_1); Serial.print("\t"); Serial.print(right_0); Serial.print("\t"); Serial.println(right_1); #endif digitalWrite(MotorLeftA, left_0); analogWrite(MotorLeftB, left_1); digitalWrite(MotorRightA, right_0); analogWrite(MotorRightB, right_1); } void Dolly::SetMotors_0(int Left, int Right, bool Slowly) { byte tmpLeft_0, tmpLeft_1, tmpRight_0, tmpRight_1; if(Slowly) { Left = constrain(Left, PrevMotorLeft - VelocityStep, PrevMotorLeft + VelocityStep); Right = constrain(Right, PrevMotorRight - VelocityStep, PrevMotorRight + VelocityStep); } ConvertVelocity2Controls(Left, tmpLeft_0, tmpLeft_1); ConvertVelocity2Controls(Right, tmpRight_0, tmpRight_1); SetMotors(tmpLeft_0, tmpLeft_1, tmpRight_0, tmpRight_1); long CurrTime = millis(); PrevMotorTime = CurrTime; PrevMotorLeft = Left; PrevMotorRight = Right; PrevCallTime = micros()/MKSinterval; // для статистики } void Dolly::Motors(int Left, int Right, bool Slowly) // set motors Velocity: Left, Right from -255 to 255 { #ifdef DOLLY_DEBUG_MOTORS Serial.print("Motors:\t"); Serial.print(Left); Serial.print("\t"); Serial.println(Right); #endif Left = constrain(Left, -255, 255); Right = constrain(Right, -255, 255); TargetLeft = Left; TargetRight = Right; SetMotors_0(Left, Right, Slowly); } void Dolly::MotorLeft(int Left, bool Slowly) // set motors Velocity: Left, Right from -255 to 255 { Left = constrain(Left, -255, 255); TargetLeft = Left; int Right = TargetRight; SetMotors_0(Left, Right, Slowly); } void Dolly::MotorRight(int Right, bool Slowly) // set motors Velocity: Left, Right from -255 to 255 { Right = constrain(Right, -255, 255); TargetRight = Right; int Left = TargetLeft; SetMotors_0(Left, Right, Slowly); } ////////////////////////////////////////////////////////////////////////// // функция основного цикла управления всеми устройствами char ValcDelta[4][4] = { // что делать со счетчиками при переходе из одного состояния в другое // 0 1 10 11 second index { 0, +1, -1, 0}, // 0 first index {-1, 0, 0, +1}, // 1 first index {+1, 0, 0, -1}, // 10 first index { 0, -1, +1, 0}}; // 11 first index byte ValcErrors[4][4] = { // перечень ошибок (есть ошибка - 1, нет ошибки - 0) { 0, 0, 0, 1}, { 0, 0, 1, 0}, { 0, 1, 0, 0}, { 1, 0, 0, 0}}; void Dolly::Run() // each cycle routine for Motors & Valcoders { long CurrTime = millis(); // Motors part if ((CurrTime - PrevMotorTime) >= TimeConstant) { if((TargetLeft != PrevMotorLeft) || (TargetRight != PrevMotorRight)) { int Left = constrain(TargetLeft, PrevMotorLeft - VelocityStep, PrevMotorLeft + VelocityStep); int Right = constrain(TargetRight, PrevMotorRight - VelocityStep, PrevMotorRight + VelocityStep); byte tmpLeft_0, tmpLeft_1, tmpRight_0, tmpRight_1; ConvertVelocity2Controls(Left, tmpLeft_0, tmpLeft_1); ConvertVelocity2Controls(Right, tmpRight_0, tmpRight_1); SetMotors(tmpLeft_0, tmpLeft_1, tmpRight_0, tmpRight_1); PrevMotorTime += TimeConstant; PrevMotorLeft = Left; PrevMotorRight = Right; } if((CurrTime - PrevMotorTime) > TimeConstant) PrevMotorTime = CurrTime; } // Valcoders part if (ValcodersInited) { byte ValcoderLeft = (digitalRead(ValcoderLeftB) << 1) | digitalRead(ValcoderLeftA); byte ValcoderRight = (digitalRead(ValcoderRightB) << 1) | digitalRead(ValcoderRightA); // bit 0 - A, bit 1 - B // changes at Left Valcoder Channel if (ValcoderLeft != PrevValcoderLeft) { char delta = ValcDelta[PrevValcoderLeft][ValcoderLeft]; DirectionLeft = delta; ValcoderCounterLeft += delta; ValcoderCounterAbsLeft++; ChangeStateCounter++; ValcoderErrorsLeft += ValcErrors[PrevValcoderLeft][ValcoderLeft]; int DT = CurrTime - PrevValcoderTimeLeft; ValIntLeft[(ValcoderCounterAbsLeft + ValcoderTimeOutLeft) % 4] = DT; ValState[ChangeStateCounter % 16] = ValcoderLeft | (ValcoderRight << 2); ValTime[ChangeStateCounter % 16] = CurrTime; if (MinDLeft > DT) MinDLeft = DT; if (MaxDLeft < DT) MaxDLeft = DT; PrevValcoderLeft = ValcoderLeft; PrevValcoderTimeLeft = CurrTime; } // Left Valcoder Channel Timeout if ((CurrTime - PrevValcoderTimeLeft) >= MaxValcoderTimeInterval) { ValcoderTimeOutLeft++; DirectionLeft = 0; ValIntLeft[(ValcoderCounterAbsLeft + ValcoderTimeOutLeft) % 4] = MaxValcoderTimeInterval; PrevValcoderTimeLeft = CurrTime; } // changes at Right Valcoder Channel if (ValcoderRight != PrevValcoderRight) { char delta = -ValcDelta[PrevValcoderRight][ValcoderRight]; DirectionRight = delta; ValcoderCounterRight += delta; ValcoderCounterAbsRight++; ChangeStateCounter++; ValcoderErrorsRight += ValcErrors[PrevValcoderRight][ValcoderRight]; int DT = CurrTime - PrevValcoderTimeRight; ValIntRight[(ValcoderCounterAbsRight + ValcoderTimeOutRight) % 4] = DT; ValState[ChangeStateCounter % 16] = ValcoderLeft | (ValcoderRight << 2); ValTime[ChangeStateCounter % 16] = CurrTime; if (MinDRight > DT) MinDRight = DT; if (MaxDRight < DT) MaxDRight = DT; PrevValcoderRight = ValcoderRight; PrevValcoderTimeRight = CurrTime; } // Left Valcoder Channel Timeout if ((CurrTime - PrevValcoderTimeRight) >= MaxValcoderTimeInterval) { ValcoderTimeOutRight++; DirectionRight = 0; ValIntRight[(ValcoderCounterAbsRight + ValcoderTimeOutRight) % 4] = MaxValcoderTimeInterval; PrevValcoderTimeRight = CurrTime; } } // Statistic part - only first 4294967 ms if(CurrTime < 4294967) { long CurrMKS = micros()/MKSinterval; long DT = CurrMKS - PrevCallTime; DT = constrain(DT, 0, 63); StatDT[DT]++; PrevCallTime = CurrMKS; // для статистики } // LED & Buzzer part int phase = (CurrTime / 100) % 8; for(byte i = 0; i < 4; i++) { if(LEDpin[i] < 127) digitalWrite(LEDpin[i], (OUTmodes[LEDmode[i]] >> phase) & 1); } if(BuzzerPin < 127) { // !!! здесь выбор: брать за основу частоту вызова RUN или 1 мс byte BuzzPhase = (Counter >> BuzzerShift) & 1; // 0 или 1 со звуковой частотой // byte BuzzPhase = (CurrTime >> BuzzerShift) & 1; // 0 или 1 со звуковой частотой digitalWrite(BuzzerPin, (OUTmodes[BuzzerMode] >> phase) & BuzzPhase); } if (DoElCurrentMeasure) { ElCurrent[Counter % 16] = analogRead(ElCurrentPin); } if (DoVoltageMeasure) { Voltage[Counter % 16] = analogRead(VoltagePin); } Counter++; } //////////////////////////////////////////////////////////////////////////// // конец функции основного цикла void Dolly::PrintDT() // вывод статичтики в Serial { for (int i = 0; i < 64; i++) { Serial.print("i:\t"); Serial.print((i+1)*MKSinterval); Serial.print("\tDT:\t"); Serial.println(StatDT[i]); } Serial.print("Elapsed time:\t"); Serial.println(millis()); Serial.print("Left ms/pulse range:\t"); Serial.print(MinDLeft); Serial.print("\t-\t"); Serial.println(MaxDLeft); Serial.print("Right ms/pulse range:\t"); Serial.print(MinDRight); Serial.print("\t-\t"); Serial.println(MaxDRight); } void Dolly::LineSensorInit(byte Left, byte Middle, byte Right) // line sensor pins { LineSensorLeft = Left; LineSensorMiddle = Middle; LineSensorRight = Right; } //////////////////////////////////////////////////////////////////////////// // звук и светодиоды void Dolly::BlinkInit(byte LED, byte pin) // инициализация светодиодов (от 0 до 3) { if((LED >= 0) && (LED < 4)) { LEDpin[LED] = pin; pinMode(LEDpin[LED], OUTPUT); Serial.print("BlinkInit LED: "); Serial.print(LED); Serial.print(", pin: "); Serial.println(pin); } } void Dolly::SetBlinkMode(byte LED, byte mode) // настройка мигания светодиодов { if((LED >= 0) && (LED < 4)) { LEDmode[LED] = constrain(mode, 0, MaxOUTmodes); Serial.print("BlinkMode LED: "); Serial.print(LED); Serial.print(", mode: "); Serial.println(mode); } } void Dolly::BuzzerInit(byte pin) // инициализация буззера { BuzzerPin = pin; pinMode(BuzzerPin, OUTPUT); } void Dolly::SetBuzzerMode(byte mode) // гастройка режима буззера: 0 - нет звука { BuzzerShift = InitialShift + ((mode >> 6) & 3); Serial.print("BuzzerShift: "); Serial.println(BuzzerShift); BuzzerMode = constrain(mode & 0x3f, 0, MaxOUTmodes); Serial.print("Buzzer mode: "); Serial.println(BuzzerMode); Serial.print("RUN Counter: "); Serial.println(Counter); Serial.print("BuzzerPin: "); Serial.println(BuzzerPin); } //////////////////////////////////////////////////////////////////////////// // измерение тока и напряжения void Dolly::ElCurrentInit(byte pin, bool permit) // { ElCurrentPin = pin; if (permit) PermitElCurrent(); else ForbidElCurrent(); Serial.print("ElCurrent pin: "); Serial.println(pin); } void Dolly::VoltageInit(byte pin, bool permit) // { VoltagePin = pin; if (permit) PermitVoltage(); else ForbidVoltage(); Serial.print("Voltage pin: "); Serial.println(pin); } int Dolly::GetElCurrent() // возвращает силу тока в 14-разрядной величине (от 0 до 1023*16, середина 511*16) { if(DoElCurrentMeasure) { int result = ElCurrent[0]; for (byte i = 1; i < 16; i++) result += ElCurrent[i]; return result; } else return -1; } int Dolly::GetVoltage() // возвращает напряжение в 14-разрядной величине (от 0 до 1023*16, середина 511*16) { if(DoVoltageMeasure) { int result = Voltage[0]; for (byte i = 1; i < 16; i++) result += Voltage[i]; return result; } else return -1; } //////////////////////////////////////////////////////////////////////////// // обмен по I2C void Dolly::receiveEvent(int howMany) // процедура приема данных от мастера по I2C { byte Command = Wire.read(); #ifdef DOLLY_DEBUG_COMMAND Serial.print("I2C Command: 0x"); Serial.println(Command, HEX); #endif Serial.print("Cmd: "); Serial.println(Command, HEX); if((Command & 8) == 8) // читаем мастера { int LeftMotor; int RightMotor; byte mode; switch (Command) { case 0x1a: // плавная запись левого мотора LeftMotor = Wire.read() << 8 | Wire.read(); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Left Miotor Set: "); Serial.println(LeftMotor); #endif MotorLeft(LeftMotor); break; case 0x2e: // плавная запись правого мотора RightMotor = Wire.read() << 8 | Wire.read(); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Right Miotor Set: "); Serial.println(RightMotor); #endif MotorRight(RightMotor); break; case 0x3a: // мгновенная запись левого мотора LeftMotor = Wire.read() << 8 | Wire.read(); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Left Miotor Set: "); Serial.println(LeftMotor); #endif MotorLeft(LeftMotor, false); break; case 0x4e: // мгновенная запись правого мотора RightMotor = Wire.read() << 8 | Wire.read(); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Right Miotor Set: "); Serial.println(RightMotor); #endif MotorRight(RightMotor, false); break; case 0x5b: // плавная запись двух моторов LeftMotor = Wire.read() << 8 | Wire.read(); RightMotor = Wire.read() << 8 | Wire.read(); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Both Miotors Set: "); Serial.print(LeftMotor); Serial.print(", "); Serial.println(RightMotor); #endif Motors(LeftMotor, RightMotor); break; case 0x6b: // мгновенная запись двух моторов LeftMotor = Wire.read() << 8 | Wire.read(); RightMotor = Wire.read() << 8 | Wire.read(); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Both Miotors Set: "); Serial.print(LeftMotor); Serial.print(", "); Serial.println(RightMotor); #endif Motors(LeftMotor, RightMotor, false); break; case 0xc8: // разрешение измерения силы тока PermitElCurrent(); break; case 0xc9: // запрещение измерения силы тока ForbidElCurrent(); break; case 0xd8: // разрешение измерения напряжения PermitVoltage(); break; case 0xd9: // запрещение измерения напряжения ForbidVoltage(); break; case 0xe9: // установка режима буззера mode = Wire.read(); SetBuzzerMode(mode); break; case 0xf9: // установка режима светодиодов mode = Wire.read(); SetBlinkMode((mode >> 6) & 3, mode & 0x3f); break; } // switch } // конец ветки чтения мастера else // будем отвечать мастеру { LastCommand = Command; } // конец ветки ответа мастеру } static union { float buff_float[8]; long buff_long[8]; int buff_int[16]; byte buff_byte[32]; }; void Dolly::requestEvent() // процедура передачи данных мастеру по I2C { // Serial.print("request Command: 0x"); // Serial.println(LastCommand, HEX); switch (LastCommand) { case 0x10: // пройденный путь оба (полный - 8 байтов) buff_long[0] = ValcoderCounterLeft; buff_long[1] = ValcoderCounterRight; Wire.write(buff_byte, 8); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Command 0x10 LC: "); Serial.print(buff_long[0]); Serial.print(", RC: "); Serial.println(buff_long[1]); #endif break; case 0x30: // модуль пройденного пути оба (полный - 8 байтов) buff_long[0] = ValcoderCounterAbsLeft; buff_long[1] = ValcoderCounterAbsRight; Wire.write(buff_byte, 8); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Command 0x30 LA: "); Serial.print(buff_long[0]); Serial.print(", RA: "); Serial.println(buff_long[1]); #endif break; case 0x43: // скорость оба (4 байта) buff_int[0] = GetVelocityLeft(); buff_int[1] = GetVelocityRight(); Wire.write(buff_byte, 4); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Command 0x43 LV: "); Serial.print(buff_int[0]); Serial.print(", RV: "); Serial.println(buff_int[1]); #endif break; case 0x80: // запрос количества ошибок обоих валкодеров (8 байт) buff_long[0] = ValcoderErrorsLeft; buff_long[1] = ValcoderErrorsRight; Wire.write(buff_byte, 8); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Command 0x80 LE: "); Serial.print(buff_long[0]); Serial.print(", RE: "); Serial.println(buff_long[1]); #endif break; case 0x81: // запрос интервалов между сменами состояний обоих валкодеров (8 байт) buff_int[0] = MinDLeft; buff_int[1] = MaxDLeft; buff_int[2] = MinDRight; buff_int[3] = MaxDRight; Wire.write(buff_byte, 8); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Command 0x81 LD: "); Serial.print(buff_int[0]); Serial.print(" - "); Serial.print(buff_int[1]); Serial.print(", RD: "); Serial.print(buff_int[2]); Serial.print(" - "); Serial.println(buff_int[3]); #endif break; case 0x82: // запрос количества таймаутов обоих валкодеров (8 байт) buff_long[0] = ValcoderTimeOutLeft; buff_long[1] = ValcoderTimeOutRight; Wire.write(buff_byte, 8); #ifdef DOLLY_DEBUG_COMMAND Serial.print("Command 0x82 LT: "); Serial.print(buff_long[0]); Serial.print(", RT: "); Serial.println(buff_long[1]); #endif break; case 0xc2: // запрос силы тока buff_int[0] = GetElCurrent(); Wire.write(buff_byte, 2); break; case 0xd2: // запрос напряжения buff_int[0] = GetVoltage(); Wire.write(buff_byte, 2); break; case 0x90: // запрос статистики по интервалам времени между проходами цикла case 0x91: case 0x92: case 0x93: case 0x94: case 0x95: case 0x96: case 0x97: request90(); break; case 0xf0: // line sensor state buff_long[0] = (digitalRead(LineSensorLeft) & 1) + ((digitalRead(LineSensorMiddle) & 1) << 1) + ((digitalRead(LineSensorRight) & 1) << 2); Wire.write(buff_long[0]); break; default: Serial.print(" !!! Error Read Command "); Serial.println(LastCommand, HEX); break; } LastCommand = 0; } void Dolly::request90() // фрагмент ответа на запрос статистики (команды 0x90-0x97) { memcpy(&buff_byte, &StatDT[(LastCommand & 0x0f)*8], 32); Wire.write(buff_byte, 32); #ifdef DOLLY_DEBUG_COMMAND Serial.print(" Statistic, page "); Serial.println(LastCommand & 0xf); #endif } Dolly dolly;(продолжение следует)
Часть 3
Исследуем мотор
(продолжение следует)
Часть 4
Исследуем серводвигатель
(продолжение следует)
Часть 5
I2C периферия - компас
(продолжение следует)
Часть 6
I2C периферия - акселерометр и гироскоп
(продолжение следует)
Часть 7
Датчики расстояния
(продолжение следует)
Часть 8
Внешнее управление - IR Control
(продолжение следует)
Часть 9
I2C периферия - дискетка на EEPROM
(продолжение следует)
Часть 10
Составляем карту
(продолжение следует)
Тексты программы Processing не привожу. Имея опыт использования этого инструмента ровно 0 дней, трудно написать программу, которая не показалась бы смешной опытному программисту.
начхай на "опытных" програмистов и выкладывай всё.. опытные пусть идет пасутся... а то развелось тут понимаешь.. худють топчут
начхай на "опытных" програмистов и выкладывай всё.. опытные пусть идет пасутся... а то развелось тут понимаешь.. худють топчут
500 строк за 3 вечера: выкладывать такое - неуважение и к себе и к окружающим. Это - типичный одноразовый код. Усугубляемый незнанием Processing'а. В нем вообще структуры предусмотрены? Или их нужно объевлять как class? И почему я не могу сразу задать массивы нужной мне длины, а вынужден под каждую пару чисел писать выделение памяти?
500 строк за 3 вечера: выкладывать такое - неуважение и к себе и к окружающим. Это - типичный одноразовый код. Усугубляемый незнанием Processing'а. В нем вообще структуры предусмотрены? Или их нужно объевлять как class? И почему я не могу сразу задать массивы нужной мне длины, а вынужден под каждую пару чисел писать выделение памяти?
а я его вообще в глаза никогда не видел. нафиг он нужен то.
Это типа "не занимать, сюдой щас придуть!" :)
С "картографом" не совсем соглашусь. Робот сможет мерять то пространство куда ОН можнт проехать.
Когда мне сообщают что у меня код написан не по фэншую, то предлагаю переписать самому или пойти на Йух. Поэтому стыд в ентом деле не уместен.
Когда мне сообщают что у меня код написан не по фэншую, то предлагаю переписать самому или пойти на Йух. Поэтому стыд в ентом деле не уместен.
помню, помню... дискусию про "лапшу" и удобо читаемость... злой ты
Это типа "не занимать, сюдой щас придуть!" :)
С "картографом" не совсем соглашусь. Робот сможет мерять то пространство куда ОН можнт проехать.
Да, конечно.
Как-то робототехника пока находится в том состоянии, что мы, люди, пытаемся создать для роботов наиболее комфортные условия для работы. Иначе они не справятся.
В частности, предполагается, что если мы решили закрыть дверь в комнату, то для робота Вселенная заканчивается по внешней поверхности двери. И даже расставленные по полу картонные коробки отделяют реальное пространство от несуществующего.
Кстати, пришедший ко мне в посылке китайский робот уже был оснащен датчиками линии (по сути три пороговых дальномера). Зачем картографу датчик линии - я не знаю. Вот и думаю, может переделать их в датчики края, чтобы обезопасить робота от падений с лестницы, со стола и прочих мест, где он может оказаться.
Когда мне сообщают что у меня код написан не по фэншую, то предлагаю переписать самому или пойти на Йух. Поэтому стыд в ентом деле не уместен.
Неужели у кого-то вызвал серьезный интерес мой явно одноразовый код?
Нет, если серьезно интересно, создайте отдельную темку в разделе "Программирование" и задайте там мне прямой вопрос - я размещу там текст программы и даже отвечу на некоторые вопросы по нему.
PS. Интересно, сможет робот-картограф послужить аргументом в споре с БТИ?
Меня - не заинтересовал, это если честно. Но так принято - если пишут в разделе проект, то выкладывают код. Может кто-то повторить 1-1 захочет?
С БТИ - не сможет. Или Вы будете его сертифицировать по ГОСТам?
у нас просто есть в планах проект который будет частью общей системы... если интересно присоединиться. то можешь накорябать мне в гугл+ . я обьясню в чем твой подход не очень правилен и как это можно исправить и сделать это частью общей системы :) у нас простони рук ни времени не хватает все сразу развивать.
Ты мне в хангауте уже надоел. Нафига я тебе еще в гуглеплюсе писать буду?!?!?!
Ты мне в хангауте уже надоел. Нафига я тебе еще в гуглеплюсе писать буду?!?!?!
а я вообще не тебе. и не нада тут ревновать :) а то щас все бросим и бум строить робота
Меня - не заинтересовал, это если честно. Но так принято - если пишут в разделе проект, то выкладывают код. Может кто-то повторить 1-1 захочет?
Код я выложил.
Который можно запустить на Ардуине. Форум ведь посвящен ей, а не Процессингу. Тем более, что в выложенном скетче кроме бинарного предусмотрен и текстовый вывод в консоль.
у нас просто есть в планах проект который будет частью общей системы... если интересно присоединиться. то можешь накорябать мне в гугл+ . я обьясню в чем твой подход не очень правилен и как это можно исправить и сделать это частью общей системы :) у нас простони рук ни времени не хватает все сразу развивать.
Вы всерьез полагаете, что меня может заинтересовать проект, о котором я ничего не знаю?
И потом, на данном этапе мой проект меня вполне устраивает. Тем более, что основная его цель - это не изготовить что-то, а понять возможности как контроллера, так и более или менее стандартной периферии для него. Ну, например, какой точности позиционирования можно добиться на коленке. Мне кажется, что вполне естественно, когда в первые недели знакомства с новой сущностью познавательные интересы существено преобладают над изготовительными.
Вы всерьез полагаете, что меня может заинтересовать проект, о котором я ничего не знаю?
И потом, на данном этапе мой проект меня вполне устраивает. Тем более, что основная его цель - это не изготовить что-то, а понять возможности как контроллера, так и более или менее стандартной периферии для него. Ну, например, какой точности позиционирования можно добиться на коленке. Мне кажется, что вполне естественно, когда в первые недели знакомства с новой сущностью познавательные интересы существено преобладают над изготовительными.
ну отлично. пойду за попкорном. колхоз дело добровольное. интересно просто как будут решаться некоторые проблемы с построением карты. ну например когда робот тупо не может проехать куда то.. имхо но карта будет какая то убогая...
да кстати для картографии комнат не нужно никуда особо ездить.. можно просто взять лидар хотя бы баксов за 80. он есть в исполненнии для ардуино с библиотеками. и просто отсканировать всю комнату :)
ну отлично. пойду за попкорном. колхоз дело добровольное. интересно просто как будут решаться некоторые проблемы с построением карты.
Мне тоже интересно. Собственно, ради этого интереса и вожусь с ним. :)
Есть проблемы, которые либо вообще не решаются, либо не решаются на данном уровне развития техники, либо решаются за совсем другие деньги. Согласитесь, составление карт видимой и обратной сторон Луны - это немного разные по ресурсоемкости задачи. Хотя, казалось бы, какая разница.
да кстати для картографии комнат не нужно никуда особо ездить.. можно просто взять лидар хотя бы баксов за 80. он есть в исполненнии для ардуино с библиотеками. и просто отсканировать всю комнату :)
Желательно рентгеновский лидар, чтобы построить план не единственной комнаты, а всей квартиры.
PS. Собственно, меня интересует в первую очередь доступная простыми средствами точность позиционирования. А "картограф" - это уже вторичное. Вплоть до не совсем удачного названия. Но ведь надо как-то назвать.
Собственно, меня интересует в первую очередь доступная простыми средствами точность позиционирования. А "картограф" - это уже вторичное. Вплоть до не совсем удачного названия. Но ведь надо как-то назвать.
позиционирование оно всегда относительно чего то... вот как только определишся относительно чего, тогда и можно смотреть решения... а их навалом и не дорого.
а мне интересно как будет решаться проблема движения не по прямой? есть валкодер как на картинке хорошо. но подстройки скорости нет. то есть изза разброса параметров моторов, сопротивления, движение в лучшем случае будет по дуге
это если не учитывать разные поверхности по котором будет ездить. математически вроде как можно рассчитать расстояние по разнице количества импульсов с валкодера и зная точный диаметр колеса. но как компенсировать поверхность хз.
ставить еще колесо с валкодером без мотора? чтобы по нему проверять моторы. может один уже буксует. но колесо только покажется движется шасси или нет, а не в какую сторону. то есть если случилось что одно колесо забуксовало, шасси повернулось. по колесу контрольному определили что случилось что то. остановили. но шасси уже повернулось
компас теоретически может эту проблему решит, но говорят он очень чувствителен к электромагнитным наводкам (сам не проверял). так что есть над чем подумать
так что есть над чем подумать
80 баксов за лидар на 40 метров.. покроет большинство комнат...
так что есть над чем подумать
80 баксов за лидар на 40 метров.. покроет большинство комнат...
ага. но тут получается путь проще. это не значит что это нереальный хоть и геморра много. с лидаром конечно намного проще
думаю вопрос не стоит как сделать картограф чтобы получать качественные карты, а личный интерес и энтузиазм
думаю вопрос не стоит как сделать картограф чтобы получать качественные карты, а личный интерес и энтузиазм
прям вечер воспоминаний какой то...
-Товарищ капитан, а может лучше не ломами а вениками подметать? Оно и чище будет.
-А мне не нужно чище, мне нужно что бы вы заипались и спали ночью, а не в самоходы бегали!
ну если так то и катера и самолеты мне бессмысленно собирать. купи готовый и балуйся. или твой проект с "искуственным интеллектом". подобное наверно уже есть
а мне интересно как будет решаться проблема движения не по прямой? есть валкодер как на картинке хорошо. но подстройки скорости нет. то есть изза разброса параметров моторов, сопротивления, движение в лучшем случае будет по дуге
это если не учитывать разные поверхности по котором будет ездить. математически вроде как можно рассчитать расстояние по разнице количества импульсов с валкодера и зная точный диаметр колеса. но как компенсировать поверхность хз.
ставить еще колесо с валкодером без мотора? чтобы по нему проверять моторы. может один уже буксует. но колесо только покажется движется шасси или нет, а не в какую сторону. то есть если случилось что одно колесо забуксовало, шасси повернулось. по колесу контрольному определили что случилось что то. остановили. но шасси уже повернулось
компас теоретически может эту проблему решит, но говорят он очень чувствителен к электромагнитным наводкам (сам не проверял). так что есть над чем подумать
ну если так то и катера и самолеты мне бессмысленно собирать. купи готовый и балуйся. или твой проект с "искуственным интеллектом". подобное наверно уже есть
дело не в этом... ты же собираешь катер из подходящих материалов.. а не тупа из гавна найденого в кладовке.. типа старывй ботинок и пропелер от бабушкиного вентилятора... ты ж не ограничиваеш себя в материялах для проекта.. типа хочу блин катер из туалетной бумаги.. типа это мой вызов всему миру..
в материалах ограничиваю, вариантов много, да и не все по трудоемкости мне подходят. некоторые по цене. но не из туалетной бумаги конечно))
в материалах ограничиваю, вариантов много, да и не все по трудоемкости мне подходят. некоторые по цене. но не из туалетной бумаги конечно))
не ну понятно что не из позолоченого титана пилишь... но тут то разговор идет о лидаре за 80 баксов... а не за 800
наверно хочется просто. я например в катере вместо того чтобы использовать готовый драйвер за 8-10 баксов, магазинной ардуины 3бакса, преобразователя за 1бакс сделал свою под конкретную задачу и небольшими габаритами. и основывался на удобстве использования и минимума проводов и контактов, влага все таки
и пока устранил все недостатки было 3 версии платы. точнее 2 + доп. диоды
хотя мог и на магазинных сделать, спаять, приделать клеммы, залить эпоксидкой, но не стал
наверно все таки основывается на том хочу сделать именно так
наверно хочется просто. я например в катере вместо того чтобы использовать готовый драйвер за 8-10 баксов, магазинной ардуины 3бакса, преобразователя за 1бакс сделал свою под конкретную задачу и небольшими габаритами. и основывался на удобстве использования и минимума проводов и контактов, влага все таки
и пока устранил все недостатки было 3 версии платы. точнее 2 + доп. диоды
хотя мог и на магазинных сделать, спаять, приделать клеммы, залить эпоксидкой, но не стал
наверно все таки основывается на том хочу сделать именно так
ну так по факту обошлось дороже чем если бы купил :)
у меня? как раз нет)))
а мне интересно как будет решаться проблема движения не по прямой? есть валкодер как на картинке хорошо. но подстройки скорости нет. то есть изза разброса параметров моторов, сопротивления, движение в лучшем случае будет по дуге
Валкодер в принципе не способен подстраивать скорость. Это - датчик, а не исполнительное устройство.
это если не учитывать разные поверхности по котором будет ездить. математически вроде как можно рассчитать расстояние по разнице количества импульсов с валкодера и зная точный диаметр колеса. но как компенсировать поверхность хз.
Пока поверхность предполагаю абсолютно ровной. Кстати, проходимость робота ожидается довольно низкой из-за того, что передняя и задняя опоры сделаны на шариках. Думаю, для таких уже 5 мм - непреодолимое препятствие. Но точно не знаю, как дойдет до "полевых испытаний", - проверю.
Пока робота целиком даже не собирал, проверяю отдельные детали.
ставить еще колесо с валкодером без мотора? чтобы по нему проверять моторы. может один уже буксует. но колесо только покажется движется шасси или нет, а не в какую сторону. то есть если случилось что одно колесо забуксовало, шасси повернулось. по колесу контрольному определили что случилось что то. остановили. но шасси уже повернулось
Пробуксовка - отдельная проблема. Пока считаю, что робот должен объезжать препятствия, не доезжая до них. Если окажется, что это не так, тогда буду думать.
компас теоретически может эту проблему решит, но говорят он очень чувствителен к электромагнитным наводкам (сам не проверял). так что есть над чем подумать
Аналогично. Компас заказал сразу вместе с тележкой, но пока даже не распаковывал. Сейчас продолжаю возиться с валкодерами - пытаюсь приспособить к ним аппаратный счетчик. Но под руками ничего кроме 555ИЕ7 не нашлось. А для него нужный режим счета не документирован. Пытаюсь понять, можно приспособить или нет. Если окажется, что нет, перейду к моторам, а потом к серве. Кстати, по серве получил интересные результаты. Только ток через нее пытался измерить на резисторе 0.1 Ома. По измерениям получается до 3 А. Но, скорее всего, дело в сопротивлении подводящих проводов и контактов, которые сами по себе больше 0.1 Ома, а то откуда в USB взяться трем амперам.
Не надо.
У GPS погрешность 10-30 м, а у меня характерный размер единицы метров, а желаемая точность 1-2 см.
Не надо.
У GPS погрешность 10-30 м, а у меня характерный размер единицы метров, а желаемая точность 1-2 см.
Боюсь, колеса такой точности не обеспечат.
Разговор о "лидаре за 80 баксов" пойдет только после того, как я обнаружу, что либо без него никак не справиться, либо, что лидар - оптимальное решение.
Пока собираюсь использовать одновременно ИК и УЗ датчтки и сравнивать их показания (хотя не очень понятно, как учитывать существенно различающиеся диаграммы направленности).
Что-то мне подсказывает, что лазерная мышка может довольно точно перемещение отслеживать.
Что-то мне подсказывает, что лазерная мышка может довольно точно перемещение отслеживать.
гыыыыы это вам ваша безграмотность подсказывает :)
Но должна же и от безграмотности быть какая-то польза.
Но должна же и от безграмотности быть какая-то польза.
ну теоретически. если на пузо роботу прилепить мыш лазерную и обеспечить плотное касание к поверхности, то конечно будет этакий сканер пола с 1300dpi..
но мне так видиться что это будет несколько трудновато в плане механики...
Что-то мне подсказывает, что лазерная мышка может довольно точно перемещение отслеживать.
гыыыыы это вам ваша безграмотность подсказывает :)
Да уж куда мне до такого гения, как ты.
Но должна же и от безграмотности быть какая-то польза.
ну теоретически. если на пузо роботу прилепить мыш лазерную и обеспечить плотное касание к поверхности, то конечно будет этакий сканер пола с 1300dpi..
но мне так видиться что это будет несколько трудновато в плане механики...
Но соизмеримо с энкодерором или валкодером.
Да уж куда мне до такого гения, как ты.
и это верно! :)
Но соизмеримо с энкодерором или валкодером.
не факт... например по надраеному паркету нифига не хочет мыша нормально ездить :(
Но соизмеримо с энкодерором или валкодером.
не факт... например по надраеному паркету нифига не хочет мыша нормально ездить :(
Выкини свою мышь! У меня defender made in Китай, работает ювелирно....уже почти год на двух батарейках по радио.