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\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的更多相关文章
随机推荐
- 【Excle数据透视表】如何按照地区交替填充背景颜色
现存在如下数据透视表 需要根据地区填充不同的背景颜色 步骤 选定数值区域→开始→条件格式→新建规则,出现如下窗口: 公式:=MOD(COUNT(1/(MATCH($A$4:$A4,$A$4:$A4,) ...
- ASP.NET基本对象介绍
ASP.NET能够成为一个庞大的软件体系,与它提供了大量的对象类库有很大的关系.这些类库中包含许多封装好的内置对象,开发人员可以直接使用这些对象的方法和属性,因此用较少的代码量就能轻松完成很多对象. ...
- C#中的里氏替换原则
里氏转换原则 子类可以赋值给父类对象 父类对象可以强制转化为对应的子类对象 里氏替换原则直观理解就是"子类是父类",反过来就说不通了. 就像男人是人对的,但人是男人就不对了. 这样 ...
- MQTT--入门 续
1.消息模型: MQTT是一种基于代理的发布/订阅的消息协议.提供一对多的消息分发,解除应用程序耦合.一个发布者可以对应多个订阅者,当发布者发生变化的时候,他可以将消息一一通知给所有的订阅者.这种模 ...
- logstash5安装并实现mariadb数据写入到elasticsearch
java环境这里默认安装了 ,一般源码安装,这里就不说了 一.安装logstash 安装logstash可以用yum安装,也可以用源码安装: yum安装: 1.导入GPG: rpm --import ...
- requests ssl 报错
使用requests下载日志出现HTTPSConnectionPool(host='***', port=443): Max retries exceeded with url: ******(Cau ...
- 实际项目中，看 ECharts 和 HighCharts 渲染性能对比，表面看衣装，本质看内功！！！
最近做项目,使用的是echarts显示图表数据,但是数据量比较多的时候,有卡顿的情况.后来同事拿echarts和HighCharts做了对比,仅供大家参考.同时感谢同事做的工作. 一.查询1天的源数据 ...
- win10--vs2015--libjpeg--64位库的编译过程记录
win10--vs2015--libjpeg--64位库的编译过程记录 1. 下载源代码: http://libjpeg.sourceforge.net/ 或者 http://www.ij ...
- MySQL中in（常量列表）的执行计划
我们在写sql的时候,经常用到in,in后面跟一堆常量列表,如id.有人说in的效率很高,而有人说很低:有人说in能使用索引,还有人说in不能使用索引... 到底是一个怎样的情况呢?我们分析以下几种情 ...
- Spring Boot 如何在类中应用配置文件
如何在类中应用配置文件 优先级: 当前目录子目录的/config > 当前目录 > classpath的/config包 > classpath的根目录 即:越靠近的优先级越高 ** ...