/***************************************************************

**  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\n",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("\n");

     }

     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\n",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\n");

		  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\n",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\n");

		  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\n",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\n");

	        break;

		 }}}

       }

   }

   else if(ReadSer.FindS_Type == 0x05) // 超声波

   {

   }

   else if(ReadSer.FindS_Type == 0xAA) // 搜索串口结束

   {

	   printf("Pthread Over! \n");

	   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\n");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("\n");

	//printf("LF:%d RF:%d LB:%d RB:%d UD:%d FX:%d\n",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("\n");

  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("\n");

 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("\n");

  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("*********************************************************************\n");

	printf("************   Start detecting serial port numbers!   ***************\n");

	printf("*********************************************************************\n");

	FindMotorDriver_Serial();

	printf("*********************************************************************\n");

	printf("Motor Find is over!\n");

	printf("*********************************************************************\n");

	FindSignBoard_Serial();

	printf("*********************************************************************\n");

	printf("SignBoard Find is over!\n");

	printf("*********************************************************************\n");

	FindPowerH_Serial();

	printf("*********************************************************************\n");

	printf("High Power Find is over!\n");

	printf("*********************************************************************\n");

	FindPowerL_Serial();

	//pthread_cancel(SerialData);

	ReadSer.FindS_Type = 0xAA;

	sleep(1);

	printf("*********************************************************************\n");

        printf("************           Detecting is finish!           ***************\n");

	printf("*********************************************************************\n");

	printf("LF:%02d RF:%02d LB:%02d RB:%02d UD:%02d FX:%02d\n",SerialNumber[0],SerialNumber[1],SerialNumber[2],SerialNumber[3],SerialNumber[4],SerialNumber[5]);

	printf("SB:%02d HP:%02d LP:%02d\n",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\n");

	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\n");

	//exit(1);

	return 0xFF;

  }

  printf("Serial Config Complete\n");

  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\n",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\n",Serial_nFd,SerialName);

  return State;

}

#endif /* ROBOT_PKG_INCLUDE_NODE_FIND_SERIAL_H_ */

最新文章

  1. ORA-00821: Specified value of sga_target 3072M is too small, needs to be at least 12896M
  2. 【chrome插件】web版微信接入图灵机器人API实现自动回复
  3. Java 中extends与implements使用方法
  4. Android touch事件处理流程
  5. TeX Live安装配置等默认目录
  6. Console API 与命令行
  7. MyBatis动态SQL使用，传入参数Map中的Key判断
  8. DDD开发框架ABP之导航菜单
  9. [Java解惑]异常
  10. Modelsim的demo入门教程
  11. kafka集群中常见错误的解决方法：kafka.common.KafkaException: Should not set log end offset on partition
  12. Fire Again CodeForces - 35C （BFS）
  13. Android studio 编译出现的问题记录
  14. 并发包学习之-atomic包
  15. 欧拉筛，线性筛，洛谷P2158仪仗队
  16. html-定位
  17. Docker 安装私有镜像库的简单使用
  18. MariaDB基于GTID主从复制及多主复制
  19. 喵哈哈村的魔法考试 Round #6 (Div.3) 题解
  20. UVA-10497 Sweet Child Makes Trouble （计数+高精度）

热门文章

  1. http://www.bootcss.com/p/font-awesome/design.html
  2. Lambda Expression in C#
  3. POJ1328 Radar Installation 【贪心&amp;#183;区间选点】
  4. Oracle SOA套件12c
  5. jquery的json的遍历
  6. web开发中的路径问题
  7. webAPI 405
  8. Linux中openmpi配置
  9. 第一篇： Ansible 介绍
  10. laravel学习之路3 数据库相关