Роботизированная рука Arduino на колесной платформе: схема и программа

В этом уроке мы рассмотрим как совместить ранее рассмотренную нами роботизированную платформу Mecanum Wheels с напечатанной на 3D-принтере роботизированной рукой на основе платы Arduino, которые будут работать в "связке" и автоматически выполнять заданные операции. Видео с работой этого амбициозного проекта вы можете посмотреть в конце статьи.

Обзор проекта

Итак, мы можем управлять колесным роботом Mecanum с помощью специального приложения Android, как было объяснено в предыдущей статье. В дополнение к этому, теперь в приложении есть кнопки для управления рукой робота.

В оригинальном приложении для управления роботизированной рукой на самом деле были ползунки для управления сочленениями робота, но это вызывало некоторые проблемы со стабильностью руки. Таким образом, рука работает намного лучше, поэтому я предоставлю эту обновленную версию приложения для управления роботизированной рукой и код Arduino для оригинального проекта роботизированной руки.

Тем не менее, самая крутая особенность этого робота — способность запоминать движения, а затем автоматически их повторять.

Используя кнопку Save, мы можем просто сохранить положения двигателей для каждого шага. Затем нам нужно просто нажать кнопку Run, и робот автоматически повторит сохраненные движения снова и снова.

Создание робота Arduino

Итак, у нас уже собрана колесная платформа Mecanum.

Кроме того, у меня есть напечатанные на 3D-принтере детали руки робота и серводвигатели, и теперь я покажу вам, как их собрать вместе.

Файлы для загрузки 3D-моделей и STL

Вот 3D-модель этого проекта.

Эту 3D-модель, а также файлы STL для 3D-печати можно скачать на сайте Cults3D.

Сборка

Первый сервопривод руки робота будет установлен непосредственно на верхней крышке платформы механического колеса.

Я отметил место и с помощью сверла диаметром 10 мм просверлил несколько отверстий.

Затем, используя рашпиль, я прорезал отверстия и затем точно подогнал отверстие для сервопривода. Я закрепил сервопривод на верхней пластине с помощью четырех болтов М3 и гаек.

Закрепление первого сервопривода роботизированной руки

Затем на выходном валу этого сервопривода, используя круглый рог, который идет в комплекте с сервоприводом, нам нужно прикрепить следующую часть или талию руки робота. Однако мы можем заметить, что таким образом часть остается примерно на 8 мм выше пластины. Поэтому я прикрепил два куска 8-миллиметровых досок МДФ, чтобы талия могла скользить по ним, и соединение было бы более устойчивым.

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

Далее у нас плечевой сервопривод. Мы просто устанавливаем его на место и крепим к 3D-печатной детали с помощью саморезов.

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

Следует отметить, что перед тем, как закрепить детали, нам нужно убедиться, что деталь имеет полный диапазон движения. Здесь я также добавил резинку к плечевому суставу, чтобы она немного помогала сервоприводу, поскольку этот сервопривод несет вес остальной части руки, а также полезную нагрузку.

Аналогичным образом я собрал остальную часть руки робота.

Далее нам нужно собрать механизм захвата. Захват управляется серводвигателем SG90, на который мы сначала прикрепляем специально разработанную зубчатую связь. Мы соединяем эту связь с другой зубчатой связью с другой стороны, которая крепится с помощью болта и гаек M4. На самом деле, все остальные связи соединяются с помощью болтов и гаек M4.

Первоначально 3D-модель захвата имела отверстия диаметром 3 мм, но у меня не было достаточно болтов М3, поэтому я расширил отверстия сверлом диаметром 4 мм и вместо них использовал болты М4. Собрав механизм захвата, я закрепил его на последнем сервоприводе, и на этом манипулятор робота был готов.

Затем я занялся прокладкой кабелей. Я пропустил провода сервопривода через специально предназначенные отверстия в руке робота. Используя 10-миллиметровое сверло, я сделал отверстие в верхней пластине, чтобы провода могли пройти.

С помощью стяжки я закрепил все провода вместе, и теперь осталось подключить их к плате Arduino.

Схема робота

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

Необходимые компоненты

Плата Arduino Mega 2560 (купить на AliExpress).
Bluetooth модуль HC-05 (купить на AliExpress).
Шаговый двигатель NEMA 17 (купить на AliExpress).
Сервомоторы MG-996 (купить на AliExpress).
Сервомоторы SG 90 (купить на AliExpress).
DRV8825 Stepper Driver Module (модуль драйвера шагового двигателя) (купить на AliExpress).
Li-Po аккумулятор.

Реклама: ООО "АЛИБАБА.КОМ (РУ)" ИНН: 7703380158

Сборка электронной части проекта

В предыдущей статье мы рассматривали, как работает колесная часть робота Mecanum, а также сделали для нее специальную печатную плату.

Я включил регулятор напряжения 5 В на эту печатную плату, чтобы мы могли сделать этот проект или подключить серводвигатели, поскольку они работают при 5 В. Регулятор напряжения — это LM350, который может выдерживать ток до 3 А. Все шесть сервоприводов руки робота могут потреблять ток от 2 А до 3 А, что означает, что он может их выдерживать, но это приведет к сильному нагреву регулятора.

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

Я подключил провода сигналов сервоприводов к цифровым контактам Arduino с номера 5 по 10, а для питания использовал разъем 5 В на печатной плате. Наконец, я протолкнул все провода внутрь платформы и закрепил на ней верхнюю пластину с помощью двух гаек.

Вот и все, теперь мы закончили этот проект.

Объяснение кода Arduino

