Исполнитель: Потемкин М.С., 641-об
Научный руководитель: Русинов В.Л.
Разработка системы управления роботом манипулятором с использованием Arduino
Схват для робота манипулятора управляется с помощью двух сервоприводов под управлением микроконтроллера Arduino
(https://pp.userapi.com/c855620/v855620328/109cb/-Sx1RJ_qkaY.jpg)
(https://pp.userapi.com/c855620/v855620328/109d3/ZyX7rhm7aH8.jpg)
Рисунок 1 - Схват
Разработка ручного управленияВ ручном управлении используется матричная клавиатура. Матричная клавиатура подключается к входам 2-9 МК. Серводвигатели управляющие губками и поворотом схвата подключаются к 10 и 11 выходам.
(https://pp.userapi.com/c851224/v851224862/e62e6/MNmtxZptiEg.jpg)
Рисунок 2 - Схема подключения матричной клавиатуры и сервоприводов к Arduino
При создании кода использовались библиотечные функции Keypad.h для работы с матричной клавиатурой и Servo.h для работы с сервоприводом. Для считывания значений с клавиатуры используются библиотечные функции:
• getKey() - возвращает кнопку, которая была нажата;
• getState() - возвращает текущее состояние одной из клавиш;
• attach() - привязывает привод к указанному выходу;
• write() - передает значение для вала сервопривода.
Для управлением угла поворота используются счетчики:
• i - значение угла поворота, отвечающий за поворот схвата;
• s - значение угла поворота, отвечающий управление губками схвата.
Идентификация нажатой кнопкой производится с помощью конструкции switch case, при нажатии или удержании соответствующей кнопки происходит увеличение/уменьшение счетчика и далее происходит запись счетчика в значение для управление сервоприводом. В переменные ug1 и ug2 записываются считанные значения угла поворота сервопривода, для сверки с заданными, с помощью COM порта. Функция constrain Ограничивает значение переменной заданными пределами.
Ниже приведен код разработанный в среде программирования Arduino - Arduino IDE
#include <Keypad.h>
#include <Servo.h>
Servo servo1;
Servo servo2;
int i = 0;
int s = 0;
int ug1;
int ug2;
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] =
{
{'*', '7', '4', '1'}, // здесь мы располагаем названия наших клавиш, как на клавиатуре,для удобства пользования
{'0', '8', '5', '2'},
{'#', '9', '6', '3'},
{'D', 'C', 'B', 'A'}
};
byte rowPins[ROWS] = {5, 4, 3, 2};
byte colPins[COLS] = {9, 8, 7, 6};
Keypad customKeypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );//инициализация объекта класса New Keypad
void setup()
{
servo1.attach(11);
servo2.attach(10);
servo1.write(0);
servo2.write(0);
Serial.begin(9600);
}
void loop()
{
char key = customKeypad.getKey(); //getKey() показывает была ли нажата та или иная клавиша
switch (key)
{
case '4':
while ( customKeypad.getState()) // Постоянное получение сигналов
{
i = constrain(i, 0, 140);
i--;
servo1.write(i);
delay(25);
key = customKeypad.getKey();
ug1 = servo1.read();
}
break;
case '6':
while ( customKeypad.getState())
{
i = constrain(i, 0, 140);
i++;
servo1.write(i);
delay(25);
key = customKeypad.getKey();
ug1 = servo1.read();
}
break;
case '2':
while ( customKeypad.getState())
{
s = constrain(s, 0, 70);
s--;
servo2.write(s);
delay(25);
key = customKeypad.getKey();
ug2 = servo2.read();
}
break;
case '8':
while ( customKeypad.getState())
{
s = constrain(s, 0, 70);
s++;
servo2.write(s);
delay(25);
key = customKeypad.getKey();
ug2 = servo2.read();
}
break;
}
Serial.println(i);
Serial.println(s);
Serial.println(ug1);
Serial.println(ug2);
}
Разработка управления через ПКБыло разработано оконное приложение в программе Microsoft Visual Studio на языке программирования C#. С использованием библиотеки System.IO.Ports для работы с COM портами.
(https://pp.userapi.com/c855620/v855620328/109c4/uSaCQHnEOew.jpg)
Рисунок 3 - Приложение для управления схватом
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO.Ports; //Подключение класса для работы с COM портом
namespace UPRAVLENIE_RUKOY
{
public partial class Form1 : Form
{
bool isConnected = false;
SerialPort port;//Создание объекта класса
private void init()
{
comboBox1.Items.Clear();
// Получаем список COM портов доступных в системе
string[] portnames = SerialPort.GetPortNames();
// Проверяем есть ли доступные
if (portnames.Length == 0)
{
MessageBox.Show("COM PORT не найден");
}
foreach (string s in portnames) //перебор значений в portnames
{
//добавляем доступные COM порты в список
comboBox1.Items.Add(s);
}
}
public Form1()
{
InitializeComponent();
init();//Вызов метода init
}
private void button2_Click(object sender, EventArgs e)
{
if (!isConnected)
{
if (comboBox1.GetItemText(comboBox1.SelectedItem) == "")
{
MessageBox.Show("COM PORT не выбран!");
}
else
{
connectToArduino();
label6.Text = comboBox1.GetItemText(comboBox1.SelectedItem);
label7.Text = "9600";
}
}
else
{
disconnectFromArduino();
label6.Text = "";
label7.Text = "";
}
}
private void connectToArduino()
{
isConnected = true;
string selectedPort = comboBox1.GetItemText(comboBox1.SelectedItem);
port = new SerialPort(selectedPort, 9600, Parity.None, 8, StopBits.One);
port.Open();
button2.Text = "Отключиться";
}
private void disconnectFromArduino()
{
isConnected = false;
port.Close();
button2.Text = "Подключиться";
}
private void button1_Click(object sender, EventArgs e)
{
if (isConnected == true)
{
int znach_ugla = Convert.ToInt16(textBox1.Text);
if (znach_ugla >= 0 && znach_ugla <= 180)
{
String UgolServo = "#A";
UgolServo = UgolServo + Convert.ToString(znach_ugla,16);
UgolServo = UgolServo + "|";
port.Write(UgolServo);
Ugol_povorota.Text = "Угол поворота: " + Convert.ToString(textBox1.Text) + "°";
}
else
{
MessageBox.Show("Неправильно заданное значение");
}
}
else { }
}
private void button3_Click(object sender, EventArgs e)
{
if (isConnected == true)
{
int znach_rast = Convert.ToInt16(textBox2.Text);
if (znach_rast >= 0 && znach_rast <= 50)
{
String UgolServo2 = "#B";
UgolServo2 = UgolServo2 + Convert.ToString(znach_rast,16);
UgolServo2 = UgolServo2 + "|";
port.Write(UgolServo2);
Rasstoyanie.Text = "Расстояние: " + Convert.ToString(textBox2.Text)+"мм";
}
else
{
MessageBox.Show("Неправильно заданное значение");
}
}
else { }
}
}
}
Для разработки программы для Arduino использовался «самодельный» протокол на ASCII(заимствован из интернета).
Функции hexToByte(),hexToInt16(),hexToInt32() предназначены для преобразования шестнадцатеричного байтового значения строки в байтовую строку, в двухбайтовое число и четырехбайтное число соответственно.
В будущем планируется модифицировать этот код или вовсе постараться обойтись без протокола, а использовать стандартные методы работы с последовательным портом.
const char StaPack = '#'; // Признак начала пакета данных
const char EndPack = '|'; // Признак окончания пакета данных
#include <Servo.h> //используем библиотеку для работы с сервоприводом
Servo servo1; //объявляем переменную servo типа Servo
Servo servo2; //объявляем переменную servo типа Servo
void setup()
{
Serial.begin(9600);
servo1.attach(11);
servo2.attach(10);
servo1.write(0);
servo2.write(0);
}
byte hexToByte (String StrControlHex) { //0 : 255 (1 байт)
uint8_t HEX16 = 0; // число 16-е из символа
uint8_t exp16 = 1; // степень числа 16
uint8_t decBy = 0; // число 10-е расчитанное без знака
StrControlHex.remove(0, 2); //отрезаем управляющие символы (2шт)
int i = StrControlHex.indexOf('|'); //определяем длину строки
if (i == -1) return 0; //пришел не байт
if (i > 2) return 0; //пришел не байт
StrControlHex.remove(i, 1); //отрезаем управляющие символы ('|')
for (int j = StrControlHex.length() - 1; j >= 0; j--) {
HEX16 = StrControlHex.charAt(j);
if (HEX16 >= 48 && HEX16 <= 57) HEX16 = map(HEX16, 48, 57, 0, 9);
if (HEX16 >= 65 && HEX16 <= 70) HEX16 = map(HEX16, 65, 70, 10, 15);
if (HEX16 >= 97 && HEX16 <= 102) HEX16 = map(HEX16, 97, 102, 10, 15);
decBy = decBy + HEX16 * exp16;
exp16 = exp16 * 16;
}
return decBy; //возвращаем десятичное число 1 байт без знака
}
int16_t hexToInt16 (String StrControlHex) { // -32768 : 32767 (2 байта)
uint8_t HEX16 = 0; // число 16-е из символа
uint16_t exp16 = 1; // степень числа 16
int16_t dec16 = 0; // число 10-е расчитанное со знаком
StrControlHex.remove(0, 2); //отрезаем управляющие символы (2шт)
int i = StrControlHex.indexOf('|'); //определяем длину строки
if (i == -1) return 0; //пришло не 2 байта
if (i > 4) return 0; //пришло не 2 байта
StrControlHex.remove(i, 1); //отрезаем управляющие символы ('|')
for (int j = StrControlHex.length() - 1; j >= 0; j--) {
HEX16 = StrControlHex.charAt(j);
if (HEX16 >= 48 && HEX16 <= 57) HEX16 = map(HEX16, 48, 57, 0, 9);
if (HEX16 >= 65 && HEX16 <= 70) HEX16 = map(HEX16, 65, 70, 10, 15);
if (HEX16 >= 97 && HEX16 <= 102) HEX16 = map(HEX16, 97, 102, 10, 15);
dec16 = dec16 + HEX16 * exp16;
exp16 = exp16 * 16;
}
return dec16; //возвращаем десятичное число int16_t со знаком
}
int32_t hexToInt32 (String StrControlHex) { // -2147483648 : 2147483647 (4 байта)
uint8_t HEX16 = 0; // число 16-е из символа
uint32_t exp16 = 1; // степень числа 16
int32_t dec32 = 0; // число 10-е расчитанное со знаком
StrControlHex.remove(0, 2); //отрезаем управляющие символы (2шт)
int i = StrControlHex.indexOf('|'); //определяем длину строки
if (i == -1) return 0; //пришло не 4 байта
if (i > 8) return 0; //пришло не 4 байта
StrControlHex.remove(i, 1); //отрезаем управляющие символы ('|')
for (int j = StrControlHex.length() - 1; j >= 0; j--) {
HEX16 = StrControlHex.charAt(j);
if (HEX16 >= 48 && HEX16 <= 57) HEX16 = map(HEX16, 48, 57, 0, 9);
if (HEX16 >= 65 && HEX16 <= 70) HEX16 = map(HEX16, 65, 70, 10, 15);
if (HEX16 >= 97 && HEX16 <= 102) HEX16 = map(HEX16, 97, 102, 10, 15);
dec32 = dec32 + HEX16 * exp16;
exp16 = exp16 * 16;
}
return dec32; //возвращаем десятичное число int32_t со знаком
}
void loop() {
clearPack: // переходим сюда если приняли мусор /отвалились по таймауту/
char IncomChar;
String StrControl = "";
while (Serial.available() > 0) { // выбираем все байты которые пришли в буфер Serial
IncomChar = Serial.read();
if (IncomChar == StaPack) { // пришел символ начала посылки данных
StrControl += IncomChar; // плюсуем пришедший символ к строке
ReceptionPacket: // начало приема пакета
if (Serial.available() > 0) {
IncomChar = Serial.read();
StrControl += IncomChar; // плюсуем пришедший символ к строке
if (IncomChar == EndPack) break; // выходим из цикла выбирая байт если приняли весь пакет
}
goto ReceptionPacket; // переход в начало приема пакета
}
} // конец цикла по выбору байтов
if (StrControl != "") {
switch (StrControl.charAt(1)) { // применяем полученую строку
case 'A':
servo1.write(hexToInt16(StrControl)*0.75);
break;
case 'B':
servo2.write(-1.75*hexToInt16(StrControl)+87.5);
break;
}
}
}
(https://a.radikal.ru/a04/1904/84/e88c3958e709.gif)
Рисунок 4 - Демонстрация работы схвата
Разработка системы управления шаговыми двигателями и получения сигналов с концевых ДТИДля управления положениями робота манипулятора по трем осям применяются шаговые двигатели: GD57STH76-2006A для управления по оси Z и GD57STH56-2808A для управления по осям X и Y. Шаговые двигатели управляются Arduino с помощью драйверов TB 6560. ШД получают питание от драйвера который в свою очередь подключен к блоку питания 24 В.
(https://pp.userapi.com/c845417/v845417353/1ea8ca/odHS7N2boO4.jpg)
Рисунок 5 - схема подключений ШД
(https://pp.userapi.com/c849332/v849332353/16bc9d/UZgLUiDXa3c.jpg)
Рисунок 6 - схема подключений драйвера ШД
При разработке схемы подключений концевых датчиков к Arduino потребовалось использовать делитель напряжений. Т.к. датчик требует минимального напряжения 6 В, что МК обеспечить не может. В связи с этим потребовалось использовать источник питания на 10 В. Напряжение, создаваемое на выходе датчика, равно напряжению питания, поэтому необходимо использовать делитель напряжения, чтобы уменьшить напряжение 10 В на сигнальном выводе до значения, близкого к 5 В, поддерживаемого Arduino.
Схема делителя напряжения - это очень распространенная схема, которая принимает более высокое напряжение и преобразует его в более низкое с помощью пары резисторов. Формула для расчета выходного напряжения основана на законе Ом и показана ниже.
(https://pp.userapi.com/c845417/v845417353/1ea8e9/lj_5r89LAOk.jpg)
где:
V
S - напряжение источника, измеренное в вольтах (В),
R
1 - сопротивление 1-го резистора, измеренное в Омах (Ом).
R
2 - сопротивление 2-го резистора, измеренное в Омах (Ом).
V
OUT - выходное напряжение, измеренное в вольтах (В),
Выбрав из ряда номиналов резисторов E24 R1 = 2.2 Ом, получим
(https://pp.userapi.com/c845417/v845417353/1ea8e2/o9RV0dSx4rM.jpg)
(https://pp.userapi.com/c847019/v847019353/1e033f/IuFWylPvGbI.jpg)
Рисунок 7 - схема подключения датчика
(https://pp.userapi.com/c848632/v848632353/165d52/sxtNBKRxkms.jpg)
Рисунок 8 - схема соединения ШД и концевых датчиков к Arduino
Серводвигатели управляющие губками и поворотом схвата подключаются к 10 и 11 входам
1) Как сервоприводы управляются входами?
2) Пусть это даже будут выходы, каким образом задается направление движения?
С помощью библиотечной функции Servo.write() задается желаемый угол от 0 до 180°. Микроконтроллер на выходе формирует управляющее напряжение в зависимости от заданного угла.Сервомотор имеет встроенный потенциометр, который соединен с выходным валом. Поворотом вала, сервопривод меняет значение напряжения на потенциометре. Плата анализирует напряжение входного сигнала и сравнивает его с напряжением на потенциометре, исходя из полученной разницы, мотор вращается выравнивая напряжение на выходе и на потенциометре.
(https://pp.userapi.com/c849128/v849128694/15fe5b/7XYCoHsTRIw.jpg)
Когда я вижу, какой сложный код в состоянии разработать наши студенты да еще на языке, который не изучался на занятиях, я испытываю противоречивые чувства. С одной стороны, - это удовлетворение. Какие молодцы! Чему-то все-таки научились! Не зря я работал. А с другой, - какие мерзавцы, могли бы и раньше показать, на что они способны, еще при изучении программирования и алгоритмизации, ну или хотя бы сейчас на "Программном обеспечении систем управления".
Но это я так, к слову. Спасибо за ответ.
Студенты - нелинейные существа!!!
В связи с тестированием программы на модели были выявлены проблемы, и были изменения в статье(коде программы для Arduino). Были назначены начальные задания в ноль градусов обоих сервоприводов при старте системы: servo1.write(0); servo2.write(0); Также для соответствия задания положения схвата в соответствием с валом был введен масштабный коэффициент = 0.75, т.к. при положении схвата в 0 градусов ему соответствует 0 градусов на валу, а при положении 180 градусов,на валу соответствует 140 градусов. Расчет масштабного коэффициента который в соответствие от заданного расстояния между губками схвата задает соответствующий угол поворота для вала сервопривода находится на стадии расчета.
Ничего не понятно! Изобразите решение графически!
Ничего не понятно! Изобразите решение графически!
В связи с тестированием программы на модели были выявлены проблемы, и были изменения в статье(коде программы для Arduino). Были назначены начальные задания в ноль градусов обоих сервоприводов при старте системы: servo1.write(0); servo2.write(0); Также для соответствия задания положения схвата в соответствием с валом был введен масштабный коэффициент = 0.75, т.к. при положении схвата в 0 градусов ему соответствует 0 градусов на валу, а при положении 180 градусов,на валу соответствует 140 градусов. Расчет масштабного коэффициента который в соответствие от заданного расстояния между губками схвата задает соответствующий угол поворота для вала сервопривода находится на стадии расчета.
(https://pp.userapi.com/c846420/v846420296/1d14f0/Q-ce-t3xjt0.jpg)(https://pp.userapi.com/c846420/v846420296/1d14f7/xISdhFCymbM.jpg)(https://pp.userapi.com/c846420/v846420296/1d14fe/FnjbwVJ2YMg.jpg)
Андрей Николаевич, вам что нибудь стало понятно, про 180 и 140 градусов?
Не очень. Туповат я стал для таких дел...
Согласен с ran. Как то звучит плохо, когда читаешь текст доклада не автоматически.
Михаил, составьте структурно-функциональную схему процесса начинающегося с момента вашего желания задействовать схват, до момента перемещения губок схвата. Рассмотрите подробно эту цепочку.
:) :) :) Это можно сразу отправлять на "Секс по телефону"...
По-моему очень красиво получилось! :-) Впрочем, как говорила моя соседка - "оговорочка по Фрейду!"
Андрей Николаевич, вам что нибудь стало понятно, про 180 и 140 градусов?
Необъяснимо, но факт, не могу объяснить почему идет такая разница между заданием и реальным положением схвата. При считывании угла сервопривод показывает 140 градусов, когда схват отклонен на 180. Также возникли проблемы с выводом уравнения движения губок, т.е. перевод заданного расстояния в угол поворота вала сервопривода, если даже и удавалось вывести то все работало наоборот. Пришлось вставить
смешной код для временного решения проблем. Полный код для Arduino можно будет увидеть в статье.
if(hexToInt16(StrControl)==10) servo2.write(70);
if(hexToInt16(StrControl)==11) servo2.write(68.25);
if(hexToInt16(StrControl)==12) servo2.write(66.5);
if(hexToInt16(StrControl)==13) servo2.write(64.75);
if(hexToInt16(StrControl)==14) servo2.write(63);
if(hexToInt16(StrControl)==15) servo2.write(61.25);
if(hexToInt16(StrControl)==16) servo2.write(59.5);
if(hexToInt16(StrControl)==17) servo2.write(57.75);
if(hexToInt16(StrControl)==18) servo2.write(56);
if(hexToInt16(StrControl)==19) servo2.write(54.25);
if(hexToInt16(StrControl)==20) servo2.write(52.5);
if(hexToInt16(StrControl)==21) servo2.write(50.75);
if(hexToInt16(StrControl)==22) servo2.write(49);
if(hexToInt16(StrControl)==23) servo2.write(47.25);
if(hexToInt16(StrControl)==24) servo2.write(45.5);
if(hexToInt16(StrControl)==25) servo2.write(43.75);
if(hexToInt16(StrControl)==26) servo2.write(42);
if(hexToInt16(StrControl)==27) servo2.write(40.25);
if(hexToInt16(StrControl)==28) servo2.write(38.5);
if(hexToInt16(StrControl)==29) servo2.write(36.75);
if(hexToInt16(StrControl)==30) servo2.write(35);
if(hexToInt16(StrControl)==31) servo2.write(33.25);
if(hexToInt16(StrControl)==32) servo2.write(31.5);
if(hexToInt16(StrControl)==33) servo2.write(29.75);
if(hexToInt16(StrControl)==34) servo2.write(28);
if(hexToInt16(StrControl)==35) servo2.write(26.25);
if(hexToInt16(StrControl)==36) servo2.write(24.5);
if(hexToInt16(StrControl)==37) servo2.write(22.75);
if(hexToInt16(StrControl)==38) servo2.write(21);
if(hexToInt16(StrControl)==39) servo2.write(19.25);
if(hexToInt16(StrControl)==40) servo2.write(17.5);
if(hexToInt16(StrControl)==41) servo2.write(15.75);
if(hexToInt16(StrControl)==42) servo2.write(14);
if(hexToInt16(StrControl)==43) servo2.write(12.25);
if(hexToInt16(StrControl)==44) servo2.write(10.5);
if(hexToInt16(StrControl)==45) servo2.write(8.75);
if(hexToInt16(StrControl)==46) servo2.write(7);
if(hexToInt16(StrControl)==47) servo2.write(5.25);
if(hexToInt16(StrControl)==48) servo2.write(3.5);
if(hexToInt16(StrControl)==49) servo2.write(1.75);
if(hexToInt16(StrControl)==50) servo2.write(0);
В целый тип потребовалось преобразовывать, чтобы if-ы можно было использовать? Это не смешно, а грустно!
Нарисуйте статическую зависимость и задайте ее формулой. Только не говорите, что Вас этому не учили! Помните волшебное слово polyfit? (А оно, скорее всего, и понадобится, поскольку зависимость вроде как линейная.)
В целый тип потребовалось преобразовывать, чтобы if-ы можно было использовать? Это не смешно, а грустно!
Нарисуйте статическую зависимость и задайте ее формулой. Только не говорите, что Вас этому не учили! Помните волшебное слово polyfit? (А оно, скорее всего, и понадобится, поскольку зависимость вроде как линейная.)
Огромное вам спасибо за помощь!!!
Начертив график получил
(https://pp.userapi.com/c849232/v849232529/16286b/knUvq-vAEpk.jpg)
графики такого типа описываются уравнением y=-kx+b
где k=1.75
Составив уравнение получим: при y=70, x=10,
70=-1.75*10+b
Откуда
b=70+17.5=87.5
И в итоге получаем уравнение y=-1.75*x+87.5
где y-угол положения вала сервопривода; x-заданное расстояние.
Кстати была добавлена анимация руки в последней картинке статьи
Наука победила!
Кстати была добавлена анимация руки в последней картинке статьи
Классно! Есть что-то в этом японское. Кстати, Япония - 4-я экономика мира (впереди США, Китай и Индия), при численности населения немногим меньше нашего.
Не забывайте, обо всех изменениях в тексте доклада писать в сообщениях!!!
Куда пропала левая полуось в деморолике?
Была обновлена анимация схвата!
Ура, Михаил полуось нашел!
Когда надо будет, найдется все (https://www.youtube.com/watch?v=z2njRx0AjIs).
Михаил, к вам вопрос: ..И, главное, ответьте на вопрос: чем эта позорная Omron-овская демонтированная система управления была хуже современной Alibaba-евской, которая сейчас там стоит, но ничего не делает?..(от ran)
http://forum.kafedra-appie.ru/index.php?topic=282.0 (http://forum.kafedra-appie.ru/index.php?topic=282.0)
Михаил, к вам вопрос: ..И, главное, ответьте на вопрос: чем эта позорная Omron-овская демонтированная система управления была хуже современной Alibaba-евской, которая сейчас там стоит, но ничего не делает?..(от ran)
http://forum.kafedra-appie.ru/index.php?topic=282.0 (http://forum.kafedra-appie.ru/index.php?topic=282.0)
Ответил!
Не зря проходит конференция. Что бы нам еще такого починить?...
Были обновления:
- Название статьи "Управление схватом робота с использованием Arduino" ->"Разработка системы управления роботом манипулятором с использованием Arduino"
- Добавлен раздел -"Разработка системы управления шаговыми двигателями и получения сигналов с концевых ДТИ"