日志
數字舵機驅動程序 半雙工可讀
熱度 2 |
用數字舵機的人少啊，太貴了，沒經費支持一般人真的傷不起。所以網上可供參考的代碼也很難找。下面是我死磕數據手冊后寫出來的數字舵機驅動程序，我們用的是AX-12牌子的，但一般的總線舵機協議都一樣，寄存器表一大同小異，多以也可以用在其他數字舵機上。
AX-12提供的是單線半雙工串口。要想既能發送數據又能讀取數據，有兩個辦法。一是在電路上改進，這個電路很簡單，在AX-12舵機數據手冊的前面就有介紹。二是將串口配置為半雙工模式。第二只方法更簡單。我就是用這種方法。在我的上一篇博客中就有介紹STM32的半雙工串口的配置方法。
另外，我的舵機驅動是用在機器人上的，多以其功能不能只是讓一個或多個舵機轉到指定角度，還要讓所有舵機協調合作，讓機器人完成一個連續的動作。這就要將所有的數據按規定的格式存放，并按規定的時間發送出去。我的數據是一幀一幀組成的，每一幀代表一個動作，多幀連起來就是一個連續的動作。而每一幀動作所需的時間則放在這一幀的頭部。發送這一幀后將延時那么長時間，否則舵機將沒有時間去完成這一幀的動作就被要求執行下一個動作。
#include "duoji.h"
#include "usart.h"
u16 frames[SIZE][13] = {
{200, 512, 80, 512, 80, 512, 80, 512, 80, 512, 80, 512, 80},
{200, 512, 80, 512, 80, 512, 80, 512, 80, 512, 80, 512, 80}
};
//移動一個舵機, 參數：id，目標位置，運動速度
void move_one_duoji(u8 id, u16 angle, u16 speed)
{
u8 data_buf[13];
u8 i = 0;
data_buf[0] = 0xff;
data_buf[1] = 0xff;
data_buf[2] = 0xfe;
data_buf[3] = 0x09;
data_buf[4] = 0x83;
data_buf[5] = 0x1e;
data_buf[6] = 0x04;
data_buf[7] = id;
data_buf[8] = (u8)(angle & 0x00ff);
data_buf[9] = (u8)((angle >> 8) & 0x00ff);
data_buf[10] = (u8)(speed & 0x00ff);
data_buf[11] = (u8)((speed >> 8) & 0x00ff);
data_buf[12] = 0;
for(i=2; i<12; i++)
data_buf[12] += data_buf[ i];
data_buf[12] = ~data_buf[12];
send_bytes(data_buf, 13);
// send_bytes(data_buf, 13);
}
//發送一幀數據
void send_one_frame(const u16 *frame_buf)
{
u8 data_buf[38];
u8 i = 0;
data_buf[0] = 0xff;
data_buf[1] = 0xff;
data_buf[2] = 0xfe;
data_buf[3] = 34;////////
data_buf[4] = 0x83;
data_buf[5] = 0x1e;
data_buf[6] = 0x04;
for(i=0; i<6; i++)
{
data_buf[5*i+7] = i+1;
data_buf[5*i+8] = (u8)(frame_buf[2*i+1] & 0x00ff);
data_buf[5*i+9] = (u8)((frame_buf[2*i+1] >> 8) & 0x00ff);
data_buf[5*i+10] = (u8)(frame_buf[2*i+2] & 0x00ff);
data_buf[5*i+11] = (u8)((frame_buf[2*i+2] >> 8) & 0x00ff);
}
data_buf[37] = 0;
for(i=2; i<37; i++)
data_buf[37] += data_buf[ i];
data_buf[37] = ~data_buf[37];
// send_bytes(data_buf, 38);
// send_bytes(data_buf, 38);
send_bytes(data_buf, 38);
}
//發送連續幀，執行一個動作
void send_one_action(u16 action[][13], u8 num)
{
int i = 0;
for(i=0; i<num; ++i)
{
// send_one_frame(action[ i]);
send_one_frame(action[ i]);
delay_ms((action[ i][0]));
}
}
//讀一個舵機角度
u16 read_one_duoji(u8 id)
{
u8 databuf[8];
u8 i = 0;
u16 angle;
//發送讀指令
databuf[0] = 0xff;
databuf[1] = 0xff;
databuf[2] = id;
databuf[3] = 0x04; //Length=4
databuf[4] = 0x02; //READ DATA
databuf[5] = 0x24; //Present Position(L) Adress
databuf[6] = 0x02; //
databuf[7] = 0;
for (i = 2; i < 7; i++)
{
databuf[7] += databuf[ i];
}
databuf[7] = (u8) (~databuf[7]);
send_bytes(databuf, 8);
// delay_ms(1);
memset(databuf, 0, 8);
read_bytes(databuf, 8);
// for(i=0; i<8; ++i)
// printf("buf[%d] = %d ", i, databuf[ i]);
// printf("\r\n");
angle = ((u16)(databuf[6])<<8) + (u16)(databuf[5]);
// printf("angle=%d\n", angle);
return angle;
}
//讀取所有舵機，我這里有6個舵機
void read_all_duoji(u16 *angle)
{
u16 i = 1;
for(i=1; i<=6; ++i){
// printf("read duoji %d\r\n", i);
angle[i-1] = read_one_duoji(i);
}
}
//激活扭矩 status: 0->卸力， 1->激活扭矩
void torque(u8 status)
{
u8 finaldata[20];
u8 i;
finaldata[0] = 0xff;
finaldata[1] = 0xff;
finaldata[2] = 0xfe;
finaldata[3] = 16;
finaldata[4] = 0x83;
finaldata[5] = 0x18;
finaldata[6] = 0x01;
for(i=0; i<6; ++i)
{
finaldata[i*2+7] = i+1;
finaldata[i*2+8] = status;
}
finaldata[19] = 0;
for (i = 2; i < 19; i++)
{
finaldata[19] += finaldata[ i];
}
finaldata[19] = (u8) (~finaldata[19]);
send_bytes(finaldata, 20);
}
//讀moving
u8 read_moving(u8 id)
{
u8 databuf[8];
u8 i = 0;
u8 moving;
//發送讀指令
databuf[0] = 0xff;
databuf[1] = 0xff;
databuf[2] = id;
databuf[3] = 0x04; //Length=4
databuf[4] = 0x02; //READ DATA
databuf[5] = 0x2e; //Adress
databuf[6] = 0x01; //
databuf[7] = 0;
for (i = 2; i < 7; i++)
{
databuf[7] += databuf[ i];
}
databuf[7] = (u8) (~databuf[7]);
send_bytes(databuf, 8);
memset(databuf, 0, 7);
read_bytes(databuf, 7);
moving = databuf[5];
return moving;
}
//讀允許電壓范圍
void read_power_rang(u8 id)
{
u8 databuf[8];
u8 i = 0;
u8 lower_rang;
u8 upper_rang;
//發送讀指令
databuf[0] = 0xff;
databuf[1] = 0xff;
databuf[2] = id;
databuf[3] = 0x04; //Length=4
databuf[4] = 0x02; //READ DATA
databuf[5] = 0x0c; //Adress
databuf[6] = 0x02; //
databuf[7] = 0;
for (i = 2; i < 7; i++)
{
databuf[7] += databuf[ i];
}
databuf[7] = (u8) (~databuf[7]);
send_bytes(databuf, 8);
memset(databuf, 0, 8);
read_bytes(databuf, 8);
lower_rang = databuf[5];
upper_rang = databuf[6];
printf("id %d: error = %d lower_rang = %d upper_rang = %d\r\n", id, databuf[4], lower_rang, upper_rang);
}
//設置允許電壓范圍
void set_power_rang(u8 lower, u8 upper)
{
u8 finaldata[26];
u8 i;
finaldata[0] = 0xff;
finaldata[1] = 0xff;
finaldata[2] = 0xfe;
finaldata[3] = 22; //
finaldata[4] = 0x83;
finaldata[5] = 0x0c;
finaldata[6] = 0x02;
for (i = 0; i < 6; i++)
{
finaldata[i * 3 + 7] = i + 1;; //ID
finaldata[i * 3 + 8] = lower;
finaldata[i * 3 + 9] = upper;
}
finaldata[25] = 0;
for (i = 2; i < 25; i++)
{
finaldata[25] += finaldata[ i];
}
finaldata[25] = (u8) (~finaldata[25]);
send_bytes(finaldata, 26);
}
//讀取最大扭矩
void read_max_torque(u8 id)
{
u8 databuf[8];
u8 i = 0;
u16 max_torque;
//發送讀指令
databuf[0] = 0xff;
databuf[1] = 0xff;
databuf[2] = id;
databuf[3] = 0x04; //Length=4
databuf[4] = 0x02; //READ DATA
databuf[5] = 0x0e; //Adress
databuf[6] = 0x02; //
databuf[7] = 0;
for (i = 2; i < 7; i++)
{
databuf[7] += databuf[ i];
}
databuf[7] = (u8) (~databuf[7]);
send_bytes(databuf, 8);
memset(databuf, 0, 8);
read_bytes(databuf, 8);
max_torque = ((u16)(databuf[6])<<8) + (u16)(databuf[5]);
printf("id %d: error = %d max_torque = %d\r\n",id, databuf[4], max_torque);
}
void set_max_torque(u16 torque)
{
u8 finaldata[26];
u8 i;
finaldata[0] = 0xff;
finaldata[1] = 0xff;
finaldata[2] = 0xfe;
finaldata[3] = 22; //
finaldata[4] = 0x83;
finaldata[5] = 0x0e;
finaldata[6] = 0x02;
for (i = 0; i < 6; i++)
{
finaldata[i * 3 + 7] = i + 1;; //ID
finaldata[i * 3 + 8] = (u8)(torque & 0x00ff);
finaldata[i * 3 + 9] = (u8)((torque>>8) & 0x00ff);
}
finaldata[25] = 0;
for (i = 2; i < 25; i++)
{
finaldata[25] += finaldata[ i];
}
finaldata[25] = (u8) (~finaldata[25]);
send_bytes(finaldata, 26);
}
void read_return_time(u8 id)
{
u8 databuf[8];
u8 i = 0;
u16 return_time;
//發送讀指令
databuf[0] = 0xff;
databuf[1] = 0xff;
databuf[2] = id;
databuf[3] = 0x04; //Length=4
databuf[4] = 0x02; //READ DATA
databuf[5] = 0x05; //Adress
databuf[6] = 0x01; //
databuf[7] = 0;
for (i = 2; i < 7; i++)
{
databuf[7] += databuf[ i];
}
databuf[7] = (u8) (~databuf[7]);
send_bytes(databuf, 8);
memset(databuf, 0, 7);
read_bytes(databuf, 7);
return_time = databuf[5]*2;
printf("id %d: error = %d return_time = %d\r\n", id,databuf[4], return_time);
}
//設置返回時間
void set_return_time(u16 time)
{
u8 time_data = time/2;
u8 finaldata[20];
u8 i;
finaldata[0] = 0xff;
finaldata[1] = 0xff;
finaldata[2] = 0xfe;
finaldata[3] = 16;
finaldata[4] = 0x83;
finaldata[5] = 0x05; //Torque Enable. Adress
finaldata[6] = 0x01;
for(i=0; i<6; ++i)
{
finaldata[i*2+7] = i+1;
finaldata[i*2+8] = time_data;
}
finaldata[19] = 0;
for (i = 2; i < 19; i++)
{
finaldata[19] += finaldata[ i];
}
finaldata[19] = (u8) (~finaldata[19]);
send_bytes(finaldata, 20);
}
//讀取狀態返回水平
void read_return_status(u8 id)
{
u8 databuf[8];
u8 i = 0;
//發送讀指令
databuf[0] = 0xff;
databuf[1] = 0xff;
databuf[2] = id;
databuf[3] = 0x04; //Length=4
databuf[4] = 0x02; //READ DATA
databuf[5] = 0x10; //Adress
databuf[6] = 0x01; //
databuf[7] = 0;
for (i = 2; i < 7; i++)
{
databuf[7] += databuf[ i];
}
databuf[7] = (u8) (~databuf[7]);
send_bytes(databuf, 8);
memset(databuf, 0, 7);
read_bytes(databuf, 7);
printf("id %d: error = %d return_status = %d\r\n", id, databuf[4], databuf[5]);
}
//設置狀態返回水平
void set_return_status(u8 status)
{
u8 finaldata[20];
u8 i;
finaldata[0] = 0xff;
finaldata[1] = 0xff;
finaldata[2] = 0xfe;
finaldata[3] = 16;
finaldata[4] = 0x83;
finaldata[5] = 0x10; //Torque Enable. Adress
finaldata[6] = 0x01;
for(i=0; i<6; ++i)
{
finaldata[i*2+7] = i+1;
finaldata[i*2+8] = status;
}
finaldata[19] = 0;
for (i = 2; i < 19; i++)
{
finaldata[19] += finaldata[ i];
}
finaldata[19] = (u8) (~finaldata[19]);
send_bytes(finaldata, 20);
}
//讀取旋轉余量和斜率
void read_margin_and_slope(u8 id)
{
u8 databuf[10];
u8 i = 0;
//發送讀指令
databuf[0] = 0xff;
databuf[1] = 0xff;
databuf[2] = id;
databuf[3] = 0x04; //Length=4
databuf[4] = 0x02; //READ DATA
databuf[5] = 0x1a; //Adress
databuf[6] = 0x04; //
databuf[7] = 0;
for (i = 2; i < 7; i++)
{
databuf[7] += databuf[ i];
}
databuf[7] = (u8) (~databuf[7]);
send_bytes(databuf, 8);
memset(databuf, 0, 10);
read_bytes(databuf, 10);
printf("id %d: 旋轉余量 = %d, %d 斜率 = %d, %d\r\n", id, databuf[5], databuf[6], databuf[7], databuf[8]);
}
//設置旋轉余量和斜率
void set_margin_and_slope(u8 margin, u8 slope)
{
u8 finaldata[38];
u8 i;
finaldata[0] = 0xff;
finaldata[1] = 0xff;
finaldata[2] = 0xfe;
finaldata[3] = 34;
finaldata[4] = 0x83;
finaldata[5] = 0x1a; //Torque Enable. Adress
finaldata[6] = 0x04;
for(i=0; i<6; ++i)
{
finaldata[i*5+7] = i+1;
finaldata[i*5+8] = margin;
finaldata[i*5+9] = margin;
finaldata[i*5+10] = slope;
finaldata[i*5+11] = slope;
}
finaldata[37] = 0;
for (i = 2; i < 37; i++)
{
finaldata[37] += finaldata[ i];
}
finaldata[37] = (u8) (~finaldata[37]);
send_bytes(finaldata, 38);
}
其中讀的函數是這樣寫的
//讀一個字節
u8 read_byte(void)
{
while(USART_GetFlagStatus(USART1,USART_FLAG_RXNE) == RESET);
USART_ClearFlag(USART1, USART_FLAG_RXNE);
return USART_ReceiveData(USART1);
}
//讀多個字節
void read_bytes(u8 *buf, u8 num)
{
u8 i = 0;
read_byte();
for(i=0; i<num; ++i)
{
buf[ i] = read_byte();
}
}
