Проектирование манипулятора

Рис. 5.5. Схема платформы для манипулятора

Платформа изготавливалась целиком из фанеры толщиной 10 мм, все элементы платформы скреплены саморезами (Рис. 5.6).



Рис. 5.6. Платформа для манипулятора

5.3 Сборка манипулятора

Все детали схвата скреплены болтами диаметром 4 мм, они не слижком затянуты, чтобы схват работал без затруднений. На рисунке 5.7 изображен собранный схват (вид сверху и снизу).

Рис. 5.7. Собранный схват манипулятора

На рисунке 5.8 изображено скрепленные 1 ое и 2 ое звено, это основная часть конструкции манипулятора, её вес составляет 260 грамм.

Рис. 5.8. 1-ое и 2-ое звенья манипулятора

Схват крепится к 2 му звену через специальное шарнирное соединение (Рис. 5.9).

Рис. 5.9. Шарнирное соединение

Нижняя неподвижная часть основания манипулятора крепится болтами к платформе (Рис. 5.10), затем к ней крепится верхняя подвижная часть основания.

Рис. 5.10. Нижняя часть основания

Полностью собранный робот, установленный на платформу и подключенный к плате Arduino и блоку питания показан на рисунке 5.11



Рис. 5.11. Готовый манипулятор



6. Разработка системы управления манипулятором

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

Система управления манипулятором разработана на основе Arduino Uno которая подключена к компьютеру через COM порт, по которому поступает на Arduino питание и команды для управления сервоприводами манипулятора. Четы сервопривода подключены к Arduino к цифровым портам под номерами 6, 8, 10, 12 . Сервоприводы подключены параллельно к отдельному источнику питания, номинальное напряжение питания 7В. Также земля (GND) сервоприводов подключена к Arduino к контакту GND.

Сервопривод «servo 0», подключенный к pin 6 (Рис. 6.1), управляет поворотом основания манипулятора, модель сервопривода SR430. «servo 1», подключенный к pin 8, управляет поворотом 1 го звена манипулятора, модель сервопривода DF15RMG. «servo 2», подключенный к pin 10, управляет поворотом 2 го звена манипулятора, модель сервопривода RDS3115. «servo 3», подключенный к pin 12, управляет сжатием / разжатием механического схвата манипулятора, модель сервопривода SR430.

При подключении Arduino к питанию, в ней тут же запускается программа и посылает сигналы на сервоприводы, чтобы они повернулись в начальное положение. Далее программа начинает ожидать команды, поступающие в COM порт с компьютера. Команды являются строками, содержащими значения углов поворотов сервоприводов манипулятора. Как только поступает команда (строка) в COM порт, программа считывает данные углов поворота из этой строки и отправляет сигналы управления с новыми значениями на сервоприводы.



6.2 Разработка копмьютерной программы для управления манипулятором

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

Код программы написан на языке C# в среде Visual Studio [21]. Внешне программа выглядит в виде формы, на которой расположены отдельные панели, выполняющие свои функции, например: панель для ручного управления роботом, которое выполняется с помощью кнопок; панель настройки скорости движения манипулятора; панель программирования манипулятора, которая позволяет задать и запустить программу автоматического управления роботом; панель информации, где указываются данные о состоянии манипулятора. На рисунке 6.3 показана копия экрана программы в которой стоят обозначения отдельных её функций, которые далее будут более подробно описаны.

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



7. Тестирование работы манипулятора

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

Сначала подключаются сервоприводы к Arduino согласно схеме на рисунке 6.1, далее Arduino подключается по COM порту к компьютеру. При подачи питания на сервоприводы манипулятор становится в начальное положение.

При запуске компьютерной программы управления манипулятором, она сразу определяет подключенный COM порт: выводится сообщение «COM порт подключен!». Для проверки правильности подключения сервоприводов к Arduino в ручном режиме управления проверяется каждое звено по отдельности, т.е. при нажатии кнопки должен отреагировать соответствующий сервопривод. Все сервоприводы подключены верно. Для проверки стабильности программы внезапно отключаем COM порт Arduino от компьютера, программа вывела сообщение «COM порт не подключен!», робот остался в том же положении в каком и был и никаких действий самостоятельно не производит. После обратного подключения COM порта нажимаю кнопку «Поиск COM портов» и программа тут же находит этот COM порт и подключается к нему, теперь возможно дальнейшее управление манипулятором. При ручном управлении манипулятором по осям координат, рабочая точка передвигается строго вдоль оси, по которой её передвигаю. Все звенья работают синхронно, т.к. для каждого нового движения просчитывается обратная задача кинематики.

При движении манипулятора заметны некоторые небольшие резкие рывки сервоприводов что приводит к колебанию всей конструкции манипулятора. Это происходит из за того, что сервоприводы работают не с достаточно высокой точностью и на малые углы поворота (например в 1 градус) могут не отреагировать. При этом программа Arduino поворачивает сервоприводы с шагом в 0.01 градуса, т.е. с программной точки зрения движение должно происходить плавно.

Далее выполняется проверка программного управления. Подвожу в ручную положение рабочей точки манипулятора в нужную позицию и нажимаю кнопку «Добавить положение», данная позиция сохраняется. Таким образом создаётся вся программа целиком и добавляю команды задержки в некоторых местах. Нажимаю кнопку «Запуск», манипулятор начинает движение проходя через все запрограммированные точки и ожидает заданное количество времени на командах задержки, при движении манипулятора есть возможность регулировать скорость его движения в реальном времени.



