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

换道逻辑调通

paint_robot_new-v1.5
Lizongdi 2 weeks ago
parent
cef919640e
commit
e4f45242e4
3 changed files with 54 additions and 22 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. 1
      RBcore/include/BHBF.h
  2. 59
      RBcore/msp_Timer.c
  3. 10
      project/paint_robot_new/paint_robot_new.c

1
RBcore/include/BHBF.h
View File

@ -121,6 +121,7 @@ typedef enum {
TIMER_CMD_STRAIGHT_DRIVE, // 机器人直线行驶命令格式为BHBF_straight_drive_Cmd
TIMER_CMD_RESET_STRAIGHT, // 上一条命令支持计时前进，这里重置计时
TIMER_CMD_TURN_ANGLE, // 机器人转向目标角度（带PID）
TIMER_CMD_TURN_UP, // 机器人转向头朝上
} BHBF_Cmd_e;
#define THREAD_DEFAULT_STACK 4096

59
RBcore/msp_Timer.c
View File

@ -20,6 +20,7 @@
#include "msg_center.h"
#include "rd_time.h"
#include "tim.h"
#include "lua_base.h"
/*----------------------------------------------*
* 外部变量说明 *
@ -62,22 +63,30 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
static int g_dletAngle = 0; //PID最终计算补偿值
static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
static int bTimerStarted = 0; //直线行驶距离单次触发标志
static int bstrgightfinish = 0; //直线行驶完成
switch (pstMsg->m_uiMsgID)
{
case TIMER_CMD_RESET_STRAIGHT:
{
uiLastSendTick = 0;
bTimerStarted = 0;
bstrgightfinish = 0;
break;
}
case TIMER_CMD_STRAIGHT_DRIVE:
{
BHBF_straight_drive_Cmd stCmd = {0};
static BHBF_straight_drive_Cmd stCmd = {0};
if (pstMsg->m_uiDataLen >= sizeof(stCmd))
{
if (1 == bstrgightfinish) // 这里换道完成认为第一步旋转已经完成不再根据计时卡状态
{
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0);
break;
}
RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
if (stCmd.m_iTime > 0 && 0 == bTimerStarted)
if (stCmd.m_iTime >= 0 && 0 == bTimerStarted)
{
uiLastSendTick = Rd_GetTime();
bTimerStarted = 1; // 开始计时
@ -119,17 +128,14 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
aiMotorSpeed[1] = g_dletAngle ;
}
if (stCmd.m_iTime > 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
if (stCmd.m_iTime >= 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
{
aiMotorSpeed[0] = 0;
aiMotorSpeed[1] = 0;
bstrgightfinish = 1;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 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;
}
@ -139,10 +145,18 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
if (pstMsg->m_uiDataLen >= sizeof(int))
{
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
if (1 == bstrgightfinish) // 这里换道完成认为第一步旋转已经完成不再根据角度卡状态
{
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
break;
}
if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50)
{
aiMotorSpeed[0] = 0;
aiMotorSpeed[1] = 0;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000)
{
@ -157,12 +171,35 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
aiMotorSpeed[1] = g_dletAngle ;
}
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
}
break;
}
case TIMER_CMD_TURN_UP:
{
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
if (abs(g_RF_Angle_Roll - g_stCV.RobotUpAngleValue) <= 50)
{
aiMotorSpeed[0] = 0;
aiMotorSpeed[1] = 0;
}
else if (abs(g_RF_Angle_Roll - g_stCV.RobotUpAngleValue) <= 1000)
{
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, g_stCV.RobotUpAngleValue, 1, 0,0.5, 10);
aiMotorSpeed[0] = -g_dletAngle ;
aiMotorSpeed[1] = g_dletAngle;
}
else
{
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, g_stCV.RobotUpAngleValue, 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 COMMON_CMD_SHOW_INFO:
{
lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\nbstrgightfinish = %d\ng_RF_Angle_Roll = %d\n",
g_dletAngle, uiLastSendTick, bTimerStarted, bstrgightfinish, g_RF_Angle_Roll);
break;
}
default:

10
project/paint_robot_new/paint_robot_new.c
View File

@ -177,13 +177,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
}
case CUSTOM_CMD_STRAIGHT_DRIVE:
{
BHBF_straight_drive_Cmd stCmd = {0};
if (pstMsg->m_uiDataLen >= sizeof(stCmd))
{
RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
int iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_UP, NULL, 0);
break;
}
case CUSTOM_CMD_TURN_ANGLE:
@ -194,7 +188,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
if (iTargetAngle == g_stCV.RobotUpAngleValue)
{
log_d("change line success");
}
else if (iTargetAngle == g_stCV.RobotLeftAngleValue)
{

Write Preview
Loading…
Cancel
Save