omap_arm.c

在室內環境中可結合式子母機器人系統

#include<stdio.h>
#include<stdlib.h>
#include<sys/socket.h>
#include<sys/types.h>
#include<sys/stat.h>
#include<fcntl.h>
#include<unistd.h>
#include<sys/ioctl.h>
#include<sys/soundcard.h>
#include<signal.h>
#include<sys/mman.h>
#include<sys/time.h>
#include<netinet/in.h>
#include<arpa/inet.h>
#include<netdb.h>
#include<termios.h>
#include<string.h>
#include<math.h>

#include"SVM_data.h"

#define	V1x_cpi	1600
#define	V1y_cpi	1600
#define	V2x_cpi	1880
#define	V2y_cpi	1880
#define	pi	3.1415926
#define n_pi	-3.1415926


#define AUDIO_TRACKING_DEV "/dev/dsptask/VGtracking"
#define SHARED_MEM_SIZE 32*1024

#define audio_file "/audio"
#define AUDIO_DEV "/dev/dsp"
#define AUDIO_DSP_DEVICE_ID 3587

#define SAMPLING_RATE_8000 8000
#define SAMPLING_RATE_16000 16000
#define SAMPLING_RATE_48000 48000

#define BUFFER_SIZE	32*1024

#define	f0x	-1
#define	f0y	0
#define f1x	0.5
#define	f1y	-0.866025403
#define	f2x	0.5
#define	f2y	0.866025403
#define	speed	110
#define	R_wheel	4.0

int	uart_init(void);
struct	position get_pos(int fd);
double	sensor_decode(char* data,int n);
unsigned short  sensor_decode_1(char* data,int n);
double	SVM_x(double *de);
double	SVM_RL_x(double *de);
double	SVM_RL_x_var(double *de);
void	turn_R(int u_fd,double angle);    //  angle : degree
void	turn_L(int u_fd,double angle);	  //  angle : degree
void	go_forward(int u_fd,int distance);  // distance : cm
void	go_forward_fast(int u_fd,int distance);  // distance : cm
void	go_backward(int u_fd,int distance);  // distance : cm
void	go_backward_fast(int u_fd,int distance);  // distance : cm
int	go_forward_1(int u_fd,double distance);  // distance : cm
int	go_forward_2(int u_fd,int distance);  // distance : cm

void	A_go(int u_fd,int	distance);
void	A_back(int u_fd,int	distance);
void	stop_run(int u_fd);
void	swap(double *a,double *b);
void	go_up(int u_fd);
void	go_down(int u_fd);
void	go_direction(int u_fd,double a,double b,double c);
void	go_circle_R(int uart_fd);
void	go_circle_L(int uart_fd);
int	cal_round(double num);
char	*itoa(int x_int);
char	*i2command(char *motor_num,int x_int);
void	docking(int u_fd);
void	go_inside(int u_fd);
double	play_sound(unsigned short  *shar_addr);


int	len,i,j,k,count[8]={0};
unsigned short	inside_value[8],door_value[3],tmp_value;
short		tmp_diff[8];
char	recbuffer[50];
struct sockaddr_in	clnaddr_99,clnaddr_100,servaddr;
socklen_t	addr_len_99=sizeof(clnaddr_99);
socklen_t	addr_len_100=sizeof(clnaddr_100);
int	sockfd;


struct	termios my_termios;
struct	position
{
	double	x;
	double	y;
	double	deg;
};

double	test_x=0;
double	test_y=0;
double	test_rad=pi/2;


double	pos_x=0;
double	pos_y=0;
double	pos_rad=pi/2;

struct position		robot_pos,test_pos;

int docking_flag;

int audio_fd;
int	dsp_fd;
int 	socket_fd;
int	samplingRate = SAMPLING_RATE_16000;
double	delay[6];
char gumtest[50]="A2";