Заключение

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

Также была построена 3D модель манипулятора, с учётом всех длин и размеров деталей, для определения её конструктивных ограничений, а именно диапазонов углов поворотов звеньев. Эти углы определили форму рабочей зоны, максимальный радиус которой получился 570 мм от центра основания робота. Рабочая зона строилась при помощи динамической модели манипулятора, в которой манипулятор прошёлся через все крайние положения рабочей зоны.

В динамической модели были определены зависимости различных велечин таких как: сила, момент, угловая скорость и угловое ускорение, которые действовали на оси поворотов звеньев и основания. Также было смоделировано как вращающий момент, который развивают реальные сервоприводы, действовал на движение звеньев манипулятора. Этот же момент использовался для определения грузоподъёмности манипулятора, которая составила 150 грамм при максимально вытянутой руке манипулятора.

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

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

Проверка работоспособности манипулятора показала, что робот отрабатывает точно все движения, которые ему задаются в программе. При этом были заметны некоторые небольшие рывки при движении конструкции манипулятора, это происходит из-за особенностей сервоприводов. Тестирование работы манипулятора позволило определить его следующие параметры: повторяемость, которая в среднем составляла 2 мм; реальная грузоподъёмность, равная от 50 до 100 грамм в зависимости от длины плеча, что является меньше чем при моделировании, т.к. реальная масса робота оказалась немного больше ожидаемой; рабочая зона, которая получилась такая же как и при моделировании.

Так же были получены новые знания и навыки построения 3D моделей деталей, работы с 3D принтером, сборки манипулятора, разработки и тестировании его компьютерной динамической модели, разработки системы управления и программировании на языке C#.

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



Список литературы

1. Промышленные роботы и манипуляторы [электронный ресурс]. URL: http://cncnc.ru/documentation/theory_of_mechanismus_and_machines/lect_19.htm (дата обращения: 09.06.2015).

2. Клюев С.А. Компьютерное моделирование: Учебно-методическое пособие. - М.: Волжский политехнический институт, 2009. - 89 с.

3. Arduino [электронный ресурс]. URL: https://www.arduino.cc/ (дата обращения: 09.06.2015).

4. Промышленные роботы манипуляторы [электронный ресурс]. URL: http://robo24.ru/promyshlennye-roboty (дата обращения: 09.06.2015).

5. Манипуляционные роботы [электронный ресурс]. URL: http://refleader.ru/jgeyfsrnarna.html (дата обращения: 10.06.2015).

6. Лекции по УРиРТС [электронный ресурс]. URL: http://www.studfiles.ru/dir/cat39/subj1292/file13007/view135824/page2.html\ (дата обращения: 10.06.2015).

7. Схиртладзе А.Г., Выходец В.И. Оборудование машиностроительных предприятий. - М.: РПК «Политехник», 2005. - 92 с.

8. Компьютерное моделирование [электронный ресурс]. URL: http://www.inf1.info/book/export/html/215 (дата обращения: 8.03.2015).

9. Гонсалес Р., Фу К., Ли К. Робототехника. - М.: Москва «Мир», 1989. - 620 с.

10. Adams - MSC Software [электронный ресурс]. URL: http://www.mscsoftware.com/product/adams (дата обращения: 10.06.2015).

11. Universal Mechanism - моделирование динамики механических систем [электронный ресурс]. URL: http://www.umlab.ru/pages/index.php? id=1 (дата обращения: 10.06.2015).

12. NX: Siemens PLM Software [электронный ресурс]. URL: http://www.plm.automation.siemens.com/ru_ru/products/nx/ (дата обращения: 10.06.2015).

13. CATIA - Dassault Systиmes [электронный ресурс]. URL: http://www.3ds.com/ru/produkty-i-uslugi/catia/ (дата обращения: 10.06.2015).

14. SolidWorks-Russia [электронный ресурс]. URL: http://www.solidworks.ru/ (дата обращения: 10.06.2015).

15. Пособие по ADAMS [электронный ресурс]. URL: http://mmm_samgu.ssu.samara.ru/polyakov/adams/Adams_pos_new.pdf (дата обращения: 8.03.2015).

16. Fanuc M-410iB Series [электронный ресурс]. URL: http://www.fanucrobotics.com/cmsmedia/datasheets/M-410iB % 20Series_15.pdf (дата обращения: 11.06.2015).

17. Описание сервопривода DF15RMG [электронный ресурс]. Режим доступа: http://www.electronshik.ru/item/df15rmg-tilt-kit-20kg-1114314 (дата обращения: 12.06.2015).

18. Описание сервопривода RDS3115 [электронный ресурс]. Режим доступа: http://www.aliexpress.com/snapshot/6494586405.html? orderId=65771461539714 (дата обращения: 12.06.2015).

19. Описание сервопривода SR430 [электронный ресурс]. Режим доступа: http://robot-kit.ru/print_product_info.php/products_id/438 (дата обращения: 12.06.2015).

20. Описание Google SketchUp 8 [электронный ресурс]. Режим доступа: http://reviewsoft.ru/Windows/Audio-Video/Audio-Recorders/google-sketchup/ (дата обращения: 12.06.2015).

21. Visual Studio - Microsoft Developer Tools [электронный ресурс]. Режим доступа: https://www.visualstudio.com/ru-ru/visual-studio-homepage-vs.aspx (дата обращения: 13.06.2015).



