How to control multiple Robotis Dynamixel AX-12 from a .NET Micro Framework environment.

The first step is to wire everything up, we use port 0 and 1 for serial RX and TX, and digital port 2 for controlling the half duplex communication. For this we need an 74LS241, take a look at the setup:

http://robottini.altervista.org/dynamixel-ax-12a-and-arduino-how-to-use-the-serial-port

or

http://savageelectronics.blogspot.com/2011/01/arduino-y-dynamixel-ax-12.html

We need a 74LS241 connected this way:

• Pin 2 to Pin 3 (data out to AX-12)
• Pin 1 to  Pin 19 (connected to Digital 2 on NETMF)
• Pin 18 (connected to RX on NETMF)
• Pin 17 (connected to TX on NETMF
• Pin 10 to ground
• Pin 20 to Vcc

The next step is controlling this from code, the TinyCLR wiki helps us a bit:

http://wiki.tinyclr.com/index.php?title=Dynamixel_AX12

Here is the code for initializing the serial port to 1.000.000 mbps:

SerialPort serial = new SerialPort("COM1", 1000000, Parity.None, 8, StopBits.One);
serial.ReadTimeout = 100;
serial.Open();

//fix the baud on COM1 to non-standard 1000000
Register U0FDR = new Register(0xE000C028);
U0FDR.Write((8 << 4) | 1);//fix the fractional divider register

AX12_PacketHandler AX12 = new AX12_PacketHandler(serial);
AX12.SetLimits(1, 205, 818);
AX12.move(1, 205);

Be sure to add references to:

Microsoft.SPOT.Hardware.SerialPorts
GHIElectronics.NETMF.Hardware (or other if you use another netmf board)
FEZPanda_GHIElectronics.NETMF.FEZ (or FEZ II or other)

And here is the library from TinyCLR, extended with some higher level functions:

using System;
using System.IO.Ports;
using Microsoft.SPOT;
using GHIElectronics.NETMF.Hardware.LowLevel;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.FEZ;

namespace Dynamixel_AX12
{
    class AX12_PacketHandler
    {
        private SerialPort _ser;
        private byte[] _packet = new byte[20];
        public const byte BrodcastID = 0xfe;
        Register PINSEL0 = new Register(0xE002C000);
        OutputPort Direction_High;

        public enum Instruction : byte
        {
            AX_PING = 0x01,
            AX_READ_DATA = 0x02,
            AX_WRITE_DATA = 0x03,
            AX_REG_WRITE = 0x04,
            AX_ACTION = 0x05,
            AX_RESET = 0x06,
            AX_SYNC_WRITE = 0x83, 
        }

        public enum Address : byte
        {
            AX_ID = 0x01,
            AX_CW_LIMIT = 0x06,
            AX_CCW_LIMIT = 0x08,
            AX_LED = 0x19,
        }

        public AX12_PacketHandler(SerialPort ser)
        {
            if (ser.PortName != "COM1")
                throw new Exception("Only COM1 is supported for now");

            Direction_High = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di2, false);

            _ser = ser;
        }

        private byte CalcCRC()
        {
            int len = _packet[3] + 2;
            byte crc = 0;
            for (int i = 2; i < len + 1; i++)
            {
                crc += _packet[i];
            }
            return (byte)(0xFF - crc);
        }

        public void Send(byte ID, Instruction ins, byte[] param)
        {
            int length = 0;
            if (param != null)
            {
                length = param.Length;
            }

            _packet[0] = 0xFF;
            _packet[1] = 0XFF;
            _packet[2] = ID;
            _packet[3] = (byte)(length + 2);//len
            _packet[4] = (byte)ins;

            for (int i = 5; i < length + 5; i++)
            {
                _packet[i] = param[i - 5];
            }    
            _packet[length+5] = CalcCRC();

            // enable UART0 TX pin
            Direction_High.Write(true);

            // send data
            _ser.Write(_packet, 0, length+6);
            //wait till all is sent
            while (_ser.BytesToWrite > 0)
                ;

            //discard echo on RX
            _ser.DiscardInBuffer();
            // disconnect UART TX pin
            Direction_High.Write(false);

            //the response is now coming back so you must read it
        }

        public void ReadResponse(out byte ID, out byte len, out byte error, byte[] parameters)
        {
            int temp = _ser.Read(_packet, 0, _packet.Length);
            if (temp < 5)
            {
                ID = 0;
                len = 0;
                error = 0xff;
                return;// throw new Exception("reading too soon?");
            }
            Debug.Print("OK");
            if (_packet[0] != 0xff || _packet[1] != 0xff)
                throw new Exception("Unexpected data");

            ID = _packet[2];
            len = (byte)(_packet[3] - 2);
            error = _packet[4]; // 16 = CRC error
            for (int i = 0; i < len; i++)
            {
                parameters[i] = _packet[5 + i];
            }
        }

        public byte move(byte ID, int value)
        {
            byte len, error;
            byte[] buf = { 0x1E, (byte)value, (byte)(value >> 8)};

            Send(ID, AX12_PacketHandler.Instruction.AX_WRITE_DATA, buf);
            //Thread.Sleep(100);
            ReadResponse(out ID, out len, out error, null);

            if (error != 0)
                Debug.Print("error: " + error);

            return error;
        }

        public byte ping(byte ID)
        {
            byte len,error;

            Send(ID, AX12_PacketHandler.Instruction.AX_PING, null);
            //Thread.Sleep(100);
            ReadResponse(out ID, out len, out error, null);

            return error;
        }

        public byte ReadLimits(byte ID, out int CW, out int CCW)
        {
            byte len, error;
            byte[] par = new byte[20];
            byte[] buf = { 0x06, 0x04 };
            Send(ID, AX12_PacketHandler.Instruction.AX_READ_DATA, buf);
            //Thread.Sleep(100);
            ReadResponse(out ID, out len, out error, par);

            CW = par[0] + (par[1] << 8);
            CCW = par[2] + (par[3] << 8);
            return error;
        }

        // 511 is halfway (150 degrees)
        // use 205, 818 as limits for 180 degrees (150-90 to 150+90)
        public byte SetLimits(byte ID,  int CW,  int CCW)
        {
            byte len, error;
            byte[] buf = { 0x06, (byte)CW, (byte)(CW >> 8), (byte)CCW, (byte)(CCW >> 8) };

            Send(ID, AX12_PacketHandler.Instruction.AX_WRITE_DATA, buf);
            //Thread.Sleep(100);
            ReadResponse(out ID, out len, out error, null);

            return error;
        }
    }
}