Writing Dynamixel AX-12+ position

This entry is part 3 of 5 in the series Bioloid C++ tutorial

The intent of this code is to be clear and easy to understand, it not pretend to be the best code to do the job.

This method create the command to write the AX-12+ position :

int Dynamixel::getSetAX12PositionCommand(byte id, short goal)
    int pos = 0;
    byte numberOfParameters = 0;

    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, &hexH, &hexL);
    buffer[pos++] = hexL;
    buffer[pos++] = hexH;

    // bodyLength
    buffer[3] = (byte)(numberOfParameters + 3);

    byte checksum = checkSumatory(buffer, pos);
    buffer[pos++] = checksum;

    return pos;

int Dynamixel::setPosition(SerialPort *serialPort, int idAX12, int position)
	int error=0;

	int n=getSetAX12PositionCommand(idAX12, position);
	long l=serialPort->sendArray(buffer,n);

	n=serialPort->getArray(bufferIn, 8);

	if (n>4 && bufferIn[4] == 0)
		printf("nid=<%i> set at pos=<%i>n", idAX12, position);
	else {
		printf("nid=<%i> error: <%i>n", idAX12, bufferIn[4]);
		bf(bufferIn, n);

	return error;

Series Navigation<< Reading Dynamixel AX-12+ positionLinux C++ Dynamixel reading and writing example >>

5 comments on “Writing Dynamixel AX-12+ position

  1. Aurelien says:

    Hi (again) 🙂 assume I have four AX 12 to control at 50Hz. As I have to send one instruction at a time, it means that I should have a maximum of 1000/50/4 = 5ms between two instructions. However (having set each AX 12’s return delay time to 0 and return status level to 0), if I do so, after a short period (a few seconds) the servos get “stuck” with the last instruction and won’t listen to any further instruction…! I assume this is related to the half-duplex UART, so I should “force” the direction to TX. I do not see any of this in your Serial class. Any hints? Thanks a lot

    • Aurelien says:

      Actually there is the Sync_write instruction. But if I go beyond 10-20Hz, at some point the servos freeze and remain in the last “read” state…Is it normal?

    • Yes (AFAIK) you should add a delay, about 3-8 ms for wired connections, and until 30-40 ms for wireless connections (BT. Zig).

      • Aurelien says:

        That sounds better! However, in wheel mode, wired, if I put a delay inferior to approx. 40ms between my SYNCWRITE instructions, at some point the servos get stuck (for example, at speed 800) and won’t read anything more… 😐
        any ideas?
        Maybe this is related to the continuous rotation mode, I should try such rates (8ms) for position control.
        But if you tell me that you succeeded with a delay of 8ms and with the code you posted, there is hope for me! 😀

  2. “Programatically” I have used mainly position control, not continuous mode, but I think that both should work in a similar way. Sometimes I have had errors sabotaging my work 🙂

    In a few days I want to upload several programs source code I hope could help you.

%d bloggers like this: