Moving Dynamixels with sync_wr

[Rob]

Ike,

Your better off ditching the CM5 now and getting Jon Hylands USB Bus board.

ArnaudBuy has already written some C# code for the AX12s which I think uses the FT232 chip in Jon's board see:

http://robosavvy.com/forum/viewtopic.ph ... t&start=15

He doesn't program via a com port but via D2XX Driver for the FTDI chip in unmanaged code.

I am not sure if this code will work, I am yet to start using Jons board but I think its a good start. I am working on other stuff with my Bioloid for the next 2 weeks after that I will start testing this code. If I can get it to work with Jons board I will post it.

If you get it to work can you let me know?

Cheers
Rob

[Ike]

I am actually looking to go wireless, but have been very tempted by Jon's USB board.

For anyone who is interested in some C# code to work with in Toss Mode, here is something I hobbled together from various sources. The WritePacket code is from the RoboticsLibrary, modified to talk to a COM port. You will need to put a GUI around it (comPort, txtOutput window, and a few buttons/sliders) but it works well if you are looking to send commands longer than 60 characters, or just want to bypass the CM5.

This is a work in progress, so there is some junk/redundant code. Also, please note that most of this was created from pieces of code written by people who are much smarter than I am; all credit goes to the original authors. You can tell the parts that I wrote... they haven't the vaguest sense of good coding practices.

Code: Select all

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO.Ports;
using System.Text;
using System.Text.RegularExpressions;
using System.Threading;
using System.Windows.Forms;

namespace BioloidChat
{
    public partial class BioloidToss : Form
    {
        #region Dynamixel IDs

        public const byte ID01 = 0x01;
        public const byte ID02 = 0x02;
        public const byte ID03 = 0x03;
        public const byte ID04 = 0x04;
        public const byte ID05 = 0x05;
        public const byte ID06 = 0x06;
        public const byte ID07 = 0x07;
        public const byte ID08 = 0x08;
        public const byte ID09 = 0x09;
        public const byte ID10 = 0x0A;
        public const byte ID11 = 0x0B;
        public const byte ID12 = 0x0C;
        public const byte ID13 = 0x0D;
        public const byte ID14 = 0x0E;
        public const byte ID15 = 0x0F;
        public const byte ID16 = 0x10;
        public const byte ID17 = 0x11;
        public const byte ID18 = 0x12;
        public const byte BroadcastID = 0xFE;

        #endregion


        #region Instruction Set

        // Instruction Set
        public const byte InstPing = 0x01;
        public const byte InstReadData = 0x02;
        public const byte InstWriteData = 0x03;
        public const byte InstRegWrite = 0x04;
        public const byte InstAction = 0x05;
        public const byte InstReset = 0x06;
        public const byte InstDigitalReset = 0x07;
        public const byte InstSystemRead = 0x0C;
        public const byte InstSystemWrite = 0x0D;
        public const byte InstSyncWrite = 0x83;
        public const byte InstSyncRegWrite = 0x84;
       
        #endregion

        #region Control Table

        // Control Table
        public const byte AddressModelNumberLow = 0X00;
        public const byte AddressModelNumberHigh = 0X01;
        public const byte AddressVersionOfFirmware = 0X02;
        public const byte AddressID = 0X03;
        public const byte AddressBaudRate = 0X04;
        public const byte AddressReturnDelayTime = 0X05;
        public const byte AddressCWAngleLimitLow = 0X06;
        public const byte AddressCWAngleLimitHigh = 0X07;
        public const byte AddressCCWAngleLimitLow = 0X08;
        public const byte AddressCCWAngleLimitHigh = 0X09;
        public const byte AddressSystemData2 = 0x0A;
        public const byte AddressHighestLimitTemperature = 0X0B;
        public const byte AddressLowestLimitVoltage = 0X0C;
        public const byte AddressHighestLimitVoltage = 0X0D;
        public const byte AddressMaxTorqueLow = 0X0E;
        public const byte AddressMaxTorqueHigh = 0X0F;
        public const byte AddressStatusReturnLevel = 0X10;
        public const byte AddressAlarmLed = 0X11;
        public const byte AddressAlarmShutdown = 0X12;
        public const byte AddressOperatingMode = 0X13;
        public const byte AddressDownCalibrationLow = 0X14;
        public const byte AddressDownCalibrationHigh = 0X15;
        public const byte AddressUpCalibrationLow = 0X16;
        public const byte AddressUpCalibrationHigh = 0X17;

