Исполнитель: Потемкин М.С., 641-об
Научный руководитель: Русинов В.Л.
Разработка системы управления роботом манипулятором с использованием Arduino
Схват для робота манипулятора управляется с помощью двух сервоприводов под управлением микроконтроллера Arduino
Рисунок 1 - Схват
Разработка ручного управленияВ ручном управлении используется матричная клавиатура. Матричная клавиатура подключается к входам 2-9 МК. Серводвигатели управляющие губками и поворотом схвата подключаются к 10 и 11 выходам.
Рисунок 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 портами.
Рисунок 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;
}
}
}
Рисунок 4 - Демонстрация работы схвата
Разработка системы управления шаговыми двигателями и получения сигналов с концевых ДТИДля управления положениями робота манипулятора по трем осям применяются шаговые двигатели: GD57STH76-2006A для управления по оси Z и GD57STH56-2808A для управления по осям X и Y. Шаговые двигатели управляются Arduino с помощью драйверов TB 6560. ШД получают питание от драйвера который в свою очередь подключен к блоку питания 24 В.
Рисунок 5 - схема подключений ШД
Рисунок 6 - схема подключений драйвера ШД
При разработке схемы подключений концевых датчиков к Arduino потребовалось использовать делитель напряжений. Т.к. датчик требует минимального напряжения 6 В, что МК обеспечить не может. В связи с этим потребовалось использовать источник питания на 10 В. Напряжение, создаваемое на выходе датчика, равно напряжению питания, поэтому необходимо использовать делитель напряжения, чтобы уменьшить напряжение 10 В на сигнальном выводе до значения, близкого к 5 В, поддерживаемого Arduino.
Схема делителя напряжения - это очень распространенная схема, которая принимает более высокое напряжение и преобразует его в более низкое с помощью пары резисторов. Формула для расчета выходного напряжения основана на законе Ом и показана ниже.
где:
V
S - напряжение источника, измеренное в вольтах (В),
R
1 - сопротивление 1-го резистора, измеренное в Омах (Ом).
R
2 - сопротивление 2-го резистора, измеренное в Омах (Ом).
V
OUT - выходное напряжение, измеренное в вольтах (В),
Выбрав из ряда номиналов резисторов E24 R1 = 2.2 Ом, получим
Рисунок 7 - схема подключения датчика
Рисунок 8 - схема соединения ШД и концевых датчиков к Arduino
