Управление движением робота с одного джойстика

В данной статье будет рассмотрен алгоритм управления 4-х колесным роботом с помощью одного джойстика.

Принцип действия

Его составной частью будет алгоритм «архивации» скорости.

Для начала рассмотрим принцип движения нашего робота (немного известной вам теории). Его ходовая часть состоит из 4-х мотор-редукторов (по одному на каждое колесо). Поэтому движение происходит следующим образом:

Вперед. Все 4 двигателя вращаются вперед с равной скоростью.

Назад. Все 4 двигателя вращаются назад с равной скоростью.

Плавный поворот направо. Правые двигатели вращаются медленнее левых. Но все 4 вращаются в одну сторону.

Плавный поворот налево. Левые двигатели вращаются медленнее правых. Но все 4 вращаются в одну сторону.

Резкий поворот направо. Левые двигатели вращаются вперед, правые назад. Если все двигатели вращаются с одной скоростью, то робот развернется на месте.

Резкий поворот налево. Правые двигатели вращаются вперед, левые двигатели вращаются назад. Если все 4 вращаются с одной скоростью, то робот развернется на месте.

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

Схема соединения

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

Теперь рассмотрим как считываются данные с джойстика на пульте. VCC модуля соединяем с 5V Ардуино, GND с GND, потенциометр горизонтальной оси с A0, вертикальной с A1, кнопку не подключаем, так как задействовать не будем. Схема приведена ниже. Вид модуля может отличаться, но пины будут такими же (кроме случаев с отдельным подводом VCC и GND к каждому потенциометру и кнопке).

Считав данные с джойстика функциями analogRead(A0) (можно заменить А0 на horizontalSticPin) и analogRead(A1) (можно заменить А1 на verticalSticPin) получим следующие диапазоны значений. Вперед 0 на вертикальной оси, назад 1023 на вертикальной оси, резкий поворот вправо 0 на горизонтальной оси, резкий разворот влево 1023 на горизонтальной оси. Значения между ними будут давать градации поворотов: положение 0,0 даст плавный правый поворот вперед, а значение 1023,1023 даст плавный левый поворот назад.

Алгоритм

Исходя из вышеописанного составляем алгоритм, преобразующий показания горизонтальной и вертикальной осей джойстика в две «вертикальные» оси скорости двигателей.

Вот его принцип:

1. Вводим следующие константы:
   - verticalSticCenter – значения, соответствующие центральному положению джойстика в вертикальной оси (средняя скорость робота равна 0);
   - horizontalSticPin – значения, соответствующие центральному положению джойстика в горизонтальной оси (скорость разворота равна 0)
   - k – коэффициент отклонения, от центральных положений, позволяет исключить влияния «шумов» на показания в положении полной остановки робота)
2. Считываем показания вертикальной оси потенциометра в переменную speed, хранящую значение средней скорости. Считанные значения нужно перевести в диапазон типа byte, разделив на 4 (можно использовать функцию map), в таком случае средняя скорость будет изменяться от 255 до 0. Затем вычесть из них значение verticalSticCenter, теперь средняя скорость изменяется от 128 до -128. Но для удобства расчетов хотелось бы видеть изменения скорости в диапазоне от -128 до 128 (тогда центральное положение будет давать 0, что очень важно), для этого делим показания на единицу.
3. Смотрим, если значения относительной скорости отличаются от 0 менее чем на k (джойстик находиться в центральном положении по вертикальной оси), то приравниваем ее к 0.
4. Считываем значения горизонтальной оси в переменную horizontal_value так же приведя к диапазону byte, будем называть их скоростью поворота.
5. Если значения скорости поворота отклонились более, чем на k от центра в право, то скорость левого двигателя приравниваем к сумме средней скорости и величине отклонения в угловой скорости от условного 0 (вычитаем значения скорости поворота из значений ее центра), а скорость правого к их разности.
6. Если значения скорости поворота отклонились более, чем на k от центра влево, то скорость левого двигателя приравниваем к разности средней скорости и величине отклонения от условного центра (из скорости поворота вычитаем значения условного 0), а правого к их сумме.
7. Не выполнение условий пунктов 5 и 6 говорит о том, что значения скорости поворота лежат в пределах центра (условного 0), в этом случае приравниваем скорости обоих двигателей к средней скорости.
8. Теперь необходимо ограничить полученные значения скоростей, что бы по модулю они не выходили за половину максимального значения типа byte для предотвращения переполнения переменных в пункте 9. Просто сравним значения с константно-объявленной границей, и если они больше, то приравняем их к ней.
9. Сейчас скорости двигателей лежат в диапазоне от -128 до 128, но мы хотим хранить их в массиве типа byte, который не может хранить отрицательные числа, поэтому «зашифруем их»:
   - 0 элемент массива data будет хранить скорость левого двигателя, а 1 скорость второго.
   - Если скорость будет отрицательной, мы положим в data ее модуль, а если положительной, то прибавим к ней половину диапазона значений типа byte. Таким образом значения от 0 до 128 хранят скорость вращения назад, а значения от 128 до 255 вперед.

