ARM Linux控制CDS5516

/*
* ServoCDS55XX.h
*
*  Created on: Sep 28, 2013
*      Author: wgh
*/

#ifndef SERVOCDS55XX_H
#define SERVOCDS55XX_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <string.h>
#include <stdarg.h>
#include <sys/ioctl.h>
#include <sys/cdefs.h>
typedef unsigned char uchar;
typedef unsigned int uint;

#define CFG_PRINT_WRITE
#define CFG_PRINT_FUNC
/*
* 下边所有函数中的使用的buf参数都需要在外部定义,参考具体指令的长度,buf的长度要取可能
* 用到的最长指令的长度,所有函数中的ms请按具体时间定义,(write_port中默认有1s延时...)
*/

void sleep(int ms);

int open_port(const char *name);

int write_port(int fd, const uchar *data, int len);
int read_port(int fd, const uchar *data, int len);

//int write_servo(int fd, const uchar *data, int len);
//int read_servo(int fd, const uchar *data, int len);
int parse_servo_msg(const uchar *data, int len);

int cfg_port(int fd, int nSpeed, int nBits, char nEvent, int nStop);

int read_servo_ct(int fd, uchar *buf, int ms, uchar id,  uchar addr, uchar len);
int read_servo_baudrate(int fd, uchar *buf, int ms, uchar id);
int read_servo_temperature(int fd, uchar *buf, int ms, uchar id);
int read_servo_position(int fd, uchar *buf, int ms, uchar id);

int write_servo_op(int fd, uchar *buf, int ms, uchar id, uchar op, uchar len, ...);
int write_servo_ct(int fd, uchar *buf, int ms, uchar id, uchar addr, uchar len, ...);
int write_servo_reg(int fd, uchar *buf, int ms, uchar id, uchar addr, uchar len, ...);
int write_servo_sync(int fd, uchar *buf, int ms, uchar servoCnt, uchar addr, uchar len, ...);

int action_servo(int fd, uchar *buf, int ms, uchar id);
int reset_servo(int fd, uchar *buf, int ms, uchar id);
int ping_servo(int fd, uchar *buf, int ms, uchar id);

int set_servo_id(int fd, uchar *buf, int ms, uchar id);
int set_servo_clockwise_limit(int fd, uchar *buf, int ms, uchar id, uchar cll, uchar clh);
int set_servo_anticlockwise_limit(int fd, uchar *buf, int ms, uchar id, uchar all, uchar alh);
int set_servo_degree_limit(int fd, uchar *buf, int ms, uchar id, uint cdl, uint adl);
int set_servo_position(int fd, uchar *buf, int ms, uchar id, uint pos, uint vel);
int set_servo_motorMode(int fd, uchar *buf, int ms, uchar id);
int set_servo_speed(int fd, uchar *buf, int ms, uchar id, uint speed);

#endif

/*
* ServoCDS55XX.cpp
*
*  Created on: Sep 28, 2013
*      Author: wgh
*/

#include "ServoCDS55XX.h"

#ifdef CFG_PRINT_FUNC
#define PRINT_FUNC_MSG(id, ms)	\
printf("%s : id(%d) delay(%d)....\n", __func__, id, ms)
#else
#define PRINT_FUNC_MSG
#endif

void sleep(int ms)
{
//struct timespec ts = {
//	ms/1000, (ms%1000)*1000*1000 };
//nanosleep(&ts, NULL);
usleep(1000*ms);
}

