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. 497
      project/paint_robot_new/paint_robot_new.c

497
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,6 +219,236 @@ static void Move_Halt_AngleError(void)
} }
} }
void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode)
{
// 急停
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(g_iLeft_Compensation);
}
else if (_iMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
{
Vertical_Lane_Change_From_Left_To_Right_Down_Control(g_iRight_Compensation);
}
return;
}
else if (_pstMK32->CH4_SA == -1000)
{
if (_iMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
{
Vertical_Lane_Change_From_Right_To_Left_UP_Control(g_iLeft_Compensation);
}
else if (_iMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
{
Vertical_Lane_Change_From_Left_To_Right_UP_Control(g_iRight_Compensation);
}
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));
}
}
else if(_iMode == 2 || _iMode == 3)
{
if (_pstMK32->CH13_S2 != g_iS2LastValue)
{
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));
}
else
{
//log_w("cannot open paint with 0 move speed !");
}
}
}
g_iS2LastValue = _pstMK32->CH13_S2;
}
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)
{
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)
{
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)
{
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)
{
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) static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
if (NULL == pstMsg) if (NULL == pstMsg)
@ -220,23 +456,17 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
return; 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 iMotorOK = 0;
static int CH13_S2_Value = 0;
switch (pstMsg->m_uiMsgID) switch (pstMsg->m_uiMsgID)
{ {
case CUSTOM_RESET_PAINT: case CUSTOM_RESET_PAINT:
{ {
iPaint = 1; g_iPaint = 1;
break; break;
} }
case CUSTOM_SET_STOP_OFF_PAINT: case CUSTOM_SET_STOP_OFF_PAINT:
{ {
iIsStopOffPaint = 1; g_bIsStopOffPaint = 1;
} }
case CUSTOM_GET_DAEMON_CODE: case CUSTOM_GET_DAEMON_CODE:
{ {
@ -328,7 +558,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
RD_MEMCPY(&paintstate, pstMsg->m_aucData, sizeof(paintstate)); RD_MEMCPY(&paintstate, pstMsg->m_aucData, sizeof(paintstate));
HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, paintstate); HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, paintstate);
} }
if (0 == paintstate) iPaint = 0; if (0 == paintstate) g_iPaint = 0;
break; break;
} }
case CUSTOM_GET_MK32: case CUSTOM_GET_MK32:
@ -343,7 +573,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
if (g_stMK32.CH4_SA == 0 && g_stMK32.CH5_SB == 0 && g_stMK32.CH6_SC == 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_stMK32.CH7_SD == 0 && g_stMK32.IsOnline == 1)
{ {
CH13_S2_Value = g_stMK32.CH13_S2; //防止第一次值不正确 g_iS2LastValue = g_stMK32.CH13_S2; //防止第一次值不正确
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0);
g_Is_All_Button_Reset = 1; g_Is_All_Button_Reset = 1;
} }
@ -358,9 +588,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
break; break;
} }
// 急停或者遥控器失联 // 遥控器失联或者电机报错
if ((g_stMK32.CH8_SE == -1000 && g_stMK32.CH9_SF == -1000) if (g_stMK32.IsOnline == 0 || g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
|| 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) if (g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
{ {
@ -392,230 +621,14 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200; int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200;
if (iSpeedSelection > 0) if (iSpeedSelection > 0)
{ {
iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; g_iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
} }
g_stIV.RobotMoveSpeed = iVehicleSpeed; g_stIV.RobotMoveSpeed = g_iVehicleSpeed;
MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&g_iVehicleSpeed, sizeof(int));
Move_Halt_AngleError(); Move_Halt_AngleError();
// 根据模式判断换道（仅在竖直向左或向右生效） MK32_Key_Func(&g_stMK32, g_stPV.RunMode);
if (g_stMK32.CH4_SA == 1000)
{
if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
{
Vertical_Lane_Change_From_Right_To_Left_Down_Control(iLeft_Compensation);
}
else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
{
Vertical_Lane_Change_From_Left_To_Right_Down_Control(iRight_Compensation);
}
break;
}
else if (g_stMK32.CH4_SA == -1000)
{
if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
{
Vertical_Lane_Change_From_Right_To_Left_UP_Control(iLeft_Compensation);
}
else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
{
Vertical_Lane_Change_From_Left_To_Right_UP_Control(iRight_Compensation);
}
break;
}
else // 重置前进计时器以便下一次换道重新计数
{
if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{
g_ichangLineState = 0;
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
}
}
if (0 == g_stAngleError_Ctl.m_iAnglelock)
{
// 开关喷枪
if (g_stPV.RunMode == 1)
{
if (g_stMK32.CH6_SC == -1000)
{
if (0 == iIsStopOffPaint)
{
g_stAngleError_Ctl.m_iPaintState = 0;
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
}
}
else
{
iIsStopOffPaint = 0;
g_stAngleError_Ctl.m_iPaintState = 1;
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)
{
if (g_stMK32.CH13_S2 != CH13_S2_Value)
{
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));
}
else
{
//log_w("cannot open paint with 0 move speed !");
}
}
}
CH13_S2_Value = g_stMK32.CH13_S2;
}
int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
// 自动巡航
if (g_stMK32.CH5_SB == -1000)
{
if (g_stPV.RunMode == 1)
{
if (iVehicleSpeed >= 0)
{
aiMotorSpeed[0] = iVehicleSpeed * 10;
aiMotorSpeed[1] = iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
}
}
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{
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 = iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
break;
}
else if(g_stMK32.CH5_SB == 1000)
{
if (g_stPV.RunMode == 1)
{
if (iVehicleSpeed >= 0)
{
aiMotorSpeed[0] = -iVehicleSpeed * 10;
aiMotorSpeed[1] = -iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
}
}
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{
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 = iVehicleSpeed
};
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
}
}
break;
}
// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
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 ((g_stPV.RunMode == 2 || g_stPV.RunMode == 3) && 0 == iPaint)
{
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
}
else
{
MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
}
break;
}
// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
// 摇杆角度死区判断
int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI;
if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
{
if (g_stPV.RunMode == 1)
{
if (iVehicleSpeed >= 0)
{
aiMotorSpeed[0] = iVehicleSpeed * 10;
aiMotorSpeed[1] = iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
}
}
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{
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 = 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 (g_stPV.RunMode == 1)
{
if (iVehicleSpeed >= 0)
{
aiMotorSpeed[0] = -iVehicleSpeed * 10;
aiMotorSpeed[1] = -iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
}
}
else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{
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 = 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);
}
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