基于导航定位的多机器人协作研究 阶段性成果

工作进行到几天为止终于取得了阶段性的成果，首先是PC和mini2440进行无线网络通信，再次是mini2440和单片机进行串口通信，最后是单片机输出引脚控制电机驱动电路板，控制电机运动。代码：

#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>

#include     <stdio.h>
#include     <stdlib.h> 
#include     <unistd.h>  
#include     <sys/types.h>
#include     <sys/stat.h>
#include     <fcntl.h> 
#include     <termios.h>
#include     <errno.h>
#include 		 <string.h>

#define SERIAL_PORT			"/dev/ttySAC1"	//串口地址
#define PORT_SPEED			9600		//串口波特率
#define	DATABITS			8		//数据位
#define STOPBITS			1		//停止位
#define	PARITY				'n'		//校验方式 （不校验）

int speed_arr[] = {B38400, B19200, B9600, B4800, B2400, B1200, B300,B38400, B19200, B9600, B4800, B2400, B1200, B300};
int name_arr[] = {38400,  19200,  9600,  4800,  2400,  1200,  300, 38400,  19200,  9600, 4800, 2400, 1200, 300};

//int setSpeed(int, int, struct &(termios));
//int setParity(int, int, int, int, struct &(termios));
int openPort(void);
int init(int ,int  ,char ,int);
void readPort(int);

int main(int argc,char *argv[])
{
//下面是串口的初始化
	char *quit = (char*)malloc(sizeof(char*));
	int fd;
       
	int j=0;
	char send_buf1[]="11111111";
	char send_buf2[]="22222222";

    char write_buf[256];

	fd = init(9600,8,'n',1);//初始化端口设置
	
	printf("configure complete\n"); 
  printf("start send and receive data...\n");

//下面是网络初始化
	int sockfd,i;
	char buffer[1024];
	struct sockaddr_in server_addr;
	struct hostent *host;
	int portnumber,nbytes;
	char *bufpoint;
	int buflength;
	//char command[]="1111";
	char command1[]={'1','1','1','1','\0'};
	char command2[]={'2','2','2','2','\0'};
	if(argc!=3)
	{
		fprintf(stderr,"Usage:%s hostname portnumber\a\n",argv[0]);
		exit(1);
	}
	if((host=gethostbyname(argv[1]))==NULL)
	{
		fprintf(stderr,"gethostname error\n");
		exit(1);
	}
	if((portnumber=atoi(argv[2]))<0)
	{
		fprintf(stderr,"Usage :%s hostname portnumber\a\n",argv[0]);
		exit(1);
	}
	if((sockfd=socket(AF_INET,SOCK_STREAM,0))==-1)
	{
		fprintf(stderr,"socket error:%s \a\n",strerror(errno));
		exit(1);
	}
	bzero(&server_addr,sizeof(server_addr));
	server_addr.sin_family=AF_INET;
	server_addr.sin_port=htons(portnumber);
	server_addr.sin_addr=*((struct in_addr *)host->h_addr);
	//server_addr.sin_addr=*((struct in_addr *)("10.1.20.109"));
	if(connect(sockfd,(struct sockaddr *)(&server_addr),sizeof(struct sockaddr))==-1)
	{
		fprintf(stderr,"connect error:%s \a\n",strerror(errno));
		exit(1);
	}

	while(1)
	{
		//for(j=0;j<100000000;j++);
	    //write(fd,send_buf,1);

		if((nbytes=read(sockfd,buffer,1024))==-1)
		{
			fprintf(stderr,"read error:%s\n",strerror(errno));
			//exit(1);
			//buffer="error string!":
		}
		buffer[nbytes]='\0';
		printf("I have received:%s \n",buffer);
		printf("shuzu changdu shi: %d\n",strlen(buffer));
		//strncpy(bufpoint,buffer,strlen(buffer));
		bufpoint=buffer;
		printf("%s  %d \n",bufpoint,sizeof(bufpoint));
		//printf("%s  %d \n",command,sizeof(command));
		printf("%s  %d \n",buffer,sizeof(buffer));
		if(strcmp(bufpoint,command1)==0)
		//if(*bufpoint==command)
		{
			printf("motor1 start!\n");
			write(fd,send_buf1,1);
			for(j=0;j<100000;j++);
		}
		if(strcmp(bufpoint,command2)==0)
		//if(*bufpoint==command)
		{
			printf("motor2 start!\n");
			write(fd,send_buf2,1);
			for(j=0;j<100000;j++);
		}
		if(nbytes==5)
			printf("received!");

		
		
	}
	close(sockfd);
	close(fd);
	exit(0);
}