        public const byte AddressTorqueEnable = 0X18;
        public const byte AddressLed = 0X19;
        public const byte AddressCWComplianceMargin = 0X1A;
        public const byte AddressCCWComplianceMargin = 0X1B;
        public const byte AddressCWComplianceSlope = 0X1C;
        public const byte AddressCCWComplianceSlope = 0X1D;
        public const byte AddressGoalPositionLow = 0X1E;
        public const byte AddressGoalPositionHigh = 0X1F;
        public const byte AddressMovingSpeedLow = 0X20;
        public const byte AddressMovingSpeedHigh = 0X21;
        public const byte AddressTorqueLimitLow = 0X22;
        public const byte AddressTorqueLimitHigh = 0X23;
        public const byte AddressPresentPositionLow = 0X24;
        public const byte AddressPresentPositionHigh = 0X25;
        public const byte AddressPresentSpeedLow = 0X26;
        public const byte AddressPresentSpeedHigh = 0X27;
        public const byte AddressPresentLoadLow = 0X28;
        public const byte AddressPresentLoadHigh = 0X29;
        public const byte AddressPresentVoltage = 0X2A;
        public const byte AddressPresentTemperature = 0X2B;
        public const byte AddressRegisteredInstruction = 0X2C;
        public const byte AddressPauseTime = 0X2D;
        public const byte AddressMoving = 0x2E;
        public const byte AddressLock = 0x2F;
        public const byte AddressPunchLow = 0x30;
        public const byte AddressPunchHigh = 0x31;

        #endregion