int open_port(const char *name)
{
int fd;
printf("open device: %s\n", name);
fd = open(name, O_RDWR /*| O_NOCTTY | O_NDELAY*/);
if(fd < 0){
printf("can't open %s!!!\n", name);
exit(1);
}else{
//fcntl(fd, F_SETFL, 0);
}
return fd;
}
int read_port(int fd, const uchar *data, int len)
{
return read(fd, (void*)data, len);
}
int write_port_one_byte(int fd, uchar data)
{
uchar buff[1];
buff[0] = data;
return write(fd, buff, 1);
//	sleep(500);
}
int write_port(int fd, const uchar *data, int len)
{
#ifdef CFG_PRINT_WRITE
int sum = 0;
printf("write(%d): ", len);
for(int i=0; i<len; i++){
printf("%2x, ", data[i]);
sum += write_port_one_byte(fd, data[i]);
}
printf(".......%d\n", sum);
#endif
//    write(fd, data, len);
sleep(1000);
return sum;
}
/*
* cfg_port : 配置串口设置,目前只支持115200,1000000
*/
int cfg_port(int fd, int nSpeed, int nBits, char nEvent, int nStop)
{
printf("cfg_port : fd(%d), nSpeed(%d), nBits(%d), nEvent(%c), nStop(%d)..\n",
fd, nSpeed, nBits, nEvent, nStop);
struct termios newOpt, oldOpt;
if(0 != tcgetattr(fd, &oldOpt)){			/* get the current options for the port */
perror("tcgetttr error...\n");
exit(1);
}
bzero(&newOpt, sizeof(newOpt));
newOpt.c_cflag |= CLOCAL | CREAD;/* enable the receiver and set local mode */
newOpt.c_cflag &= ~CSIZE;			/* mask the character size bits */

switch(nBits){
case 7:
newOpt.c_cflag |= CS7;
break;
case 8:
newOpt.c_cflag |= CS8;				/* select 8 data bits */
break;
default:
perror("bad nBits!...\n");
exit(1);
}

switch(nEvent){
case 'O':	/* odd */
newOpt.c_cflag |= PARENB;
newOpt.c_cflag |= PARODD;
newOpt.c_iflag |= (INPCK | ISTRIP);
break;
case 'E':
newOpt.c_cflag |= PARENB;
newOpt.c_cflag &= ~PARODD;
newOpt.c_iflag |= (INPCK | ISTRIP);
break;
case 'N':
newOpt.c_cflag &= ~PARENB;
//            newOpt.c_cflag &= ~INPCK;	/* enable parity checking */
break;
default:
perror("bad nEvent!...\n");
exit(1);
}
switch(nSpeed){
case 115200:
cfsetispeed(&newOpt, B115200);
cfsetospeed(&newOpt, B115200);
break;
case 1000000:
cfsetispeed(&newOpt, B1000000);
cfsetospeed(&newOpt, B1000000);
break;
default:
perror("bad baudrate...\n");
exit(1);
}
if(nStop == 1){
newOpt.c_cflag &= ~CSTOPB;
}else{
newOpt.c_cflag |= CSTOPB;
}
////newOpt.c_cflag &= CNEW_RTSCTS;					/* disable hardware flow control */
//newOpt.c_iflag &= ~(IXON | IXOFF | IXANY);		/* disable software flow control */
//    newOpt.c_oflag &= ~OPOST;  							/* choosing raw output */
//    newOpt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);	/* choosing raw input */

newOpt.c_cc[VTIME] = 150;
newOpt.c_cc[VMIN] = 0;

tcflush(fd, TCIFLUSH);
if(0 != tcsetattr(fd, TCSANOW, &newOpt)){	/* set the new options for the port */
/* other arguments : TCSADRAIN TCSAFLUSH */
perror("tcsetattr error!...\n");
exit(1);
}
printf("cfg port ok!...\n");
return 0;
}

//int read_servo(int fd, const uchar *data, int len)
//{
//    int ret = read_port(fd, data, len);
//    sleep(100);
//    return ret;
//}
//
//int write_servo(int fd, const uchar *data, int len)
//{
//    int ret = write_port(fd, data, len);
//    sleep(500);
//    return ret;
//}
/*
* parse_servo_msg :
*/
int parse_servo_msg(const uchar *data, int len)
{
for(int i=0; i<len; i++)
printf(" %2x,", data[i]);
printf("\n");
return 0;
}
int set_servo_id(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_ct(fd, buf, ms, 0xfe, 0x03, 0x01, id);
}
/*
* read_servo_ct:读舵机内存控制表
*/
int read_servo_ct(int fd, uchar *buf, int ms, uchar id, uchar addr, uchar len)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_op(fd, buf, ms, id, 0x02, 0x02, addr, len);
}
int read_servo_baudrate(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
return read_servo_ct(fd, buf, ms, id, 0x04, 0x01);
}
int read_servo_temperature(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
return read_servo_ct(fd, buf, ms, id, 0x2b, 0x01);
}
int read_servo_position(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
return read_servo_ct(fd, buf, ms, id, 0x24, 0x02);
}

int write_servo_op(int fd, uchar *buf, int ms, uchar id, uchar op, uchar len, ...)
{
PRINT_FUNC_MSG(id, ms);
int ret = 0;
buf[0] = 0xff;
buf[1] = 0xff;
buf[2] = id;
buf[3] = len+2;
buf[4] = op;
int i;
va_list vl;
va_start(vl, len);
for(i=0; i<len; i++){
buf[5+i] = (uchar)va_arg(vl, int);
}
va_end(vl);
buf[5+len] = 0;
for(int j=2; j<5+len; j++)
buf[5+len] += buf[j];
buf[5+len] = ~buf[5+len];
ret = write_port(fd, buf, 6+len);
sleep(ms);
return ret;
}
/*
* 写舵机内存控制表
*/
int write_servo_ct(int fd, uchar *buf, int ms, uchar id, uchar addr, uchar len, ...)
{
//	ret = write_servo_op(fd, buf, ms, id, 0x03, len+1, addr, vl);
PRINT_FUNC_MSG(id, ms);
int ret = 0;
buf[0] = 0xff;
buf[1] = 0xff;
buf[2] = id;
buf[3] = len+3;
buf[4] = 0x03;
buf[5] = addr;
int i;
va_list vl;
va_start(vl, len);
for(i=0; i<len; i++){
buf[6+i] = (uchar)va_arg(vl, int);
}
va_end(vl);
buf[6+len] = 0;
for(int j=2; j<6+len; j++)
buf[6+len] += buf[j];
buf[6+len] = ~buf[6+len];
ret = write_port(fd, buf, 7+len);
sleep(ms);
return ret;
}

int write_servo_reg(int fd, uchar *buf, int ms, uchar id, uchar addr, uchar len, ...)
{
PRINT_FUNC_MSG(id, ms);
int ret = 0;
buf[0] = 0xff;
buf[1] = 0xff;
buf[2] = id;
buf[3] = len+3;
buf[4] = 0x04;
buf[5] = addr;
int i;
va_list vl;
va_start(vl, len);
for(i=0; i<len; i++){
buf[6+i] = (uchar)va_arg(vl, int);
}
va_end(vl);
buf[6+len] = 0;
for(int j=2; j<6+len; j++)
buf[6+len] += buf[j];
buf[6+len] = ~buf[6+len];
ret = write_port(fd, buf, 7+len);
sleep(ms);
return ret;
}
/*
* 同步写指令
*/
int write_servo_sync(int fd, uchar *buf, int ms, uchar cnt, uchar addr, uchar len, ...)
{
PRINT_FUNC_MSG(0xfe, ms);
int ret = 0;
buf[0] = 0xff;
buf[1] = 0xff;
buf[2] = 0xfe;
buf[3] = (len+1)*cnt+4;
buf[4] = 0x83;
buf[5] = addr;
buf[6] = len;
int i;
va_list vl;
va_start(vl, len);
for(i=0; i<(len+1)*cnt; i++){
buf[7+i] = (uchar)va_arg(vl, int);
}
va_end(vl);
buf[7+(len+1)*cnt] = 0;
for(int j=2; j<7+(len+1)*cnt; j++)
buf[7+(len+1)*cnt] += buf[j];
buf[7+(len+1)*cnt] = ~buf[7+(len+1)*cnt];
ret = write_port(fd, buf, 8+(len+1)*cnt);
sleep(ms);
return ret;
}
int action_servo(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_op(fd, buf, ms, id, 0x05, 0x00);
}
/*
* reset_servo将初始化舵机的波特率为1M,不要使用
*/
int reset_servo(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
printf("warning : reset servo(%d)...!!!!!\n", id);
return write_servo_op(fd, buf, ms, id, 0x06, 0x00);
}
/*
* 查询舵机当前状态
*/
int ping_servo(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_op(fd, buf, ms, id, 0x01, 0x00);
}