Осталось посмотреть, как работает код Arduino и приложение Android. Здесь мы рассмотрим работу ключевых фрагментов кода, а полный исходный код приведен в конце статьи.

Итак, сначала нам нужно определить 6 сервоприводов, 4 шаговых двигателя и связь Bluetooth, а также определить некоторые переменные, необходимые для программы ниже. В разделе настройки мы устанавливаем максимальную скорость шаговых двигателей, определяем контакты, к которым подключены сервоприводы, инициализируем связь Bluetooth и устанавливаем руку робота в начальное положение.

#include <SoftwareSerial.h>
#include <AccelStepper.h>
#include <Servo.h>

Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;

SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)

// Define the stepper motors and the pins the will use
AccelStepper LeftBackWheel(1, 42, 43);   // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 40, 41);  // Stepper2
AccelStepper RightBackWheel(1, 44, 45);  // Stepper3
AccelStepper RightFrontWheel(1, 46, 47); // Stepper4

#define led 14

int wheelSpeed = 1500;

int lbw[50], lfw[50], rbw[50], rfw[50]; // arrays for storing positions/steps

int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;

void setup() {
  // Set initial seed values for the steppers
  LeftFrontWheel.setMaxSpeed(3000);
  LeftBackWheel.setMaxSpeed(3000);
  RightFrontWheel.setMaxSpeed(3000);
  RightBackWheel.setMaxSpeed(3000);
  pinMode(led, OUTPUT);
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  Bluetooth.begin(38400); // Default baud rate of the Bluetooth module
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);
  // Move robot arm to initial position
  servo1PPos = 90;
  servo01.write(servo1PPos);
  servo2PPos = 100;
  servo02.write(servo2PPos);
  servo3PPos = 120;
  servo03.write(servo3PPos);
  servo4PPos = 95;
  servo04.write(servo4PPos);
  servo5PPos = 60;
  servo05.write(servo5PPos);
  servo6PPos = 110;
  servo06.write(servo6PPos);
}

#include <SoftwareSerial.h>

#include <AccelStepper.h>

#include <Servo.h>

Servo servo01;

Servo servo02;

Servo servo03;

Servo servo04;

Servo servo05;

Servo servo06;

SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)

// Define the stepper motors and the pins the will use

AccelStepper LeftBackWheel(1, 42, 43); // (Type:driver, STEP, DIR) - Stepper1

AccelStepper LeftFrontWheel(1, 40, 41); // Stepper2

AccelStepper RightBackWheel(1, 44, 45); // Stepper3

AccelStepper RightFrontWheel(1, 46, 47); // Stepper4

#define led 14

int wheelSpeed = 1500;

int lbw[50], lfw[50], rbw[50], rfw[50]; // arrays for storing positions/steps

int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position

int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position

int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps

int speedDelay = 20;

int index = 0;

int dataIn;

int m = 0;

void setup() {

// Set initial seed values for the steppers

LeftFrontWheel.setMaxSpeed(3000);

LeftBackWheel.setMaxSpeed(3000);

RightFrontWheel.setMaxSpeed(3000);

RightBackWheel.setMaxSpeed(3000);

pinMode(led, OUTPUT);

servo01.attach(5);

servo02.attach(6);

servo03.attach(7);

servo04.attach(8);

servo05.attach(9);

servo06.attach(10);

Bluetooth.begin(38400); // Default baud rate of the Bluetooth module

Bluetooth.setTimeout(5);

delay(20);

Serial.begin(38400);

// Move robot arm to initial position

servo1PPos = 90;

servo01.write(servo1PPos);

servo2PPos = 100;

servo02.write(servo2PPos);

servo3PPos = 120;

servo03.write(servo3PPos);

servo4PPos = 95;

servo04.write(servo4PPos);

servo5PPos = 60;

servo05.write(servo5PPos);

servo6PPos = 110;

servo06.write(servo6PPos);

}

Затем в разделе цикла мы начинаем с проверки наличия входящих данных.

// Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.read();  // Read the data

// Check for incoming data

if (Bluetooth.available() > 0) {

dataIn = Bluetooth.read(); // Read the data

Эти данные поступают со смартфона из приложения Android, поэтому давайте посмотрим, какие данные оно на самом деле отправляет. Приложение Android создано с использованием онлайн-приложения MIT App Inventor. Оно состоит из простых кнопок, которые имеют соответствующие изображения в качестве фона. Более подробно работу с приложением MIT APP Inventor мы рассматривали в этой статье на нашем сайте.

Если мы посмотрим на блоки приложения, то увидим, что все, что оно делает, — это отправляет однобайтовые числа при нажатии кнопок.

Итак, в зависимости от нажатой кнопки, мы говорим Arduino, что делать. Например, если мы получаем число '2', платформа Mecanum Wheels будет двигаться вперед, используя пользовательскую функцию moveForward.

if (dataIn == 2) {
      m = 2;
    }
//
if (m == 2) {
      moveForward();
    }

if (dataIn == 2) {

m = 2;

}

if (m == 2) {

moveForward();

}

Эта пользовательская функция настраивает все четыре шаговых двигателя на вращение вперед.

void moveForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}

void moveForward() {

LeftFrontWheel.setSpeed(wheelSpeed);

LeftBackWheel.setSpeed(wheelSpeed);

RightFrontWheel.setSpeed(wheelSpeed);

RightBackWheel.setSpeed(wheelSpeed);

}

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

Для управления рукой робота мы используем тот же метод. Опять же, у нас есть кнопки в приложении, и при удерживании кнопок сочленения руки робота двигаются в определенном направлении.

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