int configureCodec(const int iFormat)
{
	int format;
	int STEREO = 2;
	int flag;
	mode_t mode;
	format = AFMT_S16_LE;

	// Check if device is already mounted at /dev/dsp
        printf("Checking for device connection... ");
        audio_fd = open( AUDIO_DEV, O_RDWR|O_CREAT, 0);

        // Make node /dev/dsp if not exist
        if (audio_fd == -1) {
                printf("not presents \n");
                printf("Creating node /dev/dsp ... ");
                mode = 0666;
                mode |= S_IFCHR;
                printf("MODE = %u ",mode);

                if (mknod( AUDIO_DEV, mode, AUDIO_DSP_DEVICE_ID) == 0) {
                        printf("Node /dev/dsp sucessfully created \n");
                        audio_fd = open( AUDIO_DEV, O_WRONLY); // reopen the device
                } else {
                        printf("Node /dev/dsp creation failed \n");
                        return 1;
                }
        } else {
                printf("OK \n");
        }

	flag = fcntl( audio_fd, F_GETFL, 0);
        flag &= ~O_NDELAY;
        fcntl( audio_fd, F_SETFL, flag);

	if(ioctl(audio_fd, SNDCTL_DSP_SETFMT, &format) == -1) {
		printf("ioctl failure 1\n");
		return -1;
	}
	if(format != AFMT_S16_LE) {
		printf("AFMT_S16_LE is not supported\n");
		return -1;
	}
	
	format = iFormat;
	if(ioctl(audio_fd, SNDCTL_DSP_SPEED, &format) == -1) {
		printf("ioctl failure 2\n");
		return -1;
	}
	if(format != iFormat) {
		printf("%i sample/sec is not supported\n", iFormat);
		return -1;
	}

	format = STEREO;    // AIC23 is a stereo codec
        ioctl(audio_fd, SNDCTL_DSP_CHANNELS, &format);
	return 0;
}



