Psykokwak

/****************************************************************************
#              AroBot Srv: TCP Server for remote control.
# This program is used to make a bridge between the serial controller and 
# a tcp socket.                                                           
#                                                                         
#             Copyright (C) 2007 MAMMANA Jean Charles                   
#                                                                         
# This program is free software; you can redistribute it and/or modify     
# it under the terms of the GNU General Public License as published by     
# the Free Software Foundation; either version 2 of the License, or       
# (at your option) any later version.                                     
#                                                                         
# This program is distributed in the hope that it will be useful,         
# but WITHOUT ANY WARRANTY; without even the implied warranty of           
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           
# GNU General Public License for more details.                             
#                                                                         
# You should have received a copy of the GNU General Public License       
# along with this program; if not, write to the Free Software             
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
#                                                                         
****************************************************************************/

// replace "..." for html parsing...
#include "stdio.h"
#include "string.h"   
#include "unistd.h"
#include "fcntl.h"     
#include "errno.h"   
#include "termios.h"
#include "sys/types.h"
#include "sys/stat.h"
#include "stdlib.h"
#include "stdarg.h"
#include "signal.h"

#include "pthread.h"

#include "sys/socket.h"
#include "arpa/inet.h"
#include "netinet/in.h"

#define IR_NB      3

#define BUFFSIZE        64
#define MAX(x, y)       (((x) < (y)) ? (y) : (x))



void readSocketAndSend(int *s);
int readIRAndSend();
int tty_open(char* tty_dev);
void tty_printf(char *format, ...);
void termination_handler (int signum);
int stdin_init(void);
void readValue();
int initServer(int port);
void keyevent();
void pollIR();



struct termios stdin_saved_attributes;
struct termios tty_saved_attributes;
int tty_fd;
int s_cli = 0;
int IR = 1;
pthread_t pollIR_thread;



typedef struct  s_dataformat {
        char    header;
        char    value;
}              t_dataformat;


void *cb_threadIR(void *param) {
        struct timeval tv;

        tv.tv_sec = 1;
        tv.tv_usec = 0L;

        select (0, NULL, NULL, NULL, &tv);

        while (42) {
                tv.tv_sec = 0;
                tv.tv_usec = 100000L;
                pollIR();
                select (0, NULL, NULL, NULL, &tv);     
        }
}


int main(int argc, char *argv[]) {

        printf("Arobot Server - v1.0\n - SV8 : cam vertical\n - SV7 : cam horizontal\n - SV6 : moteur gauche\n - SV5 : moteur droit\n - AD1 : IR centre\n\n");

        struct timeval tv;
        fd_set readfds;
        int rSelect;
        int s_server = 0;
        int poll_ir = 1;

        if (tty_open("/dev/ttyS0") < 0) {
              fprintf (stderr,"tty open error %s\n", strerror(errno));
                exit(EXIT_FAILURE);
        }

        if (stdin_init() < 0) {
              printf("stdin init error %s\n", strerror(errno));
                exit(EXIT_FAILURE);
        }

        if (signal (SIGINT, termination_handler) == SIG_IGN) signal (SIGINT, SIG_IGN);
        if (signal (SIGHUP, termination_handler) == SIG_IGN) signal (SIGHUP, SIG_IGN);
        if (signal (SIGTERM, termination_handler) == SIG_IGN) signal (SIGTERM, SIG_IGN);


        s_server = initServer(5432);
        if (socket == 0) {
                fprintf(stderr, "unable to create listen socket\n");
                exit(EXIT_FAILURE);
        }

       
        pthread_create(&pollIR_thread, NULL, cb_threadIR, NULL);

        while (42) {

                tv.tv_sec = 0;
                tv.tv_usec = 10000L;

                FD_ZERO(&readfds);

                FD_SET(STDIN_FILENO, &readfds);
                FD_SET(tty_fd, &readfds);
                FD_SET(s_server, &readfds);
                FD_SET(s_cli, &readfds);
               
                rSelect = select(MAX(MAX(s_server, s_cli), tty_fd) + 1, &readfds, NULL, NULL, /*&tv*/ NULL);

                if (rSelect == -1)
                        exit(EXIT_FAILURE);          

                //if (!rSelect && poll_ir) {
                //      pollIR();
                //      poll_ir = 0;
                //}

                if (FD_ISSET(tty_fd, &readfds)) {
                        if(readIRAndSend())
                                poll_ir = 1;
                }

                if (FD_ISSET(STDIN_FILENO, &readfds)) {
                        keyevent();
                }

                if (FD_ISSET(s_server, &readfds)) {
                        s_cli = accept(s_server, 0, 0);
                }

                if (FD_ISSET(s_cli, &readfds)) {
                        readSocketAndSend(&s_cli);
                }

        }
        close(s_server);
        return EXIT_SUCCESS;
}

void pollIR() {
        //printf("PollIng : %d\n", IR);
        tty_printf("AD%d\x0D", IR);
}