///////////子函数
void readPort(int fd)
{
		int i;
	  int len;
	  int n; 
	  char read_buf[256];
	  while(1)
	  {
			bzero(read_buf, sizeof(read_buf));    
	     
			while((n = read(fd, read_buf, sizeof(read_buf))) > 0)
	  	{
				//printf("Len %d \n", n);
				read_buf[n + 1] = '\0';
	 		 	printf("[%s]\n", read_buf);
				for(i=0;i<500;i++);
	  	}
  	}
}
/************************************
 *init 
 *	初始化串口
 *@global
 *	PORT_SPEED	串口波特率
 *	DATABITS	数据位
 *	STOPBITS	停止位
 *	PARITY		校验方式
 *@return
 *	-1:设置失败  !(-1)：设置成功
 ************************************/
int init(int speed ,int  databits,  char parity,int  stopbits)
{
	int fd;
	struct termios Opt;	//定义termios结构
 /*
  这个变量被用来提供一个健全的线路设置集合, 如果这个端口在被用户初始化前
  使用. 驱动初始化这个变量使用一个标准的数值集               
  struct termios{
  	unsigned short c_iflag; //输入模式标志
    unsigned short c_oflag; 	//输出模式标志
    unsigned short c_cflag; 	//控制模式标志
    unsigned short c_lflag; 	//区域模式标志或本地模式标志或局部模式
    unsigned char c_line; 	//行控制line discipline 
    unsigned char c_cc[NCC]; 	// 控制字符特性
  };
	*/
  //打开串口
  fd = openPort();
  //fcntl(fd,F_SETFL,0);
  
  //设置波特率
  //if(setSpeed(fd, PORT_SPEED, &opt) == 1)
  //{	
  	//printf("setSpeed failed!\n");
  	//exit(1);
  //}
  int i;

    //if(tcgetattr(fd, &Opt) != 0) //获取与终端相关的参数；	#并将获得信息保存在 opt 变量中
    if(tcgetattr(STDIN_FILENO, &Opt) != 0)
    {
        perror("tcgetattr fd\n");
        return 1;
    }
    //识别波特率,设置输入输出波特率
    for(i = 0; i < sizeof(speed_arr) / sizeof(int); i++)
    {
        if(speed == name_arr[i])
        {
            tcflush(fd, TCIOFLUSH);
            //设置波特率
            cfsetispeed(&Opt, speed_arr[i]);
            cfsetospeed(&Opt, speed_arr[i]);
            
            //设置数据接收方式
            if(tcsetattr(fd, TCSANOW, &Opt) != 0)
            {
                perror("tcsetattr fd");
                return 1;
            }
            tcflush(fd, TCIOFLUSH);
        }
    }
  
  //设置数据位、停止位和校验位
  //if(setParity(fd, DATABITS, STOPBITS, PARITY, &opt) == 1)
  //{
  	//printf("setParity failed!\n");
  	//exit(1);
  //} 

	if(tcgetattr(fd, &Opt) != 0)
    {
        perror("tcgetattr fd");
        return 1;
    }
		Opt.c_cflag |= (CLOCAL | CREAD);       	//CLOCAL：忽略 modem 控制线。 
							//CREAD：	打开接受者。
    switch(databits)       				//设置数据位数
    {
	    case 7:
	        Opt.c_cflag &= ~CSIZE;			//屏蔽字符大小位
	        Opt.c_cflag |= CS7;			//选择7位数据位
	        break;
	    case 8:
	        Opt.c_cflag &= ~CSIZE;
	        Opt.c_cflag |= CS8;
	        break;
	    default:
	        fprintf(stderr, "Unsupported data size.\n");
	        return 1;
    }

    switch(parity)            //设置校验位
    {
    case 'n':
        Opt.c_cflag &= ~PARENB;        			//清除校验位
        Opt.c_iflag &= ~INPCK;        			//启用输入奇偶检测。  
        break;
    case 'o':
        Opt.c_cflag |= PARENB;       			//使能校验位
        Opt.c_cflag |= PARODD;        			//奇校验
        Opt.c_iflag |= INPCK;            		//启用输入奇偶检测。
        break;
    case 'e':
        Opt.c_cflag |= PARENB;       			//使能校验位
        Opt.c_cflag &= ~PARODD;        			//偶校验
        Opt.c_iflag |= INPCK;            		//启用输入奇偶检测。
        break;
    case 's':
        Opt.c_cflag &= ~PARENB;        			//清除校验位
        Opt.c_cflag &= ~CSTOPB;       			//设置一个停止位
        Opt.c_iflag |= INPCK;            		//启用输入奇偶检测。
        break;
    default:
        fprintf(stderr, "Unsupported parity.\n");
        return 1;    
    }

    switch(stopbits)        //设置停止位
    {
    case 1:
        Opt.c_cflag &= ~CSTOPB;
        break;
    case 2:
        Opt.c_cflag |= CSTOPB;
        break;
    default:
        fprintf(stderr, "Unsupported stopbits.\n");
        return 1;
    }

    Opt.c_cflag |= (CLOCAL | CREAD);

    Opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);	
 
    Opt.c_oflag &= ~OPOST;		//OPOST ：启用具体实现自行定义的输出处理。
    Opt.c_oflag &= ~(ONLCR | OCRNL);    //OCRNL ：将输出中的回车映射为新行符  
 						//ONLCR ：(XSI) 将输出中的新行符映射为回车-换行。
    
    Opt.c_iflag &= ~(ICRNL | INLCR);		//ICRNL :将输入中的回车翻译为新行 (除非设置了 IGNCR)(否则当输入信号有 CR 时不会终止输入)。 
    						//INLCR :将输入中的 NL 翻译为 CR。（将收到的换行符号转换为Return）
    Opt.c_iflag &= ~(IXON | IXOFF | IXANY);	//IXON 	:启用输出的 XON/XOFF 流控制。
    						//IXOFF :启用输入的 XON/XOFF 流控制。
						//IXANY :(不属于 POSIX.1；XSI) 允许任何字符来重新开始输出。
    tcflush(fd, TCIFLUSH);			//清空输入缓存
    
    //MIN = 0 , TIME =0; 有READ立即回传否则传回 0,不读取任何字元
    Opt.c_cc[VTIME] = 0;        			
    Opt.c_cc[VMIN] = 10;        					
    
    if(tcsetattr(fd, TCSANOW, &Opt) != 0)	//设置数据接收方式
    {
        perror("tcsetattr fd");
        return 1;
    }

  if(tcsetattr(fd, TCSANOW, &Opt) != 0)	//TCSANOW：不等数据传输完毕就立即改变属性。
	{				//TCSADRAIN：等待所有数据传输结束才改变属性。
      perror("serial error");
      return -1;
  }
  return fd;
}
/************************************
 *openPort  
 *	打开串口
 *@global
 *	SERIAL_PORT	串口地址
 *@return
 *	-1:设置失败  !(-1)：设置成功
 ************************************/
int openPort()
{
	int fd;
	//O_RDWR:	可读写方式打开；
	//O_NOCTTY:	若打开的文件为终端机设备时则不会把该终端机当做进程控制终端机
	//O_NDELAY：	以不可阻断的方式打开文件
	fd = open(SERIAL_PORT, O_RDWR | O_NOCTTY |O_NDELAY);   
  	if(fd == -1)
  	{
		perror("open serial failed!\n");
  		exit(1);
  	}    
  	return fd;
}