STM32F4 Discovery program for moving Dynamixel

Posted on August 05, 2015 at 08:11

Hello, I am a college student and a beginner programmer, I got a project to make a library using STM32F4 Discovery to move a Dynamixel motor type XL-320. I already studied the basic communication for commanding Dynamixel XL-320 (http://support.robotis.com/en/product/dynamixel_pro/communication/instruction_status_packet.htm) and I already made some of the program list, but it doesn't work, can somebody help me? Thank you very much.

I use some of the program from HelloSpoon, to get the CRC value (https://github.com/HelloSpoon/HelloSpoon-Spark/blob/master/firmware/dxl_pro.cpp)

Here is some of the program list I made :

void Dyna_move(unsigned char ID, int Position)

{

    //length

    XL_LENGTH_L = L_Byte(XL_GOAL_P_LENGTH);

    XL_LENGTH_H = H_Byte(XL_GOAL_P_LENGTH);

    Packet_Length = XL_GOAL_P_LENGTH;

    //address

    XL_ADDRESS_L = L_Byte(XL_GOAL_POSITION);

    XL_ADDRESS_H = H_Byte(XL_GOAL_POSITION);

    //position

    POS_L = L_Byte(Position);

    POS_H = H_Byte(Position);

    //txpacket

    unsigned char DataSend[] = { XL_START1,

                                 XL_START1,

                                 XL_START2,

                                 XL_RESERVED,

                                 ID,

                                 XL_LENGTH_L,

                                 XL_LENGTH_H,

                                 XL_WRITE_DATA,

                                 XL_ADDRESS_L,

                                 XL_ADDRESS_H,

                                 POS_L,

                                 POS_H,

                                 CRC_L,

                                 CRC_H };

    for(count=0;count<(XL_GOAL_P_LENGTH+7);count++)

    {

    TxPacket[count]=DataSend[count];

    }

    //CRC

    CRC_X = update_crc (CRC_ACCUM, TxPacket , (5+Packet_Length) ) ;

    CRC_L = L_Byte(CRC_X);

    CRC_H = H_Byte(CRC_X);

    UART_SendByte(DYNAMIXEL, XL_START1);

    UART_SendByte(DYNAMIXEL, XL_START1);

    UART_SendByte(DYNAMIXEL, XL_START2);

    UART_SendByte(DYNAMIXEL, XL_RESERVED);

    UART_SendByte(DYNAMIXEL, ID);

    UART_SendByte(DYNAMIXEL, XL_LENGTH_L);

    UART_SendByte(DYNAMIXEL, XL_LENGTH_H);

    UART_SendByte(DYNAMIXEL, XL_WRITE_DATA);

    UART_SendByte(DYNAMIXEL, XL_ADDRESS_L);

    UART_SendByte(DYNAMIXEL, XL_ADDRESS_H);

    UART_SendByte(DYNAMIXEL, POS_L);

    UART_SendByte(DYNAMIXEL, POS_H);

    UART_SendByte(DYNAMIXEL, CRC_L);

    UART_SendByte(DYNAMIXEL, CRC_H);

    Delayus(100);

}

#stm32 #stm32f4 #discovery #usart