————–메인함수————–

#define F_CPU 16000000UL //내부 CPU 클록 16000000Hz

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <util/delay.h>
#include “usart.h”
#define LED_SETTING DDRA |=1<<PA0 //LED 연결 핀을 출력 상태로

#define S0 PB0 // 색 감지 센서 주파수 % 조정
#define S1 PB1 // 색 감지 센서 주파수 % 조정
#define S2 PB2 // 색상 조정
#define S3 PB3 // 색상 조정
#define S4 PB4 // TCS3200 라이트

#define nop2 {asm volatile(“ NOP “); asm volatile(“ NOP “);} // 어셈블리어 2클록 딜레이
#define nop6 {nop2; nop2; nop2;} // 어셈블리어 6클록 딜레이
#define nop12 { nop6 ; nop6 ;} // 어셈블리어 12클록 딜레이

#define ws2812b_1 (PORTA |= (1<<PA0)) // LED 켜기
#define ws2812b_0 (PORTA &= ~(1<<PA0)) // LED 끄기

volatile enum state_m{NORMAL,ROMANTIC,COLORCHANGE,BLUETOOTH} statem=0; // 모드 선언 열거

volatile unsigned int light=50; // 밝기 변수 [ 0~255 ] volatile unsigned int bright=2; // 밝기 변수 [light/25] [0~10] volatile unsigned int play=0; // 상태 사용 변수
volatile unsigned int number=0; // 타이머 오버 플로 1s 단위를 위한 변수
volatile unsigned int value=0; // 주파수 측정 변수
volatile unsigned int shift; // ROMANTIC MODE 색깔변경
volatile unsigned int t;
volatile unsigned int red_fre; // RGB R 주파수
volatile unsigned int blue_fre; // RGB B 주파수
volatile unsigned int green_fre; // RGB G 주파수

volatile enum state_t {RED , BLUE , GREEN} statet=RED ; // 주파수 열거
volatile char led[13][3];
volatile char led_romantic[156][3]={
{255, 0, 0},{245, 0, 10},{235, 0, 20},{224, 0, 31},{214, 0, 41},{204, 0, 51},{194, 0, 61},{184, 0, 71},{173, 0, 82},{163, 0, 92},
{153, 0,102},{143, 0,112},{133, 0,122},{122, 0,133},{112, 0,143},{102, 0,153},{ 92, 0,163},{ 82, 0,173},{ 71, 0,184},{ 61, 0,194},
{ 51, 0,204},{ 41, 0,214},{ 31, 0,224},{ 20, 0,235},{ 10, 0,245},{ 0, 0,255},{ 0, 0,255},{ 0, 10,245},{ 0, 20,235},{ 0, 31,224},
{ 0, 41,214},{ 0, 51,204},{ 0, 61,194},{ 0, 71,184},{ 0, 82,173},{ 0, 92,163},{ 0,102,153},{ 0,112,143},{ 0,122,133},{ 0,133,122},
{ 0,143,112},{ 0,153,102},{ 0,163, 92},{ 0,173, 82},{ 0,184, 71},{ 0,194, 61},{ 0,204, 51},{ 0,214, 41},{ 0,224, 31},{ 0,235, 20},
{ 0,245, 10},{ 0,255, 0},{ 0,255, 0},{ 10,245, 0},{ 20,235, 0},{ 31,224, 0},{ 41,214, 0},{ 51,204, 0},{ 61,194, 0},{ 71,184, 0},
{ 82,173, 0},{ 92,163, 0},{102,153, 0},{112,143, 0},{122,133, 0},{133,122, 0},{143,112, 0},{153,102, 0},{163, 92, 0},{173, 82, 0},
{184, 71, 0},{194, 61, 0},{204, 51, 0},{214, 41, 0},{224, 31, 0},{235, 20, 0},{245, 10, 0},{255, 0, 0},
//
{255, 0, 0},{245, 0, 10},{235, 0, 20},{224, 0, 31},{214, 0, 41},{204, 0, 51},{194, 0, 61},{184, 0, 71},{173, 0, 82},{163, 0, 92},
{153, 0,102},{143, 0,112},{133, 0,122},{122, 0,133},{112, 0,143},{102, 0,153},{ 92, 0,163},{ 82, 0,173},{ 71, 0,184},{ 61, 0,194},
{ 51, 0,204},{ 41, 0,214},{ 31, 0,224},{ 20, 0,235},{ 10, 0,245},{ 0, 0,255},{ 0, 0,255},{ 0, 10,245},{ 0, 20,235},{ 0, 31,224},
{ 0, 41,214},{ 0, 51,204},{ 0, 61,194},{ 0, 71,184},{ 0, 82,173},{ 0, 92,163},{ 0,102,153},{ 0,112,143},{ 0,122,133},{ 0,133,122},
{ 0,143,112},{ 0,153,102},{ 0,163, 92},{ 0,173, 82},{ 0,184, 71},{ 0,194, 61},{ 0,204, 51},{ 0,214, 41},{ 0,224, 31},{ 0,235, 20},
{ 0,245, 10},{ 0,255, 0},{ 0,255, 0},{ 10,245, 0},{ 20,235, 0},{ 31,224, 0},{ 41,214, 0},{ 51,204, 0},{ 61,194, 0},{ 71,184, 0},
{ 82,173, 0},{ 92,163, 0},{102,153, 0},{112,143, 0},{122,133, 0},{133,122, 0},{143,112, 0},{153,102, 0},{163, 92, 0},{173, 82, 0},
{184, 71, 0},{194, 61, 0},{204, 51, 0},{214, 41, 0},{224, 31, 0},{235, 20, 0},{245, 10, 0},{255, 0, 0}
}; //로맨틱 모드

void byte_out(int d){
char i;
for(i=0;i<8;i++){
if(d&0×80){ ws2812b_1; nop12; ws2812b_0; }
else { ws2812b_1; nop6; ws2812b_0; }
d<<=1;
}
} // LED 색상 제어

ISR(INT2_vect) // 색상 감지 기능 OFF , 색깔 고정 (4번 버튼)
{
play=1;
PORTB &= ~(1<<S4) ;
}

ISR(INT3_vect) // 현재 스탠드의 모드를 바꾸어줌 (5번 버튼)
{

for(int x=0 ; x<9 ; x++)
{
byte_out(0);

byte_out(0);

byte_out(0);

}
_delay_ms(50);

receive=0;
bright=2;
light=50;
statem++;
if(statem>1)
statem=0;
_delay_ms(500); //스위치 채터링 방지용 딜레이
}

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);
}
}
}

#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();
}

}
}