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_