Таким образом, мы просто отправляем одно 1-байтовое число при нажатии определенной кнопки.

Код Arduino входит в цикл while этого числа и остается там до тех пор, пока мы не нажмем кнопку, поскольку в этот момент мы отправляем число 0, что означает, что робот не должен ничего делать.

// Move servo 1 in positive direction
    while (m == 16) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo01.write(servo1PPos);
      servo1PPos++;
      delay(speedDelay);
    }
    // Move servo 1 in negative direction
    while (m == 17) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo01.write(servo1PPos);
      servo1PPos--;
      delay(speedDelay);
    }

// Move servo 1 in positive direction

while (m == 16) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo01.write(servo1PPos);

servo1PPos++;

delay(speedDelay);

}

// Move servo 1 in negative direction

while (m == 17) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo01.write(servo1PPos);

servo1PPos--;

delay(speedDelay);

}

Итак, в зависимости от нажатых кнопок сервоприводы движутся либо в положительном, либо в отрицательном направлении. Тот же принцип работы применяется ко всем серводвигателям. Для изменения скорости движения мы используем значения, поступающие от ползунка, которые находятся в диапазоне от 100 до 250.

// If arm speed slider is changed
    if (dataIn > 101 & dataIn < 250) {
      speedDelay = dataIn / 10; // Change servo speed (delay time)
    }

// If arm speed slider is changed

if (dataIn > 101 & dataIn < 250) {

speedDelay = dataIn / 10; // Change servo speed (delay time)

}

Разделив их на 10, мы получаем значения от 10 до 25, которые используются в качестве задержки в микросекундах в циклах while для управления сервоприводами.

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

// If button "SAVE" is pressed
    if (m == 12) {
      //if it's initial save, set the steppers position to 0
      if (index == 0) {
        LeftBackWheel.setCurrentPosition(0);
        LeftFrontWheel.setCurrentPosition(0);
        RightBackWheel.setCurrentPosition(0);
        RightFrontWheel.setCurrentPosition(0);
      }
      lbw[index] = LeftBackWheel.currentPosition();  // save position into the array
      lfw[index] = LeftFrontWheel.currentPosition();
      rbw[index] = RightBackWheel.currentPosition();
      rfw[index] = RightFrontWheel.currentPosition();

      servo01SP[index] = servo1PPos;  // save position into the array
      servo02SP[index] = servo2PPos;
      servo03SP[index] = servo3PPos;
      servo04SP[index] = servo4PPos;
      servo05SP[index] = servo5PPos;
      servo06SP[index] = servo6PPos;
      index++;                        // Increase the array index
      m = 0;
    }

// If button "SAVE" is pressed

if (m == 12) {

//if it's initial save, set the steppers position to 0

if (index == 0) {

LeftBackWheel.setCurrentPosition(0);

LeftFrontWheel.setCurrentPosition(0);

RightBackWheel.setCurrentPosition(0);

RightFrontWheel.setCurrentPosition(0);

}

lbw[index] = LeftBackWheel.currentPosition(); // save position into the array

lfw[index] = LeftFrontWheel.currentPosition();

rbw[index] = RightBackWheel.currentPosition();

rfw[index] = RightFrontWheel.currentPosition();

servo01SP[index] = servo1PPos; // save position into the array

servo02SP[index] = servo2PPos;

servo03SP[index] = servo3PPos;

servo04SP[index] = servo4PPos;

servo05SP[index] = servo5PPos;

servo06SP[index] = servo6PPos;

index++; // Increase the array index

m = 0;

}

Затем, когда мы нажимаем кнопку Run, мы вызываем пользовательскую функцию runSteps(). Эта пользовательская функция проходит по всем сохраненным шагам, используя некоторые циклы for и while.

if (m == 14) {
      runSteps();

      // If button "RESET" is pressed
      if (dataIn != 14) {
        stopMoving();
        memset(lbw, 0, sizeof(lbw)); // Clear the array data to 0
        memset(lfw, 0, sizeof(lfw));
        memset(rbw, 0, sizeof(rbw));
        memset(rfw, 0, sizeof(rfw));
        memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
        memset(servo02SP, 0, sizeof(servo02SP));
        memset(servo03SP, 0, sizeof(servo03SP));
        memset(servo04SP, 0, sizeof(servo04SP));
        memset(servo05SP, 0, sizeof(servo05SP));
        memset(servo06SP, 0, sizeof(servo06SP));
        index = 0;  // Index to 0
      }
    }

if (m == 14) {

runSteps();

// If button "RESET" is pressed

if (dataIn != 14) {

stopMoving();

memset(lbw, 0, sizeof(lbw)); // Clear the array data to 0

memset(lfw, 0, sizeof(lfw));

memset(rbw, 0, sizeof(rbw));

memset(rfw, 0, sizeof(rfw));

memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0

memset(servo02SP, 0, sizeof(servo02SP));

memset(servo03SP, 0, sizeof(servo03SP));

memset(servo04SP, 0, sizeof(servo04SP));

memset(servo05SP, 0, sizeof(servo05SP));

memset(servo06SP, 0, sizeof(servo06SP));

index = 0; // Index to 0

}

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

По следующей ссылке вы можете скачать это приложение, а также редактируемый файл проекта - файлы приложений для робота Arduino Robot Arm и Mecanum Wheels.

Исходный код

Вот полный код Arduino для нашего проекта робота.

/*
       Arduino Robot Arm and Mecanum Wheels Robot
          Smartphone Control via Bluetooth
       by Dejan, www.HowToMechatronics.com
*/

#include <SoftwareSerial.h>
#include <AccelStepper.h>
#include <Servo.h>

Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;

SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)

