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

换道改为状态机实现

paint_robot_new-v1.5
Lizongdi 1 week ago
parent
26ea082892
commit
fabfd2a95e
3 changed files with 115 additions and 110 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. 1
      RBcore/include/BHBF.h
  2. 43
      RBcore/msp_Timer.c
  3. 181
      project/paint_robot_new/paint_robot_new.c

1
RBcore/include/BHBF.h
View File

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

43
RBcore/msp_Timer.c
View File

@ -63,14 +63,12 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
static int g_dletAngle = 0; //PID最终计算补偿值 static int g_dletAngle = 0; //PID最终计算补偿值
static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离 static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
static int bTimerStarted = 0; //直线行驶距离单次触发标志 static int bTimerStarted = 0; //直线行驶距离单次触发标志
static int bstrgightfinish = 0; //直线行驶完成
switch (pstMsg->m_uiMsgID) switch (pstMsg->m_uiMsgID)
{ {
case TIMER_CMD_RESET_STRAIGHT: case TIMER_CMD_RESET_STRAIGHT:
{ {
uiLastSendTick = 0; uiLastSendTick = 0;
bTimerStarted = 0; bTimerStarted = 0;
bstrgightfinish = 0;
break; break;
} }
case TIMER_CMD_STRAIGHT_DRIVE: case TIMER_CMD_STRAIGHT_DRIVE:
@ -78,12 +76,6 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = {0}; BHBF_straight_drive_Cmd stCmd = {0};
if (pstMsg->m_uiDataLen >= sizeof(stCmd)) 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)); RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
if (stCmd.m_iTime >= 0 && 0 == bTimerStarted) if (stCmd.m_iTime >= 0 && 0 == bTimerStarted)
@ -132,7 +124,6 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
{ {
aiMotorSpeed[0] = 0; aiMotorSpeed[0] = 0;
aiMotorSpeed[1] = 0; aiMotorSpeed[1] = 0;
bstrgightfinish = 1;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0); MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0);
} }
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
@ -146,17 +137,11 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
{ {
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, 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) if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50)
{ {
aiMotorSpeed[0] = 0; aiMotorSpeed[0] = 0;
aiMotorSpeed[1] = 0; aiMotorSpeed[1] = 0;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, NULL, 0);
} }
else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000) else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000)
{ {
@ -174,32 +159,10 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
} }
break; break;
} }
case TIMER_CMD_TURN_UP:
{
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: case COMMON_CMD_SHOW_INFO:
{ {
lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\nbstrgightfinish = %d\ng_RF_Angle_Roll = %d\n", lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\ng_RF_Angle_Roll = %d\n",
g_dletAngle, uiLastSendTick, bTimerStarted, bstrgightfinish, g_RF_Angle_Roll); g_dletAngle, uiLastSendTick, bTimerStarted, g_RF_Angle_Roll);
break; break;
} }
default: default:

181
project/paint_robot_new/paint_robot_new.c
View File

