日志
數(shù)字舵機(jī)驅(qū)動(dòng)程序 半雙工可讀
熱度 2 |
用數(shù)字舵機(jī)的人少啊，太貴了，沒經(jīng)費(fèi)支持一般人真的傷不起。所以網(wǎng)上可供參考的代碼也很難找。下面是我死磕數(shù)據(jù)手冊后寫出來的數(shù)字舵機(jī)驅(qū)動(dòng)程序，我們用的是AX-12牌子的，但一般的總線舵機(jī)協(xié)議都一樣，寄存器表一大同小異，多以也可以用在其他數(shù)字舵機(jī)上。
AX-12提供的是單線半雙工串口。要想既能發(fā)送數(shù)據(jù)又能讀取數(shù)據(jù)，有兩個(gè)辦法。一是在電路上改進(jìn)，這個(gè)電路很簡單，在AX-12舵機(jī)數(shù)據(jù)手冊的前面就有介紹。二是將串口配置為半雙工模式。第二只方法更簡單。我就是用這種方法。在我的上一篇博客中就有介紹STM32的半雙工串口的配置方法。
另外，我的舵機(jī)驅(qū)動(dòng)是用在機(jī)器人上的，多以其功能不能只是讓一個(gè)或多個(gè)舵機(jī)轉(zhuǎn)到指定角度，還要讓所有舵機(jī)協(xié)調(diào)合作，讓機(jī)器人完成一個(gè)連續(xù)的動(dòng)作。這就要將所有的數(shù)據(jù)按規(guī)定的格式存放，并按規(guī)定的時(shí)間發(fā)送出去。我的數(shù)據(jù)是一幀一幀組成的，每一幀代表一個(gè)動(dòng)作，多幀連起來就是一個(gè)連續(xù)的動(dòng)作。而每一幀動(dòng)作所需的時(shí)間則放在這一幀的頭部。發(fā)送這一幀后將延時(shí)那么長時(shí)間，否則舵機(jī)將沒有時(shí)間去完成這一幀的動(dòng)作就被要求執(zhí)行下一個(gè)動(dòng)作。
#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}
};
//移動(dòng)一個(gè)舵機(jī), 參數(shù)：id，目標(biāo)位置，運(yùn)動(dòng)速度
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);
}
//發(fā)送一幀數(shù)據(jù)
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);
}
//發(fā)送連續(xù)幀，執(zhí)行一個(gè)動(dòng)作
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]));
}
}
//讀一個(gè)舵機(jī)角度
u16 read_one_duoji(u8 id)
{
u8 databuf[8];
u8 i = 0;
u16 angle;
//發(fā)送讀指令
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;
}
//讀取所有舵機(jī)，我這里有6個(gè)舵機(jī)
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;
//發(fā)送讀指令
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;
//發(fā)送讀指令
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);
}
//設(shè)置允許電壓范圍
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;
//發(fā)送讀指令
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;
//發(fā)送讀指令
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);
}
//設(shè)置返回時(shí)間
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);
}
//讀取狀態(tài)返回水平
void read_return_status(u8 id)
{
u8 databuf[8];
u8 i = 0;
//發(fā)送讀指令
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]);
}
//設(shè)置狀態(tài)返回水平
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);
}
//讀取旋轉(zhuǎn)余量和斜率
void read_margin_and_slope(u8 id)
{
u8 databuf[10];
u8 i = 0;
//發(fā)送讀指令
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: 旋轉(zhuǎn)余量 = %d, %d 斜率 = %d, %d\r\n", id, databuf[5], databuf[6], databuf[7], databuf[8]);
}
//設(shè)置旋轉(zhuǎn)余量和斜率
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);
}
其中讀的函數(shù)是這樣寫的
//讀一個(gè)字節(jié)
u8 read_byte(void)
{
while(USART_GetFlagStatus(USART1,USART_FLAG_RXNE) == RESET);
USART_ClearFlag(USART1, USART_FLAG_RXNE);
return USART_ReceiveData(USART1);
}
//讀多個(gè)字節(jié)
void read_bytes(u8 *buf, u8 num)
{
u8 i = 0;
read_byte();
for(i=0; i<num; ++i)
{
buf[ i] = read_byte();
}
}
