CodeBackUP_node_find

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

**  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_ */

CodeBackUP_node_find_serial的更多相关文章

随机推荐

对于刚開始学习的人Xcode最经常使用的快捷键
对于刚開始学习的人而言,好多人都是直接使用鼠标进行操作.差点儿非常少使用快捷键,从而再练习编程时比别人慢那么一点,今天就把刚開始学习的人最经常使用的几个快捷键给大家总结下,当然欢迎大家补充. (1)c ...
IIS7.5下的web.config 404应该如何配置
IIS环境下web.config的配置的问题,在IIS7.5中添加配置404页面时遇到了一些问题,记录如下: 一开始在<customError>下的<error>节点配置404 ...
R 包的安装，使用，更新
R包的使用方法包就是提供了种类繁多的函数,当然还有它的一些数据集,可以使用这些函数来操作这些数据集,来学习使用. library(),当前的工作环境里,可以使用的包包的帮助文档:help(pack ...
mongoDB Java SDK CRUD操作例子
Example: package com.sdfwerwer.test; import java.net.UnknownHostException; import com.mongodb.Basi ...
GDBus
1. https://en.wikipedia.org/wiki/D-Bus In computing, D-Bus (for "Desktop Bus"[4]), a softw ...
使用React的static方法实现同构以及同构的常见问题
代码地址请在github查看,假设有新内容.我会定时更新.也欢迎您star,issue,共同进步 1.我们服务端渲染数据从何而来 1.1 怎样写出同构的组件服务端生成HTML结构有时候并不完好.有时 ...
jar包解压与打包
首先感谢大神的指导:https://blog.csdn.net/mr_pang/article/details/47028921 1.首先准备一个能运行的jar文件,我们使用第三方解压工具进行解压wi ...
nginx 内置变量大全
HTTP核心模块支持一些内置变量,变量名与apache里的对应.比如 $http_user_agent,$http_cookie等表示HTTP请求信息的变量.更多变量:$args, 请求中的参数; $ ...
笔试真题解析 ALBB-2015 系统project师研发笔试题
4)在小端序的机器中,假设 union X { int x; char y[4]; }; 假设 X a; a.x=0x11223344;//16进制则:() y[0]=11 y[1] ...
Linux上部署Java应用+Python3环境搭建
给了Linux的测试环境,目前需要install JDK, Tomcat,此处记录下小白的操作过程. 1. 查询Linux发行版本,包括内核信息 (1) Linux查询内核信息 $ uname -a ...

CodeBackUP_node_find_serial

CodeBackUP_node_find_serial的更多相关文章

随机推荐

热门专题