Кинетическое управление мобильной платформой на Arduino


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

Для этого нам понадобится:
1) 4 двигателя с редуктором и 4 колеса, например, отсюда

2) Драйвер двигателя L289

3) Плата Arduino 2 шт, можно использовать 2 Arduino Uno, но я использую Arduino Mega и Iskra Neo (аналог Leonardo)

4) NRF24L01 2 шт

5)Трехосный акселерометр(я использую LIS331DLH)

6) Преобразователь линейный понижающий на базе ams1117-3.3, 3.3V 1A

7) Arduino proto shield

8) Отсек для батареек на 6/8 шт
9) Соединительные провода

Итак, для начала необходимо собрать мобильную платформу по следующей схеме:

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

Драйвер двигателя подключается так:
Порт драйвера Порт Arduino
12V К «+» батарейного отсека/VIN
GND К «-» батарейного отсека/GND
ENA 9
IN1 2
IN2 3
IN3 4
IN4 5
ENB 10
Подключение преобразователя
Преобразователь Arduino
Vin 5V
GND GND
Vout В питание NRF24L01

Подключение NRF24L01

NRF24L01 Arduino
1 GND
2 Vout понижающего преобразователя
3 7
4 8
5 SCK
6 MOSI
7 MISO
8 Не подключаем

В итоге у меня это выглядит так:

Далее собираем «пульт» для управления нашей машинкой по следующей схеме:

NRF24L01 подключается аналогично, единственное отличие, в том что второй контакт NRF идет в восьмой порт Arduino, а третий – в девятый.
Акселерометр подключается по шине I2c
SDA SDA
SCL SCL
GND GND
5V 5V
Для того, чтобы закрепить акселерометр на руке можно собрать на отдельной плате этот модуль, а arduino закрепить, скажем, на поясе, либо же собрать схему на proto-шилде и крепить к руке вместе с arduino. Я выбрал второй вариант.


И общий вид:

После сборки нам необходимо соотнести положения руки и показания акселерометра. Я использовал следующую схему управления (Ваши показания могут отличаться):

Положение руки Действия машинки
Рука горизонтально Не двигаемся
Рука Вверх(от 3 до 6)/Вниз (от -6 до -3) по оси X Движение вперед/назад
Рука Влево(от -10 до -3)/Вправо от (3 до 10)по оси Y Движение против/по часовой стрелки

Далее определим протокол обмена между пультом и платформой:
NRF модуль отправляет трехбайтовый массив
Data[0] – определяет направление движения 1 – вперед, 2 – назад, 0 – стоим.
Data[1] – определяет скорость движения вперед/назад
Data[2] – определяет направление движения по/против часовой стрелки: 0 – не крутимся, 1 – против часовой 2 – по часовой

Исходные коды скетчей и библиотеки (RF24-master, Troyka-IMU-master) можно найти на Github’е либо в конце статьи.

Код для определения показаний акселерометра:

//ТК я работаю с несколькими платами, и у некоторых из них
//аппаратный и программный  UART, отличаются я использую следующее "переименование"
#define SerialPC Serial
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include "troyka-imu.h"

Accelerometer accel;

void setup()
{
  // открываем последовательный порт
  SerialPC.begin(115200);
  delay(3000);
  // пока не появились данные с USB
  // ждём
  //while (!SerialUSB.available()) {}
  // выводим сообщение о начале инициализации
  SerialPC.println("Begin init...");
  // инициализация акселерометра
  accel.begin();
  // выводим сообщение об удачной инициализации
  SerialPC.println("Init completed");
}

void loop()
{
  /*
  	!!!ВАЖНО!!!
  	библиотека возвращает значение приблизительно от -10.00 до +10.00
  	Причем значения типа float(1.0, 1.53 9.87 и так далее)
  	у меня не было необходимости получать значения с сотыми,
  	Поэтому в данном коде используется тип int, дробная часть отбрасывается
  */
  // Считываем значение по оси X и выводим его в Serial
  int x = accel.readX_G();
  SerialPC.print(x);
  SerialPC.print("\t");
  // Считываем значение по оси Y и выводим его в Serial
  int y = accel.readY_G();
  SerialPC.print(y);
  SerialPC.print("\t");
  //Считываем значение по оси Z и выводим его в Serial
  int z = accel.readZ_G();
  SerialPC.print(z);
  SerialPC.print("\t\t");

  SerialPC.println("");
  delay(300);
}

