Servomoteurs ROBOTIS Dynamixel
Servomoteurs ROBOTIS Dynamixel
La société coréenne ROBOTIS propose des servomoteurs ( ensemble Moteur CC + Hacheur + Contrôleur ) performants.
L’asservissement étant géré en interne, il suffit d’envoyer une consigne de vitesse ou de position via un bus ( TTL ou RS485 ).
L’information retour de couple est également disponible.
Références Testées
- Bus: TTL
- Tension : 6.5 - 12V
- Bus: RS485
- Tension : 6.5 - 12V
Protocole Dynamixel 2.0
Analyse d’une trame :
LED ON : ( uC –> Dxl)
Header
- 0xFF 0xFF 0xFD 0x00
ID
- 0x01 : Identifiant du moteur = 1
Length
- 0x06 : Taille( Instruction + commande + Param + CRC ) = 6
Instruction
- 0x03 : Write = 0x03 ( read = 0x02 )
Command
- 0x41 : Command LED = 0x41 = 65
Parameter
- 0x01 : Led ON ( Led Off = 0x00)
CRC
- 0xCC 0xE6
Status Frame : ( Dxl –> uC)
Header
- 0xFF 0xFF 0xFD 0x00
ID
- 0x01 : Identifiant du moteur = 1
Length
- 0x04 : Taille( Instruction + commande + Param + CRC ) = 4
Instruction
- 0x55 : Status
Error Code
- 0x00 : Pas d’erreur
CRC
- 0xA1 0x0C
AUTRES EXEMPLES DE TRAMES
Contrôle des Moteurs depuis un PC
Contrôle des Moteurs par carte ST-NUCLEO
Robotis Dynamixel Shield
SOURCES DU PROJET :
WORKSPACE_F411_DXL_STM32CUBEIDE.zip
Architecture
Envoi d’une Trame
int dxl_sendPacket(int id, int instruction, int parameter_data_size, ...)
{
uint8_t frame_to_write[50];
uint16_t crc;
uint32_t length=3+parameter_data_size;
uint8_t arg;
frame_to_write[0] = 0xFF;
frame_to_write[1] = 0xFF;
frame_to_write[2] = 0xFD;
frame_to_write[3] = 0x00; // RESERVED
frame_to_write[4] = id;
frame_to_write[5] = length&0xFF; // length_LB
frame_to_write[6] = (length>>8)&0xFF; // length_HB
frame_to_write[7] = instruction; // Instruction : Write
va_list args;
va_start(args, parameter_data_size);
for(int i=0 ; i < parameter_data_size ; i++ )
{
arg = va_arg(args, int);
frame_to_write[8+i]= arg;
}
crc = dxl_updateCRC(0,frame_to_write,8+parameter_data_size);
frame_to_write[8+parameter_data_size]= (uint8_t) ( crc & 0xFF );
frame_to_write[9+parameter_data_size]= (uint8_t) ( (crc>>8) & 0xFF );
va_end(args);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, 1);
sendFrame(frame_to_write,parameter_data_size+2+8);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, 0);
return 0;
}Réception d’une Trame
Quand un octet est reçu sur UART6, cela déclenche une interruption.
La fonction de callback dxl_rcv_cb() met à jour dxl_rcvBuf .
|
|
while(p_rd != p_wr)
{
tmp = dxl_rcvBuf[(p_rd++)%BUF_SIZE];
switch(state)
{
case STATE_START : if(tmp == 0xFF) {state = STATE_HEADER_1;} break;
case STATE_HEADER_1 : if( tmp == 0xFF) {state = STATE_HEADER_2;} break;
case STATE_HEADER_2 : if( tmp == 0xFD) {state = STATE_HEADER_3;} break;
case STATE_HEADER_3 : if( tmp == 0x00) {state = STATE_ID;} break;
case STATE_ID : id = tmp ; state = STATE_LENGTH_LB; term_printf("id = %d | ",id); break;
case STATE_LENGTH_LB : length = tmp ; state = STATE_LENGTH_HB; break;
case STATE_LENGTH_HB : length = (uint16_t)tmp << 8 | length ;
state = STATE_INSTRUCTION;
term_printf("length = %d | ",length);
length = length - 2;
break;
case STATE_INSTRUCTION : instruction = tmp ;
state = STATE_PARAM;
length--;
term_printf("instruction = 0x%x | ",instruction);
break;
case STATE_PARAM : if( length > 0 ){
param[i++] = tmp ;
length-- ;
term_printf("param %d = 0x%x | ",i,tmp);
state = STATE_PARAM;
}
if (length == 0) {
state = STATE_CRC1;
}
break;
case STATE_CRC1 : state = STATE_CRC2; term_printf("CRC1 = 0x%x | ",tmp); break;
case STATE_CRC2 : state = STATE_FINAL; term_printf("CRC2 = 0x%x ",tmp); break;
case STATE_FINAL : p_rd = p_wr; return 0; break;
}
}