xarm舵机id写入例程

主函数

#include "STC12C5A60S2.H"
#include "typedef.h"
#include "intrins.h"
#include "uart.h"
#include "LobotSerialServo.h"

void Delay1000ms() //@11.0592MHz
{
unsigned char i, j, k;

_nop_();
_nop_();
i = 43;
j = 6;
k = 203;
do
{
do
{
while (--k);
} while (--j);
} while (--i);
}

int main()
{
uartInit();
while(1)
{
LobotSerialServoSetID(254, 1);
Delay1000ms();
}
}

uart.c

#include "uart.h"
#include "LobotSerialServo.h"

#include <string.h>
void uartInit() //²¨ÌØÂÊ115200
{
PCON &= 0x7F; //??????
SCON = 0x50; //8???,?????
AUXR |= 0x04; //???????????Fosc,?1T
BRT = 0xFD; //?????????????
AUXR |= 0x01; //??1?????????????????
AUXR |= 0x10; //??????????
ES = 1; //??Uart??
EA = 1; //???
}

uint8_t busy = 0; //???????

/*********************************************************************************
* Function: uartWriteByte
* Description:??????????
* Parameters: dat:????
* Return: ???
* Others:
**********************************************************************************/
void uartWriteByte(uint8_t dat)
{
while (busy);
busy = 1;
SBUF = dat;
}

/*********************************************************************************
* Function: uartWriteBuf
* Description:??????
* Parameters: buf:???????,len:??????
* Return: ???
* Others:
**********************************************************************************/
void uartWriteBuf(uint8_t *buf, uint8_t len)
{
ES = 1;
while (len--) {
while (busy);
busy = 1;
SBUF = *buf++;
}
}

/*********************************************************************************
* Function: receiveHandle
* Description: ??????,??????,????
* Parameters: ?????
* Return: ???
* Others:
**********************************************************************************/

uint8_t UART_RX_BUF[24];
bool isUartRxCompleted = false;

void receiveHandle(void) interrupt 4
{
uint8_t rx;
static bool isGotFrameHeader = false;
static uint8_t frameHeaderCount = 0;
static uint8_t dataLength = 2;
static uint8_t dataCount = 0;

if (RI) { //????
RI = 0; //Çå³þ±êʶ
rx = SBUF; //¶ÁÈ¡½ÓÊռĴæÆ÷

if(!isGotFrameHeader)
{
if(rx == 0x55)
{
frameHeaderCount++;
if(frameHeaderCount == 2)
{
frameHeaderCount = 0;
isGotFrameHeader = true;
dataCount = 1;
}
}
else
{
isGotFrameHeader = false;
dataCount = 0;
frameHeaderCount = 0;
}
}
if(isGotFrameHeader)
{
UART_RX_BUF[dataCount] = rx;
if(dataCount == 3)
{
dataLength= UART_RX_BUF[dataCount];
if(dataLength < 3 || dataLength > 8)
{
dataLength = 3;
isGotFrameHeader = false;
}
}
dataCount++;
if(dataCount == dataLength + 3)
{
if(isUartRxCompleted == false)
{
isUartRxCompleted = true;
memcpy(LobotRxBuf, UART_RX_BUF, dataCount+2);
}
isGotFrameHeader = false;
}
}
}
if (TI) {
TI = 0;
busy = 0;
}
}

bool isRxCompleted(void)
{
if(isUartRxCompleted)
{
isUartRxCompleted = false;
return true;
}
else
{
return false;
}
}

LobotSerialServo.c

#include "uart.h"
#include "LobotSerialServo.h"
#include <stdarg.h>
#include <string.h>

#define LobotSerialWrite uartWriteBuf /*ÐÞ¸ÄuartWriteBufΪÄã×Ô¼ºµÄ´®¿Ú·¢Ëͺ¯Êý,Ãû×Ö¿ÉÒÔ×Ô¶¨Ò壬µ«º¯ÊýÔ­ÐÍÐèÓëÏÂÏàͬ*/
/*uartWriteBuf(uint8_t *buf, uint8_t len); º¯ÊýÔ­ÐÍ*/