Код для пульта, все, что относится к Serial используется только для отладки

#define SerialPC Serial
//Минимальное и максимальное значения ШИМ для движения вперед/назад
#define MIN_PWM 100
#define MAX_PWM 250
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <troyka-imu.h>
//Библиотеки для работы с NRF24
#include <SPI.h>
#include "RF24.h"
//Переменные, определяющие направление движения
bool left = false;
bool right = false;
bool top = false;
bool down = false;

// создаём объект для работы с акселерометром
Accelerometer accel;
/* Hardware configuration: Set up nRF24L01 radio on SPI bus plus pins 9 & 10 */
RF24 radio(9, 10);
const uint64_t pipe = 0xF0F1F2F3F4LL; // индитификатор передачи, "труба"
//Массив данных
byte data[] = {0, 0, 0};
void setup() {
  //Задержка перед включением пульта
  //Нужна была для отладки
  //delay(5000);
  //инициализация Serial соединения
  SerialPC.begin(115200);
  SerialPC.println("setup");

  // инициализация акселерометра
  SerialPC.println("Init accel");
  accel.begin();
  SerialPC.println("Accel init");
  delay(100);

  //Инициализация NRF24L01
  SerialPC.println("Init radio");
  radio.begin();
  SerialPC.println("radio init");
  delay(100);
  //Установка параметров NRF24L01
  SerialPC.println("Set radio param");
  // канал (0-127)
  radio.setChannel(9);
  // скорость, RF24_250KBPS, RF24_1MBPS или RF24_2MBPS
  // RF24_250KBPS на nRF24L01 (без +) неработает.
  // меньше скорость, выше чувствительность приемника.
  radio.setDataRate(RF24_1MBPS);
  // мощность передатчика, RF24_PA_MIN=-18dBm, RF24_PA_LOW=-12dBm, RF24_PA_MED=-6dBM,
  radio.setPALevel(RF24_PA_HIGH);
  // открываем трубу на передачу.
  radio.openWritingPipe(pipe);
  SerialPC.println("end set radio param");
  delay(100);

  SerialPC.println("end setup");
}
//В этих переменных будем хранить значения от акселерометра
int x, y, z;
//Скорость
int speed = 0;

void loop() {
  SerialPC.println("loop");

  //Читаем значения от акселерометра
  SerialPC.print("read x: ");
  x = accel.readX_G();
  SerialPC.println(x);
  SerialPC.print("read y: ");
  y = accel.readY_G();
  SerialPC.println(y);
  SerialPC.print("read z: ");
  z = accel.readZ_G();
  Serial.println(z);
  SerialPC.println("end read.");

  //Устанавливаем значения направления
  //и скорости "по умолчанию"
  //ТЕ нет поворота, нет движения, скорость = 0

  //Нет поворота
  left = false;
  right = false;

  //Нет движения
  down = false;
  top = false;
  speed = 0;
  //Определяем есть ли поворот руки
  //Поворот направо
  if ( y > 3) {
    right = true;
  }//Поворот налево
  else if ( y < -3) {
    left = true;
  }

  //Определяем наклоняется ли рука
  //Рука вниз
  if ( x <= -3) {
    down = true;
    //Приводим значение x в диапазон от -6 до -3, затем
    //отображаем это значение на диапазон от MIN_PWM до MAX_PWM
    speed = map(constrain(x, -6, -3), -3, -6, MIN_PWM, MAX_PWM);
  }//рука вверх
  else if (x >= 3) {
    top = true;
    //Приводим значение x в диапазон от 3 до 6, затем
    //отображаем это значение на диапазон от MIN_PWM до MAX_PWM
    speed = map(constrain(x, 3, 6), 3, 6, MIN_PWM, MAX_PWM);
  }
  //Заносим значения в массив, который будем отправлять
  //Направление вперед/назад
  //конструкция a = (b) ? 1 : 2; равносильна
  // if (b) { a = 1; } else {a = 2;}
  data[0] = (top) ? 1 : (down) ? 2 : 0;
  //Скорость
  data[1] = speed;
  //Движение по/против часовой стрелки
  data[2] = (left) ? 1 : (right) ? 2 : 0;

  //Выводим значения перед отправкой в Serial
  //Для отладки
  for (int i = 0; i < 3; i++) {
    SerialPC.print(data[i]);
    SerialPC.print(" ");
  }
  SerialPC.println("");
  SerialPC.println("clear buff");
  //Отчищаем буфер перед отправкой (необходимо на некоторых модулях)
  radio.flush_tx();
  SerialPC.println("sending...");
  //Пытаемся отправить
  if (!radio.write(data, sizeof(data))) {
    //Если не удалось отправить выводим в Serial сообщение об ошибке
    SerialPC.println("fail");
  }

  delay(100);
  SerialPC.println("end loop");
}

