· Port 설정
//Dynamixel
this.timer.Interval = 200;
this.timer.Tick += new EventHandler(timer_Tick);
this.timer.Enabled = true;
this.seriaport.BaudRate = 1000000;
this.seriaport.Encoding = Encoding.Default;
this.seriaport.Parity = Parity.None;
this.seriaport.DataBits = 8;
this.seriaport.StopBits = StopBits.One;
this.seriaport.Handshake = Handshake.None;
this.seriaport2.BaudRate = 1000000;
this.seriaport2.Encoding = Encoding.Default;
this.seriaport2.Parity = Parity.None;
this.seriaport2.DataBits = 8;
this.seriaport2.StopBits = StopBits.One;
this.seriaport2.Handshake = Handshake.None;

// initialize the components (controls) of the window
this.InitializeComponent();
string[] Ports = SerialPort.GetPortNames();
foreach (string i in Ports)
{
ComboBox1.Items.Add(i);
}
ComboBox1.SelectedIndex = ComboBox1.Items.Count – 1;
myDynamixel = new Dynamixel(this.seriaport);
string[] Ports2 = SerialPort.GetPortNames();
foreach (string i in Ports2)
{
ComboBox2.Items.Add(i);
}
ComboBox2.SelectedIndex = ComboBox1.Items.Count – 1;
myDynamixel2 = new Dynamixel(this.seriaport2);
}

·Wearable 값 읽기
private void serialPort_DataReceived2 (object sender,Serial Data Received Event Argse)
{
string buff;
buff = this.seriaport2.ReadLine();
if (buff.Length < 32) return;
//Console.WriteLine(buff.Length);
if (buff.Substring(0, 2) == “WM”)
{
_sensor[0] = Convert.ToInt32(buff.Substring(3, 4));
_sensor[1] = Convert.ToInt32(buff.Substring(8, 4));
_sensor[2] = Convert.ToInt32(buff.Substring(13, 4));
_sensor[3] = Convert.ToInt32(buff.Substring(18, 4));
_sensor[4] = Convert.ToInt32(buff.Substring(23, 4));
_sensor[5] = Convert.ToInt32(buff.Substring(28, 4));
}
}

· 모터 속도 조절
private void MotorSpeed()
{
if (this.seriaport.IsOpen)
{
for (int i = 0; i <= 10; i++)
{
myDynamixel.ax12_write2(i, Dynamixel.AX_GOAL_SPEED_L, 1023);
myDynamixel.ax12_write2(i, Dynamixel.AX_TORQUE_LIMIT_L, 1023);
}

}
}
void SendSerialData(SerialPort _serial, byte[] buffer)
{
try
{
_serial.Write(buffer, 0, buffer.Length);
}
catch
{ }
}
void SendSerialData(SerialPort _serial, byte[] buffer, int Length)
{
try
{
_serial.Write(buffer, 0, Length);
}
catch
{ }
}

· 모터 움직이기
private void moveVactor()
{
if (this.seriaport.IsOpen)
{
_motorData[0] = (1024 – (int)_sensor[1]);
_motorData[1] = (1024 – (int)_sensor[2]);
_motorData[2] = (1024 – (int)_sensor[3]);
_motorData[3] = (1024 – (int)_sensor[4]);
_motorData[4] = (1024 – (int)_sensor[5]);
if (_sensor[0] > 1000)
_motorData[5] = (int)(512 – ((int)((_sensor[0] – 7) / 100) * 1024 / 31));
else
_motorData[5] = 512;
myDynamixel.ax12_sync_write(Dynamixel.AX_GOAL_POSITION_L, 2, 10, _motorData);
}
}

· ROBOTIS OpenCM 소스코드
/* Minimum_Source*/
#include <stdio.h>
#define AX_START 255

#define AX_READ_DATA 2
#define AX_WRITE_DATA 3
#define AX_LENGTH 5
#define AX_TORQIUELIMIT 34
#define AX_NOW 36
#define sendData(args) (Serial1.write(args))
Dynamixel Dxl(1);
int pp[5];
char buff[50]={0};
int temp=0;
void setup() {
// put your setup code here, to run once:
Dxl.begin(3);
for(int i=1;i<=5;i++){
settorque(i);
}
}

void settorque(unsigned char ID)
{
unsigned int Checksum;
unsigned int MaxTorque = 1;
Checksum = (~(ID + AX_LENGTH + AX_WRITE_DATA + AX_TORQIUELIMIT + MaxTorque + (MaxTorque>>8)))&0xFF; //패킷설정
gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 1);

sendData(AX_START); //시리얼 통신 보내기
sendData(AX_START);
sendData(ID);
sendData(AX_LENGTH);
sendData(AX_WRITE_DATA);
sendData(AX_TORQIUELIMIT);
sendData(MaxTorque);
sendData((MaxTorque>>8));

sendData(Checksum);
}

void stoptorque(unsigned char ID)
{
unsigned int Checksum;
unsigned int MaxTorque = 1023;

Checksum = (~(ID + AX_LENGTH + AX_WRITE_DATA + AX_TORQIUELIMIT + MaxTorque + (MaxTorque>>8)))&0xFF; //패킷설정
gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 1);
sendData(AX_START); //시리얼 통신 보내기
sendData(AX_START);
sendData(ID);
sendData(AX_LENGTH);
sendData(AX_WRITE_DATA);
sendData(AX_TORQIUELIMIT);
sendData(MaxTorque);
sendData((MaxTorque>>8));
sendData(Checksum);
}
void loop() {
int AdcData = analogRead(2);
for(int i=1;i<=5;i++)
{
temp=pp[i-1];
pp[i-1] = Dxl.readWord(i,AX_NOW);
if(pp[i-1]>1000)
pp[i-1]=temp;
}
sprintf(buff,”WM %04d %04d %04d %04d %04d %04d”, AdcData,pp[0],pp[1],pp[2],pp[3],pp[4],pp[5]);
SerialUSB.println(buff);
}