Точный джойстик-контроллер для робота MeArm: запись и воспроизведение координат на Arduino Uno

Компоненты и расходные материалы

• Arduino UNO — 1 шт.
• Аналоговый джойстик (generic) — 2 шт.
• Резистор 10 кОм — 2 шт.
• Резистор 680 Ом — 1 шт.
• Светодиод 5 мм красный — 1 шт.
• Микросервопривод SG90 — 4 шт.

Программное обеспечение и онлайн-сервисы

• Arduino IDE

О проекте

Контроллер на базе Arduino Uno для управления манипулятором MeArm® с двумя аналоговыми джойстиками и функцией записи/воспроизведения координат. Работает на базовой плате без дополнительных щитов. Требуется питание от USB (2А) или внешний источник 6В/2А. Протестировано на четырёх сервоприводах SG90.

Управление

• Джойстик 1 (кнопка): короткое нажатие — старт/стоп записи координат.
• Джойстик 2 (кнопка): короткое нажатие — старт/стоп воспроизведения; удержание 2 сек — автовоспроизведение в цикле.

Примечание: существует упрощённая версия без внешних резисторов и светодиода — «MeArm Robot - Recording of Coordinates (Minimalized Version)». Также есть версия для платы джойстика MeArm 1.6.1 с ИК-управлением.

Код (версия 1.3.1)

/* meArm analog joysticks version 1.3.1 - UtilStudio.com Dec 2018
 * Два аналоговых джойстика, четыре сервопривода.
 * В версии 1.3 улучшена запись координат, исправлены баги.
 * 
 * Джойстик 1: вперёд/назад, влево/вправо (база, плечо), кнопка — запись.
 * Джойстик 2: вверх/вниз, захват/разжатие (локоть, гриппер), кнопка — воспроизведение.
 * Удержание кнопки 2 сек — автовоспроизведение.
 * 
 * Пины:
 * Arduino     Stick1      Stick2      Base  Shoulder Elbow Gripper Rec/Autoplay
 * GND         GND         GND         Br    Br       Br    Br
 * 5V          VCC         VCC         Red   Red      Red   Red
 * A0          HOR         -           -     -        -     -
 * A1          VER         -           -     -        -     -
 * A2          -           HOR         -     -        -     -
 * A3          -           VER         -     -        -     -
 * PD2         BUTT        -           -     -        -     -
 * PD3         -           BUTT        -     -        -     -
 * 11          -           -           Yel   -        -     -
 * 10          -           -           -     Yel      -     -
 * 9           -           -           -     -        Yel   -
 * 6           -           -           -     -        -     Yel
 * PD4         -           -           -     -        -     -     LED
 */

#include <Servo.h>

bool repeatePlaying = false;      // циклическое воспроизведение
int delayBetweenCycles = 2000;    // пауза между циклами (мс)

int basePin = 11;      // сервопривод базы
int shoulderPin = 10;  // сервопривод плеча
int elbowPin = 9;      // сервопривод локтя
int gripperPin = 6;    // сервопривод гриппера

int xdirPin = 0;       // база - джойстик 1 (горизонталь)
int ydirPin = 1;       // плечо - джойстик 1 (вертикаль)
int zdirPin = 3;       // локоть - джойстик 2 (вертикаль)
int gdirPin = 2;       // гриппер - джойстик 2 (горизонталь)

int pinRecord = 2;     // кнопка записи (PD2, рекомендуется вместо A4)
int pinPlay = 3;       // кнопка воспроизведения (PD3, рекомендуется вместо A5)
int pinLedRecord = 4;  // светодиод индикации записи/автовоспроизведения

bool useInternalPullUpResistors = false;

const int buffSize = 512;  // размер буфера записи

// Начальные позиции сервоприводов
int startBase = 90, startShoulder = 90, startElbow = 90, startGripper = 0;
int posBase = 90, posShoulder = 90, posElbow = 90, posGripper = 0;
int lastBase = 90, lastShoulder = 90, lastElbow = 90, lastGripper = 90;

// Пределы движения
int minBase = 0, maxBase = 150;
int minShoulder = 0, maxShoulder = 150;
int minElbow = 0, maxElbow = 150;
int minGripper = 0, maxGripper = 150;

const int countServo = 4;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0, playPos = 0;

int buttonRecord = HIGH, buttonPlay = HIGH;
int buttonRecordLast = LOW, buttonPlayLast = LOW;
bool record = false, play = false, debug = false;
String command = "Manual";
int printPos = 0;
int buttonPlayDelay = 20, buttonPlayCount = 0;
bool ledLight = false;

Servo servoBase, servoShoulder, servoElbow, servoGripper;

