好久沒更新技術類日志了，今天更新下，這是一個韓國robotics機器人Dynamixel數字舵機的驅動，采用C語言編寫，支持Dynamixel 通信協議 ，案例是AVR單片。實際上可以采用FPGA，例如SOPC系統上，可控制小型機器人。
在這也拆解下CM-5的控制器，是一片atmega128A 哦！哈哈。這PCB做工一般，感覺韓國人是科技領域爆發戶，但我們有什么資格嘲笑棒子呢，至少人家都能造CPU了，我們呢？
#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的位置
}
}
|
