2.3 Вычисление углов для сервомоторов
Одной из проблем в процессе разработки кинематики робота, является вычисление углов для сервомоторов. Особенность сервомоторов такова, что формирование угла поворота происходит не передачей значения угла в градусах, а занесением в EEPROM память микроконтроллера сервомотора значений соответствия углов. То есть для установки угла поворота следует сформировать пакет инструкцию, которая запишет в адрес 0x1Е (Goal Position), угол. Кроме того, угол состоит из двух байт, старшего и младшего, которые изменятся в пределах от 0x00 до 0x03FF. Данные вычислинея обеспечат плавное движение робота. Расположение углов представлено на Рисунке 9.
Рисунок 9. Расположение углов сервомоторов
Для решения данной проблемы была написана простая программа на языке С++, которая вычисляет значения углов. Один градус приравнивается к величине 0х01. Значения полученные в процессе вычисления делятся на два массива.
Код программы:
#include "stdafx.h"
#include "iostream"
#include "math.h"
using namespace std;
int main()
{
int sotni=0;
double desatki=0;
int angle3[1025];// последее число
int mas_desatki[1025];
int mas_sotni[1025];
int k=0;
int zz=0;
double c = 0x3ff/0x3ff;
for(int i=0;i<=0x3ff;i+=c)
{
k=k+1;
j=j+1;
angle3[j]=i;
double ishodnoe=i; // допустим 256
sotni=ishodnoe/100; // тут 256/100=2 (целая часть)
desatki=ishodnoe-sotni*100; // тут 256%100=56 (остаток)
mas_desatki[k]=desatki;
mas_sotni[k]=sotni;
cout << " i:"<< " " << hex << i;
}
for (int i = 1; i < 1025; i++)
{
for(int j=i+1;j<1025;j++)
{
if (angle3[i]>angle3[j])
{
int temp = angle3[i];
angle3[i]=angle3[j];
angle3[j]=temp;
}
}
cout << angle3[i]<<endl;
}
system("pause");
}
3 Реализация алгоритмов управления роботом
Управление роботом реализовано через кабель, подключенный по USB. Приложение на компьютере написано на языке C#. После нажатия кнопки в форме, происходит связь с микроконтроллером платы Arduino.
Serial.write
CDS5516
Atmega2560
Atmega8U2
USB
SERIAL.SYS
Visual
Studio SerialPort
Serial.write
Рисунок 10. Коммуникация приложения и сервомоторов
Код файла blink.ino
//файл прошивки микроконтроллера Arduino 2560
int angle11; // переменная для старшего байта угла альфа
int angle21;//переменная для младшего байта угла альфа
int angle12;//пременная для старшего байта угла бета
int angle22;//пременная для младшего байта угла бета
int angle13; //пременная для старшего байта угла гамма
int angle23; //пременная для младшего байта угла гамма
/*функция для Serial1. Формирует инструкцию для записи в микроконтроллер сервомотора угла поворота. Параметрами функции является идентификатор (id), сташий и младший байт угла*/
void write_pos1(byte id, byte angle1, byte angle2)
{
byte f = 0xff; //начало
byte leng = 0x07;
byte wr = 0x03;
byte gp = 0x1e;
byte p3 = 0x00;
byte p4 = 0x02;
byte cs = (~(id+leng+wr+gp+angle2+angle1+p3+p4)&0xff);
byte z[11]={ f,f,id,leng,wr,gp,angle2,angle1,p3,p4,cs};
Serial1.write(z,11);
delay(10);
}
/*функция для Serial2*/
void write_pos2(byte id, byte angle1, byte angle2)
{
byte f = 0xff;
byte leng = 0x07;
byte wr = 0x03;
byte gp = 0x1e;
byte p3 = 0x00;
byte p4 = 0x02;
byte cs = (~(id+leng+wr+gp+angle2+angle1+p3+p4)&0xff);
byte z[11]={ f,f,id,leng,wr,gp,angle2,angle1,p3,p4,cs};
Serial2.write(z,11);
delay(10);
}
void up() //функция для подъема робота
{
write_pos1(0x01,0x00,0xaf);
write_pos2(0x01,0x01,0x00);
write_pos1(0x04,0x02,0x10);
write_pos2(0x04,0x00,0x20);
write_pos1(0x07,0x02,0x00);
write_pos2(0x07,0x01,0x00);
write_pos1(0x02,0x02,0x00);
write_pos2(0x02,0x02,0x00);
write_pos1(0x03,0x01,0x00);
write_pos2(0x03,0x01,0x00);
write_pos1(0x05,0x02,0x00);
write_pos2(0x05,0x02,0x00);
write_pos1(0x06,0x01,0x00);
write_pos2(0x06,0x01,0x00);
write_pos1(0x08,0x02,0x00);
write_pos2(0x08,0x02,0x00);
write_pos1(0x09,0x01,0x00);
write_pos2(0x09,0x01,0x00);
delay(1000);
}
/*функции «reg_write_serial1», «void reg_write_serial2» для управления сервомоторами. Формирует параветры, для последующей записи через широковещательный адрес. Параметрами функции является идентификатор, сташрий и младший байт угла поворота. */
/*функция для Serial1*/
void reg_write_serial1(byte id_r,byte angle_r1,byte angle_r2)
{
byte f = 0xff;
byte leng = 0x05;
byte wr = 0x04;
byte gp = 0x1e;
byte cs = (~(id_r+leng+wr+gp+angle_r1+angle_r2)&0xff);
byte z_r[9]={ f,f,id_r,leng,wr,gp,angle_r2,angle_r1,cs};
Serial1.write(z_r,9);
delay(10);
}
/*функция для Serial1*/
void reg_write_serial2(byte id_r,byte angle_r1,byte angle_r2)
{
byte f = 0xff;
byte leng = 0x05;
byte wr = 0x04;
byte gp = 0x1e;
byte cs = (~(id_r+leng+wr+gp+angle_r1+angle_r2)&0xff);
byte z_r[9]={ f,f,id_r,leng,wr,gp,angle_r2,angle_r1,cs};
Serial2.write(z_r,9);
delay(10);
}
/*функция передает данные в микроконтроллер сервомора по широковещательному адресу. Одновременно для Serial1и Serial2*/
void action()
{
byte x[6]={0xff,0xff,0xfe,0x02,0x05,0xfa};
Serial1.write(x,6);
Serial2.write(x,6);
}
void setup() //установка начальных параметров
{
Serial.begin(9600); //скорость общения с платой Arduino
Serial1.begin(1000000); //скорость общения с сервомоторами
Serial2.begin(1000000);
}
void loop()
{
if(Serial.available()) //разрешение работы с последовательным портом
{
int c = Serial.read(); //чтение данных из последовательного порта и занемсение их в переменную
switch(c) //исходя из значения переменной происходит выбор инструкции
{
case 'f': //поворот влево
{
for(int i=0;i<2;i++)
{
up();
reg_write_serial1(0x02,0x00,0x6f);
action();
delay(10);
reg_write_serial2(0x05,0x00,0x6f);
action();
delay(10);
reg_write_serial1(0x08,0x00,0x6f);
action();
delay(1500);
up();
reg_write_serial2(0x02,0x00,0x6f);
action();
delay(10);
reg_write_serial1(0x05,0x00,0x6f);
action();
delay(10);
reg_write_serial2(0x08,0x00,0x6f);
action();
delay(1500);
up();
}
}break;
case '1': //активация робота
{
for(int i=0;i<10;i++)
{
reg_write_serial1(0x01,0x00,0xaf);
reg_write_serial2(0x01,0x01,0x00);
action();
reg_write_serial2(0x04,0x00,0x20);
reg_write_serial1(0x04,0x02,0x10);
action();
reg_write_serial1(0x07,0x02,0x00);
reg_write_serial2(0x07,0x01,0x00);
action();
reg_write_serial1(0x02,0x00,0xaf);
reg_write_serial2(0x02,0x00,0xaf);
action();
reg_write_serial1(0x03,0x00,0x00);
reg_write_serial2(0x03,0x00,0x00);
action();
reg_write_serial1(0x05,0x00,0xaf);
reg_write_serial2(0x05,0x00,0xaf);
action();
reg_write_serial1(0x06,0x00,0x00);
reg_write_serial2(0x06,0x00,0x00);
action();
reg_write_serial1(0x08,0x00,0xaf);
reg_write_serial2(0x08,0x00,0xaf);
action();
reg_write_serial1(0x09,0x00,0x00);
reg_write_serial2(0x09,0x00,0x00);
action();
}
}break;
Файл Form.cs содержит код управления роботом, написанный на языке С#
Код FORM1.cs
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; //работа с последовательным портом
namespace Robot
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
private void button1_Click(object sender, EventArgs e)
{
serialPort1.PortName=((string)comboBox1.SelectedItem); //получение имени последовательного порта
try
{
serialPort1.Open(); //открытие порта
}
catch (InvalidOperationException)
{ }
}
public string SetPortName(string defaultPortName)
{
string portName = ""; //строка для хранения имени порта
foreach (string portname in SerialPort.GetPortNames())
{
comboBox1.Items.Add(portname); //занесение имени порта в comboCox1
}
comboBox1.SelectedIndex = 0; //проверка
if (portName == "")
{
portName = defaultPortName;
}
return portName;
}
private void button2_Click(object sender, EventArgs e)
{
serialPort1.Write("1");
textBox1.Text = "activate";
}
private void button3_Click(object sender, EventArgs e)
{
serialPort1.Write("0");
textBox1.Text = "deactivate";
}
private void Form1_Load(object sender, EventArgs e)
{
serialPort1.PortName = SetPortName(serialPort1.PortName);
}
private void button4_Click(object sender, EventArgs e)
{
serialPort1.Write("e");
textBox1.Text = "UP";
}
private void button11_Click(object sender, EventArgs e)
{
//алгоритм инверсной кинематики
const int x=12; //координата х позиции для движения
const int y=10; //координата y позиции для движения
const double z_offset = 4; //высота подъема
const double cox = 1; // длина Coxa
const double tibia = 13; //длина Tibia
const double femur = 6; //длина Femur
/* angle1 и angle2 массивы для хранения старшего и млашего байтов углов. угол взят с диапазоном 10 градусов*/
int[] angle1 = {0x00,0x00,0x00,0x00,0x01,0x01,0x02,0x02,0x03,0x03,0x03};
int[] angle2 = {0x00,0x66,0xcc,0x32,0x98,0xfe,0x64,0xca,0x30,0x96,0xfc};
double L1,alfa,gamma,betta;
L1 = Math.Sqrt(Math.Pow(z_offset,2)+Math.Pow((y-cox),2));
//вычисление альфа, гамма и бетта углов инверсной кинематики
alfa = (Math.Acos(z_offset/L1)+Math.Acos((Math.Pow(tibia,2)-Math.Pow(femur,2)-Math.Pow(L1,2))/((-2)*femur*L1)))*(180/3.14);
gamma = (Math.Atan2(x,y))*(180/3.14);
betta = (Math.Acos((Math.Pow(L1,2)-Math.Pow(tibia,2)-Math.Pow(femur,2))/((-2)*tibia*femur)))*(180/3.14);
//поиск углов в массивах
int alf = Convert.ToInt32(alfa/10.0); //вычисление индекса массива
int a1 = angle1[alf];//старший байт угла альфа
int a2 = angle2[alf];//младший байт угла альфа
int bet = Convert.ToInt32(betta/10.0); //вычисление индекса массива
int b1 = angle1[bet]; //старший байт угла бетта
int b2 = angle2[bet]; //младший байт угла бетта
int gam = Convert.ToInt32(gamma/10.0); //вычисление индекса массива
int c1 = angle1[gam];//старший байт угла гамма
int c2 = angle2[gam];//младший байт угла гамма
//запись старших и младших байтов углов в последовательный порт
serialPort1.Write("h"); //запись инструкции
serialPort1.Write(a1.ToString());
serialPort1.Write(a2.ToString());
serialPort1.Write(b1.ToString());
serialPort1.Write(b2.ToString());
serialPort1.Write(c1.ToString());
serialPort1.Write(c2.ToString());
textBox1.Text = "Move"; //вывод операции
}
/*считывание данных из последовательного порта и запись значений в textBox*/
private void button12_Click(object sender, EventArgs e)
{
serialPort1.Write("s"); //активация инструкции
string readstring; //строка для считывания
readstring = serialPort1.ReadLine();//считывание данных
textBox2.AppendText(readstring + "\r\n");
}
private void button13_Click(object sender, EventArgs e)
{
serialPort1.Close();
button1.Enabled = true;
button2.Enabled = false;
button3.Enabled = false;
}
private void button9_Click(object sender, EventArgs e)
{
serialPort1.Write("g");
textBox1.Text = "Right";
}
private void button5_Click(object sender, EventArgs e)
{
serialPort1.Write("f");
textBox1.Text = "Left";
}
private void button7_Click(object sender, EventArgs e)
{
serialPort1.Write("a");
textBox1.Text = "DOWN";
}
private void button6_Click(object sender, EventArgs e)
{
serialPort1.Write("j");
textBox1.Text = "Right";
}
/*кнопка, осуществляющая передачу параметров из NumericUpDown в последовательный порт. Реализует калибровку сервомоторов, а так же позволяет управлять каждым сервомоторов по отдельности*/
private void button14_Click(object sender, EventArgs e)
{
serialPort1.Write("z");
char a = (char)Convert.ToByte(numericUpDown1.Value); //выбор старшего байта угла в пределах от 0 до 3
char d = (char)Convert.ToByte(numericUpDown2.Value); //выбор младшего байта угла в пределах от 0 до 255 (ff вшестнадцатиричной системе счисления)
char f = (char)Convert.ToByte(numericUpDown3.Value);
/*запись данных в последовательный порт*/
serialPort1.Write(a.ToString());
serialPort1.Write(d.ToString());
serialPort1.Write(f.ToString());
textBox1.Text = "collibrate";//вывод имени инструкции
}
}
}
Рисунок 11. Скриншот приложения управления роботом
Connect – соединение с платой (SerialPort.Open)
Disconnect – закрыть соединение (SerialPort.Closed)
ON – активация робота
OFF – переход робота в «свернутое» состояние
UP – подъем робота
DOWN – опустить робота
Left – поворот влево
Right – поворот вправо
Forward – движение вперед
Back – движение назад
Left1 – движение влево
Right1 – движение вправо
Collibrate – калибровка сервомоторов
Read – считывание параметров сервомоторов и вывод в textbox