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

优化宏定义并把手动四个方向移动挪到custom

paint_robot_new-v1.5
Lizongdi 1 week ago
parent
d748d34499
commit
3befb9be59
6 changed files with 72 additions and 97 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. 56
      RBcore/BHBF.c
  2. 16
      RBcore/include/BHBF.h
  3. 2
      RBcore/msp_Timer.c
  4. 2
      README.md
  5. 77
      project/paint_robot_new/paint_robot_new.c
  6. 16
      project/paint_robot_new/paint_robot_new_motors.c

56
RBcore/BHBF.c
View File

@ -54,63 +54,13 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
{ {
return; return;
} }
static int iVehicleSpeed = -1; // 表示由RD1转换来的速度值
int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
switch (pstMsg->m_uiMsgID) switch (pstMsg->m_uiMsgID)
{ {
case RBCORE_CMD_STOP_ALL: case RBCORE_CMD_STOP_ALL:
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);
break;
}
case RBCORE_CMD_MANUAL_FORWARD:
{
if (iVehicleSpeed >= 0)
{
aiMotorSpeed[0] = iVehicleSpeed * 10;
aiMotorSpeed[1] = iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
}
break;
}
case RBCORE_CMD_MANUAL_BACKWARD:
{
if (iVehicleSpeed >= 0)
{
aiMotorSpeed[0] = -iVehicleSpeed * 10;
aiMotorSpeed[1] = -iVehicleSpeed * 10;
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
}
break;
}
case RBCORE_CMD_MANUAL_TURNLEFT:
{
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));
}
break;
}
case RBCORE_CMD_MANUAL_TURNRIGHT:
{
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));
}
break;
}
case RBCORE_GET_VEHICLE_SPEED:
{
if (pstMsg->m_uiDataLen >= sizeof(int))
{
RD_MEMCPY(&iVehicleSpeed, pstMsg->m_aucData, sizeof(int));
}
break; break;
} }
default: default:

16
RBcore/include/BHBF.h
View File

@ -75,24 +75,14 @@ extern "C"{
#define MODULE_NAME_TIMER "timer" #define MODULE_NAME_TIMER "timer"
typedef enum { typedef enum {
COMMON_CMD_SHOW_INFO, // 终端信息展示 CMD_SHOW_INFO, // 终端信息展示
COMMON_CMD_STOP_ALL, // 机器人停止（建议各个控制硬件的模块都要实现） CMD_STOP_ALL, // 机器人停止（建议各个控制硬件的模块都要实现）
MOTOR_START = 0x0100, // *电机命令开始* GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化）
MOTOR_SET_SPEED, // 设置速度 MOTOR_SET_SPEED, // 设置速度
MOTOR_POWER_ENABLE, // 电机上电 MOTOR_POWER_ENABLE, // 电机上电
MOTOR_POWER_DISABLE, // 电机失电 MOTOR_POWER_DISABLE, // 电机失电
MOTOR_GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化）
MOTOR_CMD_STOP_AFTER, // 先关枪后停车 MOTOR_CMD_STOP_AFTER, // 先关枪后停车
RBCORE_START = 0x0200, // *机器人共性命令开始*
RBCORE_CMD_STOP_ALL, // 此命令统一调用机器人各个模块的COMMON_CMD_STOP_ALL RBCORE_CMD_STOP_ALL, // 此命令统一调用机器人各个模块的COMMON_CMD_STOP_ALL
RBCORE_CMD_MANUAL_FORWARD, // 机器人手动前进
RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退
RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转
RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转
RBCORE_GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化）
CUSTOM_START = 0x0300, // *机器人特性命令开始*
CUSTOM_GET_PV, // 获取PV CUSTOM_GET_PV, // 获取PV
CUSTOM_GET_MK32, // 获取MK32 CUSTOM_GET_MK32, // 获取MK32
CUSTOM_GET_TL720D_ROLL, // 获取陀螺仪横滚角（IV显示） CUSTOM_GET_TL720D_ROLL, // 获取陀螺仪横滚角（IV显示）

2
RBcore/msp_Timer.c
View File

@ -159,7 +159,7 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
} }
break; break;
} }
case COMMON_CMD_SHOW_INFO: case CMD_SHOW_INFO:
{ {
lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %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, g_RF_Angle_Roll); g_dletAngle, uiLastSendTick, bTimerStarted, g_RF_Angle_Roll);

2
README.md
View File

@ -18,7 +18,7 @@ c1((按下急停))
end end
a((Read_MK32))-->CUSTOM_GET_MK32-->MOTOR_POWER_ENABLE-->CUSTOM_GET_MOTOR_OK-->c0 a((Read_MK32))-->CUSTOM_GET_MK32-->MOTOR_POWER_ENABLE-->CUSTOM_GET_MOTOR_OK-->c0
c1-->MOTOR_POWER_DISABLE-->m0 c1-->MOTOR_POWER_DISABLE-->m0
c1-->RBCORE_CMD_STOP_ALL-->COMMON_CMD_STOP_ALL c1-->RBCORE_CMD_STOP_ALL-->CMD_STOP_ALL
``` ```

77
project/paint_robot_new/paint_robot_new.c
View File

