diff --git a/RBcore/BHBF.c b/RBcore/BHBF.c index 679663f..af3aa5a 100644 --- a/RBcore/BHBF.c +++ b/RBcore/BHBF.c @@ -55,31 +55,62 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) return; } + static int iSpeed = -1; + static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理 switch (pstMsg->m_uiMsgID) { - case RBCORE_CMD_STOP_ALL: + case CMD_STOP_ALL: { - //log_i("RBCORE_CMD_STOP_ALL"); + MsgCenter_SendTo(MODULE_NAME_MOTOR, pstMsg->m_uiMsgID, NULL, 0); break; } case RBCORE_CMD_MANUAL_FORWARD: { - log_i("RBCORE_CMD_MANUAL_FORWARD"); + if (iSpeed >= 0) + { + aiMotorSpeed[0] = iSpeed; + aiMotorSpeed[1] = iSpeed; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } break; } case RBCORE_CMD_MANUAL_BACKWARD: { - log_i("RBCORE_CMD_MANUAL_BACKWARD"); + if (iSpeed >= 0) + { + aiMotorSpeed[0] = -iSpeed; + aiMotorSpeed[1] = -iSpeed; + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } break; } case RBCORE_CMD_MANUAL_TURNLEFT: { - log_i("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_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } break; } case RBCORE_CMD_MANUAL_TURNRIGHT: { - log_i("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_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } + break; + } + case RBCORE_GET_VEHICLE_SPEED: + { + if (pstMsg->m_uiDataLen >= sizeof(int)) + { + RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int)); + iSpeed *= 10; + } break; } default: diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index a8e9f00..205e79a 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -78,18 +78,18 @@ typedef enum { LUA_SHOW_INFO, MOTOR_CMD_SET_SPEED, // 设置速度 MOTOR_CMD_SET_POSITION, // 设置位置 - MOTOR_CMD_STOP, // 停止 MOTOR_CMD_GET_STATUS, // 获取状态 MOTOR_CMD_SET_HOME, // 设置零点 MOTOR_CMD_SPEED_MODE, // 速度模式 MOTOR_CMD_POSITION_MODE, // 位置模式 MOTOR_CMD_END = 0x0100, - RBCORE_CMD_STOP_ALL, // 机器人停止 + CMD_STOP_ALL, // 机器人停止 RBCORE_CMD_MANUAL_FORWARD, // 机器人手动前进 RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退 RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转 RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转 + RBCORE_GET_VEHICLE_SPEED, // 获取机器人速度 RBCORE_CMD_END = 0x0200, CUSTOM_GET_PV, // 获取PV diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 0ddec48..4a95813 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -93,7 +93,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) 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) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_RBCORE, CMD_STOP_ALL, NULL, 0); break; } @@ -117,7 +117,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else { - MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_RBCORE, CMD_STOP_ALL, NULL, 0); } break; } diff --git a/project/paint_robot_new/paint_robot_new_motors.c b/project/paint_robot_new/paint_robot_new_motors.c index a2f6217..3612f16 100644 --- a/project/paint_robot_new/paint_robot_new_motors.c +++ b/project/paint_robot_new/paint_robot_new_motors.c @@ -62,25 +62,22 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg) { case MOTOR_CMD_SET_SPEED: { - int iSpeed = 0; - if (pstMsg->m_uiDataLen >= sizeof(int)) + int aiMotorSpeed[2] = {0}; + if (pstMsg->m_uiDataLen >= sizeof(aiMotorSpeed)) { - RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int)); + RD_MEMCPY(aiMotorSpeed, pstMsg->m_aucData, sizeof(aiMotorSpeed)); } - //log_i("Set Speed %d", iSpeed); - g_aiMotorSpeed[0] = iSpeed; - g_aiMotorSpeed[1] = iSpeed; + g_aiMotorSpeed[0] = MotorMgr_SpeedConvert(g_apstMotors[0], aiMotorSpeed[0]); + g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]); break; } - case MOTOR_CMD_STOP: + case CMD_STOP_ALL: { for (int i = 0; i < 2; i++) { - if (g_apstMotors[i] != NULL) - { - MotorMgr_SetTargetSpeed(g_apstMotors[i], 0); - } + MotorMgr_SetTargetSpeed(g_apstMotors[i], 0); } + memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed)); break; } case MOTOR_CMD_SET_HOME: @@ -174,18 +171,18 @@ void MotorTask(void *argument) // 配置左轮电机 Motor_Config_t stLeftMotorConfig = { .m_ucMotorID = 1, - .m_uiPulsePerRound = 10000, - .m_uiReductionRatio = 70, - .m_fWheelDiameter = 0.26f + .m_uiPulsePerRound = g_stCV.pulse_Per_Circle, + .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio, + .m_fWheelDiameter = g_stCV.wheel_Diameter_m }; g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL); // 配置右轮电机 Motor_Config_t stRightMotorConfig = { .m_ucMotorID = 2, - .m_uiPulsePerRound = 10000, - .m_uiReductionRatio = 70, - .m_fWheelDiameter = 0.26f + .m_uiPulsePerRound = g_stCV.pulse_Per_Circle, + .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio, + .m_fWheelDiameter = g_stCV.wheel_Diameter_m }; g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);