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

【重要问题解决】PID挪到硬件定时器中，解决控制不及时导致的抖动问题

paint_robot_new-v1.5
Lizongdi 2 weeks ago
parent
3f43369c55
commit
db614823eb
5 changed files with 206 additions and 134 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 117
      RBcore/BHBF.c
  2. 3
      RBcore/drv_interface.c
  3. 10
      RBcore/include/BHBF.h
  4. 182
      RBcore/msp_Timer.c
  5. 28
      project/paint_robot_new/paint_robot_new.c

117
RBcore/BHBF.c
View File

@ -27,8 +27,7 @@
/*----------------------------------------------* /*----------------------------------------------*
* 外部函数原型说明 * * 外部函数原型说明 *
*----------------------------------------------*/ *----------------------------------------------*/
double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP,
double Position_KI, double Position_KD, double MaxValue);
/*----------------------------------------------* /*----------------------------------------------*
* 内部函数原型说明 * * 内部函数原型说明 *
*----------------------------------------------*/ *----------------------------------------------*/
@ -58,11 +57,6 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
static int iSpeed = -1; // 表示由RD1转换来的速度值 static int iSpeed = -1; // 表示由RD1转换来的速度值
static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理 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) switch (pstMsg->m_uiMsgID)
{ {
case RBCORE_CMD_STOP_ALL: case RBCORE_CMD_STOP_ALL:
@ -71,14 +65,6 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);
break; 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: case RBCORE_CMD_MANUAL_FORWARD:
{ {
if (iSpeed >= 0) if (iSpeed >= 0)
@ -119,107 +105,6 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
} }
break; 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: case RBCORE_GET_VEHICLE_SPEED:
{ {
if (pstMsg->m_uiDataLen >= sizeof(int)) if (pstMsg->m_uiDataLen >= sizeof(int))

3
RBcore/drv_interface.c
View File

@ -256,6 +256,7 @@ void TL720D_Init(void);
void RBcore_Init(void); void RBcore_Init(void);
void controller_init(void); void controller_init(void);
void daemon_Init(void); void daemon_Init(void);
void Timer_Init(void);
void Drv_InterfaceInit(void) void Drv_InterfaceInit(void)
{ {
@ -306,4 +307,6 @@ void Drv_InterfaceInit(void)
TL720D_Init(); TL720D_Init();
daemon_Init(); daemon_Init();
Timer_Init();
} }

10
RBcore/include/BHBF.h
View File

@ -72,6 +72,7 @@ extern "C"{
#define MODULE_NAME_CUSTOM "custom" #define MODULE_NAME_CUSTOM "custom"
#define MODULE_NAME_SENDIV "sendiv" #define MODULE_NAME_SENDIV "sendiv"
#define MODULE_NAME_DAEMON "daemon" #define MODULE_NAME_DAEMON "daemon"
#define MODULE_NAME_TIMER "timer"
typedef enum { typedef enum {
COMMON_CMD_SHOW_INFO, // 终端信息展示 COMMON_CMD_SHOW_INFO, // 终端信息展示
@ -87,10 +88,6 @@ typedef enum {
RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退 RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退
RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转 RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转
RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转 RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转
RBCORE_CMD_STRAIGHT_DRIVE, // 机器人直线行驶命令格式为BHBF_straight_drive_Cmd
RBCORE_CMD_RESET_STRAIGHT, // 上一条命令支持计时前进，这里重置计时
RBCORE_CMD_TURN_ANGLE, // 机器人转向目标角度（带PID）
RBCORE_GET_TL720D_ROLL, // 获取陀螺仪横滚角（PID计算）
RBCORE_GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化） RBCORE_GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化）
CUSTOM_START = 0x0300, // *机器人特性命令开始* CUSTOM_START = 0x0300, // *机器人特性命令开始*
@ -115,6 +112,11 @@ typedef enum {
DAEMON_SET_BUTTON_RESET, // 置位ComError_Remote_Button_Reset_State DAEMON_SET_BUTTON_RESET, // 置位ComError_Remote_Button_Reset_State
DAEMON_SET_MK32_SERIAL, // 置位ComError_MK32_Serial DAEMON_SET_MK32_SERIAL, // 置位ComError_MK32_Serial
DAEMON_SET_MK32_UDP, // 置位ComError_MK32_UDP DAEMON_SET_MK32_UDP, // 置位ComError_MK32_UDP
TIMER_START = 0x0600, // *2ms定时器命令
TIMER_CMD_STRAIGHT_DRIVE, // 机器人直线行驶命令格式为BHBF_straight_drive_Cmd
TIMER_CMD_RESET_STRAIGHT, // 上一条命令支持计时前进，这里重置计时
TIMER_CMD_TURN_ANGLE, // 机器人转向目标角度（带PID）
} BHBF_Cmd_e; } BHBF_Cmd_e;
#define THREAD_DEFAULT_STACK 4096 #define THREAD_DEFAULT_STACK 4096

182
RBcore/msp_Timer.c
View File

@ -0,0 +1,182 @@
/******************************************************************************
版权所有 (C), 2018-2099, Radkil
******************************************************************************
文 件 名 : msp_Timer.c
版 本 号 : 初稿
作 者 : radkil
生成日期 : 2026年9月17日
最近修改 :
功能描述 : 硬件定时器任务
修改历史 :
1.日 期 : 2026年9月17日
作 者 : radkil
修改内容 : 创建文件
******************************************************************************/
#include "BHBF.h"
#include "msg_center.h"
#include "rd_time.h"
#include "tim.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);
/*----------------------------------------------*
* 内部函数原型说明 *
*----------------------------------------------*/
/*----------------------------------------------*
* 全局变量 *
*----------------------------------------------*/
/*----------------------------------------------*
* 模块级变量 *
*----------------------------------------------*/
static uint32_t g_uiTimModuleID = 0;
/*----------------------------------------------*
* 常量定义 *
*----------------------------------------------*/
/*----------------------------------------------*
* 宏定义 *
*----------------------------------------------*/
static void Timer_ModuleHandler(const Msg_t *pstMsg)
{
if (NULL == pstMsg)
{
return;
}
static int aiMotorSpeed[2] = {0};
static int g_dletAngle = 0; //PID最终计算补偿值
static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
static int bTimerStarted = 0; //直线行驶距离单次触发标志
switch (pstMsg->m_uiMsgID)
{
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] = (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_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_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 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;
}
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));
if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
{
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
break;
}
default:
break;
}
}
void Timer_Task(void)
{
MsgCenter_Process(g_uiTimModuleID);
}
void Timer_Init(void)
{
HAL_TIM_Base_Start_IT(&htim8);
g_uiTimModuleID = MsgCenter_Register(MODULE_NAME_TIMER, Timer_ModuleHandler);
}

28
project/paint_robot_new/paint_robot_new.c
View File

@ -177,7 +177,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd)); RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
int iTargetAngle = g_stCV.RobotUpAngleValue; int iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
break; break;
} }
@ -200,7 +200,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation, .m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上 else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
{ {
@ -209,7 +209,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation, .m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
} }
else if (iTargetAngle == g_stCV.RobotRightAngleValue) else if (iTargetAngle == g_stCV.RobotRightAngleValue)
@ -221,7 +221,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation, .m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上 else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
{ {
@ -230,7 +230,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation, .m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
} }
else if (iTargetAngle == g_stCV.RobotDownAngleValue) else if (iTargetAngle == g_stCV.RobotDownAngleValue)
@ -324,12 +324,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上 if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
{ {
iTargetAngle = g_stCV.RobotLeftAngleValue; iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上 else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
{ {
iTargetAngle = g_stCV.RobotRightAngleValue; iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
g_RB_State = 0; g_RB_State = 0;
break; break;
@ -339,12 +339,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上 if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
{ {
iTargetAngle = g_stCV.RobotRightAngleValue; iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上 else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
{ {
iTargetAngle = g_stCV.RobotLeftAngleValue; iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
g_RB_State = 0; g_RB_State = 0;
break; break;
@ -353,7 +353,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_RESET_STRAIGHT, NULL, 0); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
g_RB_State = 0; g_RB_State = 0;
} }
} }
@ -407,7 +407,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
g_RB_State = 1; g_RB_State = 1;
} }
@ -428,7 +428,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
g_RB_State = 1; g_RB_State = 1;
} }
@ -462,7 +462,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
g_RB_State = 1; g_RB_State = 1;
} }
@ -482,7 +482,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1
}; };
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
g_RB_State = 1; g_RB_State = 1;
} }

Write Preview
Loading…
Cancel
Save