3 Вычислительный эксперимент
В данном разделе представлены результаты работы программы. Для выполнения вычислительного эксперимента необходимо запустить напрямую скомпилированный EXE файл программы, либо запустить через среду разработки MS Visual Studio 2010.
Результаты вычислительных экспериментов представлены на рисунках 8-10.
На рисунке 8 представлена настроенная программа с заданными углами звеньев робота-манипулятора.
Рисунок 8 – Настроенная программа
На рисунке 9 представлен ответ программы на нажатие кнопки “Refresh”.
Рисунок 9 – Работа программы
На рисунке 10 представлено решенение обратной задачи кинематики для конкретной точки в пространстве.
Рисунок 10 – Решение обратной задачи кинематики
Заключение
В ходе данной курсовой работы была разработана математическая модель решения обратной задачи кинематики, алгоритм и программа управления роботом-манипулятором.
Программа управления трехзвенным роботом-манипулятором реализуется на универсальной ЭВМ. Для корректной работы программы необходимо иметь операционную систему Windows XP (SP 2-3)/Vista (SP 1)/ Windows 7. Программа разработана в среде разработки программного обеспечения Microsoft Visual Studio 2010.
Разработанная программа полностью соответствует техническому заданию:
– осуществлено управление трехзвенным роботом-манипулятором путем задания конечной точки расположения рабочего органа робота;
– осуществлено графическое отображение положения робота-мани-пулятора;
– предусмотрено ручное управление отдельными звеньями робота-манипулятора
– предусмотрено динамическое отслеживание текущего положения каждого звена робота-манипулятора.
– передача данных осуществляется по стандарту “RS232”;
– программа написана на языке программирования “C#”;
Объем программы: 63кб
Список литературы
1. Герберт Шилдт. – C# 4.0: полное руководство. – М.: «Вильямс», 2010. – С. 1056.
2. Троелсен Э. – Язык программирования C# 2010 и платформа .NET 4.0. 5-е изд. – М.: Вильямс, 2010. – С. 1392.
3. Тягунов О. А. - Математические модели и алгоритмы управления промышленных транспортных роботов. Информационно-измерительные и управляющие системы. – 2007. – Т. 5. – № 5. – С. 63–69.
Приложение а (обязательное) Текст программы
В данном приложении представлена программа управления роботом-манипулятором с персонального компьютера.
На рисунке ПА.1 представлен текст программы управления роботом-манипулятором с персонального компьютера.
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.Ports;
namespace Manip_Control
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
this.KeyPreview = true;
this.KeyPress += new KeyPressEventHandler(Form1_KeyPress);
}
Bitmap bmp = new Bitmap(200, 200);
SerialPort serialPort = new SerialPort();
Point Point_0 = new Point(70, 150),
Point_1,
Point_2,
Point_3;
byte[] Package = new byte[7];
byte[] Receive = new byte[6];
int[] Servo_position = new int[6];
int x, y, z;
int unit_1 = 30,
unit_2 = 20,
unit_3 = 15;
int angle_0 = 0,
angle_1 = 0,
angle_2 = 0,
angle_3 = 0;
Рисунок ПА.1 – Текст программы управления
роботом-манипулятором
double Scal_2 = 0,
Scal_3 = 0;
private void Connect_Click(object sender, EventArgs e)
{
serialPort.PortName = COMPort.Text;
serialPort.BaudRate = Convert.ToInt32(BAUDRate.Text);
serialPort.ReadTimeout = 2000;
try
{
serialPort.WriteBufferSize = 10;
serialPort.Open();
}
catch (Exception ex)
{
MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
private void Disconnect_Click(object sender, EventArgs e)
{
serialPort.Close();
}
private void Set_Click(object sender, EventArgs e)
{
try
{
Set_Packaging(Package);
serialPort.Write(Package, 0, 7);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
private void Renew_Click(object sender, EventArgs e)
{
Package[0] = 7;
Package[1] = 0;
serialPort.Write(Package, 0, 2);
for (int i = 0; i < 6; i++)
Receive[i] = (byte) serialPort.ReadByte();
textBox7.Text = Convert.ToString(Receive[0]);
textBox8.Text = Convert.ToString(Receive[1]);
textBox9.Text = Convert.ToString(Receive[2]);
textBox10.Text = Convert.ToString(Receive[3]);
textBox11.Text = Convert.ToString(Receive[4]);
textBox12.Text = Convert.ToString(Receive[5]);
}
private void Calculate_Click(object sender, EventArgs e)
{
x = Convert.ToInt16(textBox13.Text);
y = Convert.ToInt16(textBox14.Text);
z = Convert.ToInt16(textBox15.Text);
Direct_points(x, y, z);
Drawning();
}
void Set_Packaging(byte[] pack)
{
pack[0] = 0;
pack[1] = Convert.ToByte(textBox1.Text);
pack[2] = Convert.ToByte(textBox2.Text);
pack[3] = Convert.ToByte(textBox3.Text);
Рисунок ПА.1 – Продолжение
pack[4] = Convert.ToByte(textBox4.Text);
pack[5] = Convert.ToByte(textBox5.Text);
pack[6] = Convert.ToByte(textBox6.Text);
}
void ServoStepper(int Servo_num, int Direct)
{
Package[0] = 7;
Package[1] = Convert.ToByte(Servo_num);
serialPort.Write(Package, 0, 2);
try
{
serialPort.Read(Receive,0,1);
}
catch(Exception ex)
{
MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
int Cur_Angle = Receive[0];
int Stepped = Convert.ToInt16(Cur_Angle + Math.Pow(-1, Direct));
if (Stepped > 180 || Stepped < 0)
{
Stepped = Cur_Angle;
MessageBox.Show("Сервопривод в крайнем положении: " + Cur_Angle + "ᵒ");
}
Package[0] = Convert.ToByte(Servo_num);
Package[1] = Convert.ToByte(Stepped);
serialPort.Write(Package, 0, 2);
switch (Servo_num)
{
case 1: textBox7.Text = Convert.ToString(Stepped); break;
case 2: textBox8.Text = Convert.ToString(Stepped); break;
case 3: textBox9.Text = Convert.ToString(Stepped); break;
case 4: textBox10.Text = Convert.ToString(Stepped); break;
case 5: textBox11.Text = Convert.ToString(Stepped); break;
case 6: textBox12.Text = Convert.ToString(Stepped); break;
default: break;
}
}
void Direct_points(int X, int Y, int Z)
{
double X_new = Scalar(X, Z);
angle_0 = (int)aCos(Z, X_new);
Scal_3 = Scalar(X_new, Y);
angle_1 = (int)aCos(X_new, Scal_3);
Point_3 = new Point(70 + Converting(Scal_3, Math.Cos(angle_1 * Math.PI / 180)), 150 - Converting(Scal_3, Math.Sin(angle_1 * Math.PI / 180)));
Scal_2 = Scalar(unit_3, Scal_3);
angle_1 += (int)aCos_Theorem(unit_3, Scal_2, Scal_3);
Point_2 = new Point(70 + Converting(Scal_2, Math.Cos(angle_1 * Math.PI / 180)), 150 - Converting(Scal_2, Math.Sin(angle_1 * Math.PI / 180)));
angle_1 += (int)aCos_Theorem(unit_2, unit_1, Scal_2);
Point_1 = new Point(70 + Converting(unit_1, Math.Cos(angle_1 * Math.PI / 180)), 150 - Converting(unit_1, Math.Sin(angle_1 * Math.PI / 180)));
angle_2 = (int)aCos_Theorem(Scal_2, unit_1, unit_2);
angle_3 = (int)(aCos_Theorem(unit_1, unit_2, Scal_2) + aCos_Theorem(Scal_3, unit_3, Scal_2));
}
int Converting(double Lenght, double Func)
{
return (int)(Lenght * Func * 150 / 65);
}
Рисунок ПА.1 – Продолжение
double Scalar(double x, double y)
{
return Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
}
double aCos(double A, double C)
{
return 180 * Math.Acos(A / C) / Math.PI;
}
double aCos_Theorem(double A, double B, double C)
{
return 180 * Math.Acos((Math.Pow(B, 2) + Math.Pow(C, 2) - Math.Pow(A, 2)) / (2 * B * C)) / Math.PI;
}
void Form1_KeyPress(object sender, KeyPressEventArgs e)
{
if (e.KeyChar >= 97 && e.KeyChar <= 122)
{
switch (e.KeyChar)
{
case (char)113: ServoStepper(1, 0); break;//q
case (char)119: ServoStepper(1, 1); break;//w
case (char)97: ServoStepper(2, 0); break;//a
case (char)115: ServoStepper(2, 1); break;//s
case (char)122: ServoStepper(3, 0); break;//z
case (char)120: ServoStepper(3, 1); break;//x
case (char)101: ServoStepper(4, 0); break;//e
case (char)114: ServoStepper(4, 1); break;//r
case (char)100: ServoStepper(5, 0); break;//d
case (char)102: ServoStepper(5, 1); break;//f
case (char)99: ServoStepper(6, 0); break;//c
case (char)118: ServoStepper(6, 1); break;//v
}
}
}
private void Drawning()
{
Bitmap bmp = new Bitmap(200, 200);
pictureBox.Refresh();
Graphics Draw = Graphics.FromImage(bmp);
Draw.DrawLine(new Pen(Color.Black), 0, 150, 250, 150);
Draw.DrawLine(new Pen(Color.Black), 70, 0, 70, 200);
Draw.DrawLine(new Pen(Color.Blue, 3), Point_0, Point_1);
Draw.DrawLine(new Pen(Color.Blue, 3), Point_1, Point_2);
Draw.DrawLine(new Pen(Color.Blue, 3), Point_2, Point_3);
Draw.FillEllipse(new SolidBrush(Color.Red), Point_0.X - 3, Point_0.Y - 3, 6, 6);
Draw.FillEllipse(new SolidBrush(Color.Red), Point_1.X - 3, Point_1.Y - 3, 6, 6);
Draw.FillEllipse(new SolidBrush(Color.Red), Point_2.X - 3, Point_2.Y - 3, 6, 6);
Draw.FillEllipse(new SolidBrush(Color.Red), Point_3.X - 3, Point_3.Y - 3, 6, 6);
pictureBox.Image = bmp;
}
}
}
Рисунок ПА.1 – Продолжение
Приложение Б