Коды проекта

Ниже вы можете скачать или скопировать необходимые скетчи для данного урока. Далее я объясню все основные моменты программирования проекта. Также перед началом работы нужно скачать и установить необходимые библиотеки:

Теперь переходим непосредственно к коду проекта.

#include <SPI.h> // подключаем библиотеку для работы с последовательным периферийным интерфейсом
#include <RF24.h> // подключаем библиотеку для работы с самим модулем NRF24L01

#define horizontalSticPin A0  // пин на который подключена вертикальная ось джостика
#define verticalSticPin A1    // пин горизонтальной оси джостика
#define horizontalSticCenter 130 // значение "0" по горизонтальной оси в приведенных значениях
#define verticalSticCenter 125 // значение "0" по вертикальной оси в приведенных значениях
#define k 3                 // коэффициент отклонения от центра
#define max_speed 127         // максимальное значение скорости. Используется для кодировки скорости
#define min_speed -127     // минимальное значение скорости. Используется для кодировки скорости

byte data[2];  // массив, который передается роботу. Хранит "зашифрованные" скорости двигателей
int speed_left, speed_right;   // переменные, хранящие в вычислениях скорости левого и правого двигателей

int speed, horizontal_value;  // переменные, хранящие среднюю скорость и скорость поворота

const uint32_t pipe1 = 123456789; // адрес данного модуля связи в канале

RF24 radio(9, 10); // пины связи с модулем. Можно использовать любые (порты 15-19 CSN CE MOSI MISO SCK)

/*-------------------- функция "зашифровки" значений скорости ---------------*/

void getValue() {
  speed = -1 * (analogRead(verticalSticPin) / 4 - verticalSticCenter); // 2.
  if (speed < k && speed > -1 * k) speed = 0;                          // 3.
  horizontal_value = analogRead(horizontalSticPin) / 4;                // 4.
  if (horizontal_value < horizontalSticCenter - k) {                   // 5.
    speed_left = speed + (horizontalSticCenter - horizontal_value);
    speed_right = speed - (horizontalSticCenter - horizontal_value);
  }
  else if (horizontal_value > horizontalSticCenter + k) {              // 6.
    speed_left = speed - (horizontal_value - horizontalSticCenter);
    speed_right = speed + (horizontal_value - horizontalSticCenter);
  } else {                                                             // 7.
    speed_left = speed;
    speed_right = speed;
  }
  // 8.
  if (speed_left > max_speed) speed_left = max_speed;
  if (speed_right > max_speed) speed_right = max_speed;
  if (speed_left < min_speed) speed_left = min_speed;
  if (speed_right < min_speed) speed_right = min_speed;
  // 9.
  if (speed_left <= 0) data[0] = byte(-1 * speed_left);
  else if (speed_left > 0) data[0] = byte(128 + speed_left);
  if (speed_right <= 0)data[1] = byte(-1 * speed_right);
  else if (speed_right > 0) data[1] = byte(128 + speed_right);
}

/*--------- функция отправки значений модулю связи -----------*/

void sendData() {
  radio.write(&data, sizeof(data)); 
}

/*-------- функция вывода данных в терминал ------------------*/

void printData() {
  Serial.print(speed); // вывод средней скорости
  Serial.print("  "); // вывод разделяющего занчения пробела
  Serial.print(horizontal_value); // вывод скорости поворота
  Serial.print("  ");
  Serial.print(data[0]); // вывод скорости левого двигателя
  Serial.print("  ");
  Serial.print(data[1]); // вывод скорости правого двигателя
  Serial.println(); // перевод строки
}

void setup() {
  Serial.begin(9600);   // запуск последовательного порта (терминала) со скоростью 9600 
  radio.begin();                // запуск канала связи с радио модулем
  radio.setDataRate(RF24_2MBPS); // установка скорости обмена данными RF24_1MBPS или RF24_2MBPS
  radio.setCRCLength(RF24_CRC_8); // установка длины контрольной суммы 8-bit or 16-bit
  radio.setPALevel(RF24_PA_MAX); // установка уровеня питания усилителя RF24_PA_MIN(минимальный), RF24_PA_LOW(низкий), RF24_PA_HIGH(высокий) и RF24_PA_MAX(максимальный)
  // соответствует уровням:    -18dBm,      -12dBm,      -6dBM,           0dBm
  radio.setChannel(120);         // установка канала
  radio.setAutoAck(false);       // выключение автоответа
  radio.powerUp();               // включение или пониженное потребление powerDown - powerUp
  radio.openWritingPipe(pipe1);   // открытие канала отправки данных
}

