Измерение скорости, пройденного пути и угла поворота с помощью Arduino и датчика LM393

Роботы с каждым годом все больше внедряются в нашу жизнь, делая ее проще. Большинству выпускаемых сегодня роботов (например, роботам для доставки чего-либо) необходима система навигации в пространстве, с помощью которой они и ориентируются в окружающем мире. Существует много способов построения системы, способной определять местоположение и ориентацию робота – с помощью GPS, радиочастотной триангуляции, акселерометров, гироскопов и т.д. Каждая из этих технологий имеет свои преимущества и недостатки. В этой статье мы будем использовать простой и легко доступный датчик скорости LM393 (H206) для измерения с помощью платы Arduino таких важных для передвигающегося в пространстве робота параметров как скорость, пройденный путь (дистанция) и угол его поворота. Используя знания об этих параметрах, робот сможет безопасно передвигаться в окружающем пространстве.

Внешний вид проекта для измерения скорости, пройденного пути и угла с помощью Arduino и датчика LM393

Вы можете посмотреть следующие проекты роботов на нашем сайте, построенные на основе платы Arduino:

Также вы можете посмотреть все проекты по тематике робототехника на нашем сайте.

В данном проекте мы сконструируем робота, которой будет запитываться от литиевого аккумулятора и управляться с помощью джойстика. Во время работы робота мы сможем измерять его скорость, пройденную дистанцию и угол поворота робота и отображать эти параметры в режиме реального времени на экране ЖК дисплея. В данном проекте мы будем только измерять и отображать эти параметры, а как их следует потом использовать – это уже остается на ваше усмотрение.

Датчик скорости LM393 (H206)

Основную роль в нашем проекте играет датчик скорости LM393, поэтому кратко рассмотрим принцип его работы. Датчик скорости LM393 (H206) состоит из интегрированного в него инфракрасного датчика и микросхемы компаратора напряжения LM393, поэтому он и получил название датчика скорости LM393. Также в состав датчика входит пластина с сетчатой градуировкой, которую необходимо смонтировать на вращающейся оси двигателя. Внешний вид данного датчика показан на следующем рисунке.

Внешний вид датчика скорости LM393 (H206)

Установка датчика H206

Установка данных типов датчиков немного капризна. Они могут устанавливаться только на двигатели, которые имеют ось, выступающие с обоих концов двигателя. Поэтому одна сторона оси прикрепляется к колесу, а к другой стороне оси прикрепляется пластина с сетчатой градуировкой как показано на следующем рисунке.

Установка датчика скорости H206 на колесо

Поскольку при таком креплении получается что и колесо, и пластина с делениями датчика смонтированы на одной оси, это будет обозначать что они вращаются с одинаковой скоростью. То есть измеряя скорость вращения пластины мы можем определить скорость вращения колеса. При установке убедитесь в том, что отверстия (деления) пластины попадают в область действия инфракрасного датчика потому что только он сможет определить число отверстий, которое прошло через него. При желании вы можете на свой вкус модифицировать механическую часть проекта – главное чтобы соблюдались указанные условия.

Пластина с сетчатой градуировкой имеет 20 делений (слотов, отверстий). Это значит что за время полного оборота колеса инфракрасный датчик сможет обнаружить 20 пропусков (отверстий). Поскольку в нашем проекте мы будем использовать датчики установленные на обоих колеса робота это значит что мы сможем определять и угол поворота робота, но не очень точно, к сожалению. Однако точность определения угла поворота робота можно значительно повысить если вы дополните этот проект робота гироскопом – на нашем сайте вы можете прочитать статью о его подключении к плате Arduino.

Работа схемы

Схема робота на основе Arduino и датчика скорости LM393 представлена на следующем рисунке.

Схема робота на основе Arduino и датчика скорости LM393Вычислительные и управляющие задачи в схеме возложены на плату of Arduino Nano. Колеса робота приводятся в движение двумя электродвигателями постоянного тока при помощи драйвера двигателя на основе H-моста L298N. Джойстик используется для управления скоростью и направлением движения робота, а два датчика скорости H206 используются для измерения скорости, пройденной дистанции и угла поворота робота. Измеренные значения этих параметров отображаются на экране ЖК дисплея 16×2. Потенциометр, подключенный к ЖК дисплею, можно использовать для управления его контрастностью, а резистор служит для ограничения тока для управления яркостью подсветки дисплея.

Схема запитывается от литиевого элемента на 7.4V. Эти 7.4V подаются на контакт 12V драйвера двигателя, а регулятор напряжения драйвера двигателя преобразует их в стабилизированное напряжение +5V, которое используется для питания платы Arduino, ЖК дисплея, датчиков и джойстика.

