Three C# classes for a simple example for reading and writing the position of a AX-12 Dynamixel servo. You can use it with the USB2Dynamixel or using only the serial port: in “Manage mode” send “T” to put firmware in “Toss Mode”.
Here you can find several combinations of hardware, firmware and programming tools.
The main
.
class Program
{
static void Main(string[] args)
{
int error=0;
int idAX12=15;
SerialPort2Dynamixel serialPort = new SerialPort2Dynamixel();
Dynamixel dynamixel = new Dynamixel();
if (serialPort.open("COM1")==false) {
dynamixel.sendTossModeCommand(serialPort);
int pos=dynamixel.getPosition(serialPort, idAX12);
if (pos>250 && pos dynamixel.setPosition(serialPort, idAX12, pos-100);
else
Console.Out.WriteLine("nPosition under 250 or over 1023", pos);
serialPort.close();
}
else {
Console.Out.WriteLine("nCan't open serial port");
error=-1;
}
}
}
Serial Port
public class SerialPort2Dynamixel
{
const int HeaderSize = 4;
const int PacketLengthByteInx = 3;
const int BufferSize = 1024;
const int MaximuTimesTrying = 250;
const int waitTime = 5;
private string portName = "COM3";
private byte[] buffer = new byte[BufferSize];
private SerialPort serialPort = new SerialPort();
public bool open(String com)
{
bool error = false;
portName = com;
serialPort.PortName = com;
serialPort.BaudRate = 57600;
serialPort.DataBits = 8;
serialPort.Parity = Parity.None;
serialPort.StopBits = StopBits.One;
try
{
if (serialPort.IsOpen)
Console.WriteLine("Serial port is already open");
else
serialPort.Open();
}
catch (Exception exc)
{
Console.WriteLine(exc.Message);
error = true;
}
return error;
}
public void close()
{
if (serialPort.IsOpen)
serialPort.Close();
Console.WriteLine("Conecction closed!");
}
private void cleanConnection()
{
serialPort.DiscardInBuffer();
}
public byte[] query(byte[] buffer, int pos)
{
byte[] outBuffer = null;
try
{
serialPort.Write(buffer, 0, pos);
System.Threading.Thread.Sleep(waitTime);
outBuffer = rawRead();
}
catch (Exception exc)
{
Console.Out.WriteLine(exc.Message);
}
return outBuffer;
}
public void rawWrite(byte[] buffer, int pos)
{
try
{
serialPort.Write(buffer, 0, pos);
}
catch (Exception exc)
{
Console.Out.WriteLine(exc.Message);
}
}
public byte[] rawRead()
{
byte[] localbuffer = null;
int n = serialPort.BytesToRead;
if (n != 0)
{
localbuffer = new byte[n];
try
{
serialPort.Read(localbuffer, 0, n);
}
catch (Exception exc)
{
Console.Out.WriteLine(exc.Message);
}
}
return localbuffer;
}
}<code>
Dynamixel
class Dynamixel
{
protected static int MaxBufferSize = 1024;
protected byte[] buffer = new byte[MaxBufferSize];
private static byte checkSumatory(byte[] data, int length)
{
uint cs = 0;
for (int i = 2; i < length; i++)
{
cs += data[i];
}
cs = ~cs;
return (byte)(cs & 0x0FF);
}
public static void toHexHLConversion(int pos, out byte hexH, out byte hexL)
{
ushort uPos = (ushort)pos;
hexH = (byte)(uPos >> 8);
hexL = (byte)uPos;
}
public static ushort fromHexHLConversion(byte hexH, byte hexL)
{
return (ushort)((hexL << 8 ) + hexH);
}
public static short fromHexHLConversionToShort(byte hexH, byte hexL)
{
return (short)((hexL << 8 ) + hexH);
}
public static void toHexHLConversion(int pos, out string hexH, out string hexL)
{
string hex;
int lon, start;
hex = String.Format("{0:X4}", pos);
lon = hex.Length;
if (lon < 2)
{
hexL = hex;
hexH = "0";
}
else
{
start = lon - 2;// lon = 4, start = 2; lon=3, start=1
hexL = hex.Substring(start);
hexH = hex.Substring(0, start);
}
}
public void sendTossModeCommand(SerialPort2Dynamixel sp2d)
{
byte[] buffer = { (byte)'t', (byte)'r' };
sp2d.rawWrite(buffer, 2);
System.Threading.Thread.Sleep(100);
sp2d.rawRead();
}
private static int getReadPositionCommand(byte[] buffer, byte id)
{
//OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM
int pos = 0;
buffer[pos++] = 0xff;
buffer[pos++] = 0xff;
buffer[pos++] = id;
buffer[pos++] = 4; // bodyLength = 4
buffer[pos++] = 2; //the instruction, rawRead => 2
// pos registers 36 and 37
buffer[pos++] = 0x24;
//bytes to rawRead
buffer[pos++] = 2;
byte checksum = checkSumatory(buffer, pos);
buffer[pos++] = checksum;
return pos;
}
private static int getSetPositionCommand(byte[] buffer, byte id, short goal)
{
int pos = 0;
byte numberOfParameters = 0;
//OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM
buffer[pos++] = 0xff;
buffer[pos++] = 0xff;
buffer[pos++] = id;
// bodyLength
buffer[pos++] = 0; //place holder
//the instruction, query => 3
buffer[pos++] = 3;
// goal registers 30 and 31
buffer[pos++] = 0x1E;// 30;
//bytes to write
byte hexH = 0;
byte hexL = 0;
toHexHLConversion(goal, out hexH, out hexL);
buffer[pos++] = hexL;
numberOfParameters++;
buffer[pos++] = hexH;
numberOfParameters++;
// bodyLength
buffer[3] = (byte)(numberOfParameters + 3);
byte checksum = checkSumatory(buffer, pos);
buffer[pos++] = checksum;
return pos;
}
public short getPosition(SerialPort2Dynamixel sp2d, int id)
{
//byte[] localbuffer = new byte[MaxBufferSize];
int size = getReadPositionCommand(buffer, (byte)id);
byte[] res = sp2d.query(buffer, size);
short position = -1;
if (res != null)
{
int length = res.Length;
if (res != null && length > 4 && res[4] == 0)
{
byte l = 0;
byte h = res[5];
if (length > 6)
{
l = res[6];
}
position = fromHexHLConversionToShort(h, l);
}
}
return position;
}
public bool setPosition(SerialPort2Dynamixel sp2d, int id, int goal)
{
bool couldSet = false;
short position = (short)goal;
int size = getSetPositionCommand(buffer, (byte)id, (short)goal);
byte[] res = sp2d.query(buffer, size);
//ushort value = 1;
if (res != null && res.Length > 4 && res[4] == 0)
couldSet = true;
return couldSet;
}
}
The zip with the full example for Visual Studio 2008