void setup() {
  Serial.begin(9600);
  
  if (useInternalPullUpResistors) {
    pinMode(pinRecord, INPUT_PULLUP);
    pinMode(pinPlay, INPUT_PULLUP);
  } else {
    pinMode(pinRecord, INPUT);
    pinMode(pinPlay, INPUT);
  }
  
  pinMode(xdirPin, INPUT);
  pinMode(ydirPin, INPUT);
  pinMode(zdirPin, INPUT);
  pinMode(gdirPin, INPUT);
  pinMode(pinLedRecord, OUTPUT);
  
  servoBase.attach(basePin);
  servoShoulder.attach(shoulderPin);
  servoElbow.attach(elbowPin);
  servoGripper.attach(gripperPin);
  
  StartPosition();
  digitalWrite(pinLedRecord, HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void loop() {
  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  // Обработка удержания кнопки воспроизведения для авторежима
  if (buttonPlay == LOW) {
    buttonPlayCount++;
    if (buttonPlayCount >= buttonPlayDelay) repeatePlaying = true;
  } else buttonPlayCount = 0;

  // Переключение режима воспроизведения
  if (buttonPlay != buttonPlayLast) {
    if (record) record = false;
    if (buttonPlay == LOW) {
      play = !play;
      repeatePlaying = false;
      if (play) StartPosition();
    }
  }

  // Переключение режима записи
  if (buttonRecord != buttonRecordLast) {
    if (buttonRecord == LOW) {
      record = !record;
      if (record) {
        play = false; repeatePlaying = false; recPos = 0;
      } else if (debug) PrintBuffer();
    }
  }

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  // Чтение джойстиков с фильтрацией мёртвой зоны
  float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
  float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);

  if (abs(dx) < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  posBase += dx;
  posShoulder += dy;
  posElbow += dz;
  posGripper += dg;

  // Режим воспроизведения
  if (play) {
    if (playPos >= recPos) {
      playPos = 0;
      if (repeatePlaying) {
        delay(delayBetweenCycles);
        StartPosition();
      } else play = false;
    }
    
    bool endOfData = false;
    while (!endOfData) {
      if (playPos >= buffSize - 1) break;
      if (playPos >= recPos) break;
      
      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x3000;
      endOfData = data & 0x4000;
      
      switch (servoNumber) {
        case 0x0000: posBase = angle; break;
        case 0x1000: posShoulder = angle; break;
        case 0x2000: posElbow = angle; break;
        case 0x3000: posGripper = angle; dg = posGripper - lastGripper; break;
      }
      playPos++;
    }
  }

  // Ограничение диапазонов
  posBase = constrain(posBase, minBase, maxBase);
  posShoulder = constrain(posShoulder, minShoulder, maxShoulder);
  posElbow = constrain(posElbow, minElbow, maxElbow);
  posGripper = constrain(posGripper, minGripper, maxGripper);

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  
  bool waitGripper = false;
  if (dg < 0) { posGripper = minGripper; waitGripper = true; }
  else if (dg > 0) { posGripper = maxGripper; waitGripper = true; }
  servoGripper.write(posGripper);
  
  if (play && waitGripper) delay(1000);

  // Запись изменений в буфер
  if (lastBase != posBase || lastShoulder != posShoulder || 
      lastElbow != posElbow || lastGripper != posGripper) {
    
    if (record && recPos < buffSize - countServo) {
      int buffPos = 0;
      if (lastBase != posBase) { buffAdd[buffPos] = posBase; buffPos++; }
      if (lastShoulder != posShoulder) { buffAdd[buffPos] = posShoulder | 0x1000; buffPos++; }
      if (lastElbow != posElbow) { buffAdd[buffPos] = posElbow | 0x2000; buffPos++; }
      if (lastGripper != posGripper) { buffAdd[buffPos] = posGripper | 0x3000; buffPos++; }
      
      buffAdd[buffPos - 1] |= 0x4000;  // метка конца кадра
      for (int i = 0; i < buffPos; i++) buff[recPos + i] = buffAdd[i];
      recPos += buffPos;
    }
    
    command = "Manual"; printPos = 0;
    if (play) { command = "Play"; printPos = playPos; }
    else if (record) { command = "Record"; printPos = recPos; }
    
    Serial.print(command); Serial.print("\t");
    Serial.print(printPos); Serial.print("\t");
    Serial.print(posBase); Serial.print("\t");
    Serial.print(posShoulder); Serial.print("\t");
    Serial.print(posElbow); Serial.print("\t");
    Serial.print(posGripper); Serial.print("\t");
    Serial.print(record); Serial.print("\t");
    Serial.println(play);
  }

  lastBase = posBase; lastShoulder = posShoulder;
  lastElbow = posElbow; lastGripper = posGripper;

  // Индикация светодиодом
  if (repeatePlaying) ledLight = !ledLight;
  else { if (ledLight) ledLight = false; if (record) ledLight = true; }
  digitalWrite(pinLedRecord, ledLight);
  
  delay(50);
}

void PrintBuffer() {
  for (int i = 0; i < recPos; i++) {
    int data = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x3000;
    bool endOfData = data & 0x4000;
    Serial.print("Servo="); Serial.print(servoNumber);
    Serial.print("\tAngle="); Serial.print(angle);
    Serial.print("\tEnd="); Serial.print(endOfData);
    Serial.print("\tData="); Serial.println(data, BIN);
  }
}

void StartPosition() {
  int angleBase = servoBase.read();
  int angleShoulder = servoShoulder.read();
  int angleElbow = servoElbow.read();
  int angleGripper = servoGripper.read();
  
  Serial.print(angleBase); Serial.print("\t");
  Serial.print(angleShoulder); Serial.print("\t");
  Serial.print(angleElbow); Serial.print("\t");
  Serial.println(angleGripper);
  
  posBase = startBase; posShoulder = startShoulder;
  posElbow = startElbow; posGripper = startGripper;
  
  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoGripper.write(posGripper);
}

Схемы

Схема подключения джойстик-контроллера для MeArm® с записью координат. Также показана тестовая плата с двумя подключёнными сервоприводами (для четырёх см. схему).