// Define the stepper motors and the pins the will use
AccelStepper LeftBackWheel(1, 42, 43);   // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 40, 41);  // Stepper2
AccelStepper RightBackWheel(1, 44, 45);  // Stepper3
AccelStepper RightFrontWheel(1, 46, 47); // Stepper4

#define led 14

int wheelSpeed = 1500;

int lbw[50], lfw[50], rbw[50], rfw[50]; // arrays for storing positions/steps

int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;

void setup() {
  // Set initial seed values for the steppers
  LeftFrontWheel.setMaxSpeed(3000);
  LeftBackWheel.setMaxSpeed(3000);
  RightFrontWheel.setMaxSpeed(3000);
  RightBackWheel.setMaxSpeed(3000);
  pinMode(led, OUTPUT);
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  Bluetooth.begin(38400); // Default baud rate of the Bluetooth module
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);
  // Move robot arm to initial position
  servo1PPos = 90;
  servo01.write(servo1PPos);
  servo2PPos = 100;
  servo02.write(servo2PPos);
  servo3PPos = 120;
  servo03.write(servo3PPos);
  servo4PPos = 95;
  servo04.write(servo4PPos);
  servo5PPos = 60;
  servo05.write(servo5PPos);
  servo6PPos = 110;
  servo06.write(servo6PPos);
}

void loop() {
  // Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.read();  // Read the data

    if (dataIn == 0) {
      m = 0;
    }
    if (dataIn == 1) {
      m = 1;
    }
    if (dataIn == 2) {
      m = 2;
    }
    if (dataIn == 3) {
      m = 3;
    }
    if (dataIn == 4) {
      m = 4;
    }
    if (dataIn == 5) {
      m = 5;
    }
    if (dataIn == 6) {
      m = 6;
    }
    if (dataIn == 7) {
      m = 7;
    }
    if (dataIn == 8) {
      m = 8;
    }
    if (dataIn == 9) {
      m = 9;
    }
    if (dataIn == 10) {
      m = 10;
    }
    if (dataIn == 11) {
      m = 11;
    }
    if (dataIn == 12) {
      m = 12;
    }
    if (dataIn == 14) {
      m = 14;
    }
    if (dataIn == 16) {
      m = 16;
    }
    if (dataIn == 17) {
      m = 17;
    }
    if (dataIn == 18) {
      m = 18;
    }
    if (dataIn == 19) {
      m = 19;
    }
    if (dataIn == 20) {
      m = 20;
    }
    if (dataIn == 21) {
      m = 21;
    }
    if (dataIn == 22) {
      m = 22;
    }
    if (dataIn == 23) {
      m = 23;
    }
    if (dataIn == 24) {
      m = 24;
    }
    if (dataIn == 25) {
      m = 25;
    }
    if (dataIn == 26) {
      m = 26;
    }
    if (dataIn == 27) {
      m = 27;
    }

    // Move the Mecanum wheels platform
    if (m == 4) {
      moveSidewaysLeft();
    }
    if (m == 5) {
      moveSidewaysRight();
    }
    if (m == 2) {
      moveForward();
    }
    if (m == 7) {
      moveBackward();
    }
    if (m == 3) {
      moveRightForward();
    }
    if (m == 1) {
      moveLeftForward();
    }
    if (m == 8) {
      moveRightBackward();
    }
    if (m == 6) {
      moveLeftBackward();
    }
    if (m == 9) {
      rotateLeft();
    }
    if (m == 10) {
      rotateRight();
    }

    if (m == 0) {
      stopMoving();
    }

    // Mecanum wheels speed
    if (dataIn > 30 & dataIn < 100) {
      wheelSpeed = dataIn * 20;
    }

    // Move robot arm
    // Move servo 1 in positive direction
    while (m == 16) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo01.write(servo1PPos);
      servo1PPos++;
      delay(speedDelay);
    }
    // Move servo 1 in negative direction
    while (m == 17) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo01.write(servo1PPos);
      servo1PPos--;
      delay(speedDelay);
    }
    // Move servo 2
    while (m == 19) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo02.write(servo2PPos);
      servo2PPos++;
      delay(speedDelay);
    }
    while (m == 18) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo02.write(servo2PPos);
      servo2PPos--;
      delay(speedDelay);
    }
    // Move servo 3
    while (m == 20) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo03.write(servo3PPos);
      servo3PPos++;
      delay(speedDelay);
    }
    while (m == 21) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo03.write(servo3PPos);
      servo3PPos--;
      delay(speedDelay);
    }
    // Move servo 4
    while (m == 23) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo04.write(servo4PPos);
      servo4PPos++;
      delay(speedDelay);
    }
    while (m == 22) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo04.write(servo4PPos);
      servo4PPos--;
      delay(speedDelay);
    }
    // Move servo 5
    while (m == 25) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo05.write(servo5PPos);
      servo5PPos++;
      delay(speedDelay);
    }
    while (m == 24) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo05.write(servo5PPos);
      servo5PPos--;
      delay(speedDelay);
    }
    // Move servo 6
    while (m == 26) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo06.write(servo6PPos);
      servo6PPos++;
      delay(speedDelay);
    }
    while (m == 27) {
      if (Bluetooth.available() > 0) {
        m = Bluetooth.read();
      }
      servo06.write(servo6PPos);
      servo6PPos--;
      delay(speedDelay);
    }

    // If arm speed slider is changed
    if (dataIn > 101 & dataIn < 250) {
      speedDelay = dataIn / 10; // Change servo speed (delay time)
    }

    // If button "SAVE" is pressed
    if (m == 12) {
      //if it's initial save, set the steppers position to 0
      if (index == 0) {
        LeftBackWheel.setCurrentPosition(0);
        LeftFrontWheel.setCurrentPosition(0);
        RightBackWheel.setCurrentPosition(0);
        RightFrontWheel.setCurrentPosition(0);
      }
      lbw[index] = LeftBackWheel.currentPosition();  // save position into the array
      lfw[index] = LeftFrontWheel.currentPosition();
      rbw[index] = RightBackWheel.currentPosition();
      rfw[index] = RightFrontWheel.currentPosition();

      servo01SP[index] = servo1PPos;  // save position into the array
      servo02SP[index] = servo2PPos;
      servo03SP[index] = servo3PPos;
      servo04SP[index] = servo4PPos;
      servo05SP[index] = servo5PPos;
      servo06SP[index] = servo6PPos;
      index++;                        // Increase the array index
      m = 0;
    }

    // If button "RUN" is pressed
    if (m == 14) {
      runSteps();

      // If button "RESET" is pressed
      if (dataIn != 14) {
        stopMoving();
        memset(lbw, 0, sizeof(lbw)); // Clear the array data to 0
        memset(lfw, 0, sizeof(lfw));
        memset(rbw, 0, sizeof(rbw));
        memset(rfw, 0, sizeof(rfw));
        memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
        memset(servo02SP, 0, sizeof(servo02SP));
        memset(servo03SP, 0, sizeof(servo03SP));
        memset(servo04SP, 0, sizeof(servo04SP));
        memset(servo05SP, 0, sizeof(servo05SP));
        memset(servo06SP, 0, sizeof(servo06SP));
        index = 0;  // Index to 0
      }
    }
  }
  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();

  // Monitor the battery voltage
  int sensorValue = analogRead(A0);
  float voltage = sensorValue * (5.0 / 1023.00) * 3; // Convert the reading values from 5v to suitable 12V i
  //Serial.println(voltage);
  // If voltage is below 11V turn on the LED
  if (voltage < 11) {
    digitalWrite(led, HIGH);
  }
  else {
    digitalWrite(led, LOW);
  }
}
void moveForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveBackward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void moveSidewaysRight() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveSidewaysLeft() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void rotateLeft() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void rotateRight() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void moveRightForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveRightBackward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftForward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftBackward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void stopMoving() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(0);
}

