《2023 DigiKey 汽车应用创意挑战赛》-AGV安全距离检测系统

xiaoshen-372360 · 发表于 2024-1-4 23:51:43

AGV安全距离检测系统

前段时间一直在做超声波的调试，很多模拟量的问题没有解决这段时间降模拟量和通信做了调整，目前完成了Modbus-RTU的数据通信以及超声波测距的工作，整体的项目进展目前已经基本完成了，所以这里来和大家做一下汇报，分享一下自己的成果，首先从整体的项目规划以及框图开始说起，可能会比较繁琐一点，但是我还是觉得尽量详细的介绍我们现在的东西，也许会对大家带来一些启发。

1 项目介绍AGV在现在物流运输和分拣中起到了重要作用，通常情况下是由上级电脑完成系统路径的规划，然后车辆按照分配路线将物流包裹配送到对应的区域内，从而完成大量包裹的分拣的工作，节约了很多人力成本，也避免了由于人为操作失误造成物流停滞、延迟的负面影响。因此AGV在物流分拣的重要性可想而知。在AGV的整体路径规划中，AGV的智能性也将起到至关重要的作用，其中最主要的一点就是安全距离检测，可以判断对于障碍物的判断，以及车辆急停、车辆会根据障碍物距离做出相应的减速、停止、转向等动作。

2 硬件方案

整体的硬件方案是STM32F103CBT6作为主控，通过分时复用完成多多个超声波探头的然后通过RS485或者CAN总线完成数据的传输，通常情况下，RS485和CAN总线在实时性上是有一些距离的。目前的话只做了RS485这块，支持Modbus_RTU的协议。整体还配备了温度采集传感器，可以通过温度来进行声音测距的温度补偿，计算出当前温度的声速，从而对测量距离进行修正。

2.1 硬件原理图

整体的原理图如上图，采用八个国内的超声波测距芯片，然后使用DCDC，完成12V到3.3V的电压转换，通过两个8通道的选择开关完成对每个传感器的数据选择和读取，通过TTL串口实现数的采集。软件内配置状态机，可以正常的完成采集。

整体的电路板如上图。每个传感器都有自己的驱动芯片。

3 软件流程图

3.1 状态机切换机制

状态机是在控制最常用的一种机制，可以避免阻塞，完成程序的流畅运行，同样避免了不必要的程序运行占用资源。目前我使用的状态机一共设置了几个状态，每个状态有对应状态的工作任务。

目前一共分为：空闲状态、命令发送状态、回复等待状态、超时状态、异常状态。

3.2 代码展示
3.2.1 状态机代码

void get_Data(struct CSB_Get *CSB_Struct)
{
int i=0;
unsigned int Status=CSB_Struct->Stttus;
unsigned int Data=0;
if(CSB_Struct->Channel_Count>CSB_Struct->Channel_MAX)
{
CSB_Struct->Channel_Count=0;/*-- 如果当前通道计数大于最大通道--那么这次就作废---*/
return ;
}
else
{
switch(Status)
{
/*--------*/
case Status_Ideal:/*--数据采集之前的数据初始化工作 ---*/
{
for(i=0;i<CSB_Struct->Channel_MAX;i++)
{
CSB_Struct->CSB_Data_Stable[i]=CSB_Struct->CSB_Curent_Data[i];
SysTemInfo.Sys_Data.CSB_Data[i]=CSB_Struct->CSB_Curent_Data[i]/1000;
}
select(CSB_Struct->Channel_Count); /*-- 选择发送通道 --*/
CSB_Struct->TimeOut_Flag=0; /*-- 超时标志清零--*/
CSB_Struct->UART_GetData_Flag=0; /*-- 串口接收数据标志清零--*/
CSB_Struct->Uart_GetData_Count=0; /*-- 串口接收数量计数清零 ---*/
CSB_Struct->CSB_Curent_Data[CSB_Struct->Channel_Count]=0;/*当前通道长度清零--*/
CSB_Struct->TimeStart_Flag=1;
CSB_Struct->TimeOutCount=0;
CSB_Struct->Uart_Data[0]=0;
CSB_Struct->Uart_Data[1]=0;
CSB_Struct->Uart_Data[2]=0;
CSB_Struct->Stttus=Status_SendCND;
}break;
case Status_SendCND:
{
select(CSB_Struct->Channel_Count); /*-- 选择发送通道 --*/
Send_Cmd(); /*-- 发送超声波的数据采集命令 --*/
timer_enable(TIMER1); /*-- 开启定时器 -------*/
CSB_Struct->Uart_Data[2]=0;
CSB_Struct->Stttus=Status_WaitReply;
}break;
case Status_WaitReply :
{
if(CSB_Struct->UART_GetData_Flag==1)
{
Data =(Data|(CSB_Struct->Uart_Data[0]<<16) |(CSB_Struct->Uart_Data[1]<<8)|(CSB_Struct->Uart_Data[2]));
CSB_Struct->CSB_Curent_Data[CSB_Struct->Channel_Count]=Data;
CSB_Struct->Channel_Count++;
CSB_Struct->Stttus=Status_Ideal;
}
/*-- 如果超时 --*/
if(CSB_Struct->TimeOut_Flag==1)
{
CSB_Struct->Stttus=Status_TimeOut;
}
}break;
case Status_TimeOut: /*-- 直接跳转到空闲模式，继续下一个通道的测量 ----*/
{
CSB_Struct->Channel_Count++;
CSB_Struct->TimeStart_Flag=0;
CSB_Struct->TimeOutCount=0;
timer_disable(TIMER1); /*-- 开启定时器 -------*/
CSB_Struct->Stttus=Status_Ideal;
}break;
/*--------*/
default:
{
CSB_Struct->TimeOut_Flag=0; /*-- 超时标志清零--*/
CSB_Struct->UART_GetData_Flag=0; /*-- 串口接收数据标志清零--*/
CSB_Struct->Uart_GetData_Count=0; /*-- 串口接收数量计数清零 ---*/
CSB_Struct->Uart_Data[0]=0;
CSB_Struct->Uart_Data[1]=0;
CSB_Struct->Uart_Data[2]=0;
CSB_Struct->Channel_Count=0;
CSB_Struct->TimeStart_Flag=0;
timer_disable(TIMER1); /*-- 开启定时器 -------*/
}
}
}
}