@ -189,7 +189,7 @@ static void Move_Halt_AngleError(void)
{ {
if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed) if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed)
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
} }
return; return;
} }
@ -202,7 +202,7 @@ static void Move_Halt_AngleError(void)
{ {
if(Rd_GetTime() - g_stAngleError_Ctl.m_ipaintOffCount > 1000) if(Rd_GetTime() - g_stAngleError_Ctl.m_ipaintOffCount > 1000)
{ {
MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪 MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);//先关枪
g_stAngleError_Ctl.m_iAnglelock = 1; g_stAngleError_Ctl.m_iAnglelock = 1;
return; return;
} }
@ -252,8 +252,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode)) if (pstMsg->m_uiDataLen >= sizeof(uiFailtCode))
{ {
RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode)); RD_MEMCPY(uiFailtCode, pstMsg->m_aucData, sizeof(uiFailtCode));
if (uiFailtCode[0] == 1) g_stIV.Left_Motor_Err = uiFailtCode[1]; if (uiFailtCode[0] == 1)
else g_stIV.Right_Motor_Err = uiFailtCode[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; break;
} }
@ -266,6 +274,11 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
if (1 == iCurrentSpeed[0]) if (1 == iCurrentSpeed[0])
{ {
g_stIV.CurrentSpeed = iCurrentSpeed[1]; 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; break;
@ -301,7 +314,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
g_ichangLineState = 1; // 走到这说明换道第一次转完了 g_ichangLineState = 1; // 走到这说明换道第一次转完了
break; break;
} }
case COMMON_CMD_STOP_ALL: case CMD_STOP_ALL:
{ {
HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, 1); HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, 1);
g_stAngleError_Ctl.m_iPaintState = 1; g_stAngleError_Ctl.m_iPaintState = 1;
@ -382,8 +395,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
} }
g_stIV.RobotMoveSpeed = iVehicleSpeed; g_stIV.RobotMoveSpeed = iVehicleSpeed;
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int)); MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
Move_Halt_AngleError(); Move_Halt_AngleError();
@ -459,12 +471,18 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
CH13_S2_Value = g_stMK32.CH13_S2; 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_stMK32.CH5_SB == -1000)
{ {
if (g_stPV.RunMode == 1) if (g_stPV.RunMode == 1)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); 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) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
@ -485,7 +503,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
if (g_stPV.RunMode == 1) if (g_stPV.RunMode == 1)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); 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) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
@ -512,7 +535,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
else else
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
} }
break; break;
} }
@ -525,8 +548,13 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
if (g_stPV.RunMode == 1) if (g_stPV.RunMode == 1)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); 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) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
if (0 == g_stAngleError_Ctl.m_iAnglelock) if (0 == g_stAngleError_Ctl.m_iAnglelock)
@ -545,7 +573,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
{ {
if (g_stPV.RunMode == 1) if (g_stPV.RunMode == 1)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); 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) else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
{ {
@ -563,19 +596,29 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
} }
else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance) else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0); 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) else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
{ {
MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0); 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 else
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
} }
break; break;
} }
case COMMON_CMD_SHOW_INFO: case CMD_SHOW_INFO:
{ {
lua_print("MK32 Info\nRxIndex:%d\tIsOnline:%d\n", g_stMK32.RxIndex, g_stMK32.IsOnline); 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("CH0_RY_H\t%d\n", g_stMK32.CH0_RY_H);

16
project/paint_robot_new/paint_robot_new_motors.c
View File

@ -97,7 +97,7 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1); HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1);
break; break;
} }
case MOTOR_GET_VEHICLE_SPEED: case GET_VEHICLE_SPEED:
{ {
if (pstMsg->m_uiDataLen >= sizeof(int)) if (pstMsg->m_uiDataLen >= sizeof(int))
{ {
@ -112,7 +112,7 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
if (1 == g_iLastVehicleMove) if (1 == g_iLastVehicleMove)
{ {
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_SET_STOP_OFF_PAINT, NULL, 0); MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_SET_STOP_OFF_PAINT, NULL, 0);
MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪 MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);//先关枪
iIsDelayStop = 1; iIsDelayStop = 1;
} }
@ -134,14 +134,14 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
} }
break; break;
} }
case COMMON_CMD_STOP_ALL: case CMD_STOP_ALL:
{ {
memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed)); memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
uiLastTime = Rd_GetTime(); uiLastTime = Rd_GetTime();
g_iLastVehicleMove = 0; g_iLastVehicleMove = 0;
break; break;
} }
case COMMON_CMD_SHOW_INFO: case CMD_SHOW_INFO:
{ {
lua_print("iVehicleSpeed = %d\nuiLastTime = %d\ng_isMotorInit = %d\ng_iLastVehicleMove = %d\n", lua_print("iVehicleSpeed = %d\nuiLastTime = %d\ng_isMotorInit = %d\ng_iLastVehicleMove = %d\n",
iVehicleSpeed, uiLastTime, g_isMotorInit, g_iLastVehicleMove); iVehicleSpeed, uiLastTime, g_isMotorInit, g_iLastVehicleMove);
@ -178,14 +178,6 @@ static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize)
// 调用电机管理模块解析响应 // 调用电机管理模块解析响应
MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)_pBuffer, _iSize); MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)_pBuffer, _iSize);
if (1 == ucMotorID)
{
MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_LEFT_MOTOR, NULL, 0);
}
else if (2 == ucMotorID)
{
MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_RIGHT_MOTOR, NULL, 0);
}
} }
void MotorTask(void *argument) void MotorTask(void *argument)

Write Preview
Loading…
Cancel
Save