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

增加换道逻辑

paint_robot_new-v1.5
Lizongdi 3 weeks ago
parent
6226c29dc8
commit
9395c993da
3 changed files with 175 additions and 28 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. 63
      RBcore/BHBF.c
  2. 10
      RBcore/include/BHBF.h
  3. 126
      project/paint_robot_new/paint_robot_new.c

63
RBcore/BHBF.c
View File

@ -60,6 +60,9 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
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:
@ -115,49 +118,72 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
}
break;
}
case RBCORE_CMD_VERTICAL:
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))
{
int iMode[2] = {0};//iMode[0]正数表示前进，负数表示倒退 iMode[1]是目标角度
if (pstMsg->m_uiDataLen >= sizeof(iMode))
RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
if (stCmd.m_iTime > 0 && 0 == bTimerStarted)
{
RD_MEMCPY(&iMode, pstMsg->m_aucData, sizeof(iMode));
if (0 == iMode[0])
uiLastSendTick = Rd_GetTime();
bTimerStarted = 1; // 开始计时
}
if (0 == stCmd.m_iMode)
{
break;
}
if (abs(g_iPIDAngle - iMode[1]) <= g_stCV.PID_mid.PID_Angle)
if (abs(g_iPIDAngle - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle)
{
if (abs(g_iPIDAngle - iMode[1]) < g_stCV.PID_low.PID_Angle)
if (abs(g_iPIDAngle - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle)
{
g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1],
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, iMode[1],
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 (iMode[0] > 0)
if (stCmd.m_iMode > 0)
{
aiMotorSpeed[0] = iSpeed - g_dletAngle ;
aiMotorSpeed[1] = iSpeed + g_dletAngle ;
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] = -iSpeed - g_dletAngle ;
aiMotorSpeed[1] = -iSpeed + g_dletAngle ;
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, iMode[1],
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;
}
@ -185,6 +211,11 @@ static void RBcore_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)
{
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
}
break;
}

10
RBcore/include/BHBF.h
View File

@ -86,7 +86,8 @@ typedef enum {
RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退
RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转
RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转
RBCORE_CMD_VERTICAL, // 机器人竖直前进（带PID）
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, // 获取机器人速度（内部速度归一化）
@ -98,11 +99,18 @@ typedef enum {
CUSTOM_GET_SPEED, // 获取速度（电机回码）
CUSTOM_GET_FAULT_CODE, // 获取错误码（电机回码）
CUSTOM_CMD_PAINTGUN, // 控制喷枪
CUSTOM_CMD_STRAIGHT_DRIVE, // RBCORE_CMD_STRAIGHT_DRIVE命令停止后回调
CUSTOM_CMD_TURN_ANGLE, // RBCORE_CMD_TURN_ANGLE命令停止后回调
SENDIV_START = 0x0400, // *IV发送线程命令（一般低优先级事项也放到这）*
SENDIV_SET_IV, // 设置IV
} BHBF_Cmd_e;
typedef struct {
int m_iMode; //正数表示前进，负数表示倒退
int m_iAngle; //目标角度
int m_iTime; //行驶时间(单位毫秒)计时，负数表示一直行走
} BHBF_straight_drive_Cmd;
/*==============================================*
* project-wide global variables *
*----------------------------------------------*/

126
project/paint_robot_new/paint_robot_new.c
View File

@ -58,6 +58,11 @@ static IV_struct_define g_stIV;
#define IV_SEND_TIME 500 // IV上报周期（单位毫秒）
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);
}
static void Custom_ModuleHandler(const Msg_t *pstMsg)
{
if (NULL == pstMsg)
@ -65,6 +70,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
return;
}
static int iLeft_Compensation = 0;
static int iRight_Compensation = 0;
switch (pstMsg->m_uiMsgID)
{
case CUSTOM_GET_FAULT_CODE:
@ -106,6 +113,76 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
}
break;
}
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_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
break;
}
case CUSTOM_CMD_TURN_ANGLE:
{
int iTargetAngle = 0;
if (pstMsg->m_uiDataLen >= sizeof(int))
{
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
if (iTargetAngle == g_stCV.RobotUpAngleValue)
{
log_d("change line success");
}
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()
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_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()
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_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()
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_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()
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
else if (iTargetAngle == g_stCV.RobotDownAngleValue)
{
}
}
break;
}
case CUSTOM_CMD_PAINTGUN:
{
int paintstate = -1;
@ -153,19 +230,42 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int));
}
int iTargetAngle = 0;
// 根据模式判断换道（仅在竖直向左或向右生效）
if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
if (g_stMK32.CH4_SA == 1000)
{
if (g_stMK32.CH4_SA == -1000)
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
{
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0);
iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
{
iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
break;
}
else if(g_stMK32.CH4_SA == 1000)
else if (g_stMK32.CH4_SA == -1000)
{
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0);
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
{
iTargetAngle = g_stCV.RobotRightAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
{
iTargetAngle = g_stCV.RobotLeftAngleValue;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
}
break;
}
else // 重置前进计时器以便下一次换道重新计数
{
if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_RESET_STRAIGHT, NULL, 0);
}
}
// 自动巡航
@ -199,8 +299,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
}
else
{
int iMode[2] = {1, g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_VERTICAL, iMode, sizeof(iMode));
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
@ -211,8 +315,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
}
else
{
int iMode[2] = {-1 , g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_VERTICAL, iMode, sizeof(iMode));
BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1
};
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)

Write Preview
Loading…
Cancel
Save