复制代码

3.2.2 通信代码展示 这里仅展示部分代码

case ReadMultRegister : //--功能码：03 -- 读多个寄存器----
{
MB->ProtocalStr.MB_DataNum=MB->MB_RxData[MB_DataNum]<<8|MB->MB_RxData[MB_DataNum+1];
MB->ProtocalStr.CRCData=MB->MB_RxData[MB->MB_RxData_Len-1]<<8|MB->MB_RxData[MB->MB_RxData_Len-2];//--填入CRC数值--
//--进入这里，说明地址和功能码都满足了，现在需要做的事判断输出数量是否在规定范围内--
//--IO数量的计算应当从起始地址+数量来计算--如果起始地址加上数量之后大于IO的点数，
//--此时应当报错处理--
DataLimit=MB->ProtocalStr.MB_StartAddr + MB->ProtocalStr.MB_DataNum;//--起始地址+读取数量---
if((DataLimit>=1)&&(DataLimit<MaxiunReadREG_Data))
{
MB->MB_TxData[TX_MB_Addr]=MB->MB_RxData[MB_Addr];//--填充站号---
MB->MB_TxData[TX_MB_FunCode]=MB->MB_RxData[MB_FunCode];//--填充功能码--
//--读取线圈的函数----unsigned short ReadMultReg_03(unsigned char *Buffer,unsigned short StartAddr,unsigned short Length)
RTN_DataLen=ReadMultReg_03(&MB->MB_TxData[TX_MB_DataBase],MB->ProtocalStr.MB_StartAddr,MB->ProtocalStr.MB_DataNum);
//--填充字节数--
MB->MB_TxData[TX_MB_TxNum]=(unsigned char )(RTN_DataLen&0xff); //--填充字节数--
//--开始填充数据--
//计算CRC---校验和----校验是按照从包头开始 -- 一直到数据结束---
CRC_Data=usMBCRC16( MB->MB_TxData, RTN_DataLen+3 );//--计算CRC的数值
RTN_DataLen=RTN_DataLen+5; //--这里是包含了CRC校验的----站号 +功能码 +数量+CRCData*2=5
MB->MB_TxData_Len=RTN_DataLen;//--传入带发送的字节长度--
MB->MB_TxData[RTN_DataLen-2]=(CRC_Data&0xff); //--CRC-H
MB->MB_TxData[RTN_DataLen-1]=((CRC_Data>>8)&0xff); //--CRC-L
// 完成数据包的组包
//-- 发送标志----
MB->SendFlag=1;
}
else/*--说明--超出范围了---应当返回异常码------ --*/
{
MB->MB_TxData[MB_Addr]=(0x80+ MB->MB_RxData[MB_FunCode]);//--填充站号--
MB->MB_TxData[MB_FunCode]=MB_ERR_Output_Outof_RangeNum;//--填充功能码--错误码--
MB->MB_TxData_Len=2;
MB->SendFlag=1;
}
}
break ;
//--
case ReadinputRegister : //--功能码：04 -- 读输入寄存器----
{
MB->ProtocalStr.MB_DataNum=MB->MB_RxData[MB_DataNum]<<8|MB->MB_RxData[MB_DataNum+1];
MB->ProtocalStr.CRCData=MB->MB_RxData[MB->MB_RxData_Len-1]<<8|MB->MB_RxData[MB->MB_RxData_Len-2];//--填入CRC数值--
DataLimit=MB->ProtocalStr.MB_StartAddr + MB->ProtocalStr.MB_DataNum;//--起始地址+读取数量---
if((DataLimit>=1)&&(DataLimit<MaxiunReadREG_Data))
{
MB->MB_TxData[TX_MB_Addr]=MB->MB_RxData[MB_Addr];//--填充站号---
MB->MB_TxData[TX_MB_FunCode]=MB->MB_RxData[MB_FunCode];//--填充功能码--
//--读取线圈的函数----unsigned short ReadMultReg_03(unsigned char *Buffer,unsigned short StartAddr,unsigned short Length)
RTN_DataLen=ReadInputReg_04(&MB->MB_TxData[TX_MB_DataBase],MB->ProtocalStr.MB_StartAddr,MB->ProtocalStr.MB_DataNum);
//--填充字节数--
MB->MB_TxData[TX_MB_TxNum]=(unsigned char )(RTN_DataLen&0xff); //--填充字节数--
//--开始填充数据--
//计算CRC---校验和----校验是按照从包头开始 -- 一直到数据结束---
CRC_Data=usMBCRC16( MB->MB_TxData, RTN_DataLen+3 );//--计算CRC的数值
RTN_DataLen=RTN_DataLen+5; //--这里是包含了CRC校验的----站号 +功能码 +数量+CRCData*2=5
MB->MB_TxData_Len=RTN_DataLen;//--传入带发送的字节长度--
MB->MB_TxData[RTN_DataLen-2]=(CRC_Data&0xff); //--CRC-H
MB->MB_TxData[RTN_DataLen-1]=((CRC_Data>>8)&0xff); //--CRC-L
// 完成数据包的组包
//-- 发送标志----
MB->SendFlag=1;
}
else/*--说明--超出范围了---应当返回异常码------ --*/
{
MB->MB_TxData[MB_Addr]=(0x80+ MB->MB_RxData[MB_FunCode]);//--填充站号--
MB->MB_TxData[MB_FunCode]=MB_ERR_Output_Outof_RangeNum;//--填充功能码--错误码--
MB->MB_TxData_Len=2;
MB->SendFlag=1;
}
}
break ;
//----
case WriteSingelRegister : //--功能码：06 -- 写单个寄存器----
{
MB->ProtocalStr.MB_DataNum=1;
//--站号-功能码-起始地址 - 数量字节计数-- 寄存器数值--
DataLimit=MB->ProtocalStr.MB_StartAddr+MB->ProtocalStr.MB_DataNum;
//--进入这里，说明地址和功能码都满足了，现在需要做的事判断输出数量是否在规定范围内--
if((MB->ProtocalStr.MB_DataNum>=1)&&(MB->ProtocalStr.MB_DataNum<=0x07D0))
{
//--站号-功能码-寄存器地址- 寄存器值
for(i=0;i<6;i++)
MB->MB_TxData[i]=MB->MB_RxData[i];
//--写多个寄存器的函数----unsigned short WriteMultRegister_16(unsigned char *Buffer,unsigned short StartAddr,unsigned short Length)
RTN_DataLen=WriteSingelReg_06(&MB->MB_RxData[4],MB->ProtocalStr.MB_StartAddr,MB->ProtocalStr.MB_DataNum);
//--填充字节数--
// MB->MB_TxData[TX_MB_TxNum]=(unsigned char )(RTN_DataLen&0xff); //--填充字节数--
//--开始填充数据--
//计算CRC---校验和----校验是按照从包头开始 -- 一直到数据结束---
CRC_Data=usMBCRC16( MB->MB_TxData, RTN_DataLen+6 );//--计算CRC的数值
RTN_DataLen=RTN_DataLen+8; //--这里是包含了CRC校验的----站号 +功能码 +数量+CRCData*2=5
MB->MB_TxData_Len=RTN_DataLen;//--传入带发送的字节长度--
MB->MB_TxData[RTN_DataLen-2]=(CRC_Data&0xff); //--CRC-H
MB->MB_TxData[RTN_DataLen-1]=((CRC_Data>>8)&0xff); //--CRC-L
// 完成数据包的组包
//-- 发送标志----
MB->SendFlag=1;
}
else/*--说明--超出范围了---应当返回异常码------ --*/
{
MB->MB_TxData[MB_Addr]=(0x80+ MB->MB_RxData[MB_FunCode]);//--填充站号--
MB->MB_TxData[MB_FunCode]=MB_ERR_Output_Outof_RangeNum;//--填充功能码--错误码--
MB->MB_TxData_Len=2;
MB->SendFlag=1;
}
}
break ;

复制代码

4 实物测试图

由于电路没有处理好，所以目前可以稳定测试的距离在40cm以内，下面是Modbus-RTU 的测试界面。

[项目提交] 《2023 DigiKey 汽车应用创意挑战赛》-AGV安全距离检测系统

站长推荐 /5