You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
256 lines
7.6 KiB
256 lines
7.6 KiB
/******************************************************************************
|
|
|
|
版权所有 (C), 2018-2099, Radkil
|
|
|
|
******************************************************************************
|
|
文 件 名 : BHBF.c
|
|
版 本 号 : 初稿
|
|
作 者 : radkil
|
|
生成日期 : 2026年6月8日
|
|
最近修改 :
|
|
功能描述 : 机器人主程序
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年6月8日
|
|
作 者 : radkil
|
|
修改内容 : 创建文件
|
|
|
|
******************************************************************************/
|
|
#include "BHBF.h"
|
|
#include "common.h"
|
|
#include "msg_center.h"
|
|
|
|
/*----------------------------------------------*
|
|
* 外部变量说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 外部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP,
|
|
double Position_KI, double Position_KD, double MaxValue);
|
|
/*----------------------------------------------*
|
|
* 内部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 全局变量 *
|
|
*----------------------------------------------*/
|
|
static uint32_t g_uiRBcoreModuleID = 0;
|
|
|
|
/*----------------------------------------------*
|
|
* 模块级变量 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 常量定义 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 宏定义 *
|
|
*----------------------------------------------*/
|
|
static void RBcore_ModuleHandler(const Msg_t *pstMsg)
|
|
{
|
|
if (NULL == pstMsg)
|
|
{
|
|
return;
|
|
}
|
|
|
|
static int iSpeed = -1; // 表示由RD1转换来的速度值
|
|
static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
|
|
static int g_dletAngle = 0; //PID最终计算补偿值
|
|
static int32_t g_iPIDAngle = 0; //当前角度
|
|
|
|
static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
|
|
static int bTimerStarted = 0; //直线行驶距离单次触发标志
|
|
switch (pstMsg->m_uiMsgID)
|
|
{
|
|
case RBCORE_CMD_STOP_ALL:
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
|
|
break;
|
|
}
|
|
case RBCORE_GET_TL720D_ROLL:
|
|
{
|
|
if (pstMsg->m_uiDataLen >= sizeof(int32_t))
|
|
{
|
|
RD_MEMCPY(&g_iPIDAngle, pstMsg->m_aucData, sizeof(int32_t));
|
|
}
|
|
break;
|
|
}
|
|
case RBCORE_CMD_MANUAL_FORWARD:
|
|
{
|
|
if (iSpeed >= 0)
|
|
{
|
|
aiMotorSpeed[0] = iSpeed;
|
|
aiMotorSpeed[1] = iSpeed;
|
|
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
|
|
}
|
|
break;
|
|
}
|
|
case RBCORE_CMD_MANUAL_BACKWARD:
|
|
{
|
|
if (iSpeed >= 0)
|
|
{
|
|
aiMotorSpeed[0] = -iSpeed;
|
|
aiMotorSpeed[1] = -iSpeed;
|
|
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
|
|
}
|
|
break;
|
|
}
|
|
case RBCORE_CMD_MANUAL_TURNLEFT:
|
|
{
|
|
if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
|
|
{
|
|
aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
|
|
aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
|
|
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
|
|
}
|
|
break;
|
|
}
|
|
case RBCORE_CMD_MANUAL_TURNRIGHT:
|
|
{
|
|
if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
|
|
{
|
|
aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
|
|
aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
|
|
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
|
|
}
|
|
break;
|
|
}
|
|
case RBCORE_CMD_RESET_STRAIGHT:
|
|
{
|
|
uiLastSendTick = 0;
|
|
bTimerStarted = 0;
|
|
break;
|
|
}
|
|
case RBCORE_CMD_STRAIGHT_DRIVE:
|
|
{
|
|
BHBF_straight_drive_Cmd stCmd = {0};
|
|
if (pstMsg->m_uiDataLen >= sizeof(stCmd))
|
|
{
|
|
RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
|
|
|
|
if (stCmd.m_iTime > 0 && 0 == bTimerStarted)
|
|
{
|
|
uiLastSendTick = Rd_GetTime();
|
|
bTimerStarted = 1; // 开始计时
|
|
}
|
|
|
|
if (0 == stCmd.m_iMode)
|
|
{
|
|
break;
|
|
}
|
|
|
|
if (abs(g_iPIDAngle - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle)
|
|
{
|
|
if (abs(g_iPIDAngle - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle)
|
|
{
|
|
g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
|
|
g_stCV.PID_low.Kp, g_stCV.PID_low.Ki, g_stCV.PID_low.Kd, 10);
|
|
}
|
|
else
|
|
{
|
|
g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
|
|
g_stCV.PID_mid.Kp, g_stCV.PID_mid.Ki, g_stCV.PID_mid.Kd, 10);
|
|
}
|
|
if (stCmd.m_iMode > 0)
|
|
{
|
|
aiMotorSpeed[0] = (g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ;
|
|
aiMotorSpeed[1] = (g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ;
|
|
}
|
|
else
|
|
{
|
|
aiMotorSpeed[0] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ;
|
|
aiMotorSpeed[1] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
|
|
g_stCV.PID_high.Kp, g_stCV.PID_high.Ki, g_stCV.PID_high.Kd,50);
|
|
aiMotorSpeed[0] = -g_dletAngle ;
|
|
aiMotorSpeed[1] = g_dletAngle ;
|
|
}
|
|
|
|
if (stCmd.m_iTime > 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
|
|
{
|
|
aiMotorSpeed[0] = 0;
|
|
aiMotorSpeed[1] = 0;
|
|
}
|
|
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
|
|
|
|
if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case RBCORE_CMD_TURN_ANGLE:
|
|
{
|
|
int iTargetAngle = 0;
|
|
if (pstMsg->m_uiDataLen >= sizeof(int))
|
|
{
|
|
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
|
|
if (abs(g_iPIDAngle - iTargetAngle) <= 50)
|
|
{
|
|
aiMotorSpeed[0] = 0;
|
|
aiMotorSpeed[1] = 0;
|
|
}
|
|
else if (abs(g_iPIDAngle - iTargetAngle) <= 1000)
|
|
{
|
|
g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iTargetAngle, 1, 0,0.5, 10);
|
|
aiMotorSpeed[0] = -g_dletAngle ;
|
|
aiMotorSpeed[1] = g_dletAngle;
|
|
}
|
|
else
|
|
{
|
|
g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iTargetAngle, 2, 0, 0.5, 85);
|
|
aiMotorSpeed[0] = -g_dletAngle ;
|
|
aiMotorSpeed[1] = g_dletAngle ;
|
|
}
|
|
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
|
|
|
|
if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
|
|
{
|
|
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case RBCORE_GET_VEHICLE_SPEED:
|
|
{
|
|
if (pstMsg->m_uiDataLen >= sizeof(int))
|
|
{
|
|
RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int));
|
|
iSpeed *= 10;
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void RBcore_Task(void *argument)
|
|
{
|
|
while(1)
|
|
{
|
|
// 处理消息
|
|
MsgCenter_ProcessWait(g_uiRBcoreModuleID, 2);
|
|
}
|
|
}
|
|
|
|
void RBcore_Init(void)
|
|
{
|
|
g_uiRBcoreModuleID = MsgCenter_Register(MODULE_NAME_RBCORE, RBcore_ModuleHandler);
|
|
|
|
const osThreadAttr_t GF_Dispatch_attributes = {
|
|
.name = MODULE_NAME_RBCORE,
|
|
.stack_size = 2048,
|
|
.priority = (osPriority_t) osPriorityRealtime1,
|
|
};
|
|
(void)osThreadNew(RBcore_Task, NULL, &GF_Dispatch_attributes);
|
|
}
|
|
|
|
|