Приложения

Приложение 1

Код Matlab для вычисления математической динамической модели манипулятора

L1 = 0.2;

L2 = 0.2;

Q1 = pi/4; dQ1 = 0; ddQ1 = 0;

Q2 = pi/4; dQ2 = pi/8; ddQ2 = pi/6;

m1 = 0.21;

m2 = 0.18;

g = 9.80665;

C1 = cos(Q1); S1 = sin(Q1);

C2 = cos(Q1); S2 = sin(Q1);

C12 = cos (Q1+Q2); S12 = sin (Q1+Q2);

A1_0 = [C1 S1 0 L1*C1

S1 C1 0 L1*S1

0 0 1 0

0 0 0 1];

A2_1 = [C2 - S2 0 L2*C2

S2 C2 0 L2*S2

0 0 1 0

0 0 0 1];

A2_0 = A1_0 * A2_1;

MQ = [0 -1 0 0

1 0 0 0

0 0 0 0

0 0 0 0];

U11 = MQ * A1_0;

U21 = MQ * A2_0;

U22 = A1_0 * MQ * A2_1;

J1 = [1/3*m1*L1^2 0 0 -1/2*m1*L1

0 0 0 0

0 0 0 0

-1/2*m1*L1 0 0 m1];

J2 = [1/3*m2*L2^2 0 0 -1/2*m2*L2

0 0 0 0

0 0 0 0

-1/2*m2*L2 0 0 m2];

D11 = 1/3*m1*L1^2 + 4/3*m2*L2^2 + m2*C2*L2^2;

D12 = 1/3*m2*L2^2 + 1/2*m2*L2^2*C2;

D22 = 1/3*m2*L2^2;

h1 = -1/2*m2*S2*L2^2*dQ2^2 - m2*S2*L2^2*dQ1*dQ2;

h2 = 1/2*m2*S2*L2^2*dQ1^2;

c1 = 1/2*m1*g*L1*C1 + 1/2*m2*g*L2*C12 + m2*g*L1*C1;

c2 = 1/2*m2*g*L2*C12;

D = [D11 D12

D12 D22];

ddQ = [ddQ1

ddQ2];

h = [h1

h2];

c = [c1

c2];

T = D*ddQ + h + c;

Приложение 2

Код программы Arduino для управления сервоприводами манипулятора

#include «Servo.h»

// Обьекты Servo

Servo servo0;

Servo servo1;

Servo servo2;

Servo servo3;

String s=»»;

int i;

int q0, nq0 = 94; // начальное положене угла серво0

int q1, nq1 = 144; // начальное положене угла серво1

int q2, nq2 = 180; // начальное положене угла серво2

int q3, nq3 = 50; // начальное положене угла серво3

float p0, p1, p2, p3;

void setup()

{

Serial.begin(9600);

servo0.attach(6); // серво1 подключен к пин 6

servo1.attach(8); // серво1 подключен к пин 8

servo2.attach(10); // серво2 подключен к пин 10

servo3.attach(12); // серво3 подключен к пин 12

q0 = nq0;

q1 = nq1;

q2 = nq2;

q3 = nq3;

servo0.write(q0); // Повернуть серво0 на q0 градусов

servo1.write(q1); // Повернуть серво1 на q1 градусов

servo2.write(q2); // Повернуть серво2 на q2 градусов

servo3.write(q3); // Повернуть серво3 на q3 градусов

p0=q0;

p1=q1;

p2=q2;

p3=q3;

}

void loop()

{

while (Serial.available())

{ // считывание данных с COM порта

char c = Serial.read(); // читаем символ

s += c; // добавляем к строке

if (c == '!') // если считали символ, строка закончилась

{

i=0;

q0=0;

q1=0;

q2=0;

q3=0;

while (s[i]!= ' ')

{

q0=q0*10;

q0=q0+(int (s[i]) - 48);

i++;

}

i++;

while (s[i]!= ' ')

{

q1=q1*10;

q1=q1+(int (s[i]) - 48);

i++;

}

i++;

while (s[i]!= ' ')

{

q2=q2*10;

q2=q2+(int (s[i]) - 48);

i++;

}

i++;

while (s[i]!= '!')

{

q3=q3*10;

q3=q3+(int (s[i]) - 48);

i++;

}

s = «»;

}

}

// повороты сервоприводов

if (p0<q0)

{

p0=p0+0.01;

servo0.write(p0); // Повернуть серво0 на p0 градусов

}

if (p0>q0)

{

p0=p0-0.01;

servo0.write(p0); // Повернуть серво0 на p0 градусов

}

if (p1<q1)

{

p1=p1+0.01;

servo1.write(p1); // Повернуть серво1 на p1 градусов

}

if (p1>q1)

{

p1=p1-0.01;

servo1.write(p1); // Повернуть серво1 на p1 градусов

}

if (p2<q2)

{

p2=p2+0.01;

servo2.write(p2); // Повернуть серво2 на p2 градусов

}

if (p2>q2)

{

p2=p2-0.01;

servo2.write(p2); // Повернуть серво2 на p2 градусов

}

if (p3<q3)

{

p3=p3+0.01;

servo3.write(p3); // Повернуть серво3 на p3 градусов

}

if (p3>q3)

{

p3=p3-0.01;

servo3.write(p3); // Повернуть серво2 на p2 градусов

}

delay (0.01);

}