        public BioloidToss()
        {
            InitializeComponent();
        }
        private void Form1_Load(object sender, EventArgs e)
        {
            Control.CheckForIllegalCrossThreadCalls = false;
            this.comPort.ReadTimeout = 1000;
            this.comPort.Open();
        }
        private void Form1_FormClosed(object sender, FormClosedEventArgs e)
        {
            this.comPort.Close();
        }
        private byte[] HexStringToByteArray(string s)
        {
            s = s.Replace(" ", "");
            byte[] buffer = new byte[s.Length / 2];
            for (int i = 0; i <s> 0)
            {
                Byte[] comByte = new byte[this.comPort.BytesToRead];
                this.comPort.Read(comByte, 0, this.comPort.BytesToRead);
                this.txtOutput.AppendText("<Recv>: " + ByteArrayToHexString(comByte) + "\r\n");
                this.txtOutput.SelectionStart = this.txtOutput.Text.Length;
                this.txtOutput.ScrollToCaret();

            }
        }
        private void btnOpenCOM_Click(object sender, EventArgs e)
        {
            if (this.comPort.IsOpen)
            {
                this.txtOutput.AppendText("COM2 is already open\r\n");
                this.txtOutput.SelectionStart = this.txtOutput.Text.Length;
                this.txtOutput.ScrollToCaret();
            }
            else
            {
                this.comPort.ReadTimeout = 500;
                this.comPort.Open();
                this.txtOutput.AppendText("COM2 Opened\r\n");
                this.txtOutput.SelectionStart = this.txtOutput.Text.Length;
                this.txtOutput.ScrollToCaret();
            }
        }
        private void btnCloseCOM_Click(object sender, EventArgs e)
        {
            this.comPort.Close();
            this.txtOutput.AppendText("COM2 Closed\r\n");
            this.txtOutput.SelectionStart = this.txtOutput.Text.Length;
            this.txtOutput.ScrollToCaret();
        }
        private void btnServoOff_Click(object sender, EventArgs e)
        {
            WriteInstructionPacket(BroadcastID,             //Broadcast ID
                                   InstSyncWrite,             //SyncWrite
                                   HexStringToByteArray("18" +            //Starting Address (Torque Enable)
                                   "01" +            //Number of bytes to write
                                   "01 00" +     //Disable Torque for ID 1
                                   "02 00" +     //Disable Torque for ID 2
                                   "03 00" +     //Disable Torque for ID 3
                                   "04 00" +     //Disable Torque for ID 4
                                   "05 00" +     //Disable Torque for ID 5
                                   "06 00" +     //Disable Torque for ID 6
                                   "07 00" +     //Disable Torque for ID 7
                                   "08 00" +     //Disable Torque for ID 8
                                   "09 00" +     //Disable Torque for ID 9
                                   "0A 00" +     //Disable Torque for ID 10
                                   "0B 00" +     //Disable Torque for ID 11
                                   "0C 00" +     //Disable Torque for ID 12
                                   "0D 00" +     //Disable Torque for ID 13
                                   "0E 00" +     //Disable Torque for ID 14
                                   "0F 00" +     //Disable Torque for ID 15
                                   "10 00" +     //Disable Torque for ID 16
                                   "11 00" +     //Disable Torque for ID 17
                                   "12 00"));     //Disable Torque for ID 18
        }
        private void btnStandUp_Click(object sender, EventArgs e)
        {
            WriteInstructionPacket(BroadcastID,           //Broadcast ID
                                   InstSyncWrite,             //SyncWrite
                                   HexStringToByteArray
                                   (
                                   "1E" +                 //Starting Address (Goal Position)
                                   "04" +                 //Number of bytes to write
                                   "01 FF 01 FF 00" +     //ID01 Pos 255 001 Speed 255 000
                                   "02 FF 01 FF 00" +     //ID02 Pos 255 001 Speed 255 000
                                   "03 FF 01 FF 00" +     //ID03 Pos 255 001 Speed 255 000
                                   "04 FF 01 FF 00" +     //ID04 Pos 255 001 Speed 255 000
                                   "05 FF 01 FF 00" +     //ID05 Pos 255 001 Speed 255 000
                                   "06 FF 01 FF 00" +     //ID06 Pos 255 001 Speed 255 000
                                   "07 FF 01 FF 00" +     //ID07 Pos 255 001 Speed 255 000
                                   "08 FF 01 FF 00" +     //ID08 Pos 255 001 Speed 255 000
                                   "09 FF 01 FF 00" +     //ID09 Pos 255 001 Speed 255 000
                                   "0A FF 01 FF 00" +     //ID10 Pos 255 001 Speed 255 000
                                   "0B FF 01 FF 00" +     //ID11 Pos 255 001 Speed 255 000
                                   "0C FF 01 FF 00" +     //ID12 Pos 255 001 Speed 255 000
                                   "0D FF 01 FF 00" +     //ID13 Pos 255 001 Speed 255 000
                                   "0E FF 01 FF 00" +     //ID14 Pos 255 001 Speed 255 000
                                   "0F FF 01 FF 00" +     //ID15 Pos 255 001 Speed 255 000
                                   "10 FF 01 FF 00" +     //ID16 Pos 255 001 Speed 255 000
                                   "11 FF 01 FF 00" +     //ID17 Pos 255 001 Speed 255 000
                                   "12 FF 01 FF 00")      //ID18 Pos 255 001 Speed 255 000
                                   );   
        }
        private void trackbarPosition_Changed(object sender, EventArgs e)
        {
            int Position_Low = this.trackBar1.Value % 256;
            String PosL = Position_Low.ToString("X2");
            int Position_High = this.trackBar1.Value / 256;
            String PosH = Position_High.ToString("X2");
            WriteInstructionPacket(ID01, InstWriteData, HexStringToByteArray("1E" + PosL + PosH));       
        }
        private void trackbarSpeed_Changed(object sender, EventArgs e)
        {
            int Speed_Low = this.trackBar2.Value % 256;
            String SpeedL = Speed_Low.ToString("X2");
            int Speed_High = this.trackBar2.Value / 256;
            String SpeedH = Speed_High.ToString("X2");
            WriteInstructionPacket(ID01, InstWriteData, HexStringToByteArray("20" + SpeedL + SpeedH));
        }
        private void btnSendBinary_Click(object sender, EventArgs e)
        {
           
            WriteInstructionPacket(ID01,InstWriteData,HexStringToByteArray("1EFF01"));
            //byte[] data = HexStringToByteArray("FFFF0105031EFF01D8");
            //this.comPort.Write(data, 0, data.Length);
        }
        private void btnTossMode_Click(object sender, EventArgs e)
        {
            this.comPort.WriteLine("T");
        }
    }
}


