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
3befb9be59
commit
f510d0d768
1 changed files with 255 additions and 242 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. 493
      project/paint_robot_new/paint_robot_new.c

493
project/paint_robot_new/paint_robot_new.c
View File

@ -50,6 +50,12 @@ 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表示计时行走 完成 static int g_ichangLineState = 0; // 换道状态，0表示起始状态，1表示第一次旋转完成，2表示计时行走 完成
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;
/*----------------------------------------------* /*----------------------------------------------*
* 常量定义 * * 常量定义 *
*----------------------------------------------*/ *----------------------------------------------*/
@ -66,7 +72,7 @@ static int32_t RunTime_DistanceCm_SpeedE_2MPMin(void)
} }
// 竖直从左往右作业 上端 向右换道 最终头朝上 // 竖直从左往右作业 上端 向右换道 最终头朝上
static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compensation) static void Vertical_Lane_Change_From_Left_To_Right_UP_Control()
{ {
int iTargetAngle = 0; int iTargetAngle = 0;
if (0 == g_ichangLineState) if (0 == g_ichangLineState)
@ -78,7 +84,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compe
{ {
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotLeftAngleValue - _iRight_Compensation, .m_iAngle = g_stCV.RobotLeftAngleValue - g_iRight_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
@ -92,7 +98,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compe
} }
// 竖直从左往右作业 下端 向右换道 最终头朝上 // 竖直从左往右作业 下端 向右换道 最终头朝上
static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Compensation) static void Vertical_Lane_Change_From_Left_To_Right_Down_Control()
{ {
int iTargetAngle = 0; int iTargetAngle = 0;
if (0 == g_ichangLineState) if (0 == g_ichangLineState)
@ -104,7 +110,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Com
{ {
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotRightAngleValue - _iRight_Compensation, .m_iAngle = g_stCV.RobotRightAngleValue - g_iRight_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
@ -118,7 +124,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Com
} }
// 竖直从右往左作业 上端 向左换道 最终头朝上 // 竖直从右往左作业 上端 向左换道 最终头朝上
static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compensation) static void Vertical_Lane_Change_From_Right_To_Left_UP_Control()
{ {
int iTargetAngle = 0; int iTargetAngle = 0;
if (0 == g_ichangLineState) if (0 == g_ichangLineState)
@ -130,7 +136,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compen
{ {
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotRightAngleValue + _iLeft_Compensation, .m_iAngle = g_stCV.RobotRightAngleValue + g_iLeft_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
@ -144,7 +150,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compen
} }
// 竖直从右往左作业 下端 向左换道 最终头朝上 // 竖直从右往左作业 下端 向左换道 最终头朝上
static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(int _iLeft_Compensation) static void Vertical_Lane_Change_From_Right_To_Left_Down_Control()
{ {
int iTargetAngle = 0; int iTargetAngle = 0;
if (0 == g_ichangLineState) if (0 == g_ichangLineState)
@ -156,7 +162,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(int _iLeft_Comp
{ {
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotLeftAngleValue + _iLeft_Compensation, .m_iAngle = g_stCV.RobotLeftAngleValue + g_iLeft_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
@ -213,220 +219,45 @@ static void Move_Halt_AngleError(void)
} }
} }
static void Custom_ModuleHandler(const Msg_t *pstMsg) void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode)
{
if (NULL == pstMsg)
{
return;
}
static int iLeft_Compensation = 0;
static int iRight_Compensation = 0;
static int iVehicleSpeed = 1;
static int iIsStopOffPaint = 0;
static int iPaint = 1;
static int iMotorOK = 0;
static int CH13_S2_Value = 0;
switch (pstMsg->m_uiMsgID)
{
case CUSTOM_RESET_PAINT:
{
iPaint = 1;
break;
}
case CUSTOM_SET_STOP_OFF_PAINT:
{
iIsStopOffPaint = 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); if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000)
}
}
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) 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)
{
CH13_S2_Value = 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.CH8_SE == -1000 && g_stMK32.CH9_SF == -1000)
|| 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_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
g_Is_All_Button_Reset = 0; g_Is_All_Button_Reset = 0;
break; return;
}
// 按键处于默认位置安卓界面可控
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)
{
iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
} }
g_stIV.RobotMoveSpeed = iVehicleSpeed;
MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
Move_Halt_AngleError();
// 根据模式判断换道（仅在竖直向左或向右生效） // 根据模式判断换道（仅在竖直向左或向右生效）
if (g_stMK32.CH4_SA == 1000) if (_pstMK32->CH4_SA == 1000)
{ {
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上 if (_iMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
{ {
Vertical_Lane_Change_From_Right_To_Left_Down_Control(iLeft_Compensation); Vertical_Lane_Change_From_Right_To_Left_Down_Control(g_iLeft_Compensation);
} }
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上 else if (_iMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
{ {
Vertical_Lane_Change_From_Left_To_Right_Down_Control(iRight_Compensation); Vertical_Lane_Change_From_Left_To_Right_Down_Control(g_iRight_Compensation);
} }
break; return;
} }
else if (g_stMK32.CH4_SA == -1000) else if (_pstMK32->CH4_SA == -1000)
{ {
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上 if (_iMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
{ {
Vertical_Lane_Change_From_Right_To_Left_UP_Control(iLeft_Compensation); Vertical_Lane_Change_From_Right_To_Left_UP_Control(g_iLeft_Compensation);
} }
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上 else if (_iMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
{ {
Vertical_Lane_Change_From_Left_To_Right_UP_Control(iRight_Compensation); Vertical_Lane_Change_From_Left_To_Right_UP_Control(g_iRight_Compensation);
} }
break; return;
} }
else // 重置前进计时器以便下一次换道重新计数 else // 重置前进计时器以便下一次换道重新计数
{ {
if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) if (_iMode == 2 || _iMode == 3)
{ {
g_ichangLineState = 0; 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);
@ -436,11 +267,11 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
if (0 == g_stAngleError_Ctl.m_iAnglelock) if (0 == g_stAngleError_Ctl.m_iAnglelock)
{ {
// 开关喷枪 // 开关喷枪
if (g_stPV.RunMode == 1) if (_iMode == 1)
{ {
if (g_stMK32.CH6_SC == -1000) if (_pstMK32->CH6_SC == -1000)
{ {
if (0 == iIsStopOffPaint) if (0 == g_bIsStopOffPaint)
{ {
g_stAngleError_Ctl.m_iPaintState = 0; g_stAngleError_Ctl.m_iPaintState = 0;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int)); MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
@ -448,14 +279,14 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
else else
{ {
iIsStopOffPaint = 0; g_bIsStopOffPaint = 0;
g_stAngleError_Ctl.m_iPaintState = 1; g_stAngleError_Ctl.m_iPaintState = 1;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int)); MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
} }
} }
else if(g_stPV.RunMode == 2 || g_stPV.RunMode == 3) else if(_iMode == 2 || _iMode == 3)
{ {
if (g_stMK32.CH13_S2 != CH13_S2_Value) if (_pstMK32->CH13_S2 != g_iS2LastValue)
{ {
if(g_stIV.CurrentSpeed != 0) if(g_stIV.CurrentSpeed != 0)
{ {
@ -468,23 +299,23 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
} }
} }
CH13_S2_Value = g_stMK32.CH13_S2; g_iS2LastValue = _pstMK32->CH13_S2;
} }
int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理 int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
// 自动巡航 // 自动巡航
if (g_stMK32.CH5_SB == -1000) if (_pstMK32->CH5_SB == -1000)
{ {
if (g_stPV.RunMode == 1) if (_iMode == 1)
{ {
if (iVehicleSpeed >= 0) if (g_iVehicleSpeed >= 0)
{ {
aiMotorSpeed[0] = iVehicleSpeed * 10; aiMotorSpeed[0] = g_iVehicleSpeed * 10;
aiMotorSpeed[1] = iVehicleSpeed * 10; aiMotorSpeed[1] = g_iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
} }
} }
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) else if (_iMode == 2 || _iMode == 3)
{ {
if (0 == g_stAngleError_Ctl.m_iAnglelock) if (0 == g_stAngleError_Ctl.m_iAnglelock)
{ {
@ -492,25 +323,25 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1, .m_iTime = -1,
.m_iSpeed = iVehicleSpeed .m_iSpeed = g_iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
} }
break; return;
} }
else if(g_stMK32.CH5_SB == 1000) else if(_pstMK32->CH5_SB == 1000)
{ {
if (g_stPV.RunMode == 1) if (_iMode == 1)
{ {
if (iVehicleSpeed >= 0) if (g_iVehicleSpeed >= 0)
{ {
aiMotorSpeed[0] = -iVehicleSpeed * 10; aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
aiMotorSpeed[1] = -iVehicleSpeed * 10; aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
} }
} }
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) else if (_iMode == 2 || _iMode == 3)
{ {
if (0 == g_stAngleError_Ctl.m_iAnglelock) if (0 == g_stAngleError_Ctl.m_iAnglelock)
{ {
@ -518,18 +349,18 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1, .m_iTime = -1,
.m_iSpeed = iVehicleSpeed .m_iSpeed = g_iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
} }
break; return;
} }
// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去 // 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance) if (abs(_pstMK32->CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(_pstMK32->CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
{ {
if ((g_stPV.RunMode == 2 || g_stPV.RunMode == 3) && 0 == iPaint) if ((_iMode == 2 || _iMode == 3) && 0 == g_iPaint)
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
} }
@ -537,25 +368,25 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
} }
break; return;
} }
// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效 // 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
// 摇杆角度死区判断 // 摇杆角度死区判断
int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI; 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 (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
{ {
if (g_stPV.RunMode == 1) if (_iMode == 1)
{ {
if (iVehicleSpeed >= 0) if (g_iVehicleSpeed >= 0)
{ {
aiMotorSpeed[0] = iVehicleSpeed * 10; aiMotorSpeed[0] = g_iVehicleSpeed * 10;
aiMotorSpeed[1] = iVehicleSpeed * 10; aiMotorSpeed[1] = g_iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
} }
} }
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) else if (_iMode == 2 || _iMode == 3)
{ {
if (0 == g_stAngleError_Ctl.m_iAnglelock) if (0 == g_stAngleError_Ctl.m_iAnglelock)
{ {
@ -563,7 +394,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1, .m_iTime = -1,
.m_iSpeed = iVehicleSpeed .m_iSpeed = g_iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -571,16 +402,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance) else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
{ {
if (g_stPV.RunMode == 1) if (_iMode == 1)
{ {
if (iVehicleSpeed >= 0) if (g_iVehicleSpeed >= 0)
{ {
aiMotorSpeed[0] = -iVehicleSpeed * 10; aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
aiMotorSpeed[1] = -iVehicleSpeed * 10; aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
} }
} }
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) else if (_iMode == 2 || _iMode == 3)
{ {
if (0 == g_stAngleError_Ctl.m_iAnglelock) if (0 == g_stAngleError_Ctl.m_iAnglelock)
{ {
@ -588,7 +419,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1, .m_iTime = -1,
.m_iSpeed = iVehicleSpeed .m_iSpeed = g_iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -616,6 +447,188 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); 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);
break; break;
} }
case CMD_SHOW_INFO: case CMD_SHOW_INFO:
@ -641,7 +654,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
lua_print("{%d, %d, %d, %d, %d, %ld}\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); 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", 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",
iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset); g_iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset);
break; break;
} }
default: default:

Write Preview
Loading…
Cancel
Save