OpenRCFでMyCobotを動かす

本稿では，OpenRCFでMyCobotを動かす方法を解説します．
以降で紹介するコードは「mechArm 270 M5」用に書かれている（他種の場合は一部変更が必要かもしれない）点にご注意ください．

ちなみに，本稿を執筆している時点ではMyCobotのメーカーが公式にリリースしてるC#のライブラリにはバグがある（関節の角度を読み取れない）ので要注意です．

MyCobotクラスの追加

OpenRCFに元から記述されているSerialDeviceクラスの中に，以下のMyCobotクラスを追加します．

public class MyCobot : SerialBase
{
    private ReceiveDataHandler ReceiveData = new ReceiveDataHandler();
    private static Header Header = new Header(0xfe, 0xfe);

    public MyCobot(int baudRate = 115200)
    {
        SetBaudRate(baudRate);
        SerialPort.Handshake = Handshake.None;
        SerialPort.DtrEnable = true;
        SerialPort.RtsEnable = true;
        SerialPort.DataReceived += DataReceiveEvent;
    }

    private void DataReceiveEvent(object sender, SerialDataReceivedEventArgs e)
    {
        byte[] readBytes = new byte[SerialPort.BytesToRead];
        SerialPort.Read(readBytes, 0, readBytes.Length);
        ReceiveData.Read(readBytes);
    }

    private void SerialWrite(byte[] bytes)
    {
        if (SerialPort.IsOpen) SerialPort.Write(bytes, 0, bytes.Length);
        else Console.WriteLine("Serial port is not open.");
    }

    public float[] ReceivedJointAngles { get { return ReceiveData.JointAngles; } }

    public void ConsoleWriteReceiveData()
    {
        Console.WriteLine(String.Join(" ", ReceiveData.JointAngles));
    }

    private byte speed = 30;            
    public void SetMotionSpeedPct(byte percent)
    {
        if (percent < 100) speed = percent;
        else speed = 100;
    }

    private void WriteJointAngleDeg(byte jointNo, float angle)
    {                
        byte[] sendBytes = new byte[9];
        sendBytes[0] = Header[0];
        sendBytes[1] = Header[1];
        sendBytes[2] = 0x06;
        sendBytes[3] = 0x21;
        sendBytes[4] = jointNo;
        int angle100 = (int)(100 * angle);
        sendBytes[5] = (byte)((angle100 >> 8) & 0xff);
        sendBytes[6] = (byte)(angle100 & 0xff);
        sendBytes[7] = speed;
        sendBytes[8] = 0xfa;
        SerialWrite(sendBytes);
    }
    
    public void WriteJoint1AngleDeg(float angle) { WriteJointAngleDeg(1, angle); }
    public void WriteJoint2AngleDeg(float angle) { WriteJointAngleDeg(2, angle); }
    public void WriteJoint3AngleDeg(float angle) { WriteJointAngleDeg(3, angle); }
    public void WriteJoint4AngleDeg(float angle) { WriteJointAngleDeg(4, angle); }
    public void WriteJoint5AngleDeg(float angle) { WriteJointAngleDeg(5, angle); }
    public void WriteJoint6AngleDeg(float angle) { WriteJointAngleDeg(6, angle); }          

    public void WriteJointAnglesDeg(params float[] angles)
    {
        if (angles.Length == 6)
        {
            byte[] sendBytes = new byte[18];
            sendBytes[0] = Header[0];
            sendBytes[1] = Header[1];
            sendBytes[2] = 0x0f;
            sendBytes[3] = 0x22;

            for (int i = 0; i < angles.Length; i++)
            {
                int angle100 = (int)(100 * angles[i]);
                sendBytes[4 + 2 * i] = (byte)((angle100 >> 8) & 0xff);
                sendBytes[4 + 2 * i + 1] = (byte)(angle100 & 0xff);
            }

            sendBytes[16] = speed;
            sendBytes[17] = 0xfa;
            SerialWrite(sendBytes);
        }
        else
        {
            Console.WriteLine("Number of angles is not 6.");
        }
    }

    public void WriteGripperValue(byte value)
    {
        byte[] sendBytes = { Header[0], Header[1], 0x04, 0x67, value, speed, 0xfa };
        SerialWrite(sendBytes);
    }

    public void RequestJointAnglesReply()
    {
        byte[] sendBytes = { Header[0], Header[1], 0x02, 0x20, 0xfa };
        SerialWrite(sendBytes);
    }

    public void WriteGripperEletric(bool state)
    {
        if (state == false)
        {
            byte[] sendBytes = { 0x01, 0x06, 0x01, 0x03, 0x03, 0xE8, 0x78, 0x88 };
            SerialWrite(sendBytes);
        }
        else if (state == true)
        {
            byte[] sendBytes = { 0x01, 0x06, 0x01, 0x03, 0x00, 0x00, 0x78, 0x88 };
            SerialWrite(sendBytes);
        }
    }

    private class ReceiveDataHandler
    {
        public float[] JointAngles = new float[6];
        private Packet ReadPacket = new Packet();

        public void Read(byte[] readBytes)
        {
            ReadPacket.Stack = readBytes;

            while (Header.Length < ReadPacket.Length)
            {
                if (Header.Contains(ReadPacket.Get))
                {
                    byte len = ReadPacket[Header.Length];

                    for (int i = 0; i < JointAngles.Length; i++)
                    {
                        int idx = Header.Length + 1 + 2 * i;

                        if (idx < Header.Length + len)
                        {
                            JointAngles[i] = ReadPacket.ToInt16(idx) / 100f;
                        }
                    }

                    ReadPacket.CutOut(Header.Length + len + 1); 
                }
                else
                {
                    Console.WriteLine("Error : Header does not match.");
                    ReadPacket.Reset();
                }
            }
        }

    }

}

もしエラーが出たら，クラスを貼り付ける場所（階層）が正しくない可能性が高いです．

MyCobotクラスの使用例

以下にMainWindow.xaml.csファイルの関数ButtonClick()の周辺を変更した例を示す．

SerialDevice.MyCobot MyCobot = new SerialDevice.MyCobot();
        
void Button1_Click(object sender, RoutedEventArgs e)
{
    MyCobot.PortOpen("COM13");
}

void Button2_Click(object sender, RoutedEventArgs e)
{
    MyCobot.RequestJointAnglesReply();
    MyCobot.ConsoleWriteReceiveData();
    float[] angles = MyCobot.ReceivedJointAngles;
}

void Button3_Click(object sender, RoutedEventArgs e)
{
    MyCobot.WriteJointAnglesDeg(0, 0, 0, 0, 0, 0);
}         

void Button4_Click(object sender, RoutedEventArgs e)
{
    MyCobot.WriteJointAnglesDeg(10, -10, 10, -10, 10, -10);
}

void Button5_Click(object sender, RoutedEventArgs e)
{
    MyCobot.SetMotionSpeedPct(50);            
}

ボタン３またはボタン４を押すとアームが動くので要注意です．

・COMの番号はデバイスマネージャーで要確認．
・関節の角度は度（degree）で指定する．
・関節の動作速度（MotionSpeed）はパーセントで指定する（初期値は30％）．
・関数ConsoleWriteReceiveData()は受信した関節角度をコンソールに表示する．
・関数RequestJointAnglesReply()は関節角度の返信を要求するだけ（この関数を実行した後に時間差で受信データが更新される）．つまり，ボタン２を最初に押したときは受信データの初期値（0,0,...,0）がコンソールに表示される．