comm.c

linuxFedure4环境下串口编程后台运行状态机

#include "fsm.h"
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h> 
#include <linux/poll.h>
#include "serial.h"


#define MAX_CLIENT	5
#define MAX_PATH	20
#define BUF_SIZE	256

#define MAX_CROSS	10
char *ur_path[MAX_CROSS]=
{
	"ES#",
	"S#",
	"ESS#",
	"SS#",
	"WSS#",
	"SESS#",
	"SWSS#",
	NULL
};

int cur_ur_path = 0;

typedef struct _path
{
	char start[5];
	char end[5];
	char *crosses[10];
	char dirs[10-1];
} path;

path paths[MAX_PATH]={
{
	"A",
	"B",
	{"A","2","4","3","B",NULL},
	{'N','N','W','W','\0'}
},
{
	"A",
	"C",
	{"A","2","4","C",NULL},
	{'N','N','N','\0'}
},
{
	"A",
	"D",
	{"A","2","4","3","6","D",NULL},
	{'N','N','W','N','N','\0'}
},
{
	"A",
	"E",
	{"A","2","4","5","7","E",NULL},
	{'N','N','E','N','N','\0'}
},
{	
	"B",
	"A",
	{"B","3","4","2","A",NULL},
	{'E','E','S','S','\0'}
},
{	
	"B",
	"C",
	{"B","3","4","C",NULL},
	{'E','E','N','\0'}
},
{	
	"B",
	"D",
	{"B","3","6","D",NULL},
	{'E','N','N','\0'}
},
{	
	"B",
	"E",
	{"B","3","4","5","7","E",NULL},
	{'E','E','E','N','N','\0'}
},
{	
	"C",
	"A",
	{"C","4","2","A",NULL},
	{'S','S','S','\0'}
},
{	
	"C",
	"B",
	{"C","4","3","B",NULL},
	{'S','W','W','\0'}
},
{	
	"C",
	"D",
	{"C","4","3","6","D",NULL},
	{'S','W','N','N','\0'}
},
{	
	"C",
	"E",
	{"C","4","5","7","E",NULL},
	{'S','E','N','N','\0'}
},
{	
	"D",
	"A",
	{"D","6","3","1","2","A",NULL},
	{'S','S','S','E','S','\0'}
},
{	
	"D",
	"B",
	{"D","6","3","B",NULL},
	{'S','S','W','\0'}
},
{	
	"D",
	"C",
	{"D","6","3","4","C",NULL},
	{'S','S','E','N','\0'}
},
{	
	"D",
	"E",
	{"D","6","7","E",NULL},
	{'S','E','N','\0'}
},
{	
	"E",
	"A",
	{"E","7","5","4","2","A",NULL},
	{'S','S','W','S','S','\0'}
},
{	
	"E",
	"B",
	{"E","7","5","4","3","B",NULL},
	{'S','S','W','W','W','\0'}
},
{	
	"E",
	"C",
	{"E","7","5","4","C",NULL},
	{'S','S','W','N','\0'}
},
{	
	"E",
	"D",
	{"E","7","6","D",NULL},
	{'S','W','N','\0'}
},
NULL
};


int wless_fd, serv_fd;

char answer[32];

void does(const unsigned char *none)
{
}
/*
void turn_left(const unsigned char *arc)
{
	sprintf(answer,"turn_left|%d", atoi(arc));
}

void turn_right(const unsigned char *arc)
{
	sprintf(answer,"turn_right|%d", atoi(arc));
}

void stop(const unsigned char *none)
{
	sprintf(answer,"stop");
}
*/
struct func_table funcv[]={
	{"do", does},
};

struct FSM *map_fsm[MAX_CLIENT];
char client_dir[MAX_CLIENT];
char *client_name[MAX_CLIENT]={
	"sword",
	NULL
};

int
find_client(char *name){
	int i;
	for( i = 0; i < MAX_CLIENT; i++ )
	{	
		if( !strcmp(name, client_name[i]) )
			return i;
	}

	return -1;
}

