CodeBackUP_node_find_serial

/***************************************************************
**  Find the serial number Node
**  Receive the serial data in order to determine the number
**  Version V1.0   From jinliming  CT-TC  2017-07-27
***************************************************************/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <iostream>
#include <time.h>
#include <pthread.h>
#include <stdio.h>
#include <unistd.h>
#include "inirw.h"
//#include "HR_Package/ComMsg.h"
#include "node_find_serial.h"

using namespace std;

char SerNum = 0;
int Serial_nFd;
Rd_Ser ReadSer = {0,0,0,0};
char* SerialNameBase = (char*)"/dev/ttyUSB";
unsigned char SerialRecv_Buff[64]={0};
unsigned char SerialNumber[10]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};

pthread_mutex_t ReadSer_Mutex;


/**************************************
** open serial port
***************************************/
/*unsigned char OpenSerial_Num(unsigned char num)
{
  char Str_Num[30];
  char* SerialName;
  unsigned char State = 0;

  sprintf(Str_Num,"%d",num);
  SerialName = Str_Stitching(SerialNameBase,Str_Num);
  switch(ReadSer.FindS_Type)
  {
   case 0x01:State = Serial_Config(SerialName,115200,2,1,0);break;
   case 0x02:break;
   case 0x03:break;
   case 0x04:State = Serial_Config(SerialName,115200,1,0,40);break;
  }
  printf("%d, SerialName:%s
",Serial_nFd,SerialName);
  return State;
}*/
/*****************************************
** Receive serial data
*****************************************/
void *Thread_RdSerial(void *arg)
{
  fd_set rfds;
  unsigned char i,j =0;
  unsigned char Ser_TempBuf[8]={0};
  unsigned char Ser_TempBuf80[80]={0};
  memset(SerialRecv_Buff,0,64);
  FD_ZERO(&rfds);
  FD_SET(Serial_nFd,&rfds);
  while(1)
  {
   if(ReadSer.FindS_Type == 0x01)
   {
	ReadSer.Recv_Len = read(Serial_nFd,Ser_TempBuf,8); // Receive the serial data//
	if(ReadSer.Recv_Len<=MOTOR_RECV_LEN)
    {
	 pthread_mutex_lock(&ReadSer_Mutex);
	 for(i=ReadSer.Recv_LAdd;i<MOTOR_RECV_LEN;i++){SerialRecv_Buff[i] = Ser_TempBuf[j]; j++;}
	 pthread_mutex_unlock(&ReadSer_Mutex);
	 j = 0;
	 ReadSer.Recv_LAdd+=ReadSer.Recv_Len;
	 if(ReadSer.Recv_LAdd>MOTOR_RECV_LEN)ReadSer.Recv_LAdd = 0;
	}
	else if(ReadSer.Recv_Len>254){ReadSer.Recv_Len = 0;}
	if(ReadSer.Recv_LAdd>MOTOR_RECV_LEN)ReadSer.Recv_LAdd = 0;
    if(ReadSer.Recv_LAdd == MOTOR_RECV_LEN)
    {
     if((SerialRecv_Buff[1]==0x06)&&(SerialRecv_Buff[3]==0x40))
     {
      ReadSer.Recv_LAdd = 0;
      switch(SerialRecv_Buff[0])
	  {
	   case 0x01:SerialNumber[0] = SerNum;break;//将串口号保存到数组 j=0~4 四个电机的数据在数组的前面0~4个元素
	   case 0x02:SerialNumber[1] = SerNum;break;
	   case 0x03:SerialNumber[2] = SerNum;break;
	   case 0x04:SerialNumber[3] = SerNum;break;
	   case 0x05:SerialNumber[4] = SerNum;break;
	   case 0x06:SerialNumber[5] = SerNum;break;
	  }
	  printf("It`s Motor %d Data",SerialRecv_Buff[0]);
	  printf("
");
     }
     else
     {
    	 for(i=0;i<10;i++)
    	 	printf("0x%02x ",SerialNumber[8]);
    	 ReadSer.Recv_LAdd = 0;
    	 memset(SerialRecv_Buff,0,64);
    	 printf("Not Motor Data!");
     }
    }
  }
   else if(ReadSer.FindS_Type == 0x02) // 电源板高频
   {
	  ReadSer.Recv_Len = read(Serial_nFd,SerialRecv_Buff,80); // Receive the serial data//
	  if(ReadSer.Recv_Len>254)ReadSer.Recv_Len = 0;
	  if(ReadSer.Recv_Len<40)
	  {
	  	for(i=ReadSer.Recv_LAdd;i<40;i++){Ser_TempBuf80[i] = SerialRecv_Buff[j]; j++;}
	  	j = 0;
	  	ReadSer.Recv_LAdd+=ReadSer.Recv_Len;
	  }
	  if(ReadSer.Recv_LAdd>=40)
	  {
	     for(i=0;i<40;i++){SerialRecv_Buff[i] = Ser_TempBuf80[i];}
	  }
	  if((ReadSer.Recv_Len>=22)||(ReadSer.Recv_LAdd>=40))
	  {
	   ReadSer.Recv_LAdd = 0;
	   //printf("Recv_Len = %d
",ReadSer.Recv_Len);
	   for(i=0;i<30;i++)
	   {
	    if((SerialRecv_Buff[i]==0xFF)&&(SerialRecv_Buff[i+1]==0xFE)&&(SerialRecv_Buff[i+2]==0xFE)&&(SerialRecv_Buff[i+3]==0xFF))
	    {
	     if((SerialRecv_Buff[i+18]==0xFE)&&(SerialRecv_Buff[i+19]==0xFF)&&(SerialRecv_Buff[i+20]==0xFF)&&(SerialRecv_Buff[i+21]==0xFE))
	     {
		  SerialNumber[7]= SerNum;
		  ReadSer.Recv_Flg = 0x02;
		  memset(SerialRecv_Buff,0,64);
		  printf("It`s Power High Speed Data
");
		  break;
	     }}}
	  }
   }
   else if(ReadSer.FindS_Type == 0x03) // 电源板低频
   {
	  ReadSer.Recv_Len = read(Serial_nFd,SerialRecv_Buff,80); // Receive the serial data//
	  if(ReadSer.Recv_Len>254)ReadSer.Recv_Len = 0;
	  if(ReadSer.Recv_Len<36)
	  {
	  	for(i=ReadSer.Recv_LAdd;i<36;i++){Ser_TempBuf80[i] = SerialRecv_Buff[j]; j++;}
	  	j = 0;
	  	ReadSer.Recv_LAdd+=ReadSer.Recv_Len;
	  }
	  if(ReadSer.Recv_LAdd>=36)
	  {
	     for(i=0;i<36;i++){SerialRecv_Buff[i] = Ser_TempBuf80[i];}
	  }
	  if((ReadSer.Recv_Len>=18)||(ReadSer.Recv_LAdd>=36))
	  {
		ReadSer.Recv_LAdd = 0;
	  // printf("Recv_Len = %d
",ReadSer.Recv_Len);
	   for(i=0;i<36;i++)
	   {
		if((SerialRecv_Buff[i]==0xFF)&&(SerialRecv_Buff[i+1]==0xFE)&&(SerialRecv_Buff[i+2]==0xFE)&&(SerialRecv_Buff[i+3]==0xFF))
		{
		 if((SerialRecv_Buff[i+14]==0xFE)&&(SerialRecv_Buff[i+15]==0xFF)&&(SerialRecv_Buff[i+16]==0xFF)&&(SerialRecv_Buff[i+17]==0xFE))
		 {
		  SerialNumber[8]= SerNum;
		  ReadSer.Recv_Flg = 0x03;
		  memset(SerialRecv_Buff,0,64);
		  printf("It`s Power Low Speed Data
");
		  break;
		 }}}
	  }
   }
   else if(ReadSer.FindS_Type == 0x04)//数据底板
   {
     ReadSer.Recv_Len = read(Serial_nFd,SerialRecv_Buff,80); // Receive the serial data//
     if(ReadSer.Recv_Len>254)ReadSer.Recv_Len = 0;
     if(ReadSer.Recv_Len<50)
	 {
	  for(i=ReadSer.Recv_LAdd;i<50;i++){Ser_TempBuf80[i] = SerialRecv_Buff[j]; j++;}
	  j = 0;
	  ReadSer.Recv_LAdd+=ReadSer.Recv_Len;
	 }
     if(ReadSer.Recv_LAdd>=50)
     {
    	 for(i=0;i<50;i++){SerialRecv_Buff[i] = Ser_TempBuf80[i];}
     }
     if((ReadSer.Recv_Len>=40)||(ReadSer.Recv_LAdd>=50))
     {
      ReadSer.Recv_LAdd = 0;
      //printf("Recv_Len = %d
",ReadSer.Recv_Len);
      for(i=0;i<50;i++)
      {
       if((SerialRecv_Buff[i]==0xFF)&&(SerialRecv_Buff[i+1]==0xFE)&&(SerialRecv_Buff[i+2]==0xFE)&&(SerialRecv_Buff[i+3]==0xFF))
       {
    	 if((SerialRecv_Buff[i+36]==0xFE)&&(SerialRecv_Buff[i+37]==0xFF)&&(SerialRecv_Buff[i+38]==0xFF)&&(SerialRecv_Buff[i+39]==0xFE))
		 {
    		SerialNumber[6]= SerNum;
    		ReadSer.Recv_Flg = 0x01;
    		memset(SerialRecv_Buff,0,64);
	        printf("It`s SignBoard Data
");
	        break;
		 }}}
       }
   }
   else if(ReadSer.FindS_Type == 0x05) // 超声波
   {

   }
   else if(ReadSer.FindS_Type == 0xAA) // 搜索串口结束
   {
	   printf("Pthread Over! 
");
	   pthread_exit(0);
   }
 }
  return 0;
}

unsigned char FindMotorDriver_Serial(void)
{
  unsigned char i=0,j=0;
  unsigned char SerState = 0;
  ReadSer.FindS_Type = 0x01;
  sleep(1);
  while(1)
  {
	for(i=0;i<MAX_SER_NUM;i++)
	{
	  pthread_mutex_lock(&ReadSer_Mutex);
	  SerState = OpenSerial_Num(i);
	  SerNum = i;
	  usleep(250000); //留够时间让串口数据进程读出缓冲区的数据
	  pthread_mutex_unlock(&ReadSer_Mutex);
	  for(j=0;j<MAX_MOTOR_NUM;j++)
	  {
		  if(SerState == 0xFF){printf("Step Over
");break;}//打开串口失败 直接退出当前循环
		  usleep(4000);
		  Sernd_StopMsg(Serial_nFd,j);
		  usleep(4000);
		  Sernd_StopMsg(Serial_nFd,j);
		  if(j==0)
		  {
			usleep(4000);
			Sernd_StopMsg(Serial_nFd,0);
			usleep(4000);
			Sernd_StopMsg(Serial_nFd,0);
		  }
	  }
	  close(Serial_nFd);
	}
	for(i=0;i<10;i++)
	printf("0x%02x ",SerialNumber[i]);
	printf("
");
	//printf("LF:%d RF:%d LB:%d RB:%d UD:%d FX:%d
",SerialNumber[0],SerialNumber[1],SerialNumber[2],SerialNumber[3],SerialNumber[4],SerialNumber[5]);
	break;
  }
  return 0;
}
unsigned char FindPowerH_Serial(void)
{
  unsigned char i=0,j=0;
  unsigned char SerState = 0;
  ReadSer.FindS_Type = 0x02; //电源板高频信号检测 数据长度22字节
  sleep(1);
  for(i=0;i<MAX_SER_NUM;i++)
  {
   SerState = OpenSerial_Num(i);
   SerNum = i;
   usleep(500000); //留够时间让串口数据进程读出缓冲区的数据
   close(Serial_nFd);
   if(ReadSer.Recv_Flg == 0x02)
   	{
   		//pthread_mutex_lock(&ReadSer_Mutex);
   		ReadSer.Recv_Flg = 0x00;
   		//pthread_mutex_unlock(&ReadSer_Mutex);
   		break;
   	}
  }
  for(i=0;i<10;i++)
    printf("0x%02x ",SerialNumber[i]);
  printf("
");
  return 0;
}

unsigned char FindPowerL_Serial(void)
{
 unsigned char i=0,j=0;
 unsigned char SerState = 0;
 ReadSer.FindS_Type = 0x03; //电源板低频信号检测 数据长度18字节
 sleep(1);
 for(i=0;i<MAX_SER_NUM;i++)
 {
  SerState = OpenSerial_Num(i);
  SerNum = i;
  usleep(400000); //留够时间让串口数据进程读出缓冲区的数据
  close(Serial_nFd);
  if(ReadSer.Recv_Flg == 0x03)
  {
  	ReadSer.Recv_Flg = 0x00;
  	break;
  }
 }
  for(i=0;i<10;i++)
   printf("0x%02x ",SerialNumber[i]);
  printf("
");
 return 0;
}
unsigned char FindSignBoard_Serial(void)
{
  unsigned char i=0,j=0;
  unsigned char SerState = 0;
  ReadSer.FindS_Type = 0x04; //数据底板数据检测
  sleep(1);
  for(i=0;i<MAX_SER_NUM;i++)
  {
	SerState = OpenSerial_Num(i);
	SerNum = i;
	usleep(400000); //留够时间让串口数据进程读出缓冲区的数据
	close(Serial_nFd);
	if(ReadSer.Recv_Flg == 0x01)
	{
		ReadSer.Recv_Flg = 0x00;
		break;
	}
  }
  for(i=0;i<10;i++)
  	printf("0x%02x ",SerialNumber[i]);
  	printf("
");
  return 0;
}

int main(int argc, char **argv)
{
	pthread_t SerialData;
	ReadSer.FindS_Type = 0x01;//
	OpenSerial_Num(0);
	pthread_mutex_init(&ReadSer_Mutex,NULL);
	pthread_create(&SerialData,NULL,Thread_RdSerial,NULL);
	printf("*********************************************************************
");
	printf("************   Start detecting serial port numbers!   ***************
");
	printf("*********************************************************************
");
	FindMotorDriver_Serial();
	printf("*********************************************************************
");
	printf("Motor Find is over!
");
	printf("*********************************************************************
");
	FindSignBoard_Serial();
	printf("*********************************************************************
");
	printf("SignBoard Find is over!
");
	printf("*********************************************************************
");
	FindPowerH_Serial();
	printf("*********************************************************************
");
	printf("High Power Find is over!
");
	printf("*********************************************************************
");
	FindPowerL_Serial();
	//pthread_cancel(SerialData);
	ReadSer.FindS_Type = 0xAA;
	sleep(1);
	printf("*********************************************************************
");
        printf("************           Detecting is finish!           ***************
");
	printf("*********************************************************************
");
	printf("LF:%02d RF:%02d LB:%02d RB:%02d UD:%02d FX:%02d
",SerialNumber[0],SerialNumber[1],SerialNumber[2],SerialNumber[3],SerialNumber[4],SerialNumber[5]);
	printf("SB:%02d HP:%02d LP:%02d
",SerialNumber[6],SerialNumber[7],SerialNumber[8]);
	exit(0);
	while(1);
}

 

/*
 * node_find_serial.h
 *
 *  Created on: 2017年7月29日
 *      Author: kopu
 */

#ifndef ROBOT_PKG_INCLUDE_NODE_FIND_SERIAL_H_
#define ROBOT_PKG_INCLUDE_NODE_FIND_SERIAL_H_

#define  M_LF      0
#define  M_RF      1
#define  M_LB      2
#define  M_RB      3
#define  M_UD      4
#define  M_FX      5

#define  MOTOR_RECV_LEN   8
#define  DATA_BASE_LEN    40

#define  MAX_SER_NUM      20
#define  MAX_MOTOR_NUM    6

typedef struct
{
  unsigned char Recv_Len;
  unsigned char Recv_Flg;
  unsigned char Recv_LAdd;
  unsigned char FindS_Type;
}Rd_Ser;
extern Rd_Ser ReadSer;
extern int Serial_nFd;
extern char* SerialNameBase;
/*************************************************************
**  Stitching two strings
*************************************************************/
char* Str_Stitching(char *s1, char *s2)
{
   char *result = (char*)malloc(strlen(s1)+strlen(s2)+1);//+1 for the zero-terminator
    //in real code you would check for errors in malloc here
    if (result == NULL) exit (1);
    strcpy(result, s1);
    strcat(result, s2);
    return result;
}

void Sernd_StopMsg(int fd,unsigned char motor)
{
   unsigned char Motor_Send_LF[8] = {0x01,0x06,0x00,0x40,0x00,0x00,0x88,0x1E};
   unsigned char Motor_Send_RF[8] = {0x02,0x06,0x00,0x40,0x00,0x00,0x88,0x2D};
   unsigned char Motor_Send_LB[8] = {0x03,0x06,0x00,0x40,0x00,0x00,0x89,0xFC};
   unsigned char Motor_Send_RB[8] = {0x04,0x06,0x00,0x40,0x00,0x00,0x88,0x4B};
   unsigned char Motor_Send_UD[8] = {0x05,0x06,0x00,0x40,0x00,0x00,0x89,0x9A};
   unsigned char Motor_Send_FX[8] = {0x06,0x06,0x00,0x40,0x00,0x00,0x89,0xA9};

   switch(motor)  //电机数量
   {
	case 0:  write(fd,Motor_Send_LF,8);break;
	case 1:  write(fd,Motor_Send_RF,8);break;
	case 2:  write(fd,Motor_Send_LB,8);break;
	case 3:  write(fd,Motor_Send_RB,8);break;
	case 4:  write(fd,Motor_Send_UD,8);break;
	case 5:  write(fd,Motor_Send_FX,8);break;
   }
}

/****************************************************
** function: It's Used to Config The Serial Port
****************************************************/
unsigned char Serial_Config(char *serial_name,unsigned int BaudRate,unsigned char spbit,unsigned char d_tim,unsigned char d_len)
{
 // const char *serial_name = "/dev/ttyUSB8";
  struct termios SerOpt; //the serial struct
  Serial_nFd = open(serial_name, O_RDWR|O_NOCTTY);//open the serial in a normal way
  if(Serial_nFd == -1)
  {
	perror("Err Open Serial
");
	return 0xff;
  }
  tcgetattr(Serial_nFd, &SerOpt);//save config
  bzero(&SerOpt, sizeof(SerOpt));

  tcflush(Serial_nFd, TCIOFLUSH);
  switch(BaudRate)
  {
   case 9600:  cfsetispeed(&SerOpt, B9600);cfsetospeed(&SerOpt, B9600);break;
   case 115200:cfsetispeed(&SerOpt, B115200);cfsetospeed(&SerOpt, B115200);break;
  }
  SerOpt.c_cflag &= ~CSIZE;
  SerOpt.c_cflag |= CS8;
  switch(spbit)
  {
   case 1: SerOpt.c_cflag &= ~CSTOPB; break;  // 1stop bit
   case 2: SerOpt.c_cflag |=  CSTOPB; break;  // 2 stop bit
  }
  SerOpt.c_cflag &= ~PARENB;
  SerOpt.c_cflag &= ~CRTSCTS;
  SerOpt.c_cflag |= (CLOCAL | CREAD);

  SerOpt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  SerOpt.c_oflag &= ~OPOST;

  SerOpt.c_cc[VTIME] = d_tim;  //Read has been blocked Until immediately after reading VMIN characters
  SerOpt.c_cc[VMIN]  = d_len;

  tcflush(Serial_nFd, TCIOFLUSH); //clear input output buff
  if(tcsetattr(Serial_nFd, TCSANOW, &SerOpt) != 0)
  {
	perror("serial error
");
	//exit(1);
	return 0xFF;
  }
  printf("Serial Config Complete
");
  return 0;
}
/**************************************
** open serial port
***************************************/
/*unsigned char OpenSerial_Num(unsigned char num)
{
  char Str_Num[30];
  char* SerialName;
  unsigned char State = 0;

  sprintf(Str_Num,"%d",num);
  SerialName = Str_Stitching(SerialNameBase,Str_Num);
  State = Serial_Config(SerialName,115200,2,1,0);
  printf("%d, SerialName:%s
",Serial_nFd,SerialName);
  return State;
}*/
unsigned char OpenSerial_Num(unsigned char num)
{
  char Str_Num[30];
  char* SerialName;
  unsigned char State = 0;

  sprintf(Str_Num,"%d",num);
  SerialName = Str_Stitching(SerialNameBase,Str_Num);
  switch(ReadSer.FindS_Type)
  {
   case 0x01:State = Serial_Config(SerialName,115200,2,1,0);break;
   case 0x02:State = Serial_Config(SerialName,115200,1,1,0);break;
   case 0x03:State = Serial_Config(SerialName,115200,1,1,0);break;
   case 0x04:State = Serial_Config(SerialName,115200,1,1,0);break;
  }
  printf("%d, SerialName:%s
",Serial_nFd,SerialName);
  return State;
}

#endif /* ROBOT_PKG_INCLUDE_NODE_FIND_SERIAL_H_ */