// Automatic mode custom function - run the saved steps
void runSteps() {
  while (dataIn != 13) {   // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
      if (Bluetooth.available() > 0) {      // Check for incomding data
        dataIn = Bluetooth.read();
        if ( dataIn == 15) {           // If button "PAUSE" is pressed
          while (dataIn != 14) {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.read();
              if ( dataIn == 13) {
                break;
              }
            }
          }
        }
        // If speed slider is changed
        if (dataIn > 100 & dataIn < 150) {
          speedDelay = dataIn / 10; // Change servo speed (delay time)
        }
        // Mecanum wheels speed
        if (dataIn > 30 & dataIn < 100) {
          wheelSpeed = dataIn * 10;
          dataIn = 14;
        }
      }
      LeftFrontWheel.moveTo(lfw[i]);
      LeftFrontWheel.setSpeed(wheelSpeed);
      LeftBackWheel.moveTo(lbw[i]);
      LeftBackWheel.setSpeed(wheelSpeed);
      RightFrontWheel.moveTo(rfw[i]);
      RightFrontWheel.setSpeed(wheelSpeed);
      RightBackWheel.moveTo(rbw[i]);
      RightBackWheel.setSpeed(wheelSpeed);

      while (LeftBackWheel.currentPosition() != lbw[i] & LeftFrontWheel.currentPosition() != lfw[i] & RightFrontWheel.currentPosition() != rfw[i] & RightBackWheel.currentPosition() != rbw[i]) {
        LeftFrontWheel.runSpeedToPosition();
        LeftBackWheel.runSpeedToPosition();
        RightFrontWheel.runSpeedToPosition();
        RightBackWheel.runSpeedToPosition();
      }
      // Servo 1
      if (servo01SP[i] == servo01SP[i + 1]) {
      }
      if (servo01SP[i] > servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      if (servo01SP[i] < servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
          servo01.write(j);
          delay(speedDelay);
        }
      }

      // Servo 2
      if (servo02SP[i] == servo02SP[i + 1]) {
      }
      if (servo02SP[i] > servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {
          servo02.write(j);
          delay(speedDelay);
        }
      }
      if (servo02SP[i] < servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
          servo02.write(j);
          delay(speedDelay);
        }
      }

      // Servo 3
      if (servo03SP[i] == servo03SP[i + 1]) {
      }
      if (servo03SP[i] > servo03SP[i + 1]) {
        for ( int j = servo03SP[i]; j >= servo03SP[i + 1]; j--) {
          servo03.write(j);
          delay(speedDelay);
        }
      }
      if (servo03SP[i] < servo03SP[i + 1]) {
        for ( int j = servo03SP[i]; j <= servo03SP[i + 1]; j++) {
          servo03.write(j);
          delay(speedDelay);
        }
      }

      // Servo 4
      if (servo04SP[i] == servo04SP[i + 1]) {
      }
      if (servo04SP[i] > servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j >= servo04SP[i + 1]; j--) {
          servo04.write(j);
          delay(speedDelay);
        }
      }
      if (servo04SP[i] < servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j <= servo04SP[i + 1]; j++) {
          servo04.write(j);
          delay(speedDelay);
        }
      }

      // Servo 5
      if (servo05SP[i] == servo05SP[i + 1]) {
      }
      if (servo05SP[i] > servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {
          servo05.write(j);
          delay(speedDelay);
        }
      }
      if (servo05SP[i] < servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {
          servo05.write(j);
          delay(speedDelay);
        }
      }

      // Servo 6
      if (servo06SP[i] == servo06SP[i + 1]) {
      }
      if (servo06SP[i] > servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {
          servo06.write(j);
          delay(speedDelay);
        }
      }
      if (servo06SP[i] < servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {
          servo06.write(j);
          delay(speedDelay);
        }
      }
    }
  }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