int
find_answer(int client_id, char *start, char *end, int cur_cross){
	int 	i;
	char	event[BUF_SIZE];
	path 	*p = NULL;
	int	ur_p;

	for( i = 0; i < MAX_PATH; i++ )
	{
		if( !strcmp(paths[i].start,start) && !strcmp(paths[i].end, end) )
		{
			p = &paths[i];
			break;
		}
	}	

	if( p != NULL )
	{			
		if( cur_cross == 0 )
		{
			ur_p = atoi(p->crosses[1]);
			sprintf( answer, "%s|%c", ur_path[ur_p-1], p->dirs[0] );
			return 0;
		}
	
		sprintf(event, "%c", client_dir[client_id]);
		fsm_doevent( map_fsm[client_id], event  );

		if( !strcmp(p->crosses[cur_cross], end ) )
		{
			strcpy( answer, "#|#" );
		}
		else
		{
			ur_p = atoi( p->crosses[cur_cross+1] );
			sprintf( answer, "%s|%c", ur_path[ur_p-1], p->dirs[cur_cross] );
		}

		client_dir[client_id] = p->dirs[cur_cross];
	}
}

int
comm_send( int fd, char* buf ){
	return write( fd, buf, strlen(buf) );
}

int
comm_recv( int fd, char* buf )
{
	return read(fd,buf,BUF_SIZE);
}

int
comm_send_cmd( int fd , char * cmd){
	char temp[BUF_SIZE];
	int size;

	sprintf( temp, "SERVER|CMD|%s|SERVER \0", cmd);
	size = comm_send( fd, temp );
	printf("send: %d,%s\n", size,temp);
	return 0;
}

int 
parse_ask_dir( fd, input ){
	char	*temp;
	char 	*t;
	
	char 	*start,
		*end;
	char	cur_dir;
	int	cur_cross;
	int	client_id;

	char buf[BUF_SIZE];
	char state[BUF_SIZE];
	
	temp = strstr( input, "|CLIENT" );
	*temp = '\0';
	input += strlen("CLIENT|ASK_DIR|");
	
	t = strtok( input, "|" );
	client_id = find_client(t);
	
	if( -1 == client_id )
	{
		return -1;
	}

	start = strtok( NULL, "|" );
	end = strtok( NULL, "|" );

	t = strtok( NULL, "|" );
	cur_cross = atoi(t);


	t = strtok( NULL, "|" );
	cur_dir = *t;

	if( cur_cross == 0 )
		client_dir[client_id] = cur_dir;
	else if( cur_dir != client_dir[client_id] )
	{
		sprintf(buf, "%s|stop", client_name[client_id]);
		usleep(100);
		comm_send_cmd(fd, buf);
		return -1;
	}

	

	find_answer(client_id, start, end, cur_cross);

	sprintf(buf, "%s|%s", client_name[client_id], answer );		
	usleep(100);
	comm_send_cmd( fd, buf );
	
	return 0;
}

int
parse_client_input( int fd, char *input){
	char *input_cpy;
	int len = strlen(input);

	input_cpy = malloc( len + 1 );
	strcpy( input_cpy, input );

	if( strncmp( input, "CLIENT", 6 ) != 0 ){
		free( input_cpy );
		return -1;
	}

	input += 7;

	if( strncmp( input, "ASK_DIR", 7 ) == 0 ){
		parse_ask_dir( fd, input_cpy );
		free( input_cpy );
		return 0;
	}

	free( input_cpy );
	return 0;
}


int
parse_new_job( int fd, char *input )
{	
	char	*temp;
	char 	*t;
	
	char 	*start,
			*end;
	char	*base_id;
	char	*client;
	
	int		client_id;

	char 	buf[BUF_SIZE];
	
	temp = strstr( input, "|CENTRE" );
	*temp = '\0';
	input += strlen("CENTRE|NEW_JOB|");
	
	base_id = strtok( input, "|" );
	client = strtok( NULL, "|" );
	
	start = strtok( NULL, "|" );
	end = strtok( NULL, "|" );

	sprintf(buf, "SERVER|SET_DEST|%s|%s|%s|SERVER \0", client, start, end);
	comm_send(wless_fd, buf);		//FIXME need to be changed
	printf("send: %s\n", buf);

	return 0;
}

int
parse_centre_input( int fd, char *input){
	char *input_cpy;
	int len = strlen(input);

	input_cpy = malloc( len + 1 );
	strcpy( input_cpy, input );

	if( strncmp( input, "CENTRE", 6 ) != 0 ){
		free( input_cpy );
		return -1;
	}

	input += 7;

	if( strncmp( input, "NEW_JOB", 7 ) == 0 ){
		parse_new_job( fd, input_cpy );
		free( input_cpy );
		return 0;
	}

	free( input_cpy );
	return 0;
}
/*
int
comm_wait_input( int fd ){
	int tmp_size,size;
	char tmp[BUF_SIZE];
	char buf[BUF_SIZE];
	char *input;

	memset(buf, '\0', BUF_SIZE);
	size = comm_recv(fd, buf);
	buf[size]='\0';
	while( !strstr(buf,"|CLIENT") ){
		tmp_size = comm_recv(fd, tmp);
		tmp[tmp_size] = '\0';
		strcat(buf,tmp);
		size += tmp_size;
	}
	buf[size]='\0';
	printf("before strstr, recv:%s\n",input);
	input = strstr(buf,"CLIENT|");
	printf("after strstr, recv:%s\n",input);
	parse_input( fd, input );
	return 0;
}
*/

#define WIRELESS_DEV	"/dev/ttyS0"

int open_wireless( int *fd )
{
	*fd = open( WIRELESS_DEV, O_RDWR );
	if( -1 == *fd )
	{
		perror("Can't open serial port!");
		return -1;
	}

//	set_speed( fd, 9600);
//	set_Parity(fd, 8, 1, 'N');
	set_serial(*fd);

	return 0;
}


#define CENTER_PORT 10000	
//#define CENTER_IP	"127.0.0.1"
#define CENTER_IP	"192.168.1.80"
//#define CENTER_PORT 5000	
//#define CENTER_IP	"192.168.1.88"

int connect_to_center( int *fd )
{
	struct sockaddr_in serv_addr;
	struct hostent *server;

	*fd = socket( AF_INET, SOCK_STREAM, 0 );

	if( *fd < 0 )
	{
		perror("Can't open socket!");
		return -1;
	}


	bzero( (char*)&serv_addr, sizeof(serv_addr) );
	serv_addr.sin_family = AF_INET;
	serv_addr.sin_addr.s_addr = inet_addr(CENTER_IP);
	serv_addr.sin_port = htons(CENTER_PORT);
	
	if( connect( *fd, &serv_addr, sizeof(serv_addr) ) < 0 )
	{
		perror("Can't connect to center!");
		return -1;
	}

	perror("Connect success!\n");
		return -1;
	return 0;
}

#define CEN	0
#define CAR	1

int
get_input( int role, int fd )
{
	int tmp_size;
	char tmp[BUF_SIZE];
	static int size[2];
	static char buf[2][BUF_SIZE];
	char *input;

	switch(role)
	{
	case CEN:
		buf[0][size[0]]='\0';
		tmp_size = comm_recv(fd, tmp);
		strcat(buf,tmp);
		tmp[tmp_size] = '\0';
		size[0] += tmp_size;
		
		if( strstr(buf[0],"|CENTRE") )
		{
			buf[0][size[0]]='\0';
			input = strstr(buf[0],"CENTRE|");
			printf("after strstr, recv:%s\n",input);
			parse_centre_input( fd, input );
			size[0] = 0;
		}
		break;
	case CAR:
		buf[1][size[1]]='\0';
		tmp_size = comm_recv(fd, tmp);
		strcat(buf[1],tmp);
		tmp[tmp_size] = '\0';
		size[1] += tmp_size;
		
		if( strstr(buf[1],"|CLIENT") )
		{
			buf[1][size[1]]='\0';
			input = strstr(buf[1],"CLIENT|");
			printf("after strstr, recv:%s\n",input);
			parse_client_input( fd, input );
			size[1] = 0;
		}
		break;

	}

	return 0;
}


#define FD_MAX	10

int
main()
{
	int i;
	
	struct pollfd fds[FD_MAX];
	int	pollfd_num;

	open_wireless(&wless_fd);
	connect_to_center(&serv_fd);


	for( i = 0; i < MAX_CLIENT; i++)
		map_fsm[i] = fsm_load("map.fsm", funcv, 1);

	fds[0].fd = wless_fd;
	fds[0].events = POLLRDNORM;

	fds[1].fd = serv_fd;
	fds[1].events = POLLRDNORM;

	pollfd_num = 2;

	while(1)
	{
		poll( fds, pollfd_num, 100 );		

		if( fds[0].revents & POLLRDNORM )       //FIXME POLLERR handle
		{
			get_input(CAR,fds[0].fd);
		}	

		if( fds[1].revents & POLLRDNORM )
		{
			get_input(CEN,fds[1].fd);
		}

		//comm_wait_input(wless_fd);
	}

	for( i = 0; i < MAX_CLIENT; i++)
		fsm_free(map_fsm[i]);

	close(serv_fd);
	close(wless_fd);
	return 0;
}