int set_servo_clockwise_limit(int fd, uchar *buf, int ms, uchar id, uchar cll, uchar clh)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_ct(fd, buf, ms, id, 0x06, 0x02, cll, clh);
}

int set_servo_anticlockwise_limit(int fd, uchar *buf, int ms, uchar id, uchar all, uchar alh)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_ct(fd, buf, ms, id, 0x08, 0x02, all, alh);
}

int set_servo_degree_limit(int fd, uchar *buf, int ms, uchar id, uint cdl, uint adl)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_ct(fd, buf, ms, id, 0x06, 0x04,
(uchar)(cdl), (uchar)(cdl>>8), (uchar)(adl), (uchar)(adl>>8));
}

int set_servo_position(int fd, uchar *buf, int ms, uchar id, uint pos, uint vel)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_ct(fd, buf, ms, id, 0x1e, 0x04,
(uchar)(pos), (uchar)(pos>>8), (uchar)(vel), (uchar)(vel>>8));
}

int set_servo_motorMode(int fd, uchar *buf, int ms, uchar id)
{
PRINT_FUNC_MSG(id, ms);
return set_servo_degree_limit(fd, buf, ms, id, 0x00, 0x00);
}
int set_servo_speed(int fd, uchar *buf, int ms, uchar id, uint speed)
{
PRINT_FUNC_MSG(id, ms);
return write_servo_ct(fd, buf, ms, id, 0x20, 0x02, (uchar)(speed), (uchar)(speed>>8));
}

#include <iostream>
#include "ServoCDS55XX.h"
using namespace std;

#define DEFAULT_PORT "/dev/ttySAC1"
#define DEFAULT_ID      254
#define DEFAULT_DELAY 0

#define TEST(str, func) \
printf("...test %s..\n", str);\
cnt = 3;\
do{\
func;\
if((len = read_port(fd, buf2, 1024))){\
printf("(%d)read(%d): ", cnt, len);\
parse_servo_msg(buf2, len);\
}\
cnt--;\
}while(cnt && len<=11)

int main(int argc, char **argv)
{
int i, len, cnt;
int fd;
uchar id;
uchar buf[1024], buf2[1024];
int pos;
memset(buf2, 0, sizeof(buf2));
if(argc == 3){
fd = open_port(argv[1]);
id = (uchar)atoi(argv[2]);
}else if(argc == 2){
fd = open_port(DEFAULT_PORT);
id = (uchar)atoi(argv[1]);
}
printf("servo id = %d\n", id);
cfg_port(fd, 115200, 8, 'N', 1);
//    cfg_port(fd, 1000000, 8, 'N', 1);

TEST("degree", set_servo_degree_limit(fd, buf, 2500, id, 0x00, 0x3ff));
//    TEST("pos", set_servo_position(fd, buf, 2500, id, 0x01ff, 0x0200));
//    sleep(10000);
//    TEST("set motor mode", set_servo_motorMode(fd, buf, 2500, id));
//    TEST("set motor speed", set_servo_speed(fd, buf, 2500, id, 0x2ff));
//    sleep(10000);
for(int i=0; i<10; i++){
printf("-----------%d-----------\n", i);
TEST("pos", set_servo_position(fd, buf, 2500, id, 0x02ff, 0x0200));
TEST("pos", set_servo_position(fd, buf, 2500, id, 0x0000, 0x0200));
}
//    TEST("set motor speed", set_servo_speed(fd, buf, 1000, id, 0x00));
close(fd);
return 0;
}