[BillB]

Has anyone managed to use the toss through mode via the Bluetooth/Zigbee modules? Theoretically it would be possible, although error corection, and latency may be an issue. It would be nice if this were possible without having to write special firmware for the CM5.

[Rob]

Am I right in assuming if you use Toss mode you bypass the CM5 C Lib therefore no S-Curve is applied to your commands?

[sirhoop]

Hi,

I am working with the bioloid. I want to control it him from my PC via Services implemented with MS Robotics Studio.

So I started with the sync_write command, but realized quickly that there will be a lot of problems, since it only let's me drive 3 servos at one time. So I tried out the Toss-Mode I read about in this thread. And that is working fine.
I pretty fast can put the robot in its initial pose.

But I could not find a way to leave the toss-mode again. Is there any way?

In the end the robot shall be controlled via bluetooth. We want to connect a Bluetooth-board to the ZigBee Interface as described in the Bluetooth-Hack.
I guess I have to write my own Behaviour Control Program, right? Or ist there a possibility to use the Toss-Mode for that purpose as well?


Thanks,
Alex

[Rob]

Hi Sirhoop,

In about 2 -4 weeks I will start putting together a collection of MSRS services for the AX-12 Servos. I currently have all the base code 1/3 of which is my own 2/3rds are from other sources like CrustCrawler and this forum. I will create a Codeplex project page for the open source soon.

The goal is just to get the AX-12 covered first and to allow people to choose what method they want to connect to the AX-12. The three I am looking at providing for at the moment are:
-Via the CM5 (not Toss mode I'll explain later how you don't need TOSS mode to get this to work)
-USBToDynamixel based on CrustCrawlers lib
-Via boards such as Jon Hylands USB FTDI based on ArnaudBuy's code posted to this forum

If you want to partner on this, it would be cool.

To issue a sync_write to the CM5 but not in TOSS mode you just need to add each individual goal pos + speed command as an item in a queue. When you've finished adding them, dequeue them one at a time and write each sync write to the CM5 with a small delay between writes (see code below). The swrWriteDelay is set to 50 milliseconds.

int totalQueued = commandQueue.Count;
for (int i = 0; i <= totalQueued - 1; i++)
{
port.WriteLine(commandQueue.Dequeue());
Thread.Sleep(swrWriteDelay);
}

I've found this works fine the advantage being that the CM5 still computes and executes an S-Curve for you so you get much smoother movement. I think TOSS mode bypasses this (please someone tell me if I'm wrong)??

Hope this helps
Rob

[Bullit]

That's correct, in toss mode you would have to generate your own s-curve off line and send it down one sync_write at a time.

[sirhoop]

Hi Rob,

Yes that would be great. I started my work just a week ago, so I am pretty much in an explorative phase. I realized, that with the sync-write I need an delay of about 50 ms between command, but I am not sure if that will go well with my realtime-requirements. Suppose I want to move 10 Joints at a time, then the 10th will start to move half a second after the first.

I have to do some testing in this direction. I thougth I could use the Toss-Mode, but I am a little bit worried about the s-curve problem. Sending all the commands for a s-Curve via the serial-port could result in a similar delay

Perhaps you want to write me a short Mail, so we could discuss further problems that way. ( a dotttt sommerkorn atttt googlemail dotttt com )

[Rob]

Good point the movements I am doing aren't complex i.e. > 5. I'll email you a link which talks about calculating S-Curves.