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的更多相关文章
随机推荐
- 可移动磁盘显示0kb打不开怎么办
移动硬盘.U盘打不开怎么办 为了方便共享,大多数人都购买了移动硬盘.那么如果有一天,发现移动硬盘打不开了,怎么办?下面为大家介绍移动硬盘打不开的解决方法. 未格式化故障 第一步:双击盘符出现未格式 ...
- ionic2项目创建回顾 及 react-native 报错处理
ionic2: 1.创建项目: ionic start MyIonic2Project tutorial --v2 (下载 tutorial 模板来初始化项目) ionic start MyIonic ...
- javascript洗牌算法 乱序算法 面试题
1.2种方案代码 <!DOCTYPE html> <html lang="zh"> <head> <meta charset=" ...
- 使用xib定义的UITableViewCell的复用identifier
使用xib自定义cell的时候,需要在xib中指定复用identifier(通常与类名一致即可),在编码的时候,也应该使用该identifier而不应该自定义其他identifier,否则,可能导致程 ...
- git 4种对象的理解
git中有四种基本对象类型,可以说Git的所有操作都是通过这四种对象完成的.下图是<Git版本控制管理>中文第二版的原话,顺便吐槽一下,这本书真的翻译的一般.. 下面说下我的理解吧,首先b ...
- sql CHARINDEX() 与 PATINDEX() LEN() substring() COLLATE RAISERROR
CHARINDEX() 在一个表达式中搜索另一个表达式并返回其起始位置(如果找到). CHARINDEX ( expressionToFind , expressionToSearch [ , st ...
- 解读Unity中的CG编写Shader系列3——表面剔除与剪裁模式
在上一个样例中,我们得到了由mesh组件传递的信息经过数学转换至合适的颜色区间以颜色的形式着色到物体上. 这篇文章将要在此基础上研究片段的擦除(discarding fragments)和前面剪裁.后 ...
- 红米手机连接logcat，调试信息刷屏解决办法
我买了红米,平时拿它来当作安卓测试机的 可是把它接入eclipse里面,发现它会往logcat打印大量的日志,我自己想调试一个程序的时候,自己的程序的日志一会儿就被冲刷没了 新建一个logcat me ...
- LinkedList add remove get 代码分析
add void linkLast(E e) { //e 要添加的元素 final Node<E> l = last; // 最后一个元素 final Node<E> newN ...
- oracle中导出表的结构和数据
在linux环境上: exp user_name/password@//ip_address:1521/service_name file=aa.sql tables=\(table_name\); ...