Arduino Robot Arm and Mecanum Wheels Robot

Smartphone Control via Bluetooth

by Dejan, www.HowToMechatronics.com

#include <SoftwareSerial.h>

#include <AccelStepper.h>

#include <Servo.h>

Servo servo01;

Servo servo02;

Servo servo03;

Servo servo04;

Servo servo05;

Servo servo06;

SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)

// Define the stepper motors and the pins the will use

AccelStepper LeftBackWheel(1, 42, 43); // (Type:driver, STEP, DIR) - Stepper1

AccelStepper LeftFrontWheel(1, 40, 41); // Stepper2

AccelStepper RightBackWheel(1, 44, 45); // Stepper3

AccelStepper RightFrontWheel(1, 46, 47); // Stepper4

#define led 14

int wheelSpeed = 1500;

int lbw[50], lfw[50], rbw[50], rfw[50]; // arrays for storing positions/steps

int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position

int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position

int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps

int speedDelay = 20;

int index = 0;

int dataIn;

int m = 0;

void setup() {

// Set initial seed values for the steppers

LeftFrontWheel.setMaxSpeed(3000);

LeftBackWheel.setMaxSpeed(3000);

RightFrontWheel.setMaxSpeed(3000);

RightBackWheel.setMaxSpeed(3000);

pinMode(led, OUTPUT);

servo01.attach(5);

servo02.attach(6);

servo03.attach(7);

servo04.attach(8);

servo05.attach(9);

servo06.attach(10);

Bluetooth.begin(38400); // Default baud rate of the Bluetooth module

Bluetooth.setTimeout(5);

delay(20);

Serial.begin(38400);

// Move robot arm to initial position

servo1PPos = 90;

servo01.write(servo1PPos);

servo2PPos = 100;

servo02.write(servo2PPos);

servo3PPos = 120;

servo03.write(servo3PPos);

servo4PPos = 95;

servo04.write(servo4PPos);

servo5PPos = 60;

servo05.write(servo5PPos);

servo6PPos = 110;

servo06.write(servo6PPos);

}

void loop() {

// Check for incoming data

if (Bluetooth.available() > 0) {

dataIn = Bluetooth.read(); // Read the data

if (dataIn == 0) {

m = 0;

}

if (dataIn == 1) {

m = 1;

}

if (dataIn == 2) {

m = 2;

}

if (dataIn == 3) {

m = 3;

}

if (dataIn == 4) {

m = 4;

}

if (dataIn == 5) {

m = 5;

}

if (dataIn == 6) {

m = 6;

}

if (dataIn == 7) {

m = 7;

}

if (dataIn == 8) {

m = 8;

}

if (dataIn == 9) {

m = 9;

}

if (dataIn == 10) {

m = 10;

}

if (dataIn == 11) {

m = 11;

}

if (dataIn == 12) {

m = 12;

}

if (dataIn == 14) {

m = 14;

}

if (dataIn == 16) {

m = 16;

}

if (dataIn == 17) {

m = 17;

}

if (dataIn == 18) {

m = 18;

}

if (dataIn == 19) {

m = 19;

}

if (dataIn == 20) {

m = 20;

}

if (dataIn == 21) {

m = 21;

}

if (dataIn == 22) {

m = 22;

}

if (dataIn == 23) {

m = 23;

}

if (dataIn == 24) {

m = 24;

}

if (dataIn == 25) {

m = 25;

}

if (dataIn == 26) {

m = 26;

}

if (dataIn == 27) {

m = 27;

}

// Move the Mecanum wheels platform

if (m == 4) {

moveSidewaysLeft();

}

if (m == 5) {

moveSidewaysRight();

}

if (m == 2) {

moveForward();

}

if (m == 7) {

moveBackward();

}

if (m == 3) {

moveRightForward();

}

if (m == 1) {

moveLeftForward();

}

if (m == 8) {

moveRightBackward();

}

if (m == 6) {

moveLeftBackward();

}

if (m == 9) {

rotateLeft();

}

if (m == 10) {

rotateRight();

}

if (m == 0) {

stopMoving();

}

// Mecanum wheels speed

if (dataIn > 30 & dataIn < 100) {

wheelSpeed = dataIn * 20;

}

// Move robot arm

// Move servo 1 in positive direction

while (m == 16) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo01.write(servo1PPos);

servo1PPos++;

delay(speedDelay);

}

// Move servo 1 in negative direction

while (m == 17) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo01.write(servo1PPos);

servo1PPos--;

delay(speedDelay);

}

// Move servo 2

while (m == 19) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo02.write(servo2PPos);

servo2PPos++;

delay(speedDelay);

}

while (m == 18) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo02.write(servo2PPos);

servo2PPos--;

delay(speedDelay);

}

// Move servo 3

