/************************************************************************* SBC Code Description: Basic send and receive functions between Atmega128 and SBC. Added sockets. This code is for Lavi's AI. Add AI code to set_heading(). Date: 4/19/2007 SERIAL_LAVI.C ************************************************************************** */ #include #include #include #include #include #include #include /* for socket() and bind() */ #include /* for sockaddr_in and inet_ntoa() */ #include /* for memset() */ #include /* for close() and sleep() */ #define DATAPORT "/dev/ttyS0" #define BUFFER_SIZE 9 #define CHECK_SIZE 1 #define ECHOMAX 255 /* Longest string for socket*/ //Variable used to access the serial port int data_ioport; //I2C Sensors unsigned char TC,TR,TL,CR,CL,BR,BL,BK,CMP,DRIVE_CMD; //Sensor thresholds for obstacle avoidance unsigned char cr_thresh = 25, cl_thresh = 25, br_thresh = 25, bl_thresh = 25, bk_thresh = 12, tl_thresh = 15 , tr_thresh = 15; int msg_num; //1 = Arroyo //2 = Schwartz //3 = go left //4 = go right //5 = go backward //6 = go forward //7 = stop //Open serial port with specified baud rate, port int open_serial_port(char *port, int baud); void set_heading(); void print_readings(); //Global Variables for GUI code int maxval; /*! Opens a serial port with the flags: O_RDWR - read/write mode O_NOCTTY - not the controlling terminal O_NDELAY - do not pay attentention to Data Carrier Detect (DCD) \param port a character string of the device \param baud speed to open port \return file descriptor */ int open_serial_port(char *port, int baud){ int fd; int speed; char msg[70]; struct termios opts; fd = open( port, O_RDWR | O_NOCTTY | O_NDELAY ); //may not need the nodelay if( fd <= 0 ){ sprintf(msg,"Failed: %s:%d - %s",port,baud,strerror(errno)); perror(msg); return(-1); } // Set the baud rate switch(baud){ case 1200: speed = B1200; break; case 2400: speed = B2400; break; case 4800: speed = B4800; break; case 9600: speed = B9600; break; case 19200: speed = B19200; break; case 38400: speed = B38400; break; case 57600: speed = B57600; break; default: printf("Failed: Invalid speed\n"); } /* Contents of termios struct: tcflag_t c_iflag; tcflag_t c_oflag; tcflag_t c_cflag; tcflag_t c_lflag; cc_t c_cc[NCCS]; speed_t c_ispeed; speed_t c_ospeed; */ //Bzero call throws some kind of warning based on gcc not recognizing it //so replaced called to bzero below with zero'ing c_ispeed and c_ospeed. //bzero( &opts, sizeof( opts ) ); opts.c_ispeed = 0; opts.c_ospeed = 0; opts.c_cflag = speed | CS8 | CLOCAL | CREAD /*| CRTSCTS*/; opts.c_iflag = IGNPAR; opts.c_oflag = 0; opts.c_lflag = 0; opts.c_cc[VTIME] = 0; opts.c_cc[VMIN] = 0; tcflush( fd, TCIOFLUSH ); // clean out old crap tcsetattr( fd, TCSANOW, &opts ); return fd; } /***************************************************** 7/26/06 Modified to be based on msg_num. Took out count. No longer need it. Took out cp function. ******************************************************/ void update_display() { if(msg_num==3) //system("cp -f Left.jpg Current.jpg"); system("qiv -Tfmi ~/Desktop/Serial/Left.jpg &"); else if(msg_num==4) //system("cp -f Right.jpg Current.jpg"); system("qiv -Tfmi ~/Desktop/Serial/Right.jpg &"); else if(msg_num==5) //system("cp -f Reverse.jpg Current.jpg"); system("qiv -Tfmi ~/Desktop/Serial/Reverse.jpg &"); else if(msg_num==6) //system("cp -f Forward.jpg Current.jpg"); system("qiv -Tfmi ~/Desktop/Serial/Forward.jpg &"); else if(msg_num==7) //system("cp -f Stop.jpg Current.jpg"); system("qiv -Tfmi ~/Desktop/Serial/Stop.jpg &"); else system("qiv -Tfmi ~/Desktop/Serial/Forward.jpg &"); } void speak_text(char *text) // Makes Koolio speak! { //int i; //char *text = "hello world"; char festival[240] = "echo '"; //for(i = 0;i <= text_len;i++) // festival[i + 7] = text[i]; strcat(festival,text); strcat(festival,"' | festival --tts &"); //system("echo '%s ' | festival --tts &", text); system(festival); } //Based on the current readings from the sensors, decide what course of //action to take. I.e., veer left, go straight, etc. //At the end of this function, the variable DRIVE_CMD will be set to the //code of the recommended heading. /* Heading codes: CMD# COMMAND 1 Turn left 2 Turn right 3 Veer left 4 Veer right 5 Stop motors 6 Back up 7 Drive straight 0 is > threshold: BLCLCRBRBK Action X 0 1 X X Turn Left X 1 0 X X Turn Right X 1 1 X 0 Back Up X 1 1 X 1 Stop (for now) */ void set_heading() { // This will be Lavi's AI code //DRIVE_CMD = 5; // Just for debbuging /* // Old stuff: if((BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && (BR > br_thresh)){ //not close to anything. 0000X DRIVE_CMD = 7; // Drive Straight } else if((BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && !(BR > br_thresh)) { //object on right. 0001X DRIVE_CMD = 3; // Veer Left } else if((CL > cl_thresh) && !(CR > cr_thresh)) { //object at front right. X01XX DRIVE_CMD = 1; // Turn Left } else if(!(CL > cl_thresh) && (CR > cr_thresh)) { //object on front left. X10XX DRIVE_CMD = 2; // Turn Right } else if(!(CL > cl_thresh) && !(CR > cr_thresh) && (BK > bk_thresh)){ //object ahead. X11X0 DRIVE_CMD = 6; // Back Up } else if(!(CL > cl_thresh) && !(CR > cr_thresh) && !(BK > bk_thresh)){ //object ahead and behind. X11X1 DRIVE_CMD = 5; // Stop } else if(!(BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && (BR > br_thresh)) { //object on left. 1000X DRIVE_CMD = 4; // Veer Right } else if(!(BL > bl_thresh) && (CL > cl_thresh) && (CR > cr_thresh) && !(BR > br_thresh)) { //objects on either side. 1001X DRIVE_CMD = 7; // Drive Straight } */ } //Prints current values of all sensors. void print_readings() { printf("BL: %i\n", BL); printf("CL: %i\n", CL); printf("CR: %i\n", CR); printf("BR: %i\n", BR); printf("BK: %i\n", BK); printf("TL: %i\n", TL); printf("TR: %i\n", TR); printf("TC: %i\n", TC); printf("CMP: %i\n", CMP); } int main(int argc,char *argv[]) { int i; int sock; /* Socket */ struct sockaddr_in echoServAddr; /* Local address */ struct sockaddr_in echoClntAddr; /* Client address */ int cliAddrLen; /* Length of incoming message */ char echoBuffer[ECHOMAX]; /* Buffer for echo string */ unsigned short echoServPort; /* Server port */ int recvMsgSize; /* Size of received message */ int ingnore_next_msg; char *say_it; /* Image and sound crap: // Added Aug 22, 2006 Andrew Chambers // Run find_ip.sh script to find the ip address and update it on the // koolio webpage system("/home/zardoz/find_ip.sh"); //Display image and play laughter: // system("aplay /usr/share/sounds/gnibbles/laughter.wav"); system("qiv -Tfmi ~/Desktop/Serial/Current.jpg &"); speak_text("If you don't know who it is it's Koolio"); sleep(5); system("mpg123 ~/Desktop/if_you_dont_know_koolio.mp3"); speak_text("Hello my name is Koolio and this is my show"); speak_text("Hello my name is Koolio and this is my theme song"); sleep(5); system("mpg123 ~/Desktop/theme_song1_koolio.mp3 &"); */ //1 char buffer for send 1 byte of data to Altera board. unsigned char message = 0x00; //8 byte char buffer for receiving the 10 sensor readings from the Altera board. unsigned char value[BUFFER_SIZE] = "0"; unsigned char check[CHECK_SIZE] = "0"; print_readings(); //Open connection at 9600 baud data_ioport = open_serial_port(DATAPORT, 9600); //Read and ignore the initial 0x00 byte that seems to be sent //by the Mavric board when starting up the UART while(read(data_ioport, &value, BUFFER_SIZE)==0); /* This is for ordering website: // get port number if (argc != 2) // Test for correct number of parameters { printf(stderr,"Usage: %s \n", argv[0]); exit(1); } echoServPort = atoi(argv[1]); // First arg: local port // Create socket for sending/receiving datagrams if ((sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) printf("socket() failed"); // Construct local address structure memset(&echoServAddr, 0, sizeof(echoServAddr)); // Zero out structure echoServAddr.sin_family = AF_INET; // Internet address family //echoServAddr.sin_addr.s_addr = htonl(INADDR_ANY); // Any incoming interface echoServAddr.sin_port = htons(echoServPort); // Local port // Bind to the local address if (bind(sock, (struct sockaddr *) &echoServAddr, sizeof(echoServAddr)) < 0) printf("bind() failed"); ingnore_next_msg = 0; */ /* END INITIALIZATION STUFF */ while(1) // main while(1) loop { // Code to read in sensor data from Atmega128 //Full sensor reading consists of 9 bytes (8 sonars + compass) int j = 0; while(j == 0) { if( read(data_ioport, &check, CHECK_SIZE) != 0 ) { if(check[0] == 0xFF) // 0xFF is the starting character of the sensor // data { j = 1; } } } while( read(data_ioport, &check, CHECK_SIZE) == 0 ); BR = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); BL = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); CR = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); CL = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); TL = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); TR = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); BK = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); TC = check[0]; while( read(data_ioport, &check, CHECK_SIZE) == 0 ); CMP = check[0]; //If new serial data is available, use it to update sensor variables. //If read is sucessful, read the next 7 values as well int u; // read(data_ioport, &value, BUFFER_SIZE); // while( read(data_ioport, &value, BUFFER_SIZE) == 0 ) //First value in the dataset // BR = value[0]; // BL = value[1]; // CR = value[2]; // CL = value[3]; // TL = value[4]; // TR = value[5]; // BK = value[6]; // TC = value[7]; // CMP = value[8]; //Set command for motors based on sensor data. (If no bad sonar data) if((BL != 0)&&(CL != 0)&&(CR != 0)&&(BR != 0)&&(BK != 0)&&(TR != 0)&&(TL != 0)&&(TC != 0)) { set_heading(); int j = 0; } //Send command to Altera board (which controls motors). //Dispatches a 1-byte message to the Altera board via an open //serial port connection. Locks until message successfully sent. message = DRIVE_CMD; while(write(data_ioport, &message, 1)==-1); //Debug output -- show what command is currently being sent to the motors / Altera board if(DRIVE_CMD == 1) { printf("cmd %X sent: Left\n", DRIVE_CMD); } else if(DRIVE_CMD == 2) { printf("cmd %X sent: Right\n", DRIVE_CMD); } else if(DRIVE_CMD == 3) { printf("cmd %X sent: Veer Left\n", DRIVE_CMD); } else if(DRIVE_CMD == 4) { printf("cmd %X sent: Veer Right\n", DRIVE_CMD); } else if(DRIVE_CMD == 5) { printf("cmd %X sent: Stop\n", DRIVE_CMD); } else if(DRIVE_CMD == 6) { printf("cmd %X sent: Reverse\n", DRIVE_CMD); } else if(DRIVE_CMD == 7) { printf("cmd %X sent: Forward\n", DRIVE_CMD); } print_readings(); } // Ends main while(1) /* // This is for ordering website: while(1) { // Set the size of the in-out parameter cliAddrLen = sizeof(echoClntAddr); // Block until receive message from a client if ((recvMsgSize = recvfrom(sock, echoBuffer, ECHOMAX, 0, (struct sockaddr *) &echoClntAddr, &cliAddrLen)) < 0) { printf("error"); ingnore_next_msg = 1; } if(ingnore_next_msg == 0) { //printf("Handling client %s\n", inet_ntoa(echoClntAddr.sin_addr)); //printf("Received: %s\n", echoBuffer); // Print the data received //change from char to number switch(echoBuffer[0]) { case 'a':msg_num = 1; break; case 'b':msg_num = 5; break; case 'f':msg_num = 6; break; case 'l':msg_num = 3; break; case 'r':msg_num = 4; break; case 's':if (echoBuffer[1] == 'c') msg_num = 2; else if(echoBuffer[1] == 't') msg_num = 7; else { printf("error does not match any cases"); msg_num = 0; //0 is an error; } break; case '$':for(i = 0;i < 200;i++) say_it[i] = ' '; for(i = 3;i < (int)echoBuffer[1] + 3;i++) say_it[i - 3] = echoBuffer[i]; say_it[i - 2] = 0; printf(say_it); printf("\n"); speak_text(say_it); msg_num = -1; break; default: printf("error does not match any cases"); msg_num = 0; //0 is an error; }; if (msg_num >= 0) printf("the number is %d\n", msg_num); // Print the command being sent while(write(data_ioport, &msg_num, 1)==-1); } // Ends if(ingnore_next_msg == 0) else { ingnore_next_msg = 0; } update_display(); // Updates Koolio's face } // Ends main while(1) */ //Close serial port at program end close(data_ioport); return 0; } // Ends main()