@ -48,6 +48,7 @@ static SP_MSP_MK32_Button g_stMK32;
static PV_struct_define g_stPV; static PV_struct_define g_stPV;
static IV_struct_define g_stIV; static IV_struct_define g_stIV;
static int g_Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位 static int g_Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位
static int g_ichangLineState = 0; // 换道状态，0表示起始状态，1表示第一次旋转完成，2表示计时行走 完成
/*----------------------------------------------* /*----------------------------------------------*
* 常量定义 * * 常量定义 *
@ -64,6 +65,110 @@ static int32_t RunTime_DistanceCm_SpeedE_2MPMin(void)
return (600 * (g_stPV.LaneChangeDistance + g_stCV.Vertical_ChangeLane_Compensation) / g_stCV.Lane_Change_Speed_m_per_min); return (600 * (g_stPV.LaneChangeDistance + g_stCV.Vertical_ChangeLane_Compensation) / g_stCV.Lane_Change_Speed_m_per_min);
} }
// 竖直从左往右作业 上端 向右换道 最终头朝上
static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compensation)
{
int iTargetAngle = 0;
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1,
.m_iAngle = g_stCV.RobotLeftAngleValue - _iRight_Compensation,
.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));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
// 竖直从左往右作业 下端 向右换道 最终头朝上
static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Compensation)
{
int iTargetAngle = 0;
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1,
.m_iAngle = g_stCV.RobotRightAngleValue - _iRight_Compensation,
.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));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
// 竖直从右往左作业 上端 向左换道 最终头朝上
static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compensation)
{
int iTargetAngle = 0;
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1,
.m_iAngle = g_stCV.RobotRightAngleValue + _iLeft_Compensation,
.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));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
// 竖直从右往左作业 下端 向左换道 最终头朝上
static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(int _iLeft_Compensation)
{
int iTargetAngle = 0;
if (0 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (1 == g_ichangLineState)
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1,
.m_iAngle = g_stCV.RobotLeftAngleValue + _iLeft_Compensation,
.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));
}
else if (2 == g_ichangLineState)
{
iTargetAngle = g_stCV.RobotUpAngleValue;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
static struct { static struct {
int m_iPaintState; // 喷枪状态：0=打开，1=关闭 int m_iPaintState; // 喷枪状态：0=打开，1=关闭
int m_iAnglelock; // 1=角度异常，触发 Move_Halt_AngleError 并屏蔽互斥代码 int m_iAnglelock; // 1=角度异常，触发 Move_Halt_AngleError 并屏蔽互斥代码
@ -188,70 +293,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
case CUSTOM_CMD_STRAIGHT_DRIVE: case CUSTOM_CMD_STRAIGHT_DRIVE:
{ {
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_UP, NULL, 0); g_ichangLineState = 2; // 走到这说明换道直线走完了
break; break;
} }
case CUSTOM_CMD_TURN_ANGLE: case CUSTOM_CMD_TURN_ANGLE:
{ {
int iTargetAngle = 0; g_ichangLineState = 1; // 走到这说明换道第一次转完了
if (pstMsg->m_uiDataLen >= sizeof(int))
{
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
if (iTargetAngle == g_stCV.RobotUpAngleValue)
{
}
else if (iTargetAngle == g_stCV.RobotLeftAngleValue)
{
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1,
.m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation,
.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));
}
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1,
.m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation,
.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));
}
}
else if (iTargetAngle == g_stCV.RobotRightAngleValue)
{
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1,
.m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation,
.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));
}
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
{
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1,
.m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation,
.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));
}
}
else if (iTargetAngle == g_stCV.RobotDownAngleValue)
{
}
}
break; break;
} }
case COMMON_CMD_STOP_ALL: case COMMON_CMD_STOP_ALL:
@ -340,19 +387,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
Move_Halt_AngleError(); Move_Halt_AngleError();
int iTargetAngle = 0;
// 根据模式判断换道（仅在竖直向左或向右生效） // 根据模式判断换道（仅在竖直向左或向右生效）
if (g_stMK32.CH4_SA == 1000) if (g_stMK32.CH4_SA == 1000)
{ {
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上 if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
{ {
iTargetAngle = g_stCV.RobotLeftAngleValue; Vertical_Lane_Change_From_Right_To_Left_Down_Control(iLeft_Compensation);
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; Vertical_Lane_Change_From_Left_To_Right_Down_Control(iRight_Compensation);
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
break; break;
} }
@ -360,13 +404,11 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上 if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
{ {
iTargetAngle = g_stCV.RobotRightAngleValue; Vertical_Lane_Change_From_Right_To_Left_UP_Control(iLeft_Compensation);
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; Vertical_Lane_Change_From_Left_To_Right_UP_Control(iRight_Compensation);
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
} }
break; break;
} }
@ -374,6 +416,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)
{ {
g_ichangLineState = 0;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
} }
} }

Write Preview
Loading…
Cancel
Save