Exemplo n.º 1
0
static void *callback(enum mg_event event,
                      struct mg_connection *conn,
                      const struct mg_request_info *request_info)
{
    const int buf_max = 1000;
    char result[buf_max] = "none";

    if (event == MG_NEW_REQUEST) {
        //const char* uri = request_info->uri;

        // get query args
        char cmd[32];
        char timeoutStr[5];
        int timeout;
        char str[256];

        get_qsvar(request_info, "cmd", cmd, sizeof(cmd));
        get_qsvar(request_info, "str", str, sizeof(str));
        get_qsvar(request_info, "timeout", timeoutStr, sizeof(timeoutStr));

        timeout = timeoutStr[0] == '\0' ? 5000 : atoi(timeoutStr);

        if( ! cmd[0] ) { // cmd is empty 
            /// do something useful here
        }
        else {
            if( strcasecmp(cmd, "read")==0 ) { 
                serialport_flush(fd);
                serialport_read_until(fd, str, '\n', buf_max, timeout);
                sprintf(result, "read:\n%s\n",str); // debug
            }
            else if( strcasecmp(cmd, "send")==0 ) {
                serialport_write(fd, str);
                sprintf(result, "send:\n%s\n",str);
            }
            else if( strcasecmp(cmd, "sendline")==0 ) {
                sprintf(str, "%s\r\n ", str);
                serialport_write(fd, str);
                sprintf(result, "sendline:\n%s",str);
            }
            else {
                sprintf(result, "uknown cmd '%s'",cmd);
            }
        }

        printf("result:%s\n",result);
        // Echo requested URI back to the client
        mg_printf(conn, "HTTP/1.1 200 OK\r\n"
                  "Content-Type: text/plain\r\n\r\n"
                  "%s"
                  "\n", 
                  result
                  );
 
        return "";  // Mark as processed

    } else {
        return NULL;
    }
}
Exemplo n.º 2
0
void message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message) {
	DBG("%s -> %s\n", message->topic, (char *) message->payload);

	global_data_t *g = userdata;
	if(message->payload != NULL) {
		serialport_write(g->arduino_fd, message->payload);
		serialport_write(g->arduino_fd, "\n");
	}

}
Exemplo n.º 3
0
int serialport_send_slip_static(unsigned char *data, unsigned int size)
{
    unsigned char buf[STATIC_SLIP_BUF_SIZE];
    unsigned out_pos = 0;
    for (int i = 0; i < size; ++i)
    {
        unsigned char cur = data[i];
        if (cur == 0xC0)
        {
            buf[out_pos++] = subst_C0[0];
            buf[out_pos++] = subst_C0[1];
        }
        else if (cur == 0xDB)
        {
            buf[out_pos++] = subst_DB[0];
            buf[out_pos++] = subst_DB[1];
        }
        else
            buf[out_pos++] = cur;
    }
    
    if(serialport_write(buf, out_pos) != out_pos)
    {
        LOGERR("failed sending %d bytes", out_pos);
        return 0;
    }
    serialport_drain();
    return size;
}
Exemplo n.º 4
0
int main(int argc, char* argv[]) {
  if (argc == 1) {
    printf("You have to pass the name of a serial port.\n");
    return -1;
  }

  int baudrate = B9600;
  int arduino = serialport_init(argv[1], baudrate); 
  if (arduino == -1) {
    printf("Could not open serial port %s.\n", argv[1]);
    return -1;
  }
  sleep(2);

  char line[MAX_LINE];
  while (1) {
    int rc = serialport_write(arduino, "a0\n"); 
    if (rc == -1) {
      printf("Could not write to serial port.\n");
    } else {
      serialport_read_until(arduino, line, '\n'); 
      printf("%s", line);
    }
  }
  return 0;
}
Exemplo n.º 5
0
vector<uint8_t> ModCom::comunicate(vector<uint8_t> buffer, bool readdata, int sizeread) {
	uint8_t *buffer_out = &(buffer[0]);

	uint8_t buffer_in[255];

	serialport_flush(fd);
	serialport_write(fd, (char*)buffer_out);

	if (!readdata) {
		vector<uint8_t> toreturn;
		return toreturn;
	} // if only send
	
	//serial_read(fd, buffer_in, 1, DEF_TIMEOUT);
	
	serialport_read(fd, buffer_in, sizeread, DEF_TIMEOUT);

	if (buffer_in[0] != 0xFF) {
		vector<uint8_t> toreturn;
		return toreturn;
	}
	
	vector<uint8_t> vr(buffer_in, buffer_in + sizeread);
	
	return vr;
}
int main(int argc, char** argv)
{
  /********************
   * Set up ROS node
   */
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher tableDir_pub = n.advertise<alfred_msg::FSRDirection>("table_direction", 100);
  ros::Publisher tableAngle_pub = n.advertise<alfred_msg::TableAngleStatus>("TableAngleStatus", 100);

  // Set up serial connection
  char buf[20];
  char* modemDevice =  "/dev/ttyACM0";
  if( argc > 1 )
    modemDevice = argv[1];
  int fd = serialport_init(modemDevice, BAUDRATE);  
  if(fd < 0)  return -1;
  usleep(3000 * 1000 ); 
   
  printf("Connected to Arduino, starting loop\n");
  ros::Rate rate(50);
  while(ros::ok())
  {
      // Get the X coordinate
      serialport_write(fd, "x\0");
      serialport_read_until(fd, buf, ':');
      directionMessage.x = atof(buf);
      //printf("Received X:%f\n", directionMessage.x); 
      
      // Get the Y coordinate
      serialport_write(fd, "y");
      serialport_read_until(fd, buf, ':');
      directionMessage.y = atof(buf);
      //printf("Received Y:%f\n", directionMessage.y); 

      // Get the follow indicator
      serialport_write(fd, "f");
      serialport_read_until(fd, buf, ':');
      directionMessage.followPressed = atof(buf) == 1;
      //printf("Received F:%s\n", directionMessage.followPressed ? "true":"false" );

      tableDir_pub.publish(directionMessage);
      rate.sleep();
  }
}
Exemplo n.º 7
0
int serialport_send_slip(unsigned char *data, unsigned int size)
{
    unsigned int sent;
    unsigned char cur_byte;
    
    if (size < STATIC_SLIP_BUF_SIZE / 2)
        return serialport_send_slip_static(data, size);
    
    sent = 0;
    
    while(sent != size)
    {
        cur_byte = *data++;
        if(cur_byte == 0xC0)
        {
            if(serialport_write(subst_C0, 2) != 2)
            {
                LOGERR("failed substituting 0xC0");
                return 0;
            }
        }
        else if(cur_byte == 0xDB)
        {
            if(serialport_write(subst_DB, 2) != 2)
            {
                LOGERR("failed substituting 0xDB");
                return 0;
            }
        }
        else
        {
            if(serialport_write(&cur_byte, 1) != 1)
            {
                LOGERR("failed sending byte %i", sent);
                return 0;
            }
        }
        sent++;
    }

    serialport_drain();
    
    return sent;
}
Exemplo n.º 8
0
Arquivo: lold.c Projeto: apirogov/lold
//render a frame to real device or stdout
void render_frame(const char *dev, const char *frame) {
  if (dev==NULL) { //dummy
    dummy_output(frame);
    return;
  }

  //open port if no open port
  if (s_port == -1) {
    s_port = serialport_init(dev, 115200);
    if (s_port == -1) {
      fprintf(stderr, "Could not open serial device file! Aborting.\n");
      exit(EXIT_FAILURE);
    }
  }

  //write frame
  serialport_write(s_port, frame);
  serialport_write(s_port, "\n");
}
Exemplo n.º 9
0
int WriteTX(ServoStates* st){
	short* a = st->Angle;
	char buf[128];
	
	sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d\r\n",
		a[0], a[1], a[2], a[3],
		a[4], a[5], a[6], a[7]
	);
	return serialport_write(USB, buf);
}
IplImage *process(IplImage **_img)
{
	fprintf(stderr, "Processing image:\n");
	IplImage *img = *_img;

	/* Convert to HSV */
	print_time("Converting to HSV");
	CvSize size = cvGetSize(img);
	IplImage *hsv = cvCreateImage(size, IPL_DEPTH_8U, 3);
	cvCvtColor(img, hsv, CV_BGR2HSV);

	/* Generate mask */
	CvMat *mask = cvCreateMat(size.height, size.width, CV_8UC1);
	//cvInRangeS(hsv, cvScalar(0.11*256, 0.60*256, 0.20*256, 0),cvScalar(0.14*256, 1.00*256, 1.00*256, 0), mask);
	cvInRangeS(hsv, cvScalar(0,0.6*256,0.6*256,0),cvScalar(0.21*256,256,256,0), mask);
	cvReleaseImage(&hsv);

	/* Perform morphological ops */
	print_time("Performing morphologies");
	IplConvKernel *se21 = cvCreateStructuringElementEx(21, 21, 10, 10, CV_SHAPE_RECT, NULL);
	IplConvKernel *se11 = cvCreateStructuringElementEx(11, 11, 5,  5,  CV_SHAPE_RECT, NULL);
	cvClose(mask, mask, se21);
	cvOpen(mask, mask, se11);
	cvReleaseStructuringElement(&se21);
	cvReleaseStructuringElement(&se11);

	/* Hough transform */
	IplImage *hough_in = cvCreateImage(size, 8, 1);
	cvCopy(mask, hough_in, NULL);
	int rows=size.height;
	int cols=size.width;
	int j,k;
	int breakflag=0;
	for(j=0;j<rows;j++) {
	  for(k=0;k<cols;k++)
	    {
		CvScalar val=cvGet2D(hough_in,j,k);
		if(val.val[0]==255)
		{
		  sprintf(dat,"%d-%d:",k,j);
		  int rc = serialport_write(fd, dat);
	          if(rc==-1) return 0;
		  fprintf(fp,"%d %d\n",k,j);	
		  breakflag=1;
		  break;
		}
	    }
	  if(breakflag)
	    break;
	}
	return hough_in;
}
Exemplo n.º 11
0
int serialport_send_C0(void)
{
    unsigned char b;

    b = 0xC0;
    
    if(serialport_write(&b, 1) != 1)
    {
        LOGERR("failed sending 0xC0");
        return 0;
    }
    serialport_drain();
    return 1;
}
Exemplo n.º 12
0
int main(int argc, char *argv[]) 
{

	int baudrate = B9600;  // default
	int fd;
	char cmd = 0;
	char cmdv[3] = "a";
	fd = serialport_init(ARD_LOC, baudrate);
	
	if((argv[1][0]) == 'p'){
		while(1){
			printf("Water Turret$: ");
			scanf("%c",&cmd);
			// printf("1\n");
			cmdv[0] = (char)cmd;
			// printf("2\n");
			if(cmd == 'x'){
				// printf("3\n");
				return 0;
			}
			// printf("4\n");
			serialport_write(fd, cmdv);
		}
	}
	
	serialport_write(fd, "a");
	serialport_write(fd, "w");
	serialport_write(fd, "a");
	serialport_write(fd, "w");
	
	serialport_write(fd, "d");
	serialport_write(fd, "d");
	
	serialport_write(fd, "s");
	
	serialport_write(fd, "s");
	
	


} 
Exemplo n.º 13
0
int send_string(char* in_string){

    //printf("in send_string\n");

    int rc = -1;

    if( fd == -1 ){ 
        printf("serial port not opened");
        return -1;
    }
    //sprintf(in_string, "%s", optarg);
    printf("send string:%s\n", in_string);
    rc = serialport_write(fd, in_string);
    if(rc==-1){
        printf("error writing");
        return -2;
    }

    return 0;
}
Exemplo n.º 14
0
//发送串口通讯数据包
static inline void CommSend(BYTE size)
{
	WORD sum;
    BYTE i;
    
    UartSend(0x46);
    UartSend(0xb9);
    UartSend(0x6a);
    UartSend(0x00);
    sum = size + 6 + 0x6a;
    UartSend(size + 6);
	serialport_write(handle_download_port,TxBuffer,size);
    for (i=0; i<size; i++)
    {
        sum += TxBuffer[i];
    }
    UartSend(HIBYTE(sum));
    UartSend(LOBYTE(sum));
    UartSend(0x16);

    CommInit();
}
Exemplo n.º 15
0
void web_child(int connfd, int fd){
    int n;
    char buffer[256];
    bzero(buffer,256);
    n = read(connfd,buffer,255);
    if (n < 0){
        perror("ERROR reading from socket");
        exit(1);
    }
    //printf("Here is the message: %s\n",buffer);
    //strncat(buffer,"\n", sizeof buffer);
    //n = write(connfd,"I got your message",18);
        serialport_write(fd, buffer); //I have to error check this
        FILE *fp = fopen("out1.txt","a"); /* Open file in append monde */
        fprintf(fp, "Read string: %s", buffer); /* Write output in file */
        fclose(fp);/* Close file */
    if (n < 0){
        perror("ERROR writing to socket");
        exit(1);
    }
    return;
}
Exemplo n.º 16
0
int send_string(char* in_string){

    //printf("in send_string\n");

    int rc = -1;

    if( fd == -1 ){ 
        printf("serial port not opened\n");
        return -1;
    }

    //print strings that aren't spacers
    if (in_string[0] != ' ')
        printf("string:%s\n", in_string);

    rc = serialport_write(fd, in_string);
    if(rc==-1){
        printf("error writing\n");
        return -2;
    }

    return 0;
}
int main(int argc, char *argv[])
{
    int fd = 0;
    char serialport[256];
    int baudrate = B9600;  // default
    char buf[256];
    int rc,n;
    
    if (argc==1) {
        usage();
        exit(EXIT_SUCCESS);
    }
    
    /* parse options */
    int option_index = 0, opt;
    static struct option loptions[] = {
        {"help",       no_argument,       0, 'h'},
        {"port",       required_argument, 0, 'p'},
        {"baud",       required_argument, 0, 'b'},
        {"send",       required_argument, 0, 's'},
        {"receive",    no_argument,       0, 'r'},
        {"num",        required_argument, 0, 'n'},
        {"delay",      required_argument, 0, 'd'}
    };
    
    while(1) {
        opt = getopt_long (argc, argv, "hp:b:s:rn:d:",
                           loptions, &option_index);
        if (opt==-1) break;
        switch (opt) {
            case '0': break;
            case 'd':
                n = strtol(optarg,NULL,10);
                usleep(n * 1000 ); // sleep milliseconds
                break;
            case 'h':
                usage();
                break;
            case 'b':
                baudrate = strtol(optarg,NULL,10);
                break;
            case 'p':
                strcpy(serialport,optarg);
                fd = serialport_init(optarg, baudrate);
                if(fd==-1) return -1;
                break;
            case 'n':
                n = strtol(optarg, NULL, 10); // convert string to number
                rc = serialport_writebyte(fd, (uint8_t)n);
                if(rc==-1) return -1;
                break;
            case 's':
                strcpy(buf,optarg);
                rc = serialport_write(fd, buf);
                if(rc==-1) return -1;
                break;
            case 'r':
                serialport_read_until(fd, buf, '\n');
                printf("read: %s\n",buf);
                break;
        }
    }
    
    exit(EXIT_SUCCESS);
} // end main
Exemplo n.º 18
0
int main(int argc, char *argv[])
{
    /*accelerometer related declarations*/
    const int buf_max = 2048;

    int fd = -1;
    char serialport[buf_max];
    int baudrate = 9600;  // default
    char quiet=0;
    char eolchar = ' ';
    int timeout = 5000;
    char buf[buf_max];
    int rc,n,p;
    float xDat[103],yDat[103],zDat[103];
    float sumX=0,sumY=0,sumZ=0;
    float sum1=0,sum2=0,sum3=0;
    float stdX=0,stdY=0,stdZ=0,avgX=0,avgY=0,avgZ=0,varX=0,varY=0,varZ=0,recal=0;
    int total=1;
    int calibrate=0;
    /*---------------------------------*/

//    char stdbuffer[10];
    /*socket related declarations*/    
    int sockfd, portno, n2;
    struct sockaddr_in serv_addr;
    struct hostent *server;

    char buffer[256];
    int sendMessage=1;
   /*----------------------------*/

    if (argc==1) {
        usage();
    }

    /* parse options */
    int option_index = 0, opt;
    static struct option loptions[] = {
        {"help",       no_argument,       0, 'h'},
        {"port",       required_argument, 0, 'p'},
        {"baud",       required_argument, 0, 'b'},
        {"send",       required_argument, 0, 's'},
        {"setup",      no_argument,       0, 'S'},
        {"receive",    no_argument,       0, 'r'},
        {"flush",      no_argument,       0, 'F'},
        {"num",        required_argument, 0, 'n'},
        {"delay",      required_argument, 0, 'd'},
        {"eolchar",    required_argument, 0, 'e'},
        {"timeout",    required_argument, 0, 't'},
        {"quiet",      no_argument,       0, 'q'},
        {NULL,         0,                 0, 0}
    };

    while(1) {
        opt = getopt_long (argc, argv, "hp:b:s:rSFn:d:qe:t:",
                           loptions, &option_index);
        if (opt==-1) break;
        switch (opt) {
        case '0': break;
        case 'q':
            quiet = 1;
            break;
        case 'e':
            eolchar = optarg[0];
            if(!quiet) printf("eolchar set to '%c'\n",eolchar);
            break;
        case 't':
            timeout = strtol(optarg,NULL,10);
            if( !quiet ) printf("timeout set to %d millisecs\n",timeout);
            break;
        case 'd':
            n = strtol(optarg,NULL,10);
            if( !quiet ) printf("sleep %d millisecs\n",n);
            usleep(n * 1000 ); // sleep milliseconds
            break;
        case 'h':
            usage();
            break;
        case 'b':
            baudrate = strtol(optarg,NULL,10);
            break;
        case 'p':
            if( fd!=-1 ) {
                serialport_close(fd);
                if(!quiet) printf("closed port %s\n",serialport);
            }
            strcpy(serialport,optarg);
            fd = serialport_init(optarg, baudrate);
            if( fd==-1 ) error("couldn't open port");
            if(!quiet) printf("opened port %s\n",serialport);
            serialport_flush(fd);
            break;
        case 'n':
            if( fd == -1 ) error("serial port not opened");
            n = strtol(optarg, NULL, 10); // convert string to number
            rc = serialport_writebyte(fd, (uint8_t)n);
            if(rc==-1) error("error writing");
            break;
        case 'S':
	     portno = 52002;//atoi(argv[2]);
             /* Create a socket point*/ 
             sockfd = socket(AF_INET, SOCK_STREAM, 0);
             if (sockfd < 0) 
             {
                perror("ERROR opening socket");
                exit(1);
             }
             server = gethostbyname("192.168.2.103");//gethostbyname(argv[1]);
             if (server == NULL) {
                fprintf(stderr,"ERROR, no such host\n");
                exit(0);
             }

             bzero((char *) &serv_addr, sizeof(serv_addr));
             serv_addr.sin_family = AF_INET;
             bcopy((char *)server->h_addr, 
                   (char *)&serv_addr.sin_addr.s_addr,
                         server->h_length);
             serv_addr.sin_port = htons(portno);

             /* Now connect to the server */
             if (connect(sockfd,&serv_addr,sizeof(serv_addr)) < 0) 
             {
                  perror("ERROR connecting");
                  exit(1);
              }
              while(1){
              int r;
              int a1, a2, a3;
	      char a4[sizeof(float)*3+3];
              memset (buf, 0, 256);
              r = serialport_read_until(fd, buf);
              //fprintf (stderr, "VAL: %s -->", buf);
              if (r >= 0)
              {
               sscanf (buf, "A1:%d\tA2:%d\tA3:%d", &a1, &a2, &a3);
              // fprintf (stderr, "(%d)(%d)(%d)\n", a1, a2, a3);
               if(calibrate==0){
                  sumX= sumX + a1; sumY= sumY + a2; sumZ= sumZ + a3;
                  xDat[total]=a1;yDat[total]=a2;zDat[total]=a3;
                  total++;
              if (total==100){ 
		  calibrate=1;
                  avgX = sumX/(float)total; avgY = sumY/(float)total; avgZ = sumZ/(float)total;
                  for(p = 1; p<total;p++){
                  sum1 = sum1 + pow((xDat[p]-avgX),2); sum2 = sum2 + pow((yDat[p]-avgY),2); sum3 = sum3 + pow((zDat[p]-avgZ),2);
                  }
		   varX= sum1/(float)total; varY=sum2/(float)total; varZ=sum3/(float)total;
                   stdX=sqrt(varX); stdY=sqrt(varY); stdZ=sqrt(varZ);
               }
	       }
               if(calibrate==1){snprintf(a4, sizeof a4*7, "%f %f %f %f %f %f %f %f %f", (float)a1, (float)a2,(float)a3,avgX,avgY,avgZ,stdX,stdY,stdZ);}
               while(sendMessage==1&&calibrate==1){
                /* Send message to the server*/ 
                //printf("here");
		n2 = write(sockfd,a4,strlen(a4));
                if (n2 < 0) 
                {
                        perror("ERROR writing to socket");
                        exit(1);
                }
                /* Now read server response */
                bzero(buffer,256);
                n2 = read(sockfd,buffer,255);
                    if (n2 < 0) 
                {
                        perror("ERROR reading from socket");
                        exit(1);
                }
//                printf("Received message: %s\n",buffer);
                 sendMessage=0;
                }
             sendMessage=1; 
	     }
              sendMessage=1;
              fflush (0);
              //sleep (5);
           }    
           break;
        case 's':
            if( fd == -1 ) error("serial port not opened");

            sprintf(buf, (opt=='S' ? "%s\n" : "%s"), optarg);

            if( !quiet ) printf("send string:%s\n", buf);
            rc = serialport_write(fd, buf);
            if(rc==-1) error("error writing");
            break;
        case 'r':
	  while (1)
	    {
	      int r;
	      int a1, a2, a3;

	      memset (buf, 0, 256);
	      r = serialport_read_until(fd, buf);
              fprintf (stderr, "VAL: %s -->", buf);
//              printf("%d",r);
	      if (r >= 0)
		{
		 sscanf (buf, "A1:%d\tA2:%d\tA3:%d", &a1, &a2, &a3);
		 fprintf (stderr, "(%d)(%d)(%d)\n", a1, a2, a3);
                 //fgets(stdbuffer,10,stdin);
	         //if(strcmp(stdbuffer,"getData")==0){
		 // printf("%d",a1);
                 // printf("%d",a2);
                 // printf("%d",a3);}
                //}
        /*       while(sendMessage==1){
    		/* Send message to the server 
    		n2 = write(sockfd,"measure",strlen("measure"));
    		if (n2 < 0) 
    		{
         		perror("ERROR writing to socket");
         		exit(1);
    		}
    		/* Now read server response 
    		bzero(buffer,256);
    		n2 = read(sockfd,buffer,255);
        	    if (n2 < 0) 
    		{
         		perror("ERROR reading from socket");
         		exit(1);
    		}
    		printf("Received message: %s\n",buffer);
                sendMessage=0;
   		}*/
              }
	      fflush (0);
	      usleep (10000);
	    }
            break;
        case 'F':
            if( fd == -1 ) error("serial port not opened");
            if( !quiet ) printf("flushing receive buffer\n");
            serialport_flush(fd);
            break;

        }
    }

    exit(EXIT_SUCCESS);
} // end main
Exemplo n.º 19
0
//串口数据发送程序
static inline BYTE UartSend(BYTE dat)
{
	serialport_write(handle_download_port,&dat,1);
	return dat;
}
Exemplo n.º 20
0
int main(int argc, char** argv){

   /*******************
    *  Base Drive Variables
    *
    * LOOP_RATE         defines how fast we send orders to the arduino
    * ORDER_TIMEOUT     is the maximum amount of time between recieving orders
    *                   before we shut the motors off
    *
    * BAUDRATE          baudrate to arduino
    * SERIAL_DEV        the defauly dev file pointing to the arduino
    * READ_CHAR_TIMEOUT the timeout to wait for the next character in a
    *                   message, in us
    * SERIAL_RESPONSE_TIMEOUT The timeout to wait for a response to our
    *                         query, in us
    */
   double LOOP_RATE = 10.0;
   double ORDER_TIMEOUT = 0.25;

   int BAUDRATE = B115200;
   char* SERIAL_DEV = "/dev/ttyUSB0";
   int READ_CHAR_TIMEOUT = 30;
   int SERIAL_RESPONSE_TIMEOUT = 100;
   char START_CHAR = '`';

   /********************
    * Set up ROS node
    */
   ros::init(argc, argv, "TableMotorToSerial");
   ros::NodeHandle n;

   ROS_INFO( "Starting Subscriber" );
   ros::Subscriber sub = n.subscribe("DriveTable", 2, callbackTableMotorToSerial ); 

   ros::Publisher pub = n.advertise<alfred_msg::TableEncoders>("TableEncoders", 2);

   /********************
    * Setup loop variables and start serial
    */
   char* serialDevice = SERIAL_DEV;
   if( argc > 1 )
      serialDevice = argv[1];

   int fd = serialport_init(serialDevice, BAUDRATE);
   if(fd < 0){
      ROS_ERROR( "Could not open serial device \'%s\'\n", serialDevice); 
      return -1;
   }
   usleep( 3000 * 1000 );
   
   ros::Rate rate(LOOP_RATE);
   int maxNumLoopsWithoutOrder = (int)( ORDER_TIMEOUT * LOOP_RATE );
   int numLoopsWithoutOrder(-1);
    
   
   while (n.ok()) {

      // If we haven't yet started
      if( numLoopsWithoutOrder == -1){
         tableOrder_serialArduino1 = 0.0;
         tableOrder_serialArduino2 = 0.0;
         tableOrder_serialArduino3 = 0.0;
         tableOrder_serialArduino4 = 0.0;
      }
      // Safety Check 1 : time without orders
      else if( numLoopsWithoutOrder >= maxNumLoopsWithoutOrder ){

         tableOrder_serialArduino1 = 0.0;
         tableOrder_serialArduino2 = 0.0;
         tableOrder_serialArduino3 = 0.0;
         tableOrder_serialArduino4 = 0.0;
         ROS_ERROR( "Base Motor to Serial hasn't received a new motor order in %f mS!", 1000 * ORDER_TIMEOUT );
      }
      // Safety check passed, send 
    
      if( numLoopsWithoutOrder >= 0 ) 
         numLoopsWithoutOrder++;
      if( receivedOrder_serialArduino == true ){
         numLoopsWithoutOrder = 0;
         receivedOrder_serialArduino = false;
      }

      ROS_INFO( "Sending output to table: [%f, %f, %f, %f]",
                  tableOrder_serialArduino1,
                  tableOrder_serialArduino2,
                  tableOrder_serialArduino3,
                  tableOrder_serialArduino4);
 
      //write
      std::stringstream ss;
      ss << "`" 
         << tableOrder_serialArduino1 << ";"
         << tableOrder_serialArduino2 << ";"
         << tableOrder_serialArduino3 << ";"
         << tableOrder_serialArduino4 << ";\0";
      std::string msg = ss.str();
   
      int isok = serialport_write(fd, msg.c_str() );

      // now read, and wait for the arduino to reply with the encoder values
      int encoders[4];
      char buf[10];

      // First, wait for a response      
      if( serialport_read_until(fd, buf, START_CHAR, SERIAL_RESPONSE_TIMEOUT) <= 0)
      {
         ROS_ERROR("ERROR: Timeout on serial response message start!");
         for( int i(0); i < 4; i++ )
            encoders[i]=-1;
      }
      else{
         for( int i(0); i < 4; i++ ){
            isok = serialport_read_until(fd, buf, ';',  READ_CHAR_TIMEOUT );
            if( isok <= 0 ){
               ROS_ERROR("ERROR: Timeout reading message");
               break;
            }
            encoders[i] = std::atoi(buf);
         }
         if( isok <= 0 )
            for( int i(0); i < 4; i++ )
               encoders[i]=-1;
      }

      ROS_INFO( "Publishing Encoder Vals: [%d, %d, %d, %d]",
                  encoders[0],
                  encoders[1],
                  encoders[2],
                  encoders[3]);

      alfred_msg::TableEncoders encTopic;
      encTopic.enc1 = encoders[0];
      encTopic.enc2 = encoders[1];
      encTopic.enc3 = encoders[2];
      encTopic.enc4 = encoders[3];

      pub.publish(encTopic);
   
      ROS_INFO("\n");
      rate.sleep();
      ros::spinOnce();
   }

   serialport_close(fd);
   return 0;
}