Двигатели управляются с помощью цифровых контактов 8, 9, 10 и 11 платы Arduino. Поскольку нам необходимо управлять еще и скоростью вращения двигателей мы используем сигналы ШИМ (широтно-импульсной модуляции), подаваемые на положительные контакты двигателей. Для этой цели мы используем контакты 9 и 10 платы Arduino, на которых возможно использование ШИМ сигналов. Значения положения осей X и Y джойстика подаются на аналоговые контакты A2 и A3 соответственно.

Поскольку, как мы уже знаем, датчик H206 формирует управляющее воздействие (триггер) каждый раз когда обнаруживается отверстие в пластине с делениями. Для повышения точности определения скорости мы эти управляющие воздействия будем подавать на входы внешних прерываний на контактах 2 и 3 платы Arduino. После сборки робота у нас получилась конструкция показанная на следующем рисунке. Более подробно вы ее можете рассмотреть на видео, приведенном в конце статьи.

Внешний вид проекта для измерения скорости, пройденного пути и угла с помощью Arduino и датчика LM393

Принципы измерения скорости с помощью датчика H206

При измерении скорости с помощью датчика H206 необходимо следить за тем, чтобы он обнаруживал только те отверстия, которые содержатся на его пластине. Для этого необходимо чтобы колесо и пластина вращались с одинаковой скоростью – в нашем проекте этой проблемы не должно возникнуть, поскольку мы и колесо, и пластину датчика установили на одну ось двигателя.

В нашем проекте для измерения угла поворота робота мы смонтировали датчики скорости на обоих колесах, но если вы хотите измерять только скорость, то в этом случае можно использовать только один датчик. Выход датчика подсоединен ко входу внешнего прерывания платы Arduino. Каждый раз при обнаружении отверстия (пропуска) в пластине будет формироваться прерывание и вызываться процедура обработки прерывания (ISR — Interrupt service Routine). Если вы сможете измерять время между двумя такими прерываниями, то вы сможете рассчитать и скорость вращения колеса.

В плате Arduino мы можем достаточно просто определять это время при помощи функции millis(). Значение этой функции инкрементируется (увеличивается) на 1 каждую миллисекунду начиная со времени подачи питания на плату Arduino. То есть когда мы будем вызывать эту функцию на первое возникшее прерывание мы можем сохранить значение функции millis() в переменной (pevtime в нашей программе), а при втором возникшем прерывании мы можем рассчитать время при помощи вычитания значения переменной pevtime из текущего значения функции millis().

Time taken = current time – previous time
timetaken = millis()-pevtime; //timetaken in millisec

А когда мы определим время между двумя прерываниями мы можем рассчитать и число оборотов колеса в минуту (rpm) по следующей приведенной формуле. В представленной формуле (1000/timetaken) позволяет рассчитать число оборотов колеса в секунду (RPS — Revolutions per second) и в дальнейшем оно умножается на 60 чтобы преобразовать RPS в RPM (Revolutions per minute – число оборотов в минуту).

rpm=(1000/timetaken)*60;

После расчета числа оборотов колеса в минуту (rpm) мы можем рассчитать скорость робота зная радиус его колеса по следующей формуле:

Velocity = 2π × RPS × radius of wheel.
v = radius_of_wheel * rpm * 0.104

Представленная формула рассчитывает скорость в м/с, если вы хотите рассчитывать ее в км/ч, то в этом случае замените в представленной формуле 0.104 на 0.376. Число 0.104 в нашей формуле получилось после упрощения выражения V = 2π × RPS × radius of wheel.

Аналогичные расчеты на нашем сайте мы уже рассматривали в проектах аналогового спидометра на основе платы Arduino и спидометра для велосипеда на основе приложения на Android. Но пластина датчика H206 имеем 20 слотов (отверстий) и если мы будем измерять время между каждой парой отверстий, то это будет слишком перегружать микроконтроллер. Поскольку на каждое отверстие в пластине будет генерироваться два прерывания (одно в начале и одно в конце отверстия), то за время одного полного оборота колеса мы получим 40 прерываний. Поэтому в коде программы мы будем ожидать появления этих 40 прерываний прежде чем рассчитать скорость вращения колеса.

Другой недостаток этого метода заключается в том, что скорость в нем не будет опускаться до нуля поскольку для расчета скорости с помощью прерываний мы всегда будем ждать завершения полного оборота колеса. Но этот недостаток можно достаточно просто устранить добавлением кода, который будет проверять время между двумя прерываниями и если оно будет больше определенного значения, то мы будем считать что скорость равна нулю. В следующем фрагменте кода для измерения времени между двумя прерываниями мы используем переменную dtime. Если разница между текущим значением функции millis() и значением этой переменной превышает 500, то мы приравниваем число оборотов колеса в минуту и скорость робота нулю.

Принципы измерения пройденной дистанции с помощью датчика H206

Как мы уже знаем, в нашем проекте плата Arduino за время полного оборота колеса обнаружит 40 прерываний. Очевидно, что расстояние, пройденное роботом за один оборот колеса, будет равно длине его окружности. Поскольку мы знаем радиус колеса, следовательно, мы можем рассчитать и пройденную дистанцию по следующей формуле:

