学步园

蛋疼了这么久..最后竟然不用了,好忧伤....

/*
 * 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;
}