LiZongdi
/
robot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

PID不再使用定时器中断，避免消息中心竞态错误。关闭可能导致异常的串口关中断操作

master
Lizongdi 2 days ago
parent
b87ae5d001
commit
4eb5ba8a51
4 changed files with 130 additions and 25 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 111
      RBcore/BHBF.c
  2. 2
      RBcore/drv_interface.c
  3. 2
      bspMCU/My_print.c
  4. 40
      project/paint_robot_new/paint_robot_new.c

111
RBcore/BHBF.c
View File

@ -19,15 +19,17 @@
#include "BHBF.h"
#include "common.h"
#include "msg_center.h"
#include "lua_base.h"
/*----------------------------------------------*
* 外部变量说明 *
*----------------------------------------------*/
extern volatile int32_t g_RF_Angle_Roll;
/*----------------------------------------------*
* 外部函数原型说明 *
*----------------------------------------------*/
double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP,
double Position_KI, double Position_KD, double MaxValue);
/*----------------------------------------------*
* 内部函数原型说明 *
*----------------------------------------------*/
@ -55,6 +57,11 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
return;
}
int aiMotorSpeed[2] = {0};
static int g_dletAngle = 0; //PID最终计算补偿值
static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
static int bTimerStarted = 0; //直线行驶距离单次触发标志
switch (pstMsg->m_uiMsgID)
{
case RBCORE_CMD_STOP_ALL:
@ -63,6 +70,106 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);
break;
}
case TIMER_CMD_RESET_STRAIGHT:
{
uiLastSendTick = 0;
bTimerStarted = 0;
break;
}
case TIMER_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_RF_Angle_Roll - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle)
{
if (abs(g_RF_Angle_Roll - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle)
{
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, 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_RF_Angle_Roll, 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] = (stCmd.m_iSpeed * 10) - g_dletAngle ;
aiMotorSpeed[1] = (stCmd.m_iSpeed * 10) + g_dletAngle ;
}
else
{
aiMotorSpeed[0] = -(stCmd.m_iSpeed * 10) - g_dletAngle ;
aiMotorSpeed[1] = -(stCmd.m_iSpeed * 10) + g_dletAngle ;
}
}
else
{
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, 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_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0);
}
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
}
break;
}
case TIMER_CMD_TURN_ANGLE:
{
int iTargetAngle = 0;
if (pstMsg->m_uiDataLen >= sizeof(int))
{
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50)
{
aiMotorSpeed[0] = 0;
aiMotorSpeed[1] = 0;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, NULL, 0);
}
else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000)
{
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, iTargetAngle, 1, 0,0.5, 10);
aiMotorSpeed[0] = -g_dletAngle ;
aiMotorSpeed[1] = g_dletAngle;
}
else
{
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, 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));
}
break;
}
case CMD_SHOW_INFO:
{
lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\ng_RF_Angle_Roll = %d\n",
g_dletAngle, uiLastSendTick, bTimerStarted, g_RF_Angle_Roll);
break;
}
default:
break;
}

2
RBcore/drv_interface.c
View File

@ -308,6 +308,6 @@ void Drv_InterfaceInit(void)
TL720D_Init();
daemon_Init();
Timer_Init();
//Timer_Init();
log_i("BingooRobot Init success\nversion: %s", Rd_GetBuildTime());
}

2
bspMCU/My_print.c
View File

@ -241,9 +241,7 @@ WEAK void Myprint_getchar(char *ch)
{
int ret;
__disable_irq();
ret = rd_ComRead(g_ptUartCtrl, ch, 1);
__enable_irq();
if (ret == 1)
{

40
project/paint_robot_new/paint_robot_new.c
View File

@ -96,7 +96,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(void)
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
@ -106,12 +106,12 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(void)
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
@ -122,7 +122,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(void)
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
@ -132,12 +132,12 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(void)
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
@ -148,7 +148,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(void)
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
@ -158,12 +158,12 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(void)
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
@ -174,7 +174,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(void)
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
@ -184,19 +184,19 @@ static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(void)
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
static void Lane_Change_State_Reset(void)
{
g_ichangLineState = 0;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
}
static void Robot_Move_Forward(void)
@ -237,7 +237,7 @@ static void Robot_Move_Forward_PID(void)
.m_iTime = -1,
.m_iSpeed = g_iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
@ -263,7 +263,7 @@ static void Robot_Move_Backward_PID(void)
.m_iTime = -1,
.m_iSpeed = g_iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
@ -601,7 +601,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
if (_iMode == 2 || _iMode == 3)
{
g_ichangLineState = 0;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
}
}
@ -652,7 +652,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
.m_iTime = -1,
.m_iSpeed = g_iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
return;
@ -680,7 +680,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
.m_iTime = -1,
.m_iSpeed = g_iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
return;
@ -727,7 +727,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
.m_iTime = -1,
.m_iSpeed = g_iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
}
@ -754,7 +754,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
.m_iTime = -1,
.m_iSpeed = g_iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
}

Write Preview
Loading…
Cancel
Save