Принципы измерения угла поворота робота с помощью датчика H206

Существует много способов определения угла поворота робота. Обычно для этих целей используют акселерометры или гироскопы. Но более дешевый способ, который использовали мы в нашем проекте – это установка датчиков H206 на оба колеса робота. Таким образом мы сможем узнать сколько оборотов сделало каждое колесо. На следующем рисунке показан принцип расчета угла поворота робота в соответствии с данным способом.

Принцип измерения угла поворота робота с помощью датчика H206

Когда мы только подали питание на робота предполагается что угол его поворота равен 0°. Если вращается левое колесо угол инкрементируется в отрицательную сторону, а если вращается правое колесо угол инкрементируется в положительную сторону. Для упрощения понимания рассмотрим диапазон углов от -90 до +90 как показано на выше приведенном рисунке. Поскольку оба колеса одинакового диаметра, то если одно колесо сделает один полный оборот, то робот повернется на угол 90°.

К примеру, если левое колесо сделает полный оборот (80 прерываний), то робот повернется на 90° влево. Аналогично, если правое колесо сделает полный оборот (80 прерываний), то робот повернется на -90° вправо. То есть если плата Arduino обнаружит 80 прерываний от одного колеса, то мы можем считать что робот повернулся на 90° в соответствующую сторону, поэтому для расчета углов поворота мы можем использовать формулу:

90 в этой формуле обозначает угол, покрываемый за 80 прерываний. Мы также используем модуль по основанию 360 чтобы результирующее значение никогда не превышало 36. Когда мы рассчитаем по приведенной формуле углы поворота вправо и влево, то результирующий угол поворота мы можем найти с помощью разницы между этими углами:

Объяснение программы для Arduino

Полный текст программы приведен в конце статьи, здесь же мы кратко рассмотрим ее основные фрагменты.

Основная цель работы программы – расчет скорости, пройденного пути и угла поворота робота на основе описанных выше правил. Также еще необходимо управлять движениями робота с помощью джойстика.

Первым делом в программе необходимо инициализировать используемые контакты – для управления двигателями (для этого мы используем ШИМ контакты платы Arduino поскольку нам необходимо управлять скоростью вращения двигателей) и для считываниях сигналов от джойстика.

Чтобы измерять скорость и пройденный роботом путь нам необходимо знать радиус колеса робота. Измерьте его и вставьте полученное значение в следующий фрагмент кода (в метрах). Для нашего робота радиус колеса составил 0.033 метра.

Внутри функции setup мы устанавливаем значения необходимых нам переменных в 0 и отображаем приветственное сообщение на экране ЖК дисплея. Также мы инициализируем последовательный порт для целей отладки. Датчики скорости H206 подключены к контактам 2 и 3 платы Arduino как внешние прерывания. Поэтому при обнаружении сигналов прерываний будут вызываться соответственно функции Left_ISR и Right_ISR.

Внутри функции обработки прерывания Left_ISR мы будем инкрементировать значение переменной left_intr, которая в дальнейшем будет использоваться для измерения угла поворота робота. Внутри функции Right_ISR мы будем делать то же самое, но дополнительно в этой функции мы еще будем рассчитывать скорость робота. Значение переменной rotation будет инкрементироваться при каждом поступлении прерывания и затем будут применяться выше описанные правила расчета скорости.

Внутри основного бесконечного цикла loop мы будет считывать значения осей X и Y с джойстика и в зависимости от их значений управлять движениями робота. Скорость движения робота будет зависеть от того как сильно наклонена рукоятка джойстика.

Затем мы будем рассчитывать скорость, пройденный путь и угол поворота робота по вышеописанным правилам и отображать их на экране ЖК дисплея.

Тестирование работы робота

После того как аппаратная часть проекта будет готова, загрузите программу в плату Arduino. После этого возьмите в руки джойстик и попробуйте управлять роботом – во время этого процесса на экран ЖК дисплея будет выводиться информация о скорости, пройденном пути и угле поворота робота.

Тестирование работы робота

На экране ЖК дисплея величины Lt и Rt будут представлять собой счетчики прерывания слева и справа соответственно. Значения этих счетчиков будут инкрементироваться при каждом обнаружения отверстия (пропуска) в пластине датчика скорости. Параметр S на экране дисплея будет информировать о скорости робота в м/с, а параметр D будет показывать величину пройденного пути в метрах. Когда робот стоит прямо, для него угол поворота будет отображаться равным 0°, при повороте робота против часовой стрелки угол будет становиться отрицательным, а при повороте робота по часовой стрелке – положительным.

Более подробно все эти процессы вы можете посмотреть на видео, приведенном в конце статьи.

Исходный код программы (скетча)

Видео, демонстрирующее работу робота

(Проголосуй первым!)
Загрузка...
18 просмотров

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *