robot/project/paint_robot_new/paint_robot_new.c

/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : paint_robot_new.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年7月14日
  最近修改   :
  功能描述   : 新版喷漆主逻辑
  
  修改历史   :
  1.日    期   : 2026年7月14日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "msg_center.h"
#include "lua_base.h"
#include "Protobuf/PSource/bsp_PV.pb.h"
#include "Protobuf/PSource/msp_MK32.pb.h"
#include "Protobuf/PSource/bsp_CV.pb.h"
#include "Protobuf/PSource/bsp_IV.pb.h"
#include "msp_MK32.h"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static uint32_t g_uiCustomModuleID = 0;
static SP_MSP_MK32_Button g_stMK32;
static PV_struct_define g_stPV;
static IV_struct_define g_stIV;
static int g_Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位
static int g_ichangLineState = 0; // 换道状态，0表示起始状态，1表示第一次旋转完成，2表示计时行走	完成
static int g_iRobot_Move_State = 0; // 机器人状态0表示默认，1表示调用TIMER_CMD_STRAIGHT_DRIVE

static int g_iLeft_Compensation = 0;
static int g_iRight_Compensation = 0;
static int g_bIsStopOffPaint = 0;
static int g_iVehicleSpeed = 1;
static int g_iPaint = 1;
static int g_iS2LastValue = 0;
/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

static struct {
    int m_iPaintState;       // 喷枪状态：0=打开，1=关闭
    int m_iAnglelock;        // 1=角度异常，触发 Move_Halt_AngleError 并屏蔽互斥代码
    int m_ipaintOffCount;    // Move_Halt_AngleError 中的计时，当前为 1 秒
} g_stAngleError_Ctl = {
    .m_iPaintState = -1
};

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 E_Robot_Stop(void)
{
	MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
	MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
	g_Is_All_Button_Reset = 0;
}

// 竖直从左往右作业 上端 向右换道 最终头朝上
static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(void)
{
	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 - g_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(void)
{
	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 - g_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(void)
{
	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 + g_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(void)
{
	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 + g_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 Lane_Change_State_Reset(void)
{
	g_ichangLineState = 0;
	MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
}

static void Robot_Move_Forward(void)
{
	if (g_iVehicleSpeed >= 0)
	{
		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
		aiMotorSpeed[0] = g_iVehicleSpeed * 10;
		aiMotorSpeed[1] = g_iVehicleSpeed * 10;
		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
	}
}

static void Robot_Paint_SP(void)
{
	if (g_stMK32.CH13_S2 != g_iS2LastValue)
	{
		if (0 == g_stAngleError_Ctl.m_iAnglelock)
		{
			if(g_stIV.CurrentSpeed != 0)
			{
				g_stAngleError_Ctl.m_iPaintState = 0;
				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
			}
		}
	}
	g_iS2LastValue = g_stMK32.CH13_S2;
}

static void Robot_Move_Forward_PID(void)
{
	Robot_Paint_SP();
	if (0 == g_stAngleError_Ctl.m_iAnglelock)
	{
		BHBF_straight_drive_Cmd stCmd = {
			.m_iMode = 1,
			.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
			.m_iTime = -1,
			.m_iSpeed = g_iVehicleSpeed
			};
		MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
	}
}

static void Robot_Move_Backward(void)
{
	if (g_iVehicleSpeed >= 0)
	{
		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
		aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
		aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
	}
}

static void Robot_Move_Backward_PID(void)
{
	Robot_Paint_SP();
	if (0 == g_stAngleError_Ctl.m_iAnglelock)
	{
		BHBF_straight_drive_Cmd stCmd = {
			.m_iMode = -1,
			.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
			.m_iTime = -1,
			.m_iSpeed = g_iVehicleSpeed
			};
		MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
	}
}

static void Robot_Paint_ON(void)
{
	if (0 == g_stAngleError_Ctl.m_iAnglelock)
	{
		if (0 == g_bIsStopOffPaint)
		{
			g_stAngleError_Ctl.m_iPaintState = 0;
			MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
		}
	}
}

static void Robot_Paint_OFF(void)
{
	if (0 == g_stAngleError_Ctl.m_iAnglelock)
	{
		g_bIsStopOffPaint = 0;
		g_stAngleError_Ctl.m_iPaintState = 1;
		MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
	}
}

static void Robot_Stop_After(void)
{
	if (0 == g_iPaint)
	{
		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
	}
	else
	{
		MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
	}
}

static void Robot_Stop(void)
{
	MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
}

static void Robot_Move_Left(void)
{
	if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
	{
		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
		aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
		aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
	}
}

static void Robot_Move_Right(void)
{
	if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
	{
		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
		aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
		aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
	}
}

#ifdef IS_TABLE
const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX] = {
	[E_STOP] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = E_Robot_Stop,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = E_Robot_Stop,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = E_Robot_Stop,
		},
	},
    [KEY_SA_UP] = {
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Right_To_Left_UP_Control,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Left_To_Right_UP_Control,
		},
    },
	[KEY_SA_DOWN] = {
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Right_To_Left_Down_Control,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Left_To_Right_Down_Control,
		},
    },
	[KEY_SA_DEFAULT] = {
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Lane_Change_State_Reset,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Lane_Change_State_Reset,
		},
    },
	[KEY_SB_UP] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Forward,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
		}
    },
	[KEY_SB_DOWN] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Backward,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
		}
    },
	[KEY_SC_DOWN] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Paint_OFF,
		}
    },
	[KEY_SC_DEFAULT] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Paint_OFF,
		}
    },
	[KEY_SC_UP] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Paint_ON,
		}
    },
	[JOY_DEFAULT] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Stop,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Robot_Stop_After,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Robot_Stop_After,
		},
    },
	[JOY_LEFT_UP] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Forward,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
		},
    },
	[JOY_LEFT_DWON] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Backward,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
		},
    },
	[JOY_LEFT_LEFT] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Left,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Left,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Left,
		},
    },
	[JOY_LEFT_RIGHT] = {
		[MODE_DEFAULT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Right,
		},
		[MODE_VERTICAL_LEFT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Right,
		},
		[MODE_VERTICAL_RIGHT] = {
			[SUB_MODE_DEFAULT] = Robot_Move_Right,
		},
    }
};
#else

const TKeyForest g_ActionTable = {
	.E_Stop = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, E_Robot_Stop}
	},
	.SA_Up = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, NULL},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Right_To_Left_UP_Control},
		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Left_To_Right_UP_Control}
	},
	.SA_Down = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, NULL},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Right_To_Left_Down_Control},
		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Left_To_Right_Down_Control}
	},
	.SA_Default = {
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Lane_Change_State_Reset}
	},
	.SB_Up = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Forward},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID}
	},
	.SB_Down = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Backward},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID}
	},
	.SC_Up = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_ON},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, NULL},
		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, NULL}
	},
	.SC_Down = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_OFF},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, NULL},
		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, NULL}
	},
	.SC_Default = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_OFF},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, NULL},
		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, NULL}
	},
	.Joy_Default = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Stop},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Stop_After}
	},
	.Joy_Left_Up = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Forward},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID}
	},
	.Joy_Left_Down = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Backward},
		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID}
	},
	.Joy_Left_Left = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Left}
	},
	.Joy_Left_Right = {
		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Right}
	}
};
#endif

#define IV_SEND_TIME 50 // IV上报周期（单位毫秒）

static void Move_Halt_AngleError(void)
{
	static uint32_t uiLastTime = 0; 
	if (g_stPV.RunMode != 2 && g_stPV.RunMode != 3)
	{
		return;
	}

	if (1 == g_stAngleError_Ctl.m_iAnglelock)
	{
		if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed)
		{
			MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
		}
		return;
	}
	else 
	{
		uiLastTime = Rd_GetTime();
	}

	if ((abs(g_stIV.CurrentAngle - g_stCV.RobotUpAngleValue) > g_stCV.Robot_Permitted_Angler_Error_Value_E_2D) && 0 == g_stAngleError_Ctl.m_iPaintState)
	{
		if(Rd_GetTime() - g_stAngleError_Ctl.m_ipaintOffCount > 1000)
		{
			MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);//先关枪
			g_stAngleError_Ctl.m_iAnglelock = 1;
			return;
		}
	}
	else
	{
		g_stAngleError_Ctl.m_ipaintOffCount = Rd_GetTime();
	}
}

void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
{
	g_iRobot_Move_State = 0;
	// 急停
	if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000)
	{
		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
		MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
		g_Is_All_Button_Reset = 0;
		return;
	}

	// 根据模式判断换道（仅在竖直向左或向右生效）
	if (_pstMK32->CH4_SA == 1000)
	{
		if (_iMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
		{
			Vertical_Lane_Change_From_Right_To_Left_Down_Control();
		}
		else if (_iMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
		{
			Vertical_Lane_Change_From_Left_To_Right_Down_Control();
		}
		return;
	}
	else if (_pstMK32->CH4_SA == -1000)
	{
		if (_iMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
		{
			Vertical_Lane_Change_From_Right_To_Left_UP_Control();
		}
		else if (_iMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
		{
			Vertical_Lane_Change_From_Left_To_Right_UP_Control();
		}
		return;
	}
	else // 重置前进计时器以便下一次换道重新计数
	{
		if (_iMode == 2 || _iMode == 3)
		{
			g_ichangLineState = 0;
			MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
		}
	}

	if (0 == g_stAngleError_Ctl.m_iAnglelock)
	{
		// 开关喷枪
		if (_iMode == 1)
		{
			if (_pstMK32->CH6_SC == -1000)
			{
				if (0 == g_bIsStopOffPaint)
				{
					g_stAngleError_Ctl.m_iPaintState = 0;
					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
				}
			}
			else
			{
				g_bIsStopOffPaint = 0;
				g_stAngleError_Ctl.m_iPaintState = 1;
				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
			}
		}
	}

	int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
	// 自动巡航
	if (_pstMK32->CH5_SB == -1000)
	{
		if (_iMode == 1)
		{
			if (g_iVehicleSpeed >= 0)
			{
				aiMotorSpeed[0] = g_iVehicleSpeed * 10;
				aiMotorSpeed[1] = g_iVehicleSpeed * 10;
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
		}
		else if (_iMode == 2 || _iMode == 3)
		{
			g_iRobot_Move_State = 1;
			Robot_Paint_SP();
			if (0 == g_stAngleError_Ctl.m_iAnglelock)
			{
				BHBF_straight_drive_Cmd stCmd = {
					.m_iMode = 1,
					.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
					.m_iTime = -1,
					.m_iSpeed = g_iVehicleSpeed
					};
				MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
			}
		}
		return;
	}
	else if(_pstMK32->CH5_SB == 1000)
	{
		if (_iMode == 1)
		{
			if (g_iVehicleSpeed >= 0)
			{
				aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
				aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
		}
		else if (_iMode == 2 || _iMode == 3)
		{
			g_iRobot_Move_State = 1;
			Robot_Paint_SP();
			if (0 == g_stAngleError_Ctl.m_iAnglelock)
			{
				BHBF_straight_drive_Cmd stCmd = {
					.m_iMode = -1,
					.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
					.m_iTime = -1,
					.m_iSpeed = g_iVehicleSpeed
					};
				MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
			}
		}
		return;
	}

	// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
	if (abs(_pstMK32->CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(_pstMK32->CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
	{
		if ((_iMode == 2 || _iMode == 3) && 0 == g_iPaint)
		{
			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
		}
		else
		{
			MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
		}
		return;
	}
	// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效

	// 摇杆角度死区判断
	int angle = atan2(_pstMK32->CH2_LY_V, _pstMK32->CH3_LY_H) * 180 / M_PI;

	if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		if (_iMode == 1)
		{
			if (g_iVehicleSpeed >= 0)
			{
				aiMotorSpeed[0] = g_iVehicleSpeed * 10;
				aiMotorSpeed[1] = g_iVehicleSpeed * 10;
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
		}
		else if (_iMode == 2 || _iMode == 3)
		{
			g_iRobot_Move_State = 1;
			Robot_Paint_SP();
			if (0 == g_stAngleError_Ctl.m_iAnglelock)
			{
				BHBF_straight_drive_Cmd stCmd = {
					.m_iMode = 1,
					.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
					.m_iTime = -1,
					.m_iSpeed = g_iVehicleSpeed
					};
				MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
			}
		}
	}
	else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		if (_iMode == 1)
		{
			if (g_iVehicleSpeed >= 0)
			{
				aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
				aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
		}
		else if (_iMode == 2 || _iMode == 3)
		{
			g_iRobot_Move_State = 1;
			Robot_Paint_SP();
			if (0 == g_stAngleError_Ctl.m_iAnglelock)
			{
				BHBF_straight_drive_Cmd stCmd = {
					.m_iMode = -1,
					.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
					.m_iTime = -1,
					.m_iSpeed = g_iVehicleSpeed
					};
				MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
			}
		}
	}
	else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
		{
			aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
			aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
		}
	}
	else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
	{
		if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
		{
			aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
			aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
		}
	}
	else
	{
		MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
	}
}

static void Custom_ModuleHandler(const Msg_t *pstMsg)
{
    if (NULL == pstMsg)
    {
        return;
    }

	static int iMotorOK = 0;
    switch (pstMsg->m_uiMsgID)
    {
		case CUSTOM_RESET_PAINT:
		{
			g_iPaint = 1;
        	break;
        }
		case CUSTOM_SET_STOP_OFF_PAINT:
		{
			g_bIsStopOffPaint = 1;
		}
    	case CUSTOM_GET_DAEMON_CODE:
		{
			if (pstMsg->m_uiDataLen >= sizeof(int32_t))
			{
				RD_MEMCPY(&g_stIV.SystemError, pstMsg->m_aucData, sizeof(int32_t));
			}
			break;
		}
    	case CUSTOM_GET_FAULT_CODE:
		{
			uint32_t uiFailtCode[2] = {0};
			if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode))
			{
				RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode));
				if (uiFailtCode[0] == 1)
				{
					g_stIV.Left_Motor_Err = uiFailtCode[1];
					MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_LEFT_MOTOR, NULL, 0);
				}
				else
				{
					g_stIV.Right_Motor_Err = uiFailtCode[1];
					MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_RIGHT_MOTOR, NULL, 0);
				}
			}
			break;
		}
    	case CUSTOM_GET_SPEED:
		{
			int32_t iCurrentSpeed[2] = {0};
			if (pstMsg->m_uiDataLen >= sizeof(iCurrentSpeed))
			{
				RD_MEMCPY(iCurrentSpeed, pstMsg->m_aucData, sizeof(iCurrentSpeed));
				if (1 == iCurrentSpeed[0])
				{
					g_stIV.CurrentSpeed = iCurrentSpeed[1];
					MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_LEFT_MOTOR, NULL, 0);
				}
				else
				{
					MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_RIGHT_MOTOR, NULL, 0);
				}
			}
			break;
		}
    	case CUSTOM_GET_TL720D_ROLL:
    	{
    		if (pstMsg->m_uiDataLen >= sizeof(int32_t))
			{
				RD_MEMCPY(&g_stIV.CurrentAngle, pstMsg->m_aucData, sizeof(int32_t));
			}
    		break;
    	}
		case CUSTOM_GET_PV:
		{
			if (pstMsg->m_uiDataLen >= sizeof(g_stPV))
			{
				RD_MEMCPY(&g_stPV, pstMsg->m_aucData, sizeof(g_stPV));
			}
			break;
		}
		case CUSTOM_GET_MOTOR_OK:
		{
			iMotorOK = 1;
			break;
		}
		case CUSTOM_CMD_STRAIGHT_DRIVE:
		{
			g_ichangLineState = 2; // 走到这说明换道直线走完了
			break;
		}
		case CUSTOM_CMD_TURN_ANGLE:
		{
			g_ichangLineState = 1; // 走到这说明换道第一次转完了
			break;
		}
		case CMD_STOP_ALL:
		{
			HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, 1);
			g_stAngleError_Ctl.m_iPaintState = 1;
			break;
		}
		case CUSTOM_CMD_PAINTGUN:
		{
			int paintstate = -1;
			if (pstMsg->m_uiDataLen >= sizeof(paintstate))
			{
				RD_MEMCPY(&paintstate, pstMsg->m_aucData, sizeof(paintstate));
				HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, paintstate);
			}
			if (0 == paintstate) g_iPaint = 0;
			break;
		}
		case CUSTOM_GET_MK32:
		{
			if (pstMsg->m_uiDataLen >= sizeof(g_stMK32))
			{
				RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32));
			}

		    if (g_Is_All_Button_Reset == 0)
			{
				if (g_stMK32.CH4_SA == 0 && g_stMK32.CH5_SB == 0 && g_stMK32.CH6_SC == 0
						&& g_stMK32.CH7_SD == 0 && g_stMK32.IsOnline == 1)
				{
					g_iS2LastValue = g_stMK32.CH13_S2; //防止第一次值不正确
					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0);
					g_Is_All_Button_Reset = 1;
				}
			}
			else
			{
				MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_BUTTON_RESET, NULL, 0);
			}

			if (iMotorOK == 0) // 电机还没初始化完，不执行后边
			{
				break;
			}

			// 遥控器失联或者电机报错
			if (g_stMK32.IsOnline == 0 || g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
			{
				if (g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
				{
					log_a("Motor Error ! error code: left [%d] right [%d]", g_stIV.Left_Motor_Err, g_stIV.Right_Motor_Err);
				}
				
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
				g_Is_All_Button_Reset = 0;
				break;
			}

			// 按键处于默认位置安卓界面可控
			if ((fabs(g_stMK32.CH2_LY_V) <= 200) && (fabs(g_stMK32.CH3_LY_H) <= 200)
				&& (fabs(g_stMK32.CH0_RY_H) <= 200) && (fabs(g_stMK32.CH1_RY_V) <= 200)
				&& (g_stMK32.CH4_SA == 0) && (g_stMK32.CH5_SB == 0)
				&& (g_stMK32.CH6_SC == 0) && (g_stMK32.CH7_SD == 0))
			{
				g_stIV.IsWorking = 0;
				g_stAngleError_Ctl.m_iAnglelock = 0; // 遥控器复位认为打开角度锁
				g_stAngleError_Ctl.m_ipaintOffCount = 0;
			}
			else
			{
				g_stIV.IsWorking = 1;
			}

			// 更新速度旋钮值
			int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200;
			if (iSpeedSelection > 0)
			{
				g_iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
			}
			g_stIV.RobotMoveSpeed = g_iVehicleSpeed;
			MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&g_iVehicleSpeed, sizeof(int));

			Move_Halt_AngleError();

			MK32_Key_Func(&g_stMK32, g_stPV.RunMode, SUB_MODE_DEFAULT);
			break;
		}
		case CMD_SHOW_INFO:
		{
			lua_print("MK32 Info\nRxIndex:%d\tIsOnline:%d\n", g_stMK32.RxIndex, g_stMK32.IsOnline);
			lua_print("CH0_RY_H\t%d\n", g_stMK32.CH0_RY_H);
			lua_print("CH1_RY_V\t%d\n", g_stMK32.CH1_RY_V);
			lua_print("CH2_LY_V\t%d\n", g_stMK32.CH2_LY_V);
			lua_print("CH3_LY_H\t%d\n", g_stMK32.CH3_LY_H);
			lua_print("CH4_SA  \t%d\n", g_stMK32.CH4_SA);
			lua_print("CH5_SB  \t%d\n", g_stMK32.CH5_SB);
			lua_print("CH6_SC  \t%d\n", g_stMK32.CH6_SC);
			lua_print("CH7_SD  \t%d\n", g_stMK32.CH7_SD);
			lua_print("CH8_SE  \t%d\n", g_stMK32.CH8_SE);
			lua_print("CH9_SF  \t%d\n", g_stMK32.CH9_SF);
			lua_print("CH10_LD1\t%d\n", g_stMK32.CH10_LD1);
			lua_print("CH11_RD1\t%d\n", g_stMK32.CH11_RD1);
			lua_print("CH12_S1 \t%d\n", g_stMK32.CH12_S1);
			lua_print("CH13_S2 \t%d\n", g_stMK32.CH13_S2);
			lua_print("CH14_LT \t%d\n", g_stMK32.CH14_LT);
			lua_print("CH15_RT \t%d\n", g_stMK32.CH15_RT);
			lua_print("PV Info\n");
			lua_print("{%d, %d, %d, %d, %d, %ld}\n",
					g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (long long)g_stPV.TimeStamp);
			lua_print("\niPaint = %d\ng_stAngleError_Ctl.m_iPaintState = %d\ng_stAngleError_Ctl.m_iAnglelock = %d\ng_stAngleError_Ctl.m_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\n",
					g_iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset);
			break;
		}
        default:
            break;
    }
}

void Custom_Task(void *argument)
{
	g_uiCustomModuleID = MsgCenter_Register(MODULE_NAME_CUSTOM, Custom_ModuleHandler);
    uint32_t uiLastSendTick = Rd_GetTime();
    while(1)
    {
        MsgCenter_ProcessWait(g_uiCustomModuleID, 2);

        uint32_t uiNowTick = Rd_GetTime();
        if ((int32_t)(uiNowTick - uiLastSendTick) >= IV_SEND_TIME)
        {
            uiLastSendTick = uiNowTick;
            g_stIV.SBUS_State = g_stMK32.IsOnline;
            MsgCenter_SendTo(MODULE_NAME_SENDIV, SENDIV_SET_IV, (void *)&g_stIV, sizeof(g_stIV));
        }
    }
}