void loop() {
  getValue();  // считываем и обрабатываем значения
  sendData();  // отправляем значения
  printData(); // выводим значения в терминал
}

#include <SPI.h>    // подключение библиотеку для работы с последовательным периферийным интерфейсом
#include <RF24.h>   // подключение библиотеку для работы с последовательным периферийным интерфейсом
#include <Servo.h>

RF24 radio(7, 8);   // назначение пинов связи с модулем
const uint32_t pipe = 123456789; // установка адреса данного радио модуля в канале
byte data[2];  // массив принимаемых с радио модуля значений

// пины, к которым подключен драйвер двигаеля

#define M1 6
#define M2 9
#define M3 3
#define M4 5

#define speed_border 128 // граница разделения диапазаона скорости

/*-------------- функция управления двигателями ----------------------------------------*/

void Motor(bool directoin_left, byte speed_left, bool direction_right, byte speed_right) {
  //(направление левого, скорость левого, направление правого, скрость правого) 1 это вперед, 0 это назад
  if (directoin_left == 1) {
    digitalWrite(M2, 0); // задаем направление движения
    analogWrite(M1, speed_left); // задаем скорость вращения
  }
  if (directoin_left == 0) {
    digitalWrite(M1, 0);// задаем направление движения
    analogWrite(M2, speed_left); // задаем скорость вращения
  }
  if (direction_right == 1) {
    digitalWrite(M4, 0); // задаем направление движения
    analogWrite(M3, speed_right); // задаем скорость вращения
  }
  if (direction_right == 0) {
    digitalWrite(M3, 0);// задаем направление движения
    analogWrite(M4, speed_right); // задаем скорость вращения
  }
}

/*-------------------------- функция "дешифровки" принятых значений ---------------------------------*/

void drive() {
  if (data[0] <= speed_border) {            
    if (data[1] <= speed_border) Motor(0, data[0], 0, data[1]);
    else if (data[1] > speed_border) Motor(0, data[0], 1, data[1] - speed_border);
  } else if (data[0] > speed_border) {
    if (data[1] <= speed_border) Motor(1, data[0] - speed_border, 0, data[1]);
    else if (data[1] > speed_border) Motor(1, data[0] - speed_border, 1, data[1] - speed_border);
  }
}

/*-------------------------- функция "дешифровки" принятых значений с домножением---------------------*/

void driveHigh() {
  if (data[0] <= speed_border) { 
    if (data[1] <= speed_border) Motor(0, data[0]*2, 0, data[1]*2);
    else if (data[1] > speed_border) Motor(0, data[0]*2, 1, (data[1] - speed_border)*2);
  } else if (data[0] > speed_border) {
    if (data[1] <= speed_border) Motor(1, (data[0] - speed_border)*2, 0, data[1]*2);
    else if (data[1] > speed_border) Motor(1, (data[0] - speed_border)*2, 1, (data[1] - speed_border)*2);
  }
}

/*----- функция получения данных с радио модуля ---------*/

void getData() {
  if (radio.available()){  // если новые данные доступны
    radio.read(&data, sizeof(data)); // считываем их в массив data
  }
}

/*---- функция вывода занчений в терминал ----------*/

void printData() {
  Serial.print(data[0]); 
  Serial.print("  ");
  Serial.print(data[1]);
  Serial.print("  ");
  Serial.println();
}

void setup(){
  Serial.begin(9600);       // запуск терминала (последовательного порта) со скоростью 9600
  radio.begin();            // запуск радиомодуля
  radio.setDataRate(RF24_2MBPS); // установка скорости обмена данными RF24_1MBPS или RF24_2MBPS
  radio.setCRCLength(RF24_CRC_8); // установка длины контрольной суммы 8-bit or 16-bit
  radio.setPALevel(RF24_PA_MAX); // установка уровеня питания усилителя RF24_PA_MIN(минимальный), RF24_PA_LOW(низкий), RF24_PA_HIGH(высокий) и RF24_PA_MAX(максимальный)
  // соответствует уровням:    -18dBm,      -12dBm,      -6dBM,           0dBm
  radio.setChannel(120);         // уствновка канала
  radio.setAutoAck(false);       // выключение автоответа
  radio.powerUp();               // включение или пониженное потребление powerDown - powerUp
  radio.openReadingPipe(1, pipe); // открытие канала на приём
  radio.startListening(); // запуск приема данных 
  // назначение пинов драйвера на выход
  pinMode(M1, OUTPUT); 
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);
}

void loop(){
  getData();    // считывание значений
  drive();      // обработка значений
  printData();  // вывод значений в терминал
}

Напишем этот алгоритм в функции getValue. Чтобы не перегружать код в комментариях стоят номера выше написанных пунктов, а не их текст.

Теперь нужно передать значения роботу через радио модуль и неплохо было бы вывести их в терминал. Воспользуемся функциями sendData и printData:

Осталось только вызвать функции в loop:

Переходим к роботу.

Тут нам нужно будет написать алгоритм «дешифровки значений». Для его работы понадобится функция управления двигателями.

Вначале программы объявим пины на которые подключен драйвер двигателей (если вы таковой используете). Константно зададим границу скорости (середину максимального значения типа byte). И напишем функцию управления двигателями.

Она принимает на вход 4 параметра:

• направление левого двигателя,
• его скорость,
• направление правого двигателя,
• его скорость.

1 – вращение вперед, 0 – назад. Скорость в диапазоне от 0 до 255. Как известно, ток течет от высокого потенциала к низкому, поэтому если подать на первый пин высокий сигнал, а на второй низкий, ток пойдет от первого ко второму и двигатель начнет вращаться в соответствующую сторону. Вместо постоянного высокого сигнала можно отправлять ШИМ сигнал, тогда мы сможем контролировать еще и скорость вращения, так и сделаем. Если принятое направление 1 (вперед), то на первый пин подадим низкий сигнал, а на второй ШИМ-ом принятую скорость, если направление 0, то наоборот. Для другого двигателя аналогично. Пропишем это в функции Motor.

Теперь можно писать сам алгоритм «дешифровки».

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

Этот алгоритм одинаков для обоих двигателей. Однако мы не можем передавать в функцию значения для двигателей по отдельности, для вызова функции мы должны сразу знать направления и скорости для обоих двигателей. Поэтому придется проверять условие сначала для левого, и если оно верно, то для правого и, если оно тоже верно, вызывать функцию Motor. Напишем это в функции drive. Сейчас скорость лежит в диапазоне от 0 до 128, а ШИМ сигнал имеет диапазон 0-255, совпадающий с диапазоном byte (именно поэтому использовался этот тип данных, ну еще он самый меньший из стандартных после bool, который нам не подходил). Поэтому скорость нужно умножать на 2. Предлагаю умножать значения скорости на 2 в функции driveHigh, а в drive передавать их без изменений, потом это может пригодиться.

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

Аналогично скетчу для пульта, выведем данные скоростей в терминал функцией printData. Вывод данных в терминал будет полезен при отладке программы, и потому обязательным не является.

Осталось лишь вызвать все функции в основном цикле loop.

Напоследок еще перспектива оптимизации. У каждого двигателя есть минимальные значения тока и напряжения, при которых он сможет работать, то есть двигатель робота будет вращаться только тогда, когда значения ШИМа превышают какой-то порог значений, назовем его «мертвым» интервалом. Этот порог в наибольшей степени зависит от мощности источника питания и самого двигателя, обычно он имеет значение от 30 до 70.

Поскольку двигатель в промежутке этих значений не работает, можно ими воспользоваться для кодирования чего-либо еще. При самом малом пороге в 30, мы получим 30 позиций кодировки с каждого двигателя, при комбинировании позиций с 2-х двигателей уже 900! Если от вашего робота не требуется мгновенная скорость отклика и количество отправляемых пакетов велико (>300-500), то можно потратить до 20% пакетов на отправку дополнительных кодов в «мертвых» интервалах, и вы этого не заметите.

Чем больше пакетов за 1 секунду получает робот, тем большее их количество в процентном соотношении можно потратить на отправку кодов, но чем больше их теряется, тем больший процент стоит оставлять для пакетов скорости, если вы хотите своевременного отклика. Для вылавливания этих пакетов нужно будет просто проверить, что значения скорости меньше заданной вами границы, а как обрабатывать их дальше (через математические вычисления, if или switch-case конструкции) и для чего применять подумайте сами.

Используя описанную в данной статье идею разбиения больших диапазонов на интервалы можно закодировать обе скорости для двигателей и еще какие-либо значения в одной переменной типа int, но нужно помнить, что одним пакетом мы можем передать столько кодов, сколько он содержал элементов массива, то есть при использовании массива из 2 элементов мы получали обе скорости за 1 пакет, а при использовании одной переменной нам пришлось бы использовать уже 2 пакета данный.