int tty_open(char* tty_dev) {
        struct termios options;


        tty_fd = open(tty_dev,O_RDWR| O_NOCTTY /*| O_NONBLOCK*/);

        if (tty_fd<0)
              return -1;

        tcgetattr(tty_fd,&tty_saved_attributes);
        tcgetattr(tty_fd,&options);
               
        // Set the new attributes for the serial port
        // http://linux.about.com/library/cmd/blcmdl3_termios.htm
        // http://www.gnu.org/software/libc/manual/html_node/Low_002dLevel-I_002fO.html#Low_002dLevel-I_002fO
               
        options.c_cflag |= CREAD;       // Enable receiver
        options.c_cflag |= B9600;     // Set baud rate
        options.c_cflag |= CS8;              // 8 data bit
        options.c_iflag |= IGNPAR;      // Ignore framing errors and parity errors.
//      options.c_lflag &= ~(ICANON);       // DISABLE canonical mode.
                                // Disables the special characters EOF, EOL, EOL2,
                                // ERASE, KILL, LNEXT, REPRINT, STATUS, and WERASE, and buffers by lines.
        options.c_lflag &= ~(ECHO);   // DISABLE this: Echo input characters.
        options.c_lflag &= ~(ECHOE);    // DISABLE this: If ICANON is also set, the ERASE character erases the preceding input
                                // character, and WERASE erases the preceding word.
        options.c_lflag &= ~(ISIG);   // DISABLE this: When any of the characters INTR, QUIT, SUSP,
                                // or DSUSP are received, generate the corresponding signal.
//      options.c_cc[VMIN]=1;  // Minimum number of characters for non-canonical read.
//      options.c_cc[VTIME]=0;                // Timeout in deciseconds for non-canonical read.

        (void)cfsetispeed(&options,  B9600);
        (void)cfsetospeed(&options,  B9600);

        tcsetattr(tty_fd, TCSANOW, &options);

       
        return tty_fd;
}


void tty_printf(char *format, ...) {
        va_list argptr;
        char buffer[256];
 
        va_start(argptr, format);
        vsprintf(buffer, format,argptr);
        va_end(argptr);
 
        write(tty_fd, buffer, strlen(buffer));
}

void termination_handler (int signum) {
        pthread_cancel(pollIR_thread);
        tcsetattr(STDIN_FILENO, TCSANOW, &stdin_saved_attributes);
        if (tty_fd > 0)
                tcsetattr (tty_fd, TCSANOW, &tty_saved_attributes);
        close(tty_fd);
        printf("Exit\n");
        exit(EXIT_FAILURE);
}

int stdin_init(void) {
        struct termios options;

        // Make sure stdin is a terminal
        if (!isatty(STDIN_FILENO)) {
                fprintf (stderr,"stdin is not a terminal\n");
          return -1;
        }

        // Save the terminal attributes so we can restore them later.
        tcgetattr (STDIN_FILENO, &stdin_saved_attributes);

        // Set the funny terminal modes.
        tcgetattr (STDIN_FILENO, &options);
        options.c_lflag &= ~(ICANON | ECHO); /* Clear ICANON and ECHO. */
        options.c_cc[VMIN] = 0;
        options.c_cc[VTIME] = 0;
        tcsetattr (STDIN_FILENO, TCSAFLUSH, &options);
        return 0;
}


void keyevent() {
        char c;
       
        c = getchar();
        if (c == 27) {
                c = getchar();
                if (c == 91) {
                        c = getchar(); 
                        if (c == 65) { // fleche haut
                                tty_printf("SV8I-1");
                        }              
                        if (c == 66) { // fleche bas
                                tty_printf("SV8I1");
                        }
                        if (c == 68) { // fleche gauche
                                tty_printf("SV7I-1");
                        }                                          
                        if (c == 67) { // fleche droit
                                tty_printf("SV7I1");
                        }
                        tty_printf("\x0D");               
                }
        }
        while ((c = getchar()) != '\n' && c != EOF);
}


void readValue() {
        int hsread;
        char c;
        int cr;

        cr = 1; 
        while((hsread = read(tty_fd, &c, 1)) && hsread != -1) {
                if (c >= '0' && c <= '9') {
                        printf("%c", c);
                        cr = 0;
                }
                else {
                        if (!cr)                       
                                printf("\n");
                        cr = 1;
                }
        }
}

int readIRAndSend() {

        char buffer[16] = {0};
        t_dataformat data2send;
        int hsread;
        unsigned char v;
        char ir;

        hsread = read(tty_fd, buffer, 16);
        if (hsread < 0)
                return 0;

        if (buffer[0] < '0' || buffer[0] > '9')
                return 0;

        ir = 'A' + IR - 1;
        v = strtol(buffer, (char **)NULL, 10);

        printf("%c : %d\n", ir, v);

        data2send.header = ir;
        data2send.value = v;

        if (s_cli)
                send(s_cli, (char *)&data2send, sizeof(data2send), 0);

        IR = (IR == 3) ? 1 : IR + 1;
        if (ir == ('C')) printf("--------------\n");

        return 1;
}


void readSocketAndSend(int *s) {
        char buffer[BUFFSIZE];
        int received = -1;
       
        /* Receive message */
        if ((received = recv(*s, buffer, 4, 0)) < 0) {
                *s = 0; 
                return ;
        }              
        if (buffer[0] == 'X' && received == 4) {
                tty_printf("SV7M%dSV8M%d\x0D", (unsigned char)buffer[1], (unsigned char)buffer[3]);
        }
        if (buffer[0] == 'G' && received == 4) {
                tty_printf("SV6M%dSV5M%d\x0D", (unsigned char)buffer[1], (unsigned char)buffer[3]);
        }
}


int initServer(int port) {
        int serversock;
        struct sockaddr_in echoserver;

       
        /* Create the TCP socket */
        if ((serversock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) {
              return 0;
        }
        /* Construct the server sockaddr_in structure */
        memset(&echoserver, 0, sizeof(echoserver));             /* Clear struct */
        echoserver.sin_family = AF_INET;                        /* Internet/IP */
        echoserver.sin_addr.s_addr = htonl(INADDR_ANY);         /* Incoming addr */
        echoserver.sin_port = htons(port);                     /* server port */

        /* Bind the server socket */
        if (bind(serversock, (struct sockaddr *) &echoserver, sizeof(echoserver)) < 0) {
                return 0;
        }
        /* Listen on the server socket */
        if (listen(serversock, 1) < 0) {
                return 0;
        }

        return serversock;
}