Роботы с каждым годом все больше внедряются в нашу жизнь, делая ее проще. Большинству выпускаемых сегодня роботов (например, роботам для доставки чего-либо) необходима система навигации в пространстве, с помощью которой они и ориентируются в окружающем мире. Существует много способов построения системы, способной определять местоположение и ориентацию робота – с помощью GPS, радиочастотной триангуляции, акселерометров, гироскопов и т.д. Каждая из этих технологий имеет свои преимущества и недостатки. В этой статье мы будем использовать простой и легко доступный датчик скорости LM393 (H206) для измерения с помощью платы Arduino таких важных для передвигающегося в пространстве робота параметров как скорость, пройденный путь (дистанция) и угол его поворота. Используя знания об этих параметрах, робот сможет безопасно передвигаться в окружающем пространстве.
Вы можете посмотреть следующие проекты роботов на нашем сайте, построенные на основе платы Arduino:
- следующий вдоль линии робот;
- робот, управляемый с помощью DTMF сигналов;
- робот для борьбы с огнем;
- робот для очистки пола;
- самобалансирующийся робот.
Также вы можете посмотреть все проекты по тематике робототехника на нашем сайте.
В данном проекте мы сконструируем робота, которой будет запитываться от литиевого аккумулятора и управляться с помощью джойстика. Во время работы робота мы сможем измерять его скорость, пройденную дистанцию и угол поворота робота и отображать эти параметры в режиме реального времени на экране ЖК дисплея. В данном проекте мы будем только измерять и отображать эти параметры, а как их следует потом использовать – это уже остается на ваше усмотрение.
Датчик скорости LM393 (H206)
Основную роль в нашем проекте играет датчик скорости LM393, поэтому кратко рассмотрим принцип его работы. Датчик скорости LM393 (H206) состоит из интегрированного в него инфракрасного датчика и микросхемы компаратора напряжения LM393, поэтому он и получил название датчика скорости LM393. Также в состав датчика входит пластина с сетчатой градуировкой, которую необходимо смонтировать на вращающейся оси двигателя. Внешний вид данного датчика показан на следующем рисунке.
Установка датчика H206
Установка данных типов датчиков немного капризна. Они могут устанавливаться только на двигатели, которые имеют ось, выступающие с обоих концов двигателя. Поэтому одна сторона оси прикрепляется к колесу, а к другой стороне оси прикрепляется пластина с сетчатой градуировкой как показано на следующем рисунке.
Поскольку при таком креплении получается что и колесо, и пластина с делениями датчика смонтированы на одной оси, это будет обозначать что они вращаются с одинаковой скоростью. То есть измеряя скорость вращения пластины мы можем определить скорость вращения колеса. При установке убедитесь в том, что отверстия (деления) пластины попадают в область действия инфракрасного датчика потому что только он сможет определить число отверстий, которое прошло через него. При желании вы можете на свой вкус модифицировать механическую часть проекта – главное чтобы соблюдались указанные условия.
Пластина с сетчатой градуировкой имеет 20 делений (слотов, отверстий). Это значит что за время полного оборота колеса инфракрасный датчик сможет обнаружить 20 пропусков (отверстий). Поскольку в нашем проекте мы будем использовать датчики установленные на обоих колеса робота это значит что мы сможем определять и угол поворота робота, но не очень точно, к сожалению. Однако точность определения угла поворота робота можно значительно повысить если вы дополните этот проект робота гироскопом – на нашем сайте вы можете прочитать статью о его подключении к плате Arduino.
Работа схемы
Схема робота на основе Arduino и датчика скорости LM393 представлена на следующем рисунке.
Вычислительные и управляющие задачи в схеме возложены на плату of Arduino Nano. Колеса робота приводятся в движение двумя электродвигателями постоянного тока при помощи драйвера двигателя на основе H-моста L298N. Джойстик используется для управления скоростью и направлением движения робота, а два датчика скорости H206 используются для измерения скорости, пройденной дистанции и угла поворота робота. Измеренные значения этих параметров отображаются на экране ЖК дисплея 16x2. Потенциометр, подключенный к ЖК дисплею, можно использовать для управления его контрастностью, а резистор служит для ограничения тока для управления яркостью подсветки дисплея.
Схема запитывается от литиевого элемента на 7.4V. Эти 7.4V подаются на контакт 12V драйвера двигателя, а регулятор напряжения драйвера двигателя преобразует их в стабилизированное напряжение +5V, которое используется для питания платы Arduino, ЖК дисплея, датчиков и джойстика.
Двигатели управляются с помощью цифровых контактов 8, 9, 10 и 11 платы Arduino. Поскольку нам необходимо управлять еще и скоростью вращения двигателей мы используем сигналы ШИМ (широтно-импульсной модуляции), подаваемые на положительные контакты двигателей. Для этой цели мы используем контакты 9 и 10 платы Arduino, на которых возможно использование ШИМ сигналов. Значения положения осей X и Y джойстика подаются на аналоговые контакты A2 и A3 соответственно.
Поскольку, как мы уже знаем, датчик H206 формирует управляющее воздействие (триггер) каждый раз когда обнаруживается отверстие в пластине с делениями. Для повышения точности определения скорости мы эти управляющие воздействия будем подавать на входы внешних прерываний на контактах 2 и 3 платы Arduino. После сборки робота у нас получилась конструкция показанная на следующем рисунке. Более подробно вы ее можете рассмотреть на видео, приведенном в конце статьи.
Принципы измерения скорости с помощью датчика 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 прерываний прежде чем рассчитать скорость вращения колеса.
|
1 2 3 4 5 6 7 |
if(rotation>=40) { timetaken = millis()-pevtime; //timetaken in millisec rpm=(1000/timetaken)*60; //formulae to calculate rpm pevtime = millis(); rotation=0; } |
Другой недостаток этого метода заключается в том, что скорость в нем не будет опускаться до нуля поскольку для расчета скорости с помощью прерываний мы всегда будем ждать завершения полного оборота колеса. Но этот недостаток можно достаточно просто устранить добавлением кода, который будет проверять время между двумя прерываниями и если оно будет больше определенного значения, то мы будем считать что скорость равна нулю. В следующем фрагменте кода для измерения времени между двумя прерываниями мы используем переменную dtime. Если разница между текущим значением функции millis() и значением этой переменной превышает 500, то мы приравниваем число оборотов колеса в минуту и скорость робота нулю.
|
1 2 3 4 5 6 |
/*To drop to zero if vehicle stopped*/ if(millis()-dtime>500) //ни одного прерывания не найдено на интервале 500ms { rpm= v = 0; // устанавливаем rpm и скорость в 0 dtime=millis(); } |
Принципы измерения пройденной дистанции с помощью датчика H206
Как мы уже знаем, в нашем проекте плата Arduino за время полного оборота колеса обнаружит 40 прерываний. Очевидно, что расстояние, пройденное роботом за один оборот колеса, будет равно длине его окружности. Поскольку мы знаем радиус колеса, следовательно, мы можем рассчитать и пройденную дистанцию по следующей формуле:
|
1 2 |
Distance = 2πr * number of rotations distance = (2*3.141*radius_of_wheel) * (left_intr/40) |
Принципы измерения угла поворота робота с помощью датчика H206
Существует много способов определения угла поворота робота. Обычно для этих целей используют акселерометры или гироскопы. Но более дешевый способ, который использовали мы в нашем проекте – это установка датчиков H206 на оба колеса робота. Таким образом мы сможем узнать сколько оборотов сделало каждое колесо. На следующем рисунке показан принцип расчета угла поворота робота в соответствии с данным способом.
Когда мы только подали питание на робота предполагается что угол его поворота равен 0°. Если вращается левое колесо угол инкрементируется в отрицательную сторону, а если вращается правое колесо угол инкрементируется в положительную сторону. Для упрощения понимания рассмотрим диапазон углов от -90 до +90 как показано на выше приведенном рисунке. Поскольку оба колеса одинакового диаметра, то если одно колесо сделает один полный оборот, то робот повернется на угол 90°.
К примеру, если левое колесо сделает полный оборот (80 прерываний), то робот повернется на 90° влево. Аналогично, если правое колесо сделает полный оборот (80 прерываний), то робот повернется на -90° вправо. То есть если плата Arduino обнаружит 80 прерываний от одного колеса, то мы можем считать что робот повернулся на 90° в соответствующую сторону, поэтому для расчета углов поворота мы можем использовать формулу:
|
1 2 |
int angle_left = (left_intr % 360) * (90/80); int angle_right = (right_intr % 360) * (90/80); |
90 в этой формуле обозначает угол, покрываемый за 80 прерываний. Мы также используем модуль по основанию 360 чтобы результирующее значение никогда не превышало 36. Когда мы рассчитаем по приведенной формуле углы поворота вправо и влево, то результирующий угол поворота мы можем найти с помощью разницы между этими углами:
|
1 |
angle = angle_right - angle_left; |
Объяснение программы для Arduino
Полный текст программы приведен в конце статьи, здесь же мы кратко рассмотрим ее основные фрагменты.
Основная цель работы программы – расчет скорости, пройденного пути и угла поворота робота на основе описанных выше правил. Также еще необходимо управлять движениями робота с помощью джойстика.
Первым делом в программе необходимо инициализировать используемые контакты – для управления двигателями (для этого мы используем ШИМ контакты платы Arduino поскольку нам необходимо управлять скоростью вращения двигателей) и для считываниях сигналов от джойстика.
|
1 2 3 4 5 6 |
#define LM_pos 9 // left motor #define LM_neg 8 // left motor #define RM_pos 10 // right motor #define RM_neg 11 // right motor #define joyX A2 #define joyY A3 |
Чтобы измерять скорость и пройденный роботом путь нам необходимо знать радиус колеса робота. Измерьте его и вставьте полученное значение в следующий фрагмент кода (в метрах). Для нашего робота радиус колеса составил 0.033 метра.
|
1 |
float radius_of_wheel = 0.033; //Measure the radius of your wheel and enter it here in m |
Внутри функции setup мы устанавливаем значения необходимых нам переменных в 0 и отображаем приветственное сообщение на экране ЖК дисплея. Также мы инициализируем последовательный порт для целей отладки. Датчики скорости H206 подключены к контактам 2 и 3 платы Arduino как внешние прерывания. Поэтому при обнаружении сигналов прерываний будут вызываться соответственно функции Left_ISR и Right_ISR.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 |
void setup() { rotation = rpm = pevtime = 0; //Initialize all variable to zero Serial.begin(9600); lcd.begin(16, 2); //Initialise 16*2 LCD lcd.print("Bot Monitor"); //Intro Message line 1 lcd.setCursor(0, 1); lcd.print("-CircuitDigest "); //Intro Message line 2 delay(2000); lcd.clear(); lcd.print("Lt: Rt: "); lcd.setCursor(0, 1); lcd.print("S: D: A: "); pinMode(LM_pos, OUTPUT); pinMode(LM_neg, OUTPUT); pinMode(RM_pos, OUTPUT); pinMode(RM_neg, OUTPUT); digitalWrite(LM_neg, LOW); digitalWrite(RM_neg, LOW); attachInterrupt(digitalPinToInterrupt(2), Left_ISR, CHANGE); //Left_ISR is called when left wheel sensor is triggered attachInterrupt(digitalPinToInterrupt(3), Right_ISR, CHANGE);//Right_ISR is called when right wheel sensor is triggered } |
Внутри функции обработки прерывания Left_ISR мы будем инкрементировать значение переменной left_intr, которая в дальнейшем будет использоваться для измерения угла поворота робота. Внутри функции Right_ISR мы будем делать то же самое, но дополнительно в этой функции мы еще будем рассчитывать скорость робота. Значение переменной rotation будет инкрементироваться при каждом поступлении прерывания и затем будут применяться выше описанные правила расчета скорости.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
void Left_ISR() { left_intr++;delay(10); } void Right_ISR() { right_intr++; delay(10); rotation++; dtime=millis(); if(rotation>=40) { timetaken = millis()-pevtime; //timetaken in millisec rpm=(1000/timetaken)*60; //formulae to calculate rpm pevtime = millis(); rotation=0; } } |
Внутри основного бесконечного цикла loop мы будет считывать значения осей X и Y с джойстика и в зависимости от их значений управлять движениями робота. Скорость движения робота будет зависеть от того как сильно наклонена рукоятка джойстика.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 |
int xValue = analogRead(joyX); int yValue = analogRead(joyY); int acceleration = map (xValue, 500, 0, 0, 200); if (xValue<500) { analogWrite(LM_pos, acceleration); analogWrite(RM_pos, acceleration); } else { analogWrite(LM_pos, 0); analogWrite(RM_pos, 0); } if (yValue>550) analogWrite(RM_pos, 80); if (yValue<500) analogWrite(LM_pos, 100); |
Затем мы будем рассчитывать скорость, пройденный путь и угол поворота робота по вышеописанным правилам и отображать их на экране ЖК дисплея.
|
1 2 3 4 5 6 7 8 9 10 11 |
v = radius_of_wheel * rpm * 0.104; //0.033 is the radius of the wheel in meter distance = (2*3.141*radius_of_wheel) * (left_intr/40); int angle_left = (left_intr % 360) * (90/80) ; int angle_right = (right_intr % 360) * (90/80) ; angle = angle_right - angle_left; lcd.setCursor(3, 0); lcd.print(" "); lcd.setCursor(3, 0); lcd.print(left_intr); lcd.setCursor(11, 0); lcd.print(" "); lcd.setCursor(11, 0);lcd.print(right_intr); lcd.setCursor(2, 1); lcd.print(" "); lcd.setCursor(2, 1);lcd.print(v); lcd.setCursor(9, 1); lcd.print(" "); lcd.setCursor(9, 1);lcd.print(distance); lcd.setCursor(13, 1); lcd.print(" "); lcd.setCursor(13, 1);lcd.print(angle); |
Тестирование работы робота
После того как аппаратная часть проекта будет готова, загрузите программу в плату Arduino. После этого возьмите в руки джойстик и попробуйте управлять роботом – во время этого процесса на экран ЖК дисплея будет выводиться информация о скорости, пройденном пути и угле поворота робота.
На экране ЖК дисплея величины Lt и Rt будут представлять собой счетчики прерывания слева и справа соответственно. Значения этих счетчиков будут инкрементироваться при каждом обнаружения отверстия (пропуска) в пластине датчика скорости. Параметр S на экране дисплея будет информировать о скорости робота в м/с, а параметр D будет показывать величину пройденного пути в метрах. Когда робот стоит прямо, для него угол поворота будет отображаться равным 0°, при повороте робота против часовой стрелки угол будет становиться отрицательным, а при повороте робота по часовой стрелке – положительным.
Более подробно все эти процессы вы можете посмотреть на видео, приведенном в конце статьи.
Исходный код программы (скетча)
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 |
/*-------defining pins------*/ #define LM_pos 9 // left motor #define LM_neg 8 // left motor #define RM_pos 10 // right motor #define RM_neg 11 // right motor #define joyX A2 #define joyY A3 #include <LiquidCrystal.h> const int rs = 14, en = 15, d4 = 4, d5 = 5, d6 = 6, d7 = 7; //контакты, к которым подключен ЖК дисплей LiquidCrystal lcd(rs, en, d4, d5, d6, d7); int left_intr = 0; int right_intr = 0; int angle = 0; /*Hardware details*/ float radius_of_wheel = 0.033; //измерьте радиус колес вашего робота и введите его здесь в метрах volatile byte rotation; // переменная для прерываний float timetaken,rpm,dtime; float v; int distance; unsigned long pevtime; void setup() { rotation = rpm = pevtime = 0; //Initialize all variable to zero Serial.begin(9600); lcd.begin(16, 2); //Initialise 16*2 LCD lcd.print("Vechile Monitor"); //приветственное сообщение на 1-й строке lcd.setCursor(0, 1); lcd.print("-CircuitDigest "); // приветственное сообщение на 2-й строке delay(2000); lcd.clear(); lcd.print("Lt: Rt: "); lcd.setCursor(0, 1); lcd.print("S: D: A: "); pinMode(LM_pos, OUTPUT); pinMode(LM_neg, OUTPUT); pinMode(RM_pos, OUTPUT); pinMode(RM_neg, OUTPUT); digitalWrite(LM_neg, LOW); digitalWrite(RM_neg, LOW); attachInterrupt(digitalPinToInterrupt(2), Left_ISR, CHANGE); //функция Left_ISR будет вызываться когда будет поступать прерывание от левого колеса attachInterrupt(digitalPinToInterrupt(3), Right_ISR, CHANGE);//функция Right_ISR будет вызываться когда будет поступать прерывание от правого колеса } void loop() { int xValue = analogRead(joyX); int yValue = analogRead(joyY); int acceleration = map (xValue, 500, 0, 0, 200); if (xValue<500) { analogWrite(LM_pos, acceleration); analogWrite(RM_pos, acceleration); } else { analogWrite(LM_pos, 0); analogWrite(RM_pos, 0); } if (yValue>550) analogWrite(RM_pos, 80); if (yValue<500) analogWrite(LM_pos, 100); /*сбрасываем скорость в ноль если робот остановился*/ if(millis()-dtime>500) //если не было ни одного прерывания за 500 мс { rpm= v = 0; // скорость и число оборотов колеса приравниваем нулю dtime=millis(); } v = radius_of_wheel * rpm * 0.104; //0.033 – это радиус колеса робота в метрах distance = (2*3.141*radius_of_wheel) * (left_intr/40); int angle_left = (left_intr % 360) * (90/80) ; int angle_right = (right_intr % 360) * (90/80) ; angle = angle_right - angle_left; lcd.setCursor(3, 0); lcd.print(" "); lcd.setCursor(3, 0); lcd.print(left_intr); lcd.setCursor(11, 0); lcd.print(" "); lcd.setCursor(11, 0);lcd.print(right_intr); lcd.setCursor(2, 1); lcd.print(" "); lcd.setCursor(2, 1);lcd.print(v); lcd.setCursor(9, 1); lcd.print(" "); lcd.setCursor(9, 1);lcd.print(distance); lcd.setCursor(13, 1); lcd.print(" "); lcd.setCursor(13, 1);lcd.print(angle); delay(100); } void Left_ISR() { left_intr++;delay(10); } void Right_ISR() { right_intr++; delay(10); rotation++; dtime=millis(); if(rotation>=40) { timetaken = millis()-pevtime; //время в миллисекундах rpm=(1000/timetaken)*60; //формула для расчета числа оборотов в минуту pevtime = millis(); rotation=0; } } |
Видео, демонстрирующее работу робота
Похожие статьи
(Проголосуй первым!)
Загрузка...
"Цитата": Поскольку оба колеса одинакового диаметра, то если одно колесо сделает один полный оборот, то робот повернется на угол 90°.
Нет, у Вас ошибка логики.
Угол поворота будет равен углу сегмента, для его расчета нужно знать ширину базы Вашей модели, (т.е. ширина базы/2 - радиус (R) для сегмента). Длина дуги при полном обороте колеса равен длине окружности колеса (L)/ при неполном обороте его можно определить сосчитав количество прерываний.
При этом формула расчета "угол поворота"= L/R
Ваш комментарий заставил немного призадуматься. Но ведь на представленном видео отлично видно, что при повороте примерно на 90 градусов счетчик прерываний выдает значение 84, что вполне укладывается в логику, представленную в статье
По представленному видео видно, что длина окружности колеса, как раз и примерно равна половине колесной базы тележки. Тогда угол поворота и на самом деле, на один оборот колеса будет примерно 90 гр. Но точно по видео не скажешь. Но если при повторе материала будет взято колесо меньше или больше, или тележка шире, то угол получится иным. Только поэтому я и написал, чтоб если кто решит повторить не спотыкался об эту ошибку.
А так статья понравилась. Наткнулся на него как раз, когда работал с щелевым датчиком для ардуино, и возникла необходимость смоделировать поворот на заданный угол, считываемый с эхолокатора установленного на сервике. В принципе, подход рабочий, и проще чем контролировать поворот через компас. Хоть и менее точный, но в моем случае этого достаточно.
Хорошо, спасибо вам за конструктивный комментарий к статье. Надеюсь, он окажет помощь другим пользователям, изучающим эту проблематику