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

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


你可能感兴趣的:(stm32,数字舵机,半双工,机器人,stm32)