int main(int argc,char **argv)
{
	//int	delay_power_count=0;
	
	short	audio_buffer[BUFFER_SIZE];
	char	temp[2];
	char	tmp,tmp1;
	char	angle[3];
	int	uart_fd=-1;
	//int	data_len;
	//int	samplingRate = SAMPLING_RATE_16000;
	
	int	forward_flag=0;
	int	dir_flag=0;
	int	crash_count=0;

	////struct position		robot_pos;
	double	angle_rotate=0,angle_rotate_1=0;
	char	motor_a[6],motor_b[6],motor_c[6];
	
	struct sockaddr_in	addr_svr;
	//int 	socket_fd;
	
	//short	index;
	//short	power_data[5];
	//unsigned long int	power=0;
	//int	dsp_fd;
	unsigned short *shared_addr;
	int bon_count=0;
	int bon_dir_flag=0;
	//short	R_real,R_image,L_real,L_image;
	//double	ph_R,ph_L,ph_diff;
	//double	delay[6];
	//double  delay_power[7],mean_delay=0,var_delay=0;
	//double	freq[6]={343.75,375,406.25,437.5,468.75,500};

	double	predict_x=0;
	double	predict_RL_x=0;
	double	predict_RL_x_var=0;
	double	test_delay[6]={-2.145633400000000e+001,	6.322995000000000e+000,	-4.461880000000000e-001,	-1.708740700000000e+001,	-9.860350000000000e-001,	3.368287000000000e+000};
	double	test_delay_var[7]={-2.145633400000000e+001,	6.322995000000000e+000,	-4.461880000000000e-001,	-1.708740700000000e+001,	-9.860350000000000e-001,	3.368287000000000e+000,	1.303980782466628e+002};
	
	
	memset(&addr_svr,0,sizeof(addr_svr));
	memset(&robot_pos,0,sizeof(struct position));
	memset(angle,0,3);
	memset(motor_a,0,6);
	memset(motor_b,0,6);
	memset(motor_c,0,6);
	strcpy(motor_a,"a+0\r");
	strcpy(motor_b,"b+0\r");
	strcpy(motor_c,"c+0\r");
	
/*	test_data is saved at test_data.c	*/
/*
	for(i=0;i<512;i++)
	{
		test_data2[i*2]=test_data[i];
		test_data2[i*2+1]=test_data[i+512];
	}
*/

	uart_fd=uart_init();

//////////////////////////////////////////////////
/*	clear sensor initial error data		*/
//////////////////////////////////////////////////
	for(k=0;k<5;k++)
	{
		robot_pos=get_pos(uart_fd);
		for(i=0;i<10;i++)
			for(j=0;j<30000;j++);
	}
	robot_pos.x=0;
	robot_pos.y=0;
	robot_pos.deg=0;
	pos_x=0;
	pos_y=0;
	pos_rad=pi/2;
	test_x=0;
	test_y=0;
	test_rad=pi/2;
	for(k=0;k<5;k++)
	{
		robot_pos=get_pos(uart_fd);
		for(i=0;i<10;i++)
			for(j=0;j<30000;j++);
	}

//////////////////////////////////////////////////



//////////////////////////////////////////////////
/*		 DSP memory map   		*/
/*	   start addr : 0x2f000  (DSP)		*/
/*	   Length     : 0x10000  (byte)		*/
/*	   Used memory: 0x08000  (byte)		*/
//////////////////////////////////////////////////
	if ((dsp_fd = open( AUDIO_TRACKING_DEV, O_RDWR)) < 0) {
		perror("dsp device open error\n");
		return -1;
	}
	printf("[ARM] mmapping...");
	shared_addr = (unsigned short *)mmap( NULL, SHARED_MEM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, dsp_fd, 0);
        if ((int)shared_addr < 0) {
               perror("mmap");
               return -1;
        }
        printf("Success!!\n");

//////////////////////////////////////////////////////////////////////////
/*		 Socket config   :connect to PC to play sound		*/
//////////////////////////////////////////////////////////////////////////
/*
	addr_svr.sin_family= AF_INET;
	addr_svr.sin_port= htons(1234);
        
        if(argc==2)
        	addr_svr.sin_addr.s_addr = inet_addr(argv[1]);
        else
        	addr_svr.sin_addr.s_addr = inet_addr("127.0.0.1");
        
        printf("Connect to host...");
        if((socket_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
        {
        	perror("open socket erroe\n");
        	return -1;
        }
        if(connect(socket_fd,(struct sockaddr*)&addr_svr,sizeof(addr_svr))<0)
        {
        	perror("connect error\n");
        	return -1;
        }
	printf("Success!!\n");
*/
//////////////////////////////////////////////////////////////////////////
/*		Socket config	:connect to ether_net board		*/
//////////////////////////////////////////////////////////////////////////
	printf("Socket Binding...");
	memset(&servaddr,0,sizeof(servaddr));
	
	sockfd=socket(AF_INET,SOCK_DGRAM,0);
	servaddr.sin_family=AF_INET;
	servaddr.sin_port=htons(5102);
	servaddr.sin_addr.s_addr=inet_addr("192.168.1.44");
	
	clnaddr_99.sin_family=AF_INET;
	clnaddr_99.sin_port=htons(4003);
	clnaddr_99.sin_addr.s_addr=inet_addr("192.168.1.99");  // "b" : inside
	
	clnaddr_100.sin_family=AF_INET;
	clnaddr_100.sin_port=htons(4003);
	clnaddr_100.sin_addr.s_addr=inet_addr("192.168.1.100"); // "a" : door
	
	if(bind(sockfd,(struct sockaddr *)&servaddr,sizeof(servaddr))<0)
	{
		printf("bind error\n");
		exit(0);
	}
	printf("Success!!\n");
//////////////////////////////////////////////////////////////////////////
///////////////////////////initial data///////////////////////////////////
//	while(1)
//	{
//		printf("choose the option :");
//		scanf("%s",&tmp);
	
//		go_inside(uart_fd);
		
//		forward_flag=go_forward_1(uart_fd,30);
//		if(forward_flag==0)
//			printf("No collision..\r\n");
//		else
//			printf("Crash...\r\n");
/*		robot_pos=get_pos(uart_fd);
		printf("X: %lf  Y: %lf  deg: %lf\n",robot_pos.x,robot_pos.y,robot_pos.deg);
		for(i=0;i<10;i++)
			for(j=0;j<30000;j++);
*/			
//	}
//	for(i=0;i<1500;i++)
//		for(j=0;j<60000;j++);

///////////////////////弄