[40호]립모션을 이용한 로봇 팔 제어 시스템 구현
2016 ICT 융합 프로젝트 공모전 참가상
립모션을 이용한 로봇 팔 제어 시스템 구현
글 | 부경대학교 공영훈, 김지수, 홍다운, 황호연, 박동주, 황신실
심사평
뉴티씨 실제로 구현해보면, 매우 어려운 로봇팔에 도전하였으며, 또한 립모션을 카메라를 통해 인식하여 실제 로봇의 움직임에 적용하는 등 실험이 성공적으로 된 점 등이 인상적이다. 다만, 구현의 정도에 오차가 보이고 있어서, 그러한 부분에 대한 보강작업 등이 향후 필요할 것으로 보이며, 앞으로 좀 더 보완한 후에 다시 도전하면 더 좋은 점수를 받을 수 있을 것으로 보인다. 현재도 매우 높은 점수를 받았다.
칩센 수많은 로봇팔 회사들과 개인이 시도한 분야다. 완성도가 더 떨어지는 부분은 어쩔 수 없는 것 같다.
위드로봇 leap motion을 이용해 실제 로봇팔과 인터페이스하는 작품으로 단순한 연결에 그치지 않고, UVC 카메라를 이용해 원격지의 상황을 모니터링할 수 있는 환경을 구성한 점이 훌륭합니다. 실험 결과에 대한 정량적 분석이 좀 더 명확하면 더 좋은 보고서가 될 것 같습니다.
1. 작품 제목
립모션을 이용한 로봇 팔 제어 시스템 구현
2. 작품 개요
현대 기술이 발전하면서 전자 제품과 사용자가 상호 작용할 수 있는 다양한 기술이 연구되고 있다. 자연스러운 사용자 조작환경이라고 할 수 있는 NUI(Natural User Interface)는 마우스나 키보드와 같은 입력장치를 이용하지 않고 신체의 동작을 사용한다. 그 중 손 동작을 이용한 방법은 컴퓨터 제어, 문자 입력, 증강 현실 등 다양한 분야에서 연구 중이며, 손 동작을 인식해 로봇을 제어하는 응용 프로그램이 제안되었고, 실시간으로 손 동작을 인식하기 위한 장치들의 개발이 꾸준히 시도되고 있다.
손 동작을 인식하기 위한 여러 가지 장치 중 Leap Motion은 최근 몇 년간 여러 분야에서 다양하게 사용되고 있는 모션 인식에 큰 혁신을 가져왔다. 손 동작을 3D 형태로 획득할 수 있는 이 장치는 1/100mm 단위의 미세한 움직임까지 감지하며 150도 반경에서 손가락의 움직임을 290 fps(frame per second)로 감지해 사람의 손과 손가락의 섬세한 움직임을 포착할 수 있는 장치이다. 본 작품은 이러한 Leap Motion을 이용해 사람의 손 동작을 그대로 모방해 따라하는 로봇 팔을 원격 제어할 수 있는 시스템을 목표로 제작되었다.
3. 작품 설명
3.1. 주요 동작 및 특징
본 작품은 사람의 손동작을 모방해 그대로 따라하는 로봇 팔이다. 립 모션을 이용해 사람의 손 동작을 디지털 정보로 획득한다. 그 뒤 역기구학을 적용해 로봇의 End-Effector의 이동 경로를 손의 이동경로와 일치할 수 있게 제작하였다. 그리고 손목의 움직임을 표현하기 쉽게 로봇 팔의 관절을 조립하였다. 또한 유저 인터페이스를 이용하여 전체 시스템을 관리할 수 있다. 로봇 팔에 장착된 카메라에서 획득한 영상을 유저 인터페이스에 출력하여 사용자에게 로봇의 주변 환경에 관련된 정보를 제공할 수 있다.
3.2. 전체 시스템 구성
전체 시스템 구성은 그림 1과 같다. 유저 인터페이스와 로봇 팔로 구성되어 있고 전체 시스템은 무선 랜을 이용해서 원거리 통신을 구현한다. 유저 인터페이스는 닷넷을 이용해 제작하였고, 립 모션 센서가 획득한 손 동작과 관련된 디지털 정보를 출력한다. 우리는 해당 데이터에 역기구학을 적용하여 로봇 팔 End-Effector의 끝점을 계산해주었다. 그림 1의 MCU는 ATmel사의 ATmega128을 이용하며 로봇 팔에 장착되어 각종 모듈을 제어한다. 그림 1의 UVC Camera는 로봇 팔에 부착된 비전센서를 나타내고 영상을 획득할 수 있다. Raspberry Pi는 비전 센서를 이용해 획득한 영상을 무선 랜을 통해 유저 인터페이스로 전송하고 MCU 1과 통신한다.
3.2.1. Leap Motion
그림 2는 Leap Motion의 직각 좌표계를 나타내며, 거리 단위는 mm이고 각도 단위는 rad이다. 내장된 적외선 LED와 적외선 카메라를 이용해 열 손가락과 두 손의 움직임을 인식하여 사용자에게 제공한다. Leap Motion을 사용하기 위해선 PC와 연결하고, 제공되는 API를 이용해 제작한 유저 어플리케이션을 이용한다. 로봇 팔을 제어하기 위해서 손바닥 좌표, 손의 양쪽 끝의 길이, 손바닥의 vector를 이용하여 로봇 팔의 동작을 생성하였다.
3.2.2. 로봇 팔
그림 3은 사람의 손 동작을 그대로 따라 할 수 있게 구성된 5축 로봇 팔이다. Leap Motion이 획득한 손바닥의 좌표는 역기구학을 통해 관절의 각도로 변환되고 블루투스를 이용해 로봇 팔로 전달된다. 로봇 팔에 장착된 카메라는 로봇의 주변 환경을 획득할 수 있고, 사용자 어플리케이션을 통해 출력할 수 있다. 로봇 팔에 장착된 이동기구부는 차륜구동방식을 사용하고, 사용자 어플리케이션에서 주행을 제어할 수 있다. 영상정보와 주행 제어 정보를 주고받기 위해 TCP/IP 소켓 통신을 이용하였다.
로봇 팔에 적용된 역기구학은 그림 4와 같은 방법을 통해서 계산하였다. 2축 로봇 팔에 대한 계산을 통해 2축의 각도를 계산하고, 그 후 Z좌표를 적용하여 3차원 공간을 구성하였다. 역기구학을 계산하고나면 2가지 해가 존재한다. 그 중 우리는 더 큰 값이 나올 수 있도록 해를 정하였다.
3.2.3. Embedded Board
Embedded Board역할을 할 Raspberry Pi는 무선 랜에 연결하고 UVC 카메라에서 획득한 영상데이터를 윈도우 Application으로 전송하며 ATmega128의 모드 관리에 사용된다. ATmega128보다 편리하여 무선 랜을 이용하고 영상데이터를 획득하기 위해 라즈베리파이를 이용하였다. 카메라의 영상을 획득하기 위해 V4L2을 이용한다. V4L2는 영상을 획득하기 위한 Linux 드라이버로 획득한 영상을 RGB형태로 배열에 담을 수 있다. 우리는 영상 데이터의 전송에서 속도 대신 안정성을 선택하였고, TCP 소켓 통신을 이용하여 영상을 전송한다.
UDP 소켓 통신은 ATmega128의 모드 설정 시 사용한다. 카메라에서 획득한 영상이 TCP 소켓 통신을 이용하여 윈도우 Application으로 전송 되면 사용자는 윈도우 Application에서 이동형 로봇 팔의 현재 상태를 영상을 통해 확인할 수 있으며, 사용자가 현재 필요한 모드를 결정하고 모드 변경의 명령을 내리면 UDP 소켓 통신을 통해 Raspberry Pi로 전달되고 전달된 명령은 Raspberry Pi에서 ATmega128로 I2C를 통해서 전송된다. I2C는 Raspberry Pi를 Master로 설정하여 구동한다.
3.3. 실험 및 결과
그림 5는 실험 환경이다. 종이컵을 집어 옆 박스로 옮길 때 립 모션을 통해 획득한 손바닥의 이동경로와 로봇 팔 End Effector의 이동경로를 확인하였다.
그림 6은 주어진 실험 환경에서 손바닥의 이동경로와 로봇 팔 End Effector의 이동경로를 비교해 본 그래프이다. 립 모션 센서에 손바닥이 검출되지 않을 때 로봇 팔은 주어진 기본 자세를 취한다. 립 모션 센서가 손바닥을 검출하게 되면 로봇 팔은 사람 손바닥의 위치를 추적한다. 그래프의 시작과 끝에서 확인할 수 있는 오차는 로봇 팔이 기본자세에서 손바닥의 이동경로를 추적해가면서 생기는 오차이다.
그림 7은 이동로봇의 주행 실험 결과를 나타낸다. 기준속도는 8rad/s ~ 11rad/s이며 기준속도를 일정 시간 간격으로 증가시켰을 때 기준속도와 양쪽 모터 사이에 오차가 발생하지만, 양쪽 모터의 각속도가 증가하는 모습을 확인할 수 있었다.
3.4. 결론
본 작품에서는 립모션을 이용해 손동작을 획득하고 그 손 동작을 그대로 모방해 따라하는 로봇 팔의 제어 시스템을 구현하였다. 립 모션을 로봇 제어에 사용할 수 있었고, 윈도 애플리케이션을 통해 로봇을 이동시킬 수 있었다. 구현한 로봇 제어 시스템은 비전센서를 이용해 로봇의 주변 상태를 파악한다. 또한, 주행 실험 결과에서 기준 입력과 양쪽 모터의 각속도 사이에 오차가 발생하는 것을 확인하였다. 이러한 점을 보완하기 위해 높은 성능의 센서를 장착해 현재 상태를 파악하고 각속도 오차를 줄이기 위한 연구를 진행할 것이다.
3.5. 개발환경
1. visual studio 2015 community
- .net & C#, Leap Motion Sensor
2. IAR Workbench
- ATmega128
3. Raspbian
- Raspberry pi
4. 단계별 제작 과정
4.1. 유저 인터페이스 제작과정
4.1.1. 초기모델 1
4.1.2. 초기모델 2
4.1.3. 개선된 모델
5. 기타
5.1 소스코드
5.1.1. .net
namespace LeapSerialTest
{
public partial class FrameDataForm : Form, ILeapEventDelegate
{
int CamMaxBufSize = 230400;
byte t1_flag = 0, mode_flag = 1, IP_flag = 0, SockOpen_flag = 0;
string raspberry_ip =null, pc_ip=null, SerialRxData=null;
private Controller controller;
private LeapEventListener listener;
Bitmap bmp = new Bitmap(width: 320, height: 240, format:
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
SerialPort SP = new SerialPort();
private void FrameDataForm_Load(object sender, EventArgs e)
{
string[] PortNameArr = null;string PortName = null;
int PortIndex = -1;
PortNameArr = SerialPort.GetPortNames();
do
{
PortIndex += 1;
SerialPort_cbbox.Items.Add(PortNameArr[PortIndex]);
}
while (!((PortNameArr[PortIndex] == PortName)
|| (PortIndex == PortNameArr.GetUpperBound(0))));
if (PortIndex == PortNameArr.GetUpperBound(0))
{
PortName = PortNameArr[0];
}
SerialPort_cbbox.Text = PortNameArr[0];
SerialBaud_cbbox.Items.Add(9600);
//Set the read/write timeout
P.ReadTimeout = 500;
SP.WriteTimeout = 500;
}
void newFrameHandler(Frame frame)
{
Pointable pointable = frame.Pointables.Frontmost;
Pointable rightmostInFrame = frame.Pointables.Rightmost;
Pointable leftmostInFrame = frame.Pointables.Leftmost;
Vector position = pointable.TipPosition;
Vector direction = pointable.Direction;
int L1=204, L2 = 204;//, L2 = L1*2;
int[] DyHexAngle = new int[5];
double[] DynamixelTheta = new double[5];
double theta1 = 0, theta2 = 0, x_pos = 0, y_pos = 0, z_pos = 0;
double CosTh2 = 0, SinTh2 = 0, CosTh1 = 0, SinTh1 = 0
foreach (Hand hand in frame.Hands)
{
x_pos = hand.PalmPosition.x;
y_pos = hand.PalmPosition.y;
z_pos = -hand.PalmPosition.z + 250;
CosTh2 = (Math.Pow(z_pos, 2) + Math.Pow(y_pos, 2)
- Math.Pow(L1, 2) – Math.Pow(L2, 2)) / (2 * L1 * L2);
SinTh2 = -Math.Sqrt(1 – Math.Pow(CosTh2, 2));
theta2 = (Math.Atan2(SinTh2, CosTh2) * 180) / Math.PI;
CosTh1 = (((L1 + L2 * CosTh2) * z_pos + L2 * SinTh2 * y_pos)
(Math.Pow((L1 + L2 * CosTh2), 2) + Math.Pow(L2 * SinTh2, 2)));
inTh1 = (-L2 * SinTh2 * z_pos + (L1 + L2 * CosTh2) * y_pos)
1/ (Math.Pow(L1 + L2 * CosTh2, 2) + Math.Pow(L2 * SinTh2, 2));
theta1 = (Math.Atan2(SinTh1, CosTh1) * 180) / Math.PI;
DynamixelTheta[0] = (Math.Atan2(z_pos, x_pos) * 180) / Math.PI + 60;
DynamixelTheta[1] = 240 – theta1;
DynamixelTheta[2] = 150 + (-theta2);
int extendedFingers = 0;
for (int f = 0; f < hand.Fingers.Count; f++)
{
Finger digit = hand.Fingers[f];
if (digit.IsExtended)
extendedFingers++;
}
this.displayFigerCount_text.Text = extendedFingers.ToString();
}//foreach end
DynamixelTheta[4] = Math.Sqrt(
Math.Pow((leftmostInFrame.TipPosition.x -rightmostInFrame.TipPosition.x), 2) +
Math.Pow((leftmostInFrame.TipPosition.y – rightmostInFrame.TipPosition.y), 2) +
Math.Pow((leftmostInFrame.TipPosition.z – rightmostInFrame.TipPosition.z), 2)
);
DyHexAngle[4] = (int) (DynamixelTheta[4] * 511) / 155;
DynamixelTheta[3] = rightmostInFrame.TipPosition.y
- leftmostInFrame.TipPosition.y;
DyHexAngle[3] = 511 + (int)(DynamixelTheta[3] * 511) / 300;
if(DyHexAngle[0] < 300 || DyHexAngle[0] > 850)
DyHexAngle[0] = 511;
if(DyHexAngle[1] < 500)
yHexAngle[1] = 511;
if(DyHexAngle[2] < 500)
DyHexAngle[2] = 511;
if (DyHexAngle[4] > 600 || DyHexAngle[4] < 50)
DyHexAngle[4] = 511;
PosBuf[0] = 0xff;//start bit
PosBuf[1] = 0xff;
PosBuf[2] = (byte)(DyHexAngle[0] % 255);//1st motor
PosBuf[3] = (byte)(DyHexAngle[0] / 255);
PosBuf[4] = (byte)(DyHexAngle[1] % 255);//2st motor
PosBuf[5] = (byte)(DyHexAngle[1] / 255);
PosBuf[6] = (byte)(DyHexAngle[2] % 255);//3st motor
PosBuf[7] = (byte)(DyHexAngle[2] / 255);
PosBuf[8] = (byte)(DyHexAngle[3] % 255);//4st motor
PosBuf[9] = (byte)(DyHexAngle[3] / 255);
PosBuf[10] = (byte)(DyHexAngle[4] % 255);
PosBuf[11] = (byte)(DyHexAngle[4] / 255);
PosBuf[12] = (byte)(z_pos % 255);//x,
PosBuf[13] = (byte)(z_pos / 255);
(z_pos < 0)
PosBuf[14] = (byte)0×01;
else
PosBuf[14] = 0×00;
PosBuf[15] = (byte)(y_pos % 255);//y,
PosBuf[16] = (byte)(y_pos / 255);
PosBuf[17] = 0;
PosBuf[18] = (byte)(x_pos % 255);//z,
PosBuf[19] = (byte)(x_pos / 255);
if (x_pos < 0)
PosBuf[20] = 0×01;
else
PosBuf[20] = 0×00;
PosBuf[21] = 0xff;
//serail transmit
if (SP.IsOpen && SerialRxData == “S”)
{
SP.Write(buffer: PosBuf, offset: 0, count: 22);
SerialRxData = null;
}
}
private void boundary_setting(ref double DTheta, ref int DAngle)
{
DAngle = (int)(DTheta * 1023) / 300;
if (DAngle > 1023)
{
DAngle = 1023;
}
else if (DAngle < 0)
{
DAngle = 0;
}
}
private void RxDataClear_bt_Click(object sender, EventArgs e)
{
string RxDataClear = null;
ChangeRxTextBoxClear(RxDataClear);
}
private void ImgStart_bt_Click(object sender, EventArgs e)
{
if (IP_flag == 1 && SockOpen_flag == 1)
{
t1 = new Thread(new ThreadStart(TcpCamThread));
t1.Start();
t1_flag = 1;
}
else if (IP_flag != 1)
{
MessageBox.Show(“IP를 입력하세요”, “IP Error”);
}
else if (SockOpen_flag != 1)
{
MessageBox.Show(“Sock Open 버튼을 클릭하세요”, “Scoket Error”);
}
}
private void TcpCamThread()
{
int length, byteLength = 0, CompareFlag = 0;
while (true)
{
while ((length = stream.Read(buffer: BmpBuffer, offset: byteLength,
size: CamMaxBufSize – byteLength)) != 0)
{
byteLength += length;
}
if (byteLength == CamMaxBufSize)
{
CompareFlag++;
BmpInit(BmpBuffer);
ChangePictureBox(bmp);
byteLength = 0;
}
}
}
elegate void ChangePictureBoxCallback(Bitmap bmp);
public void ChangePictureBox(Bitmap bmp)
{
if (RobotStateVideo_pbox.InvokeRequired)
RobotStateVideo_pbox.Invoke(new ChangePictureBoxCallback(ChangePictureBox),
bmp);
else
RobotStateVideo_pbox.Image = bmp;
}
}
private void BmpInit(byte[] BmpBuffer)
{
System.Drawing.Imaging.BitmapData bmpData =
bmp.LockBits(new Rectangle(0, 0, bmp.Width, bmp.Height),
System.Drawing.Imaging.ImageLockMode.ReadOnly, bmp.PixelFormat);
IntPtr ptr = bmpData.Scan0;
System.Runtime.InteropServices.Marshal.Copy(source: BmpBuffer,
startIndex: 0, destination: ptr, length: 320 * 240 * 3);
bmp.UnlockBits(bmpData);
}
private byte uart_check_sum(byte[] check_arr, int arr_length)
{
int i = 0;
byte data = 0, check = 0;
for (i = 2; i < (arr_length – 1); i++)//check sum을 구하기 위한 for문
data += check_arr[i];
check = (byte) data;
return check;
}
private void SocketClose_bt_Click(object sender, EventArgs e)
{
//stream.Close();
//client.Close();
}
private void SerialOpen_bt_Click(object sender, EventArgs e)
{
SP.PortName = Convert.ToString(SerialPort_cbbox.Text);
SP.BaudRate = Convert.ToInt32(SerialBaud_cbbox.Text);
SP.DataBits = (int)8;
SP.Parity = Parity.None;
SP.StopBits = StopBits.One;
SP.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
SP.Open();
if (SP.IsOpen)
{
SerialState_text.Text = “[" + SP.PortName.ToString() + "]
Port Open Connect\r\n\r\nConnect”;;
}
else
{
SerialState_text.Text = “[" + SP.PortName.ToString() + "]
Port Open Failed\r\n\r\nDisconnected”;
}
}
private void SerialClose_bt_Click(object sender, EventArgs e)
{
SP.Close();
SerialState_text.Text = “[" + SP.PortName.ToString() + "]
Port Close\r\n\r\nDisconnected”;
}
private void TcpSocketOpen_bt_Click(object sender, EventArgs e)
{
try
{
clientAddress = new IPEndPoint(address:
IPAddress.Parse(pc_ip), port: 10000);
serverAddress = new IPEndPoint(address:
IPAddress.Parse(raspberry_ip), port: 10000);
client = new TcpClient(clientAddress);
client.Connect(serverAddress);
stream = client.GetStream();
udpSocket = new Socket(AddressFamily.InterNetwork,
SocketType.Dgram, ProtocolType.Udp);
localEP = new IPEndPoint(address: IPAddress.Any, port: 10000);
remoteEP = new IPEndPoint(IPAddress.Parse(raspberry_ip), port: 10000);
SockOpen_flag = 1;
}
catch (SocketException e1)
{
Console.WriteLine(e1);
}
}
}
5.1.2. ATmega128
#include <ina90.h>
#include “Header/dynamixel_k.h”
#include “Header/uart_k.h”
#include “Header/i2c_k.h”
#include “Header/delay_k.h”
#include “Header/lcd_k.h”
void TestRobotArm(void)
{
unsigned char i;
trans0_ch(‘S’);
for(i=0; i<13; i++)
uart_arr[i]=receive0_ch();
dynamixel_sync_write(0×11,0×03,0×06,0×07,0×13);
delay_ms(10);
}
void StartRobotArm(void)
{
if(uart_arr[14] == 2)
{
dynamixel_sync_write(0×11,0×03,0×06,0×07,0×13);
delay_ms(10);
}
}
void BasicRobotArm(void)
{
unsigned char i;
check_sum(basic_arr,33);
for(i=0; i<33; i++)
trans1_ch( basic_arr[i] );
}
void dec_hex_convert(int dec, unsigned char* low_temp, unsigned char* high_temp )
{
*high_temp = 0×00;
*low_temp = 0×00;
*high_temp = (unsigned char)0xff&(dec/255);
*low_temp = (unsigned char)0xff&(dec%255);
}
void check_sum(unsigned char * check_arr,int arr_length)//디지털모터의 checksum계산하는 함수
{
unsigned int i=0;
unsigned char data=0, check=0;
for(i=2;i<(arr_length-1);i++)//check sum을 구하기 위한 for문
data += check_arr[i];
check = ~data;
*(check_arr+(arr_length-1))=check;
}
void SyncWriteAndRead(unsigned char ID1, unsigned char ID2, unsigned char ID3, unsigned char ID4, unsigned char ID5)
{
unsigned char i;
delay_ms(3000);
dynamixel_sync_write(ID1,ID2,ID3,ID4,ID5);
for(i=0; i<30; i++)
{
trans0_ch(SyncWriteArr[i]);
}
dynamixel_pre_pos_L_read_data(ID5,8);
dynamixel_pre_pos_H_read_data(ID5,8);
}
void dynamixel_pre_pos_L_read_data(unsigned char ID,unsigned int length)
{
pre_pos_L_read[2] = ID;
check_sum(pre_pos_L_read, 8);
__disable_interrupt();
RS485_TX;
trans0_ch(pre_pos_L_read[0]);
trans0_ch(pre_pos_L_read[1]);
trans0_ch(pre_pos_L_read[2]);
trans0_ch(pre_pos_L_read[3]);
trans0_ch(pre_pos_L_read[4]);
trans0_ch(pre_pos_L_read[5]);
trans0_ch(pre_pos_L_read[6]);
trans0_ch(pre_pos_L_read[7]);
TX_FINISH_CHECK//송신 완료를 기다림
RS485_RX;
__enable_interrupt();
while(1)
{
if(UartFlag == 1)
{
__disable_interrupt();
if(uart_i == 7)
{
trans0_ch(0xff);
uart_i = 0;
break;
}
trans0_ch(receive_data[uart_i]);
UartFlag = 0;
__enable_interrupt();
}
}
}
void dynamixel_pre_pos_H_read_data(unsigned char ID,unsigned int length)
{
pre_pos_H_read[2] = ID;
check_sum(pre_pos_H_read, 8);
__disable_interrupt();
RS485_TX;
trans0_ch(pre_pos_H_read[0]);
trans0_ch(pre_pos_H_read[1]);
trans0_ch(pre_pos_H_read[2]);
trans0_ch(pre_pos_H_read[3]);
trans0_ch(pre_pos_H_read[4]);
trans0_ch(pre_pos_H_read[5]);
trans0_ch(pre_pos_H_read[6]);
trans0_ch(pre_pos_H_read[7]);
TX_FINISH_CHECK//송신 완료를 기다림
RS485_RX;
__enable_interrupt();
while(1)
{
if(UartFlag == 1)
{
__disable_interrupt();
if(uart_i == 7)
{
trans0_ch(0xff);
uart_i = 0;
break;
}
trans0_ch(receive_data[uart_i]);
UartFlag = 0;
__enable_interrupt();
}
}
}
void dynamixel_baudrate_change(unsigned char dynamixel_baud_rate)
{
/*
=======================
* 1 => 1M *
* 16 => 115200 *
* 207 => 9600 *
=======================
*/
unsigned int length, i=0;
unsigned char BaudRateChange[]=
{
0xff,
0xff,
0xfe,
0×04, //length N+3, N= (parameter 개수 – 1)
0×03, //write_data 명령어
0×04,
dynamixel_baud_rate,
0×00 //check sum
};
length = (sizeof(BaudRateChange)/sizeof(unsigned char));
check_sum(BaudRateChange, length);
RS485_TX;
for(i=0; i<length; i++)
trans1_ch(BaudRateChange[i]);
}
void dynamixel_sync_write(unsigned char ID1, unsigned char ID2, unsigned char ID3, unsigned char ID4, unsigned char ID5)
{
unsigned int length = (sizeof(SyncWriteArr)/sizeof(unsigned char)) , i ;
SyncWriteArr[7] = ID1;
SyncWriteArr[12] = ID2;
SyncWriteArr[17] = ID3;
SyncWriteArr[22] = ID4;
SyncWriteArr[27] = ID5;
arm_control_ang_ve();
check_sum(SyncWriteArr,length);
//RS485_TX;
for(i=0; i<length; i++)
{
trans1_ch(SyncWriteArr[i]);
}
}
void arm_control_ang_ve()
{
//angle
SyncWriteArr[8]=uart_arr[2];
SyncWriteArr[9]=uart_arr[3];
SyncWriteArr[13]=uart_arr[4];
SyncWriteArr[14]=uart_arr[5];
SyncWriteArr[18]=uart_arr[6];
SyncWriteArr[19]=uart_arr[7];
SyncWriteArr[23]=uart_arr[8];
SyncWriteArr[24]=uart_arr[9];
SyncWriteArr[28]=uart_arr[10];
SyncWriteArr[29]=uart_arr[11];
//velocity
SyncWriteArr[10]=0×65;
SyncWriteArr[11]=0×00;
SyncWriteArr[15]=0×65;
SyncWriteArr[16]=0×00;
SyncWriteArr[20]=0×65;
SyncWriteArr[21]=0×00;
SyncWriteArr[25]=0×65;
SyncWriteArr[26]=0×00;
SyncWriteArr[30]=0×65;
SyncWriteArr[31]=0×00;
}
unsigned char SyncWriteArr[33] = {
//각도 인덱스 (8,9) (13,14)(18,19) (23,24)
//속도 인덱스 (10,11) (15,16) (20,21) (25,26)
0xff,
0xff,
0xfe,
0x1d,//((L+1)*n)+4,n은 모터개수
0×83,//sync write 명령어
0x1e,//control table의 명령어
0×04,//쓰고자하는 L의 길이
0×00,//아이디 17
0×00,//L의 시작
0×00,
0×00,
0×00,//L의 끝
0×00,//아이디
0×00,
0×00,
0×00,
0×00,
0×00,//아이디
0×00,
0×00,
0×00,
0×00,
0×07,//아이디
0×00,
0×00,
0×00,
0×00,
0×00,//아이디
0×00,
0×00,
0×00,
0×00,
//check sum = ~(ID + Length + Instruction + parameter1 + ~ + Parameter N)
0×00
};
unsigned char basic_arr[33] = {
0xff,
0xff,
0xfe,
0x1d,//((L+1)*n)+4,n은 모터개수
0×83,//sync write 명령어
0x1e,//control table의 명령어
0×04,//쓰고자하는 L의 길이
0×11,//아이디 17
0xff,//L의 시작
0×01,
0×65,
0×00,//L의 끝
0×03,//아이디 3
0xff,
0×01,
0×65,
0×00,
0×06,//아이디 6
0xff,
0×01,
0×65,
0×00,
0×07,//아이디 7
0xff,
0×01,
0×65,
0×00,
0×13,//아이디 19
0xff,
0×01,
0×65,
0×00,
//check sum = ~(ID + Length + Instruction + parameter1 + ~ + Parameter N)
0×00
};
unsigned char read_arm[] = {
0xff,//start code
0xff,//start code
0×00,//ID
0×04,//length
0×02,//instruction
0x2b,//parameter1
0×01,//parameter2
0×00//check sum
};
unsigned char pre_pos_L_read[] = {
0xff,//start code
0xff,//start code
0×00,//ID
0×04,//length
0×02,//instruction
0×24,//parameter1 , present speed Low , High Hex
0×01,//parameter2
0×00//check sum
};
unsigned char pre_pos_H_read[] = {
0xff,//start code
0xff,//start code
0×00,//ID
0×04,//length
0×02,//instruction
0×25,//parameter1 , present speed High , Low Hex
0×01,//parameter2
0×00//check sum
};
void dynamixel_read_data(unsigned char ID,unsigned int length)
{
read_arm[2] = ID;
check_sum(read_arm, 8);
__disable_interrupt();
RS485_TX;
trans1_ch(read_arm[0]);
trans1_ch(read_arm[1]);
trans1_ch(read_arm[2]);
trans1_ch(read_arm[3]);
trans1_ch(read_arm[4]);
trans1_ch(read_arm[5]);
trans1_ch(read_arm[6]);
TX_FINISH_CHECK//송신 완료를 기다림
RS485_RX;
__enable_interrupt();
while(1)
{
if(UartFlag == 1)
{
__disable_interrupt();
trans0_ch(receive_data[uart_i]);
if(uart_i == 7)
{
uart_i = 0;
break;
}
UartFlag = 0;
__enable_interrupt();
}
}
}
1. https://developer.leapmotion.com/documentation/csharp/api/Leap_Classes.html ,Leap Motion Developer
2. 닷넷 프로그래밍 정복, 김상형
3. 뇌를 자극하는 C#4.0 프로그래밍, 박상현
4. 열혈 C 프로그래밍, 윤성우
5. AVR ATmega128 마스터, 윤덕용
5.3. 회로도
5.3.1. ATmega128