while (m == 20) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo03.write(servo3PPos);

servo3PPos++;

delay(speedDelay);

}

while (m == 21) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo03.write(servo3PPos);

servo3PPos--;

delay(speedDelay);

}

// Move servo 4

while (m == 23) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo04.write(servo4PPos);

servo4PPos++;

delay(speedDelay);

}

while (m == 22) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo04.write(servo4PPos);

servo4PPos--;

delay(speedDelay);

}

// Move servo 5

while (m == 25) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo05.write(servo5PPos);

servo5PPos++;

delay(speedDelay);

}

while (m == 24) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo05.write(servo5PPos);

servo5PPos--;

delay(speedDelay);

}

// Move servo 6

while (m == 26) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo06.write(servo6PPos);

servo6PPos++;

delay(speedDelay);

}

while (m == 27) {

if (Bluetooth.available() > 0) {

m = Bluetooth.read();

}

servo06.write(servo6PPos);

servo6PPos--;

delay(speedDelay);

}

// If arm speed slider is changed

if (dataIn > 101 & dataIn < 250) {

speedDelay = dataIn / 10; // Change servo speed (delay time)

}

// If button "SAVE" is pressed

if (m == 12) {

//if it's initial save, set the steppers position to 0

if (index == 0) {

LeftBackWheel.setCurrentPosition(0);

LeftFrontWheel.setCurrentPosition(0);

RightBackWheel.setCurrentPosition(0);

RightFrontWheel.setCurrentPosition(0);

}

lbw[index] = LeftBackWheel.currentPosition(); // save position into the array

lfw[index] = LeftFrontWheel.currentPosition();

rbw[index] = RightBackWheel.currentPosition();

rfw[index] = RightFrontWheel.currentPosition();

servo01SP[index] = servo1PPos; // save position into the array

servo02SP[index] = servo2PPos;

servo03SP[index] = servo3PPos;

servo04SP[index] = servo4PPos;

servo05SP[index] = servo5PPos;

servo06SP[index] = servo6PPos;

index++; // Increase the array index

m = 0;

}

// If button "RUN" is pressed

