数字舵机驱动程序 半双工可读

用数字舵机的人少啊，太贵了，没经费支持一般人真的伤不起。所以网上可供参考的代码也很难找。下面是我死磕数据手册后写出来的数字舵机驱动程序，我们用的是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();
}
}