#define GET_LOW_BYTE(A) ((uint8_t)(A))
//»ñÈ¡AµÄµÍ°Ëλ
#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8))
//»ñÈ¡AµÄ¸ß°Ëλ
#define BYTE_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
//½«¸ßµØ°ËλºÏ³ÉΪʮÁùλ

uint8_t LobotRxBuf[24];

//УÑé¼ÆËã
uint8_t LobotCheckSum(uint8_t buf[])
{
uint8_t i;
uint16_t temp = 0;
for (i = 2; i < buf[3] + 2; i++)
{
temp += buf[i];
}
temp = ~temp;
i = (uint8_t)temp;
return i;
}

//дÈë¶æ»úµÄID
void LobotSerialServoSetID(uint8_t oldID, uint8_t newID)
{
uint8_t buf[7];
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
buf[2] = oldID;
buf[3] = 4;
buf[4] = LOBOT_SERVO_ID_WRITE;
buf[5] = newID;
buf[6] = LobotCheckSum(buf);

LobotSerialWrite(buf, 7);
}

//Èöæ»úÔÚÖ¸¶¨Ê±¼äתµ½Ö¸¶¨Î»ÖÃ
void LobotSerialServoMove(uint8_t id, int16_t position, uint16_t time)
{
uint8_t buf[10];
if(position < 0)
position = 0;
if(position > 1000)
position = 1000;
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
buf[2] = id;
buf[3] = 7;
buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE;
buf[5] = GET_LOW_BYTE(position);
buf[6] = GET_HIGH_BYTE(position);
buf[7] = GET_LOW_BYTE(time);
buf[8] = GET_HIGH_BYTE(time);
buf[9] = LobotCheckSum(buf);

LobotSerialWrite(buf, 10);
}

//жÔØÁ¦¾Ø
void LobotSerialServoUnload(uint8_t id)
{
uint8_t buf[7];
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
buf[2] = id;
buf[3] = 4;
buf[4] = LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE;
buf[5] = 0;
buf[6] = LobotCheckSum(buf);

LobotSerialWrite(buf, 7);
}
//¼ÓÔØÁ¦¾Ø
void LobotSerialServoLoad(uint8_t id)
{
uint8_t buf[7];
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
buf[2] = id;
buf[3] = 4;
buf[4] = LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE;
buf[5] = 1;
buf[6] = LobotCheckSum(buf);

LobotSerialWrite(buf, 7);
}
//»ñµÃ¶æ»úµ±Ç°Î»ÖÃ
int LobotSerialServoReadPosition(uint8_t id)
{
int ret;
uint8_t buf[6];

buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
buf[2] = id;
buf[3] = 3;
buf[4] = LOBOT_SERVO_POS_READ;
buf[5] = LobotCheckSum(buf);

LobotSerialWrite(buf, 6); //·¢ËÍ»ñȡλÖõÄÃüÁî
ret = LobotSerialMsgHandle(); //»ñÈ¡½ÓÊÕµ½µÄÊý¾Ý

return ret; //·µ»Ø¶æ»úµÄλÖÃ
}

int LobotSerialMsgHandle(void)
{
uint8_t cmd;
int count = 10000;
int16_t ret;

while(!isRxCompleted()) //¼ì²éÊÇ·ñ½ÓÊÕµ½Êý¾Ý£¬Ã»ÓеĻ°Ò»Ö±µÈ´ý£¬ÖªµÀcountСÓÚ0£¬·µ»Ø -1
{
count--;
if(count < 0)
return -1;
}

if(LobotCheckSum(LobotRxBuf) != LobotRxBuf[LobotRxBuf[3]+2])
{
return -2; //½øÐÐУÑ飬УÑé²»¹ýÔò·µ»Ø-2
}
cmd = LobotRxBuf[4];
switch(cmd)
{
case LOBOT_SERVO_POS_READ:
ret = (int16_t)BYTE_TO_HW(LobotRxBuf[6], LobotRxBuf[5]); //ºÏ³É16λÊý¾Ý
return ret; //·µ»ØÊý¾Ý
default:
break;
}
return 0;
}