Приложение 3

Код компьютерной программы на языке C# для управления манипулятором

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; // подключение пространства имен с классом SerialPort

using System.IO;

namespace RobotProgram

{

public partial class Form1: Form

{

double q0 = 0, q1 = 69, q2 = 38, q3 = 50; // значения начальных углов поворотов звеньев

int s0 = 1, s1 = 1, s2 = 1, s3 = 1; // скорости поворотов звеньев (изменять не нужно)

// длины звеньев, мм

int L0 = 90; // основание

int L1 = 200; // 1 ое звено

int L2 = 200; // 2 ое звено

int L3 = 170; // схват

// переменные координат и углов звеньев

double X, Y, Z, Xm, Ym, Zm, qq0, qq1, qq2;

// флаги наджатия кнопок управления

bool f0U = false, f0D = false,

f1U = false, f1D = false,

f2U = false, f2D = false,

f3U = false, f3D = false;

bool flag = true;

bool flag1 = false;

bool start_flag = false;

bool gotovo = true;

bool f_time = false;

int time1 = 0;

string str_pos = «»;

string str_pos_1 = «»;

string str1 = «»;

string str_x = «»;

string str_y = «»;

string str_z = «»;

string str_s = «»;

string str_t = «»;

int xk, yk, zk, sk, tk;

double k = 57.295779513;

double q3r;

string stroka = «»;



SerialPort Port1;

public Form1 ()

{

InitializeComponent();

}

private void Form1_Load (object sender, EventArgs e)

{

string[] myPort; // создаем массив строк

myPort = System.IO. Ports. SerialPort. GetPortNames(); // в массив помещаем доступные порты

comboBox1. Items. AddRange(myPort); // теперь этот массив заносим в список(comboBox)

if (comboBox1. Items. Count!= 0) comboBox1. SelectedIndex = 0; // если есть порты выбираем 1 ый

}

private void Form1_FormClosing (object sender, FormClosingEventArgs e)

{

if (comboBox1. Items. Count!= 0) // если есть доступный COM порт

{

if (Port1. IsOpen!= false)

{

Port1. Close(); // при закрытии программы закрываем порт (если он открыт)

}

}

}



private void button1_Click (object sender, EventArgs e) {}

private void button2_Click (object sender, EventArgs e) {}

//// управление роботом ////

//// ручное управление ////

private void timer1_Tick (object sender, EventArgs e)

{

if (angles. Checked) // если выбрано управление углами

{

label1. Text = «Q0»;

label2. Text = «Q1»;

label3. Text = «Q2»;

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

//s0 = Q0TB. Value; // скорость движения манипулятора

s0 = 1;

s1 = s0;

s2 = s0;

s3 = 1;

// настройка скорости

if ((q3r * k + s1) > 0)

{

s1 = (int) Math. Abs (Math. Round (q3r * k));

s2 = s1;

if (s1 == 0)

{

s1 = 1;

s2 = 1;

}

}

// управление углами

if (f0U)

{

q0 = q0 + s0; // увеличение q0

if (q0 > 85) q0 = 85;

}

if (f0D)

{

q0 = q0 - s0; // уменьшение q0

if (q0 < -85) q0 = -85;

}

Q0T. Text = q0. ToString(); // вывод значения угла q0

if (f1U)

{

q1 = q1 + s1; // увеличение q1

if (q1 > 102) q1 = 102;

}

if (f1D && ((q3r * k) < 0))

{

q1 = q1 - s1; // уменьшение q1

if (q1 < -42) q1 = -42;

}

Q1T. Text = q1. ToString(); // вывод значения угла q1

if (f2U)

{

q2 = q2 + s2; // увеличение q2

if (q2 > 48) q2 = 48;

}

if (f2D && ((q3r * k) < 0))

{

q2 = q2 - s2; // уменьшение q2

if (q2 < -90) q2 = -90;

}

Q2T. Text = q2. ToString(); // вывод значения угла q2

if (f3U)

{

q3 = q3 + s3; // увеличение q3

if (q3 > 90) q3 = 90;

}

if (f3D)

{

q3 = q3 - s3; // уменьшение q3

if (q3 < 0) q3 = 0;

}

Q3T. Text = q3. ToString(); // вывод значения угла q3

// корректировка q1 и q2

if ((q3r * k) > 0)

{

if (q1 < 0) q1 = q1 + s0;

if (q2 < 0) q2 = q2 + s0;

}

pryamaya_zadacha(); // решение прямой задачи

// присвоение координат

X = Xm;

Y = Ym;

Z = Zm;

}

if (coordinates. Checked) // если выбрано управление координатами

{

label1. Text = «X»;

label2. Text = «Y»;

label3. Text = «Z»;

// скорость движения манипулятора

s0 = 1;

s3 = 1;

// настройка скорости

if ((q3r * k + s1) > 0)

{

s0 = (int) Math. Abs (Math. Round (q3r * k));

if (s0 == 0) s0 = 1;

}

// управление координатами

if (f0U &&! ((flag == false) && (X > 0)))

{

X = X + s0; // увеличение X

}

if (f0D)

{

X = X - s0; // уменьшение X

}

Q0T. Text = X. ToString(); // вывод значения X

if (f1U &&! ((flag == false) && (Y > 0)))

{

Y = Y + s0; // увеличение Y

}

if (f1D &&! ((flag == false) && (Y < 0)))

{

Y = Y - s0; // уменьшение Y

}

Q1T. Text = Y. ToString(); // вывод значения Y

if (f2U && ((q3r * k) < 0) &&! ((flag == false) && (Z > 0)))

{

Z = Z + s0; // увеличение Z

}

if (f2D &&! ((flag == false) && (Z < 0)))

{

Z = Z - s0; // уменьшение Z

}

Q2T. Text = Z. ToString(); // вывод значения угла Z

// управление схватом

if (f3U)

{

q3 = q3 + s3; // разжимать схват

if (q3 > 90) q3 = 90;

}

if (f3D)

{

q3 = q3 - s3; // сжимать схват

if (q3 < 0) q3 = 0;

}

Q3T. Text = q3. ToString(); // вывод значения угла q3



obratnaya_zadacha(); // решение обратной задачи

if (double. IsNaN(qq0) || double. IsNaN(qq1) || double. IsNaN(qq2))

{ // если при решении обратной задачи значение угла будет NaN, значит это уже недосягаемая зона

label9. ForeColor = Color. Red;

label9. Text = «Достигнут край рабочей зоны!»;

flag = false; // ставим флаг, чтобы робот дальше не двигался

}

else

{

q0 = qq0;

q1 = qq1;

q2 = qq2;

flag = true;

}

korrektirovka(); // корректировка углов поворотов звеньев

}

// вывод различных значений на экран

label10. Text = «X =» + Math. Round(X).ToString();

label11. Text = «Y =» + Math. Round(Y).ToString();

label12. Text = «Z =» + Math. Round(Z).ToString();

label13. Text = «Q0 =» + q0. ToString();

label14. Text = «Q1 =» + q1. ToString();

label15. Text = «Q2 =» + q2. ToString();

label19. Text = «Схват =» + q3. ToString();

if (start_flag) Start. Text = «Стоп»;

else Start. Text = «Запуск»;

if ((gotovo == false) && (start_flag == true)) action();

if (start_flag)

{

groupBox1. Enabled = false;

angles. Checked = true;

}

else groupBox1. Enabled = true;

//// порграммное управление ////

if (start_flag && radioButton1. Checked) // одноразовое выполнение программы

{

if (listBox1. Items. Count > 0) // если есть строки

{

try

{

string st = listBox1. Items [listBox1. SelectedIndex].ToString(); // если нет выделенных строк

}

catch

{

listBox1. SetSelected (0, true); // выделяем 1 ую строку

}

if ((listBox1. Items [listBox1. SelectedIndex].ToString().Length > 0) && gotovo) // если есть строка

{

str_pos = listBox1. Items [listBox1. SelectedIndex].ToString(); // считывание строки-команды

if (String. Equals (str_pos, str_pos_1) == false) gotovo = false; // если поступила новая команда, ставим флаг на выполнение команды

}

if (gotovo) // если команда выполнена

{

if (listBox1. Items. Count > (listBox1. SelectedIndex + 1)) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду

else start_flag = false; // если строк-команд нет, программа завершена

}

}

else start_flag = false; // если строк-команд нет, программа завершена

}

if (start_flag && radioButton2. Checked) // выполнение в цикле

{

if (listBox1. Items. Count > 0) // если есть строки

{

try

{

string st = listBox1. Items [listBox1. SelectedIndex].ToString(); // если нет выделенных строк

}

catch

{

listBox1. SetSelected (0, true); // выделяем 1 ую строку

}

if ((listBox1. Items [listBox1. SelectedIndex].ToString().Length > 0) && gotovo) // если есть строка

{

str_pos = listBox1. Items [listBox1. SelectedIndex].ToString(); // считывание строки-команды

if (String. Equals (str_pos, str_pos_1) == false) gotovo = false; // если поступила новая команда, ставим флаг на выполнение команды

}

if (gotovo) // если команда выполнена

{

if (listBox1. Items. Count > (listBox1. SelectedIndex + 1)) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду

else listBox1. SetSelected (0, true); // если строк-команд нет, переходим в начало

}

}

else start_flag = false; // если строк-команд нет, программа завершена

}

if (start_flag && radioButton3. Checked) // пошаговое выполнение

{

if (listBox1. Items. Count > 0) // если есть строки

{

try

{

string st = listBox1. Items [listBox1. SelectedIndex].ToString(); // если нет выделенных строк

}

catch

{

listBox1. SetSelected (0, true); // выделяем 1 ую строку

}

if ((listBox1. Items. Count > (listBox1. SelectedIndex + 1))) // если есть следующая строка

{

if ((listBox1. Items [listBox1. SelectedIndex + 1].ToString().Length > 0) && gotovo && (flag1==false)) // если есть строка

{

if (flag1 == false) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду

str_pos = listBox1. Items [listBox1. SelectedIndex].ToString(); // считывание строки-команды

gotovo = false; // ставим флаг на выполнение команды

flag1 = true; // ставим флаг, что начали выполнение программы

}

if (gotovo && flag1) // если команда выполнена

{

flag1 = false;

start_flag = false; // программа завершена

}

}

else

{

start_flag = false; // если строк-команд нет, программа завершена

}

}

else start_flag = false; // если строк-команд нет, программа завершена

}

//// конец программного управления ////

// вывод информации о состоянии программы

if (start_flag)

{

label20. Text = «Выполнение программы»;

if (String. Equals (str1, «P»)) // если это команда позиции

{

label21. Text = «Движение к позиции»;

label22. Text = «X=» + str_x +» Y=» + str_y +» Z=» + str_z;

label23. Text = «Схват=» + str_s;

}

if (String. Equals (str1, «D»)) // если это команда задержки

{

label21. Text = «Ожидание»;

label22. Text = «time="+time1. ToString();

label23. Text = «»;

}

}

else

{

label20. Text = «»;

label21. Text = «»;

label22. Text = «»;

label23. Text = «»;

}

}

///// конец управления роботом //////

private void korrektirovka() // корректировка положения углов

{

bool f1 = false;

if (q0 > 85)

{

q0 = 85;

f1 = true;

}

if (q0 < -85)

{

q0 = -85;

f1 = true;

}

if (q1 > 102)

{

q1 = 102;

f1 = true;

}

if (q1 < -42)

{

q1 = -42;

f1 = true;

}

if (q2 > 48)

{

q2 = 48;

f1 = true;

}

if (q2 < -90)

{

q2 = -90;

f1 = true;

}

if ((q3r * k) > 0)

{

if (q1 < 0) q1 = q1 + s0;

if (q2 < 0) q2 = q2 + s0;

f1 = true;

}

pryamaya_zadacha();

if (f1)

{

X = Xm;

Y = Ym;

Z = Zm;

f1 = false;

}

}

private void pryamaya_zadacha()

{

// -Прямая_задача- //

k = 57.295779513;

double q0r = q0 / k;

double q1r = q1 / k;

double q2r = q2 / k;

q3r = - (q1r + q2r);

double sin_q0 = Math. Sin(q0r);

double sin_q1 = Math. Sin(q1r);

double sin_q2 = Math. Sin(q2r);

double sin_q3 = Math. Sin(q3r);

double cos_q0 = Math. Cos(q0r);

double cos_q1 = Math. Cos(q1r);

double cos_q2 = Math. Cos(q2r);

double cos_q3 = Math. Cos(q3r);

Xm = L2 * (cos_q0 * cos_q1 * cos_q2 - cos_q0 * sin_q1 * sin_q2) + L3 * (cos_q3 * (cos_q0 * cos_q1 * cos_q2 - cos_q0 * sin_q1 * sin_q2) - sin_q3 * (cos_q0 * cos_q1 * sin_q2 + cos_q0 * cos_q2 * sin_q1)) + L1 * cos_q0 * sin_q1;

Ym = L2 * (cos_q1 * cos_q2 * sin_q0 - sin_q0 * sin_q1 * sin_q2) + L3 * (cos_q3 * (cos_q1 * cos_q2 * sin_q0 - sin_q0 * sin_q1 * sin_q2) - sin_q3 * (cos_q1 * sin_q0 * sin_q2 + cos_q2 * sin_q0 * sin_q1)) + L1 * sin_q0 * sin_q1;

Zm = L0 - L2 * (cos_q1 * sin_q2 + cos_q2 * sin_q1) - L3 * (cos_q3 * (cos_q1 * sin_q2 + cos_q2 * sin_q1) + sin_q3 * (cos_q1 * cos_q2 - sin_q1 * sin_q2)) + L1 * cos_q1;

Xm = Math. Round(Xm);

Ym = Math. Round(Ym);

Zm = Math. Round(Zm);

}

private void obratnaya_zadacha()

{

// -Обратная_задача- //

double xx = Math. Sqrt (X * X + Y * Y);

double x1 = xx - L3;

double z1 = Z - L0;

double B = Math. Sqrt (x1 * x1 + z1 * z1);

qq0 = Math. Asin (Y / xx);

qq1 = 1.570796327 - Math. Acos((L1 * L1 + B * B - L2 * L2) / (2 * B * L2)) - Math. Asin (z1 / B);

qq2 = 1.570796327 - Math. Acos((L1 * L1 + L2 * L2 - B * B) / (2 * L1 * L2));

qq0 = Math. Round (qq0 * k);

qq1 = Math. Round (qq1 * k);

qq2 = Math. Round (qq2 * k);

}

// обработчики нажатия кнопок управления

private void Q0U_MouseDown (object sender, MouseEventArgs e)

{

f0U = true;

}

private void Q0U_MouseUp (object sender, MouseEventArgs e)

{

f0U = false;

}

private void Q0D_MouseDown (object sender, MouseEventArgs e)

{

f0D = true;

}

private void Q0D_MouseUp (object sender, MouseEventArgs e)

{

f0D = false;

}



private void Q1U_MouseDown (object sender, MouseEventArgs e)

{

f1U = true;

}

private void Q1U_MouseUp (object sender, MouseEventArgs e)

{

f1U = false;

}

private void Q1D_MouseDown (object sender, MouseEventArgs e)

{

f1D = true;

}

private void Q1D_MouseUp (object sender, MouseEventArgs e)

{

f1D = false;

}

private void Q2U_MouseDown (object sender, MouseEventArgs e)

{

f2U = true;

}

private void Q2U_MouseUp (object sender, MouseEventArgs e)

{

f2U = false;

}



private void Q2D_MouseDown (object sender, MouseEventArgs e)

{

f2D = true;

}

private void Q2D_MouseUp (object sender, MouseEventArgs e)

{

f2D = false;

}

private void Q3U_MouseDown (object sender, MouseEventArgs e)

{

f3U = true;

}

private void Q3U_MouseUp (object sender, MouseEventArgs e)

{

f3U = false;

}

private void Q3D_MouseDown (object sender, MouseEventArgs e)

{

f3D = true;

}

private void Q3D_MouseUp (object sender, MouseEventArgs e)

{

f3D = false;

}



private void trackBar1_Scroll (object sender, EventArgs e)

{

// регулировка задержки таймера1

// отвечает за скорость скорость перемещения робота

int period1 = trackBar1. Value;

period1 = (101 - period1);

timer1. Interval = period1; // уровень задержки таймера

labelTimer. Text = (101 - period1).ToString()+ "%»;

}

private void trackBar2_Scroll (object sender, EventArgs e)

{

// регулировка задержки таймера2

// отвечает за частоту отправки команд управления

int period2 = trackBar2. Value;

double period21 = 1000 / period2;

period21 = Math. Round(period21);

timer2. Interval = (int) period21; // уровень задержки таймера

label8. Text = period2. ToString() + «команд/сек»;

}

private void comboBox1_SelectedIndexChanged (object sender, EventArgs e)

{ // выбирает доступный COM порт, если он есть

if (comboBox1. SelectedItem. ToString()!= «») Port1 = new SerialPort (comboBox1. SelectedItem. ToString(), 9600);

}

private void Poisk_Click (object sender, EventArgs e)

{

// поиск доступных COM портов

string[] myPort; // создаем массив строк

myPort = System.IO. Ports. SerialPort. GetPortNames(); // в массив помещаем доступные порты

comboBox1. Items. Clear(); // очистка comboBox

comboBox1. Items. AddRange(myPort); // теперь этот массив заносим в список(comboBox)

if (comboBox1. Items. Count!= 0) comboBox1. SelectedIndex = 0;

}

private void timer2_Tick (object sender, EventArgs e)

{

// -Отправка_команды_на_Arduino- //

// преобразование углов для сервоприводов

double q0s = Math. Round (94 - q0);

double q1s = Math. Round (102 - q1);

double q2s = Math. Round (132 + q2);

string komanda = «»; // очистка строки

if (System.IO. Ports. SerialPort. GetPortNames().Length == 0) comboBox1. Items. Clear(); // если нет портов очистка comboBox

if (double. IsNaN(q0) || double. IsNaN(q1) || double. IsNaN(q2) || double. IsNaN(X) || double. IsNaN(Y) || double. IsNaN(Z))

{

label9. ForeColor = Color. Red;

label9. Text = «Выход из рабочей зоны!»;

}

else

{

komanda = q0s. ToString() + «» + q1s. ToString() +» «+ q2s. ToString() +» «+ q3. ToString() +»!»; // создание строки значений углов поворотов звеньев

textBox1. Text = komanda;

if (komanda!= stroka)

{

if (comboBox1. Items. Count!= 0) // если есть доступный COM порт

{

label9. ForeColor = Color. LimeGreen;

if (flag) label9. Text = «COM порт подключен!»;

try

{

Port1 = new SerialPort (comboBox1. SelectedItem. ToString(), 9600);

Port1. Open(); // открываем порт

Port1. Write(komanda); // отправка строки значений углов

Port1. Close(); // закрываем порт

}

catch (Exception)

{

label9. ForeColor = Color. Red;

if (flag) label9. Text = «Ошибка подключения по COM порту!»;

}

}

else

{

label9. ForeColor = Color. Red;

if (flag) label9. Text = «COM порт не подключен!»;

}

}

}

stroka = komanda;

// -Конец_отправки- //

}

private void button1_Click_1 (object sender, EventArgs e)

{

string position = «P» + X + «» + Y +» «+ Z +» " + q3;

// добавление в список позицию робота

if ((listBox1. Items. Count == 0) || (listBox1. SelectedIndex == (listBox1. Items. Count - 1)))

{

listBox1. Items. Add(position);

listBox1. SetSelected (listBox1. Items. Count - 1, true);

}

else

{

listBox1. Items. Insert (listBox1. SelectedIndex + 1, position);

}

}

private void button2_Click_1 (object sender, EventArgs e)

{

string delay = «D» + textBox2. Text;

// добавление в список позицию робота

if ((listBox1. Items. Count == 0) || (listBox1. SelectedIndex == (listBox1. Items. Count - 1)))

{

listBox1. Items. Add(delay);

listBox1. SetSelected (listBox1. Items. Count - 1, true);

}

else

{

listBox1. Items. Insert (listBox1. SelectedIndex + 1, delay);

}

}

private void button3_Click (object sender, EventArgs e)

{

// удаление выбранной строки

int p = listBox1. SelectedIndex;

if (p >= 0) listBox1. Items. RemoveAt(p);

if (p - 1 >= 0) listBox1. SetSelected (p - 1, true);

if ((p == 0) && (listBox1. Items. Count >= 1)) listBox1. SetSelected (p, true);

}

private void button4_Click (object sender, EventArgs e)

{

listBox1. Items. Clear(); // очистить поле

}

private void button6_Click (object sender, EventArgs e)

{

// сохранение текста программы двмжения робота

Stream myStream;

SaveFileDialog saveFileDialog1 = new SaveFileDialog();

saveFileDialog1. Filter = «txt files (*.txt)|*.txt»;

saveFileDialog1. FilterIndex = 2;

saveFileDialog1. RestoreDirectory = true;

if (saveFileDialog1. ShowDialog() == DialogResult.OK)

{

if ((myStream = saveFileDialog1. OpenFile())!= null)

{

StreamWriter sw = new StreamWriter(myStream);

for (int i = 0; i < listBox1. Items. Count; i++)

{

listBox1. SelectedIndex = i;

sw. WriteLine (listBox1. SelectedItem. ToString());

}

sw. Close();

myStream. Close();

}

}

}

private void button7_Click (object sender, EventArgs e)

{

// загрузка текста программы движения робота

OpenFileDialog openFileDial = new OpenFileDialog();

string line;

if (openFileDial. ShowDialog() == DialogResult.OK)

{

listBox1. Items. Clear(); // очистить поле

string fileName = openFileDial. FileName;

FileStream sss = File. Open (fileName, FileMode. Open, FileAccess. Read);

if (sss!= null)

{

StreamReader reader = new StreamReader(sss);

while ((line = reader. ReadLine())!= null)

listBox1. Items. Add(line);

sss. Close();

}

}

}

private void action()

{

// выполнение команды программы

if (String. Equals (str_pos, str_pos_1)==false) // если поступила новая команда

{

str_pos_1 = str_pos;

str1 = «»;

str_x = «»;

str_y = «»;

str_z = «»;

str_s = «»;

str_t = «»;

int i = 0;

// считывание команды

if (str_pos. Length > 0)

{

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str1 = str1 + str_pos[i];

i = i + 1;

}

if (String. Equals (str1, «P») == true) // если это команда позиции

{

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_x = str_x + str_pos[i];

i = i + 1;

}

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_y = str_y + str_pos[i];

i = i + 1;

}

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_z = str_z + str_pos[i];

i = i + 1;

}

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_s = str_s + str_pos[i];

i = i + 1;

}

xk = int. Parse (str_x);

yk = int. Parse (str_y);

zk = int. Parse (str_z);

sk = int. Parse (str_s);

///////////////////////

// -Обратная_задача- //

double xx = Math. Sqrt (xk * xk + yk * yk);

double x1 = xx - L3;

double z1 = zk - L0;

double B = Math. Sqrt (x1 * x1 + z1 * z1);

qq0 = Math. Asin (yk / xx);

qq1 = 1.570796327 - Math. Acos((L1 * L1 + B * B - L2 * L2) / (2 * B * L2)) - Math. Asin (z1 / B);

qq2 = 1.570796327 - Math. Acos((L1 * L1 + L2 * L2 - B * B) / (2 * L1 * L2));

qq0 = Math. Round (qq0 * k);

qq1 = Math. Round (qq1 * k);

qq2 = Math. Round (qq2 * k);

///////////////////////

}

