Dynamixel数字舵机驱动 (amobbs.com 阿莫电子技术论坛) -

vjcmain 发表于 2012-10-17 22:31:59

Dynamixel数字舵机驱动

本帖最后由 vjcmain 于 2012-10-17 22:41 编辑

一个控制数字舵机的设计程序，采用C语言编写，支持Dynamixel 通信协议，案例是AVR单片。实际上可以采用FPGA，例如SOPC系统上，可控制小型机器人。
顺便把Dynamixel数字舵机的中文翻译资料也贴上来吧，是别人翻译的
、

最后把CM-5主控制器拆解图也奉上
#include <avr/io.h>
#include <util/delay.h>
void InitUart0(void)
{
UCSR0A = 0x02; // 设置为倍速模式
UBRR0H = 0;
UBRR0L = 1;
UCSR0B = (1<<RXEN)|(1<<TXEN);// 接收器与发送器使能
UCSR0C = (3<<UCSZ0);
DDRE &= ~_BV(PE0); // 初始化RX 端口默认方向为输入
PORTE &= ~_BV(PE0); // 初始化RX 端口默认状态为高阻
DDRE |= _BV(PE1); // 初始化TX 端口默认方向为输出
PORTE |= _BV(PE1); // 初始化TX 端口默认状态为高电平
DDRA |= _BV(PA0); // 初始化使能端口状态方向为输出
PORTA &= ~_BV(PA0); // 初始化使能端口状态为RX 状态
DDRA |= _BV(PA1); // 初始化使能端口状态方向为输出
PORTA |= _BV(PA1); // 初始化使能端口状态方为TX 状态
}
void SendUart0Byte(unsigned char data)
{
while ( !( UCSR0A & (1<<UDRE)) );// 等待发送缓冲器为空
UDR0 = data;/* 将数据放入缓冲器，发送数据*/
}
void SetServoLimit(unsigned char id, unsigned short int cw_limit, unsigned short int ccw_limit)
{
unsigned short int temp_ccw = 0; // 临时速度，用于进行方向判别
unsigned short int temp_cw = 0;
unsigned char temp_ccw_h = 0; // 待发送数据h 位
unsigned char temp_ccw_l = 0; // 待发送数据l 位
unsigned char temp_cw_h = 0;
unsigned char temp_cw_l = 0;
unsigned char temp_sum = 0; // 校验和寄存变量

if (ccw_limit > 1023)
{
temp_ccw = 1023; // 限制速度值在可用范围内
}
else
{
temp_ccw = ccw_limit;
}
if (cw_limit > 1023)
{
temp_cw = 1023;
}
else
{
temp_cw = cw_limit;
}
temp_ccw_h = (unsigned char)(temp_ccw >> 8);
temp_ccw_l = (unsigned char)temp_ccw; // 将16bit 数据拆为2个8bit 数据
temp_cw_h = (unsigned char)(temp_cw >> 8);
temp_cw_l = (unsigned char)temp_cw; // 将16bit 数据拆为2个8bit 数据
PORTA &= ~_BV(PA1);
PORTA |= _BV(PA0); // 使总线处于主机发送状态
UCSR0A |= (1<<TXC0); // 清除UART0写完成标志
SendUart0Byte(0xFF); // 发送启动符号0xFF
SendUart0Byte(0xFF); // 发送启动符号0xFF
SendUart0Byte(id); // 发送id
SendUart0Byte(7); // 发送数据长度为参数长度+2，参数长度为3
SendUart0Byte(0x03); // 命令数据为“WRITE DATA”
SendUart0Byte(0x06); // 舵机控制寄存器首地址
SendUart0Byte(temp_cw_l); // 发送顺时针位置限制低位
SendUart0Byte(temp_cw_h); // 发送顺时针位置限制高位
SendUart0Byte(temp_ccw_l); // 发送逆时针位置限制低位
SendUart0Byte(temp_ccw_h); // 发送逆时针位置限制高位
temp_sum = id + 7 + 0x03 + 0x06 + temp_cw_l + temp_cw_h + temp_ccw_l + temp_ccw_h;
temp_sum = ~temp_sum; // 计算校验和
SendUart0Byte(temp_sum); // 发送校验和
while ( !( UCSR0A & (1<<TXC0)) ) // 等待发送完成
{ // (Waiting for finishing sending)
;
}
PORTA |= _BV(PA1);
PORTA &= ~_BV(PA0); // 使总线处于主机接收状态
_delay_ms(2); //送完成后，总线会被从机占用，反馈应答数据，所以进行延时
}
void SetServoPosition(unsigned char id, unsigned short int position, unsigned short int
velocity)
{
unsigned short int temp_velocity = 0; // 临时速度，用于进行方向判别
unsigned short int temp_position = 0;
unsigned char temp_velocity_h = 0; // 待发送数据h 位
unsigned char temp_velocity_l = 0; // 待发送数据l 位
unsigned char temp_position_h = 0;
unsigned char temp_position_l = 0;
unsigned char temp_sum = 0; // 校验和寄存变量
if (velocity > 1023)
{
temp_velocity = 1023; // 限制速度值在可用范围内
}
else
{
temp_velocity = velocity;
}
if (position > 1023)
{
temp_position = 1023;
}
else
{
temp_position = position;
}
temp_velocity_h = (unsigned char)(temp_velocity >> 8);
// 将16bit 数据拆为2个8bit 数据
temp_velocity_l = (unsigned char)temp_velocity;
temp_position_h = (unsigned char)(temp_position >> 8);
// 将16bit 数据拆为2个8bit 数据
temp_position_l = (unsigned char)temp_position;
PORTA &= ~_BV(PA1);
PORTA |= _BV(PA0); // 使总线处于主机发送状态
UCSR0A |= (1<<TXC0); // 清除UART0写完成标志
SendUart0Byte(0xFF); // 发送启动符号0xFF
SendUart0Byte(0xFF);
SendUart0Byte(id); // 发送id
SendUart0Byte(7); // 发送数据长度为参数长度+2，参数长度为3
SendUart0Byte(0x03); // 命令数据为“WRITE DATA”
SendUart0Byte(0x1E); // 舵机控制寄存器首地址
SendUart0Byte(temp_position_l); // 发送速度数据低位
SendUart0Byte(temp_position_h); // 发送速度数据高位
SendUart0Byte(temp_velocity_l); //发送位置低字节
SendUart0Byte(temp_velocity_h); // 发送位置高字节
temp_sum = id + 7 + 0x03 + 0x1E + temp_position_l + temp_position_h + temp_velocity_l +
temp_velocity_h;
temp_sum = ~temp_sum; // 计算校验和
SendUart0Byte(temp_sum); // 发送校验和 (Send the checksum)
while ( !( UCSR0A & (1<<TXC0)) ) // 等待发送完成
{ // (Waiting for finishing sending)
;
}
PORTA |= _BV(PA1);
PORTA &= ~_BV(PA0); // 使总线处于主机接收状态
_delay_ms(2); // 发送完成后，总线会被从机占用，反馈应答数据，所以进行延时
}
void SetServoVelocity(unsigned char id, signed short int velocity)
{
unsigned char temp_sign = 0; // 临时符号，用于进行方向判别
unsigned short int temp_velocity = 0; // 临时速度，用于进行方向判别
unsigned char temp_value_h = 0; // 待发送数据h 位
unsigned char temp_value_l = 0; // 待发送数据l 位
unsigned char temp_sum = 0; // 校验和寄存变量
if (velocity < 0)
{
temp_velocity = -velocity; // 如果为负数，则取绝对值
temp_sign = 1; // 设置负数符号标志
}
else
{
temp_velocity = velocity;
temp_sign = 0; // 设置正数符号标志
}
if (temp_velocity > 1023)
{
temp_velocity = 1023; // 限制速度值在可用范围内
}
temp_velocity |= (temp_sign << 10);
temp_value_h = (unsigned char)(temp_velocity >> 8);
// 将16bit 数据拆为2个8bit 数据
temp_value_l = (unsigned char)temp_velocity;
PORTA &= ~_BV(PA1);
PORTA |= _BV(PA0); // 使总线处于主机发送状态
UCSR0A |= (1<<TXC0); // 清除UART0写完成标志
SendUart0Byte(0xFF); // 发送启动符号0xFF
SendUart0Byte(0xFF); // 发送启动符号0xFF
SendUart0Byte(id); // 发送id
SendUart0Byte(5); // 发送数据长度为参数长度+2，参数长度为3
SendUart0Byte(0x03); // 命令数据为“WRITE DATA”
SendUart0Byte(0x20); // 舵机控制寄存器首地址
SendUart0Byte(temp_value_l); // 发送速度数据低位
SendUart0Byte(temp_value_h); // 发送速度数据高位
temp_sum = id + 5 + 0x03 + 0x20 + temp_value_l + temp_value_h;
temp_sum = ~temp_sum; // 计算校验和
SendUart0Byte(temp_sum); // 发送校验和
while ( !( UCSR0A & (1<<TXC0)) ) // 等待发送完成
{
;
}
PORTA |= _BV(PA1);
PORTA &= ~_BV(PA0); // 使总线处于主机接收状态
_delay_ms(2); // 发送完成后，总线会被从机占用，反馈应答数据，所以进行延时
}
int main(void)
{
InitUart0();
SetServoLimit(2,0,1023);
while(1)
{
_delay_ms(1000); //延时1s
SetServoPosition(2, 1000, 500); //控制舵机以500的速度运动到1000的位置
_delay_ms(1000); //延时1s
SetServoPosition(2, 200, 100); //控制舵机以100的速度运动到200的位置

}
}

longwu537 发表于 2012-10-19 13:09:06

这个很不错，应该对很多人有帮助吧

mandylion2008 发表于 2015-11-28 16:48:18

最近正在玩这个，下载了，感谢楼主奉献！

xiangbin099 发表于 2016-3-3 22:23:27

谢谢分享，路过看看看看

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

Dynamixel数字舵机驱动