В этом уроке мы покажем как управлять рукой робота 6DOF через интернет с помощью микроконтроллера Ардуино.
Содержание
Шаг 1. Комплектующие
Для нашего урока нам понадобится не так много деталей. Основа урока рука робота, которой мы будем управлять с помощью Ардуино Уно.
- Arduino Uno
- Шилд PHPoC WiFi Shield для Arduino
- Рука робота 6DOF
Шаг 2. Интерфейс
Рука робота имеет 6 двигателей.
- Зона A: Управляющий двигатель 2, 3, 4 (управляйте тремя соединениями)
- Зона B: Управляющий двигатель 1 (управление базой)
- Зона C: Управляющий двигатель 5 (управление вращением захвата)
- Зона D: Управляющий двигатель 6 (управление захватом)
Шаг 3. Архитектура и схема
В целом, архитектура системы по управлению рукой робота достаточно простая. Конечно, это очень громко называть схему "архитектурой", но мы с вами профессионалы.
Схема соединения выглядит таким образом:
Шаг 4. Рабочий поток
Сторона клиента (вебстраница, написана на JavaScript)
Когда пользователь прикасается пальцами или нажимает/двигает мышью по экрану, то мы можем получить координаты X и Y. Рабочий поток выглядит следующим образом:
В случае зоны А, чтобы рассчитать углы двигателя 2, 3, 4, нам нужно выполнить геометрический расчет, который мы разберем ниже.
Сторона сервера (код Arduino)
Получив набор углов от клиентской стороны, шесть двигателей постепенно перемещаются от текущих углов к новым углам. Шесть двигателей должны двигаться и достигать новых углов одновременно.
Прежде чем подробно рассказывать о том, как управлять всеми двигателями, давайте посмотрим, как управлять одним двигателем. Предположим, что мы хотим переместить двигатель с текущего угла (angle) на новый угол (new_angle). Поскольку скорость двигателя высока, мы должны замедлить ее.
Чтобы сделать это, следующие шаги повторяются до тех пор, пока двигатель не достигнет нового угла:
- Переместить мотор на небольшой шаг.
- Приостановить на небольшое время, затем переместить еще на один шаг.
Следующая диаграмма иллюстрирует приведенную выше схему, если новый угол больше, чем текущий:
Wherestep_numis - количество шагов, которые должен пройти двигатель. Step и Time заданные значения. Два последних определяют скорость и плавность.
Всё сказанное выше только для одного робота. Чтобы роботы начали перемещаться и добираться до "пункта назначения" одновременно, мы можем сделать следующее: шесть двигателей выполняют один и тот же step_num, но шаг каждого двигателя отличается друг от друга. Поэтому мы должны выбрать step_num в этом проекте максимальным.
Как правило, рабочий поток Arduino выглядит следующим образом:
Шаг 5. Расчет геометрии
Давайте рассмотрим вычисления для нашей задачи относительно геометрии:
Известно:
- C - фиксирована
- D входная для пользователя
- CB, BA, AD (обозначается соответственно b, a, d)
- Длина каждого сегмента руки.
- Находятся углы C, B, A. Решение:
- Сделайте предположение, что углы B и A одинаковы
- Добавьте дополнительные точки и сегмент
Вычисление
- Мы знаем точки C и D => мы можем рассчитать длину DC (обозначенную c).
- Мы также можем вычислить δ.
- Посмотрим на треугольник ABE, мы можем заключить, что AE = BE и ∠E = π - 2α.
Таким образом:
Закон косинусов в треугольнике CDE:
Изменим (1) и (2) на (3), имеем:
Упрощаем:
Поскольку мы знаем a, b, c и d, решая указанное выше квадратичное уравнение, мы можем вычислить значение α и β = π - α. Далее найдем γ - закон косинусов в треугольниках BDC и BDA:
Решив эту систему уравнений, мы можем вычислить γ.
Таким образом, требуемые углы: (δ + γ), β и β. Это углы двигателей 2, 3 и 4 соответственно.
Шаг 6. Исходный код
Исходный код включает два файла:
- RobotArmWeb.ino (код Arduino).
- Remote_arm.php (код веб-приложения, который загружается в PHPoC WiFi Shield или PHPoC Shield).
Вам также необходимо скачать и загрузить файл изображения flywheel.png (ниже) в PHPoC Shield.
Код Ардуино:
#include "SPI.h"
#include "Phpoc.h"
#include
int angle_init[] = {90, 101, 165, 153, 90, 120}; // when motor stands straight. In web, the angle when motor stands straight is {0, 90, 130, 180, 0, 0};
int angle_offset[] = {0, 11, -15, -27, 0, 137}; // offset between real servo motor and angle on web
int cur_angles[] = {90, 101, 165, 153, 90, 120}; // current angles of six motors (degree)
int dest_angles[] = {0, 0, 0, 0, 0, 0}; // destined angles
int angle_max[] = {180, 180, 160, 120, 180, 137};
int angle_min[] = { 0, 0, 0, 20, 0, 75};
int direction[] = {1, 1, 1, 1, 1 ,-1};
int angleSteps[] = {3, 2, 2, 2, 4 ,4}; // moving steps of each motor (degree)
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
Servo servo[6] = {servo1, servo2, servo3, servo4, servo5, servo6};
PhpocServer server(80);
PhpocClient client;
int stepNum = 0;
void setup(){
Serial.begin(9600);
Phpoc.begin(PF_LOG_SPI | PF_LOG_NET);
server.beginWebSocket("remote_arm");
servo1.attach(2); // attaches the servo on pin 2 to the servo object
servo2.attach(3); // attaches the servo on pin 3 to the servo object
servo3.attach(4); // attaches the servo on pin 4 to the servo object
servo4.attach(5); // attaches the servo on pin 5 to the servo object
servo5.attach(6); // attaches the servo on pin 6 to the servo object
servo6.attach(7); // attaches the servo on pin 7 to the servo object
for(int i = 0; i < 6; i++)
servo[i].write(angle_init[i]);
}
void loop() {
PhpocClient client = server.available();
if(client) {
String angleStr = client.readLine();
if(angleStr) {
Serial.println(angleStr);
int commaPos1 = -1;
int commaPos2;
for(int i = 0; i < 5; i++) {
commaPos2 = angleStr.indexOf(',', commaPos1 + 1);
int angle = angleStr.substring(commaPos1 + 1, commaPos2).toInt();
dest_angles[i] = angle * direction[i] + angle_offset[i];
commaPos1 = commaPos2;
}
int angle5 = angleStr.substring(commaPos1 + 1).toInt();
dest_angles[5] = angle5 * direction[5] + angle_offset[5];
stepNum = 0;
// move motors in many small steps to make motor move smooth, avoiding move motor suddently. The below is step calculation
for(int i = 0; i < 6; i++) {
int dif = abs(cur_angles[i] - dest_angles[i]);
int step = dif / angleSteps[i];
if(stepNum < step) stepNum = step; } } } // move motors step by step if(stepNum > 0) {
for(int i = 0; i < 6; i++) { int angleStepMove = (dest_angles[i] - cur_angles[i]) / stepNum; cur_angles[i] += angleStepMove; if(cur_angles[i] > angle_max[i])
cur_angles[i] = angle_max[i];
else if(cur_angles[i] < angle_min[i])
cur_angles[i] = angle_min[i];
servo[i].write(cur_angles[i]);
}
stepNum--;
delay(20);
}
}
Код клиентской части:
Шаг 6. Итоговый результат
На этом пока всё. Желаем вам отличных проектов и новых изобретений.