if (m == 14) {

runSteps();

// If button "RESET" is pressed

if (dataIn != 14) {

stopMoving();

memset(lbw, 0, sizeof(lbw)); // Clear the array data to 0

memset(lfw, 0, sizeof(lfw));

memset(rbw, 0, sizeof(rbw));

memset(rfw, 0, sizeof(rfw));

memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0

memset(servo02SP, 0, sizeof(servo02SP));

memset(servo03SP, 0, sizeof(servo03SP));

memset(servo04SP, 0, sizeof(servo04SP));

memset(servo05SP, 0, sizeof(servo05SP));

memset(servo06SP, 0, sizeof(servo06SP));

index = 0; // Index to 0

}

LeftFrontWheel.runSpeed();

LeftBackWheel.runSpeed();

RightFrontWheel.runSpeed();

RightBackWheel.runSpeed();

// Monitor the battery voltage

int sensorValue = analogRead(A0);

float voltage = sensorValue * (5.0 / 1023.00) * 3; // Convert the reading values from 5v to suitable 12V i

//Serial.println(voltage);

// If voltage is below 11V turn on the LED

if (voltage < 11) {

digitalWrite(led, HIGH);

}

else {

digitalWrite(led, LOW);

}

void moveForward() {

LeftFrontWheel.setSpeed(wheelSpeed);

LeftBackWheel.setSpeed(wheelSpeed);

RightFrontWheel.setSpeed(wheelSpeed);

RightBackWheel.setSpeed(wheelSpeed);

}

void moveBackward() {

LeftFrontWheel.setSpeed(-wheelSpeed);

LeftBackWheel.setSpeed(-wheelSpeed);

RightFrontWheel.setSpeed(-wheelSpeed);

RightBackWheel.setSpeed(-wheelSpeed);

}

void moveSidewaysRight() {

LeftFrontWheel.setSpeed(wheelSpeed);

LeftBackWheel.setSpeed(-wheelSpeed);

RightFrontWheel.setSpeed(-wheelSpeed);

RightBackWheel.setSpeed(wheelSpeed);

}

void moveSidewaysLeft() {

LeftFrontWheel.setSpeed(-wheelSpeed);

LeftBackWheel.setSpeed(wheelSpeed);

RightFrontWheel.setSpeed(wheelSpeed);

RightBackWheel.setSpeed(-wheelSpeed);

}

void rotateLeft() {

LeftFrontWheel.setSpeed(-wheelSpeed);

LeftBackWheel.setSpeed(-wheelSpeed);

RightFrontWheel.setSpeed(wheelSpeed);

RightBackWheel.setSpeed(wheelSpeed);

}

void rotateRight() {

LeftFrontWheel.setSpeed(wheelSpeed);

LeftBackWheel.setSpeed(wheelSpeed);

RightFrontWheel.setSpeed(-wheelSpeed);

RightBackWheel.setSpeed(-wheelSpeed);

}

void moveRightForward() {

LeftFrontWheel.setSpeed(wheelSpeed);

LeftBackWheel.setSpeed(0);

RightFrontWheel.setSpeed(0);

RightBackWheel.setSpeed(wheelSpeed);

}

void moveRightBackward() {

LeftFrontWheel.setSpeed(0);

LeftBackWheel.setSpeed(-wheelSpeed);

RightFrontWheel.setSpeed(-wheelSpeed);

RightBackWheel.setSpeed(0);

}

void moveLeftForward() {

LeftFrontWheel.setSpeed(0);

LeftBackWheel.setSpeed(wheelSpeed);

RightFrontWheel.setSpeed(wheelSpeed);

RightBackWheel.setSpeed(0);

}

void moveLeftBackward() {

LeftFrontWheel.setSpeed(-wheelSpeed);

LeftBackWheel.setSpeed(0);

RightFrontWheel.setSpeed(0);

RightBackWheel.setSpeed(-wheelSpeed);

}

void stopMoving() {

LeftFrontWheel.setSpeed(0);

LeftBackWheel.setSpeed(0);

RightFrontWheel.setSpeed(0);

RightBackWheel.setSpeed(0);

}

// Automatic mode custom function - run the saved steps

void runSteps() {

while (dataIn != 13) { // Run the steps over and over again until "RESET" button is pressed

for (int i = 0; i <= index - 2; i++) { // Run through all steps(index)

if (Bluetooth.available() > 0) { // Check for incomding data

dataIn = Bluetooth.read();

if ( dataIn == 15) { // If button "PAUSE" is pressed

while (dataIn != 14) { // Wait until "RUN" is pressed again

if (Bluetooth.available() > 0) {

dataIn = Bluetooth.read();

if ( dataIn == 13) {

break;

}

// If speed slider is changed

if (dataIn > 100 & dataIn < 150) {

speedDelay = dataIn / 10; // Change servo speed (delay time)

}

// Mecanum wheels speed

if (dataIn > 30 & dataIn < 100) {

wheelSpeed = dataIn * 10;

dataIn = 14;

}

LeftFrontWheel.moveTo(lfw[i]);

LeftFrontWheel.setSpeed(wheelSpeed);

LeftBackWheel.moveTo(lbw[i]);

LeftBackWheel.setSpeed(wheelSpeed);

RightFrontWheel.moveTo(rfw[i]);

RightFrontWheel.setSpeed(wheelSpeed);

RightBackWheel.moveTo(rbw[i]);

RightBackWheel.setSpeed(wheelSpeed);

while (LeftBackWheel.currentPosition() != lbw[i] & LeftFrontWheel.currentPosition() != lfw[i] & RightFrontWheel.currentPosition() != rfw[i] & RightBackWheel.currentPosition() != rbw[i]) {

LeftFrontWheel.runSpeedToPosition();

LeftBackWheel.runSpeedToPosition();

RightFrontWheel.runSpeedToPosition();

RightBackWheel.runSpeedToPosition();

}

// Servo 1

if (servo01SP[i] == servo01SP[i + 1]) {

}

if (servo01SP[i] > servo01SP[i + 1]) {

for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {

servo01.write(j);

delay(speedDelay);

}

if (servo01SP[i] < servo01SP[i + 1]) {

for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {

servo01.write(j);

delay(speedDelay);

}

// Servo 2

if (servo02SP[i] == servo02SP[i + 1]) {

}

if (servo02SP[i] > servo02SP[i + 1]) {

for ( int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {

servo02.write(j);

delay(speedDelay);

}

if (servo02SP[i] < servo02SP[i + 1]) {

for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {

servo02.write(j);

delay(speedDelay);

}

// Servo 3

if (servo03SP[i] == servo03SP[i + 1]) {

}

if (servo03SP[i] > servo03SP[i + 1]) {

for ( int j = servo03SP[i]; j >= servo03SP[i + 1]; j--) {

servo03.write(j);

delay(speedDelay);

}

if (servo03SP[i] < servo03SP[i + 1]) {

for ( int j = servo03SP[i]; j <= servo03SP[i + 1]; j++) {

servo03.write(j);

delay(speedDelay);

}

// Servo 4

if (servo04SP[i] == servo04SP[i + 1]) {

}

if (servo04SP[i] > servo04SP[i + 1]) {

for ( int j = servo04SP[i]; j >= servo04SP[i + 1]; j--) {

servo04.write(j);

delay(speedDelay);

}

if (servo04SP[i] < servo04SP[i + 1]) {

for ( int j = servo04SP[i]; j <= servo04SP[i + 1]; j++) {

servo04.write(j);

delay(speedDelay);

}

// Servo 5

if (servo05SP[i] == servo05SP[i + 1]) {

}

if (servo05SP[i] > servo05SP[i + 1]) {

for ( int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {

servo05.write(j);

delay(speedDelay);

}

if (servo05SP[i] < servo05SP[i + 1]) {

for ( int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {

servo05.write(j);

delay(speedDelay);

}

// Servo 6

if (servo06SP[i] == servo06SP[i + 1]) {

}

if (servo06SP[i] > servo06SP[i + 1]) {

for ( int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {

servo06.write(j);

delay(speedDelay);

}

if (servo06SP[i] < servo06SP[i + 1]) {

for ( int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {

servo06.write(j);

delay(speedDelay);

}

Видео, демонстрирующее сборку и работу проекта

Источник статьи

(Проголосуй первым!)

Загрузка...

133 просмотров

Роботизированная рука Arduino на колесной платформе своими руками

Обзор проекта

Создание робота Arduino

Файлы для загрузки 3D-моделей и STL

Сборка

Схема робота

Необходимые компоненты

Сборка электронной части проекта

Объяснение кода Arduino

Исходный код

Видео, демонстрирующее сборку и работу проекта

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

Обзор проекта

Создание робота Arduino

Файлы для загрузки 3D-моделей и STL

Сборка

Схема робота

Необходимые компоненты

Сборка электронной части проекта

Объяснение кода Arduino

Исходный код

Видео, демонстрирующее сборку и работу проекта

Похожие статьи

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