if (String. Equals (str1, «D»)) // если это команда задержки

{

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_t = str_t + str_pos[i];

i = i + 1;

}

tk = int. Parse (str_t);

}

}

}

// выполнение команды

if (String. Equals (str1, «P»)) // если это команда позиции

{

if (q0 < qq0)

{

q0 = q0 + 1;

if (q0 > 85) q0 = 85;

}

if (q0 > qq0)

{

q0 = q0 - 1;

if (q0 < -85) q0 = -85;

}

//Q0T. Text = q0. ToString(); // вывод значения угла q0

if (q1 < qq1)

{

q1 = q1 + 1;

if (q1 > 102) q1 = 102;

}

if ((q1 > qq1) && ((q3r * k) < 0))

{

q1 = q1 - 1;

if (q1 < -42) q1 = -42;

}

//Q1T. Text = q1. ToString(); // вывод значения угла q1

if (q2 < qq2)

{

q2 = q2 + 1;

if (q2 > 48) q2 = 48;

}

if ((q2 > qq2) && ((q3r * k) < 0))

{

q2 = q2 - 1;

if (q2 < -90) q2 = -90;

}

//Q2T. Text = q2. ToString(); // вывод значения угла q2

if (q3 < sk)

{

q3 = q3 + 1;

if (q3 > 90) q3 = 90;

}

if (q3 > sk)

{

q3 = q3 - 1;

if (q3 < 0) q3 = 0;

}

//Q3T. Text = q3. ToString(); // вывод значения угла q3

if ((q3r * k) > 0)

{

if (q1 < 0) q1 = q1 + 1;

if (q2 < 0) q2 = q2 + 1;

}

pryamaya_zadacha();

X = Xm;

Y = Ym;

Z = Zm;

// если все углы достигнуты, команда выполнена

if ((q0 == qq0) && (q1 == qq1) && (q2 == qq2) && (q3 == sk)) gotovo = true;

}

if (String. Equals (str1, «D»)) // если это команда задержки

{

if (time1 == 0) time1 = tk*10;

f_time = true;

}

}

private void Start_Click (object sender, EventArgs e)

{

if (start_flag == false) start_flag = true;

else start_flag = false;

}

private void timer3_Tick (object sender, EventArgs e)

{

if (f_time && start_flag)

{

if (time1 == 1)

{

f_time = false;

gotovo = true;

}

if (time1 > 0) time1 = time1 - 1;

}

}

private void textBox2_KeyPress (object sender, KeyPressEventArgs e)

{

// ограничение наввод только цифр

if (Char. IsDigit (e. KeyChar) || (e. KeyChar == 8))

{

}

else

{

e. Handled = true;

}

}

}

}

Размещено на Allbest.ru