ARM Linux控制CDS5516
来源：互联网 发布：amarra 4 for mac 编辑：程序博客网 时间：2024/06/06 20:31
蛋疼了这么久..最后竟然不用了,好忧伤....
/* * 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#endifvoid 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_WRITEint 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;}- ARM Linux控制CDS5516
- ARM linux进程控制相关API
- ARM Linux权限内存控制的实现
- arm-linux驱动：c文件操作控制驱动
- arm中断控制寄存器
- ARM控制流指令
- arm中断控制led
- arm 控制寄存器
- ARM按键控制LED
- Arm & Linux
- arm linux
- arm+linux
- ARM-Linux
- arm+Linux
- ARM TIMER 使用 控制 详解
- arm中断控制寄存器详解
- arm中断控制寄存器详解
- arm汇编控制led灯
- 哈密尔顿回路
- awk基础
- 2013南京网络赛 1002 Parade Show
- WEB测试--查看网页HTML源代码
- C语言strlen, strcpy, strcmp，strcat函数的实现
- ARM Linux控制CDS5516
- 程序员趣味读物：谈谈Unicode编码
- 信息检索笔记-词典及容错式检索
- JDBC基础
- 再说浏览器与服务器响应流程
- 第一次用c语言编写程序
- Linux下查看MySQL的安装路径
- JAVA事务操作
- oracle执行计划
