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