Электромоторы с энкодером
Для точного движения робота по заданной траектории нам необходимо знать скорость вращения, направление движения колес робота, пройденный путь. Решить эту задачу с помощью обычных электромоторов практически невозможно. Так как скорость движения робота сильно зависит от напряжения питания, нагрузки на электромоторы и от других параметров, которые влияют на перемещение робота.
Но мы можем установить на вал мотора независимый датчик, с помощью которого процессор будет определять скорость, направление движения. Такой датчик называется – энкодер.
Энкодер формирует сигналы, по которым процессор способен вычислить направление вращения оси электромотора, угловую скорость, пройденный путь. По вычисленным значениям процессор может регулировать частоту вращения двигателя, соответственно, скорость и направление движения.
Такая схема называется схемой с обратной связью по замкнутому контуру.
В современных электромоторах применяются две основные схемы кодирования: инкрементная и абсолютная.
Инкрементные энкодеры: Инкрементные энкодеры генерируют импульсы, смещенные по фазе относительно друг друга. Система отслеживает количество импульсов и порядок следования. И делает вывод о скорости и направлении вращения вала электромотора.
Абсолютные энкодеры: Абсолютные энкодеры выдают определенный код для каждого относительного положения вала, устраняя необходимость в контрольной точке. Этот код указывает положение вала за один полный оборот. Это позволяет осуществлять точное позиционирование.
Инкрементные энкодеры работают, производя определенное количество импульсов с равным интервалом на оборот (PPR) или на расстояние (PPM—импульсы на миллиметр или PPI—импульсы на дюйм). Когда используется один выходной канал энкодера, процессор может определить только скорость вращения. Направление вращения в такой схеме определить невозможно. В нашим моторах установлены энкодеры с двумя каналами. А и В. Каналы смещены по фазе относительно друг друга на 90 градусов.
Такой энкодер позволяет также определять направление вращения вала мотора.
В наших экспериментах мы будем работать с электромотором, на вал которого установлен инкрементный энкодер.
Инкрементный энкодер, в котором присутствуют два выходных канала сдвинутых по фазе, называется квадратурным.
Каждый канал в таком энкодере обеспечивает определенное количество импульсов с равным интервалом за один оборот, а направление движения определяется соотношением фаз одного канала с другим.
Контроллер определяет направление движения на основе соотношения фаз между каналами A и B. Когда энкодер вращается по часовой стрелке, импульс по каналу A приходит раньше, чем по каналу B. И , если энкодер вращается против часовой стрелки, то импульс по каналу В приходит раньше, чем по каналу А.
К энкодеру подходят 4 провода (два провода для питания электрической схемы энкодера, один выходной сигнал канала А и один провод – выходной сигнал канала В).
Посчитаем импульсы с энкодера.
На рисунке ниже мы видим, к каким пинам контроллера ESP32 подключены выходы энкодера и входы драйвера мотора.
Напишем код для подсчета импульсов от энкодера и полученные значения выведем в монитор порта.
В коде мы не будем прописывать команды на вращение моторов. Поворачивать вал мотора будем руками.
Для подсчета импульсов с энкодера мы будем использовать функцию ПРЕРЫВАНИЕ.
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
|
//считаем импульсы с канала А энкодера
// укажем, что канал А энкодера правого мотора подключили к пину 34
#define ENCR_A 34
/* Мы также объявим счетчик, который будет использоваться программой
прерывания для связи с функцией основного цикла и сигнализировать о
том, что произошло прерывание.
Обратите внимание, что эту переменную необходимо объявить как volatile,
поскольку она будет использоваться ISR(обработчик прерываний) и
основным кодом. В противном случае она может быть удалена из-за
оптимизации компилятора.
*/
volatile long right_wheel_pulse_count = 0;
void setup() {
/* Переход к функции настройки, мы начинаем с открытия последовательного
соединения,чтобы иметь возможность выводить результаты работы нашей
программы.
*/
Serial.begin(9600);
// Устанавливаем пин как вход с подтягивающим резистором
pinMode(ENCR_A , INPUT_PULLUP);
/* Вызываем функцию прерывания. В качестве первого аргумента мы передаем
результат вызова функции digitalPinToInterrupt , которая преобразует
номер используемого контакта в соответствующий номер внутреннего
прерывания. Следующим аргументом мы запишем функцию,которая будет
обрабатывать прерывания или, другими словами, которая будет выполняться
при возникновении прерывания на указанном выводе. И третьим аргументом
мы указываем тип изменения сигнала на нашем пине, как переход от
низкого уровня к высокому
*/
attachInterrupt(digitalPinToInterrupt(ENCR_A),right_wheel_pulse,RISING);
}
void loop() {
Serial.print(" Pulses: ");//печатаем текстовый результат
Serial.println(right_wheel_pulse_count);//значение, хранящееся в //переменной rght_wheel_pulse_count.
}
/* Функция right_wheel_pulse() - это определяемая пользователем функция,
она не имеет возвращаемого типа и не принимает никаких аргументов в
качестве входных данных. Это функция, которая выполняется, когда
происходит прерывание на выводе 34
*/
void right_wheel_pulse() {
right_wheel_pulse_count++;
}
|
После загрузки кода открываем последовательный монитор и выбираем скорость передачи 9600 бод. Начинаем вращать вал электромотора, на котором установлен энкодер. При вращении вала мотора вперед или назад в мониторе порта мы видим увеличение значения считанных импульсов. Зная количество импульсов, диаметр колеса нашего робота мы можем посчитать скорость перемещения, пройденный путь. Но с одним каналом энкодера мы не сможем определить направления вращения мотора.
В большинстве случаев в этом и нет необходимости так, как направление движения робота мы задаем сами.
Подключим к контроллеру канал энкодера В. И посчитаем импульсы энкодера канала А с учетом импульсов энкодера канала В.
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
|
#define ENCR_A 34 // пин 34 для канала А
#define ENCR_B 35 // пин 35 для канала В
volatile long right_wheel_pulse_count = 0;
void setup() {
Serial.begin(9600); // активируем последовательный порт
pinMode(ENCR_A,INPUT_PULLUP);
pinMode(ENCR_B,INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCR_A),right_wheel_pulse,RISING);
}
void loop() {
Serial.println("Result: ");
Serial.println(right_wheel_pulse_count);
}
/* Внутри функции right_wheel_pulse() мы считываем значение пина ENCR_B,
и полученное значение сохраняем в локальной переменной b. Затем мы
используем условие if, чтобы проверить, обнаружен ли сигнал, затем
увеличиваем right_wheel_pulse_count на 1 или уменьшаем так же на 1.
*/
void right_wheel_pulse(){
int b = digitalRead(ENCR_B);
if(b > 0){
right_wheel_pulse_count++;
}
else{
right_wheel_pulse_count--;
}
}
|
Загрузите код, откройте последовательный монитор и начните вращать энкодер. Вращайте вал двигателя по часовой стрелке, а также против часовой стрелки. В одном направлении значение будет увеличиваться, а в другом направлении - уменьшаться. Таким образом, мы получили возможность определить направление вращения вала электромотора.
Рассчитаем скорость вращения вала редуктора.
Что бы рассчитать скорость движения робота нам нужно узнать скорость вращения именно вала редуктора, а не вала электромотора. Так как колесо крепится на вал редуктора.
Далее. Прежде, чем писать код, определим количество импульсов, которое выдает энкодер за один оборот вала редуктора.
Для этого обратимся к характеристикам электромотора. Нам нужны два параметра.
-
Передаточное число. Передаточное число означает соотношение количества оборотов вала электромотора к числу оборотов вала редуктора. В нашем случае это значение равно 45. Т.е. мы понимаем, что за один оборот вала редуктора вал электромотора повернется 45 раз.
-
Количество импульсов энкодера за один оборот. Исходя из таблицы с характеристиками электромотора это число равно 13.
Умножаем значение передаточного числа на количество импульсов энкодера и получаем значение количества импульсов энкодера, сформированных за один оборот редуктора.
45 * 13 = 585
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
|
// Присваиваем переменной значение количества импульсов энкодера за один оборот вала редуктора равное 585.
#define ENC_COUNT_REV 585
#define ENCR_A 34
// Other encoder output to Arduino to keep track of wheel direction
// По каналу энкодера В будем отслеживать направление вращения электромотора
#define ENCR_B 35
// True = Forward; False = Reverse
boolean Direction_right = true;
// Считаем количество импульсов правого колеса
volatile long right_wheel_pulse_count = 0;
// Устанавливаем интервал для измерений равный 1 секунде
int interval = 1000;
// Счетчики миллисекунд в течение интервала
long previousMillis = 0;
long currentMillis = 0;
// Переменная для измерения частоты вращения
float rpm_right = 0;
// Переменная для измерения угловой скорости
float ang_velocity_right = 0;
float ang_velocity_right_deg = 0;
const float rpm_to_radians = 0.10471975512;
const float rad_to_deg = 57.29578;
void setup() {
// Открываем последовательный порт
Serial.begin(9600);
pinMode(ENCR_A , INPUT_PULLUP);
pinMode(ENCR_B , INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCR_A),right_wheel_pulse, RISING);
}
void loop() {
// Сохраняем время
currentMillis = millis();
if (currentMillis - previousMillis > interval) {
previousMillis = currentMillis;
// Рассчитываем количество оборотов в минуту
rpm_right = (float)(right_wheel_pulse_count * 60 / ENC_COUNT_REV);
ang_velocity_right = rpm_right * rpm_to_radians;
ang_velocity_right_deg = ang_velocity_right * rad_to_deg;
Serial.print(" Pulses: ");
Serial.println(right_wheel_pulse_count);
Serial.print(" Speed: ");
Serial.print(rpm_right);
Serial.println(" RPM");
Serial.print(" Angular Velocity: ");
Serial.print(rpm_right);
Serial.print(" rad per second");
Serial.print(" ");
Serial.print(ang_velocity_right_deg);
Serial.println(" deg per second");
Serial.println();
right_wheel_pulse_count = 0;
}
}
// Увеличиваем количество импульсов на 1
void right_wheel_pulse() {
// Считайте значение энкодера канала В для правого колеса
int val = digitalRead(ENCR_B);
if(val == LOW) {
Direction_right = false; // Вал вращается в обратном направлении
}
else {
Direction_right = true; // Вал вращается в прямом направлении
}
if (Direction_right) {
right_wheel_pulse_count++;
}
else {
right_wheel_pulse_count--;
}
}
|
Загружаем код в контроллер, открываем монитор порта и начинаем вращать вал редуктора вперед и назад. На экране последовательного порта видим следующие изображения.
Эти значения выводятся при вращении вала редуктора вперед.
Эти значения выводятся на экран при вращении вала редуктора назад.
В этом эксперименте мы рассчитали угловую скорость вращения вала редуктора.
Мы знаем диаметр нашего колеса, который крепится на вал редуктора. Он равен 65 мм. Что бы посчитать линейную скорость, нам нужно радиус колеса(в метрах) умножить на угловую скорость(в радианах в секунду).
v = r * ω
Радиус колеса(м) – 0,032 м
Угловая скорость – 40 рад/с
Линейная скорость = 0,0325 х 40 = 1,3 м/с