Код для мобильной платформы:

//Serial для соединения с ПК
#define SerialPC Serial
//Время в мс, в течение которого должна придти хотя бы 1 команда
#define TIMEOUT 1000
//Библиотеки для работы с NRF24
#include <SPI.h>
#include "RF24.h"
/* CE и CSN на 7 и 8 пинах */
RF24 radio(7, 8);
//Адрес трубы
const uint64_t pipe = 0xF0F1F2F3F4LL;
//Пины для управления моторами In1, In2, ENA(PWM)
const byte motor1[] = {2, 3, 9};
//Пины для управления моторами In3, In4, ENB (PWM)
const byte motor2[] = {4, 5, 10};
//Формат передаваемых данных
byte data[] = {0, 0, 0};
//Время прихода последней команды
unsigned long lastCommand;
void setup() {
  //Выставляем пины на выход
  for (int i = 0; i < 3; i++) {
    pinMode(motor1[i], OUTPUT);
    pinMode(motor2[i], OUTPUT);
    digitalWrite(motor1[i], LOW);
    digitalWrite(motor2[i], LOW);
  }

  //Инициализация модуля NRF24
  radio.begin();
  delay(20);
  //Настройки модуля
  // канал (0-127)
  radio.setChannel(9);
  // скорость, RF24_250KBPS/RF24_1MBPS/RF24_2MBPS.
  radio.setDataRate(RF24_1MBPS);
  //мощность передатчика RF24_PA_MIN=-18dBm, RF24_PA_LOW=-12dBm, RF24_PA_MED=-6dBM,
  radio.setPALevel(RF24_PA_HIGH);
  //Открываем трубу на чтение
  radio.openReadingPipe(1, pipe);
  //Начинаем слушать эфир
  radio.startListening();
  //устанавливаем время прихода последней команды
  lastCommand = millis();
}

void loop() {
  //Если время прошедшее с момента последней команды больше чем TIMEOUT,
  //считаем, что произошел обрыв связи и останавливаем машинку
  if (millis() - lastCommand > TIMEOUT) {
    stoped();
  }

  //Если что-то пришло по радио
  if (radio.available()) {
    //Читаем данные
    radio.read(data, sizeof(data));
    //Если 1 - едем вперед, если 2 - едем назад, иначе останавливаемся
    (data[0] == 1) ? forward() : (data[0] == 2) ? back() : stoped();
    //Если разворачиваемся 1 - налево, 2 - направо, 0 ничего не меняем
    if (data[2] == 1) {
      left();
    }
    else if (data[2] == 2) {
      right();
    }

    //Фиксируем время прихода команды
    lastCommand = millis();
  }

}
//Останавливаемся и блокируем моторы
void stoped() {
  setMotorParam(false, false, false, false, 0, 0);
}

void forward() {
  setMotorParam(true, false, true, false, data[1], data[1]);
}

void back() {
  setMotorParam(false, true, false, true, data[1], data[1]);
}

void left() {
  setMotorParam(true, false, false, true, 255, 255);
}

void right() {
  setMotorParam(false, true, true, false, 255, 255);
}
/* d11 - EN1, d12 - EN2
 * d21 - EN3, d22 - EN4
 * s1 - ENA, s2 - ENB
 */
void setMotorParam(bool d11, bool d12, bool d21, bool d22, int s1, int s2) {
  //Устанавливаем направление движения моторов
  digitalWrite(motor1[0], d11);
  digitalWrite(motor1[1], d12);

  digitalWrite(motor2[0], d21);
  digitalWrite(motor2[1], d22);

  //Устанавливаем скорость вращения моторов
  analogWrite(motor1[2], s1);
  analogWrite(motor2[2], s2);
}

Исходные коды
Библиотеки


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

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение