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

遥控器上线才使能电机

paint_robot_new-v1.5
Lizongdi 2 weeks ago
parent
8abd77074c
commit
1884d3870a
3 changed files with 87 additions and 74 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. 1
      RBcore/include/BHBF.h
  2. 1
      project/paint_robot_new/paint_robot_new.c
  3. 159
      project/paint_robot_new/paint_robot_new_motors.c

1
RBcore/include/BHBF.h
View File

@ -78,6 +78,7 @@ typedef enum {
COMMON_CMD_STOP_ALL, // 机器人停止（建议各个控制硬件的模块都要实现） COMMON_CMD_STOP_ALL, // 机器人停止（建议各个控制硬件的模块都要实现）
MOTOR_START = 0x0100, // *电机命令开始* MOTOR_START = 0x0100, // *电机命令开始*
MOTOR_SET_SPEED, // 设置速度 MOTOR_SET_SPEED, // 设置速度
MOTOR_POWER_ENABLE, // 电机上电
MOTOR_POWER_DISABLE, // 电机失电 MOTOR_POWER_DISABLE, // 电机失电
RBCORE_START = 0x0200, // *机器人共性命令开始* RBCORE_START = 0x0200, // *机器人共性命令开始*

1
project/paint_robot_new/paint_robot_new.c
View File

@ -268,6 +268,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)
{ {
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0);
g_Is_All_Button_Reset = 1; g_Is_All_Button_Reset = 1;
} }
} }

159
project/paint_robot_new/paint_robot_new_motors.c
View File

@ -43,6 +43,7 @@
static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL};
static uint32_t g_uiMotorModuleID = 0; static uint32_t g_uiMotorModuleID = 0;
static int g_aiMotorSpeed[MOTOR_INSTANCE_MAX] = {0}; static int g_aiMotorSpeed[MOTOR_INSTANCE_MAX] = {0};
static int g_isMotorInit = -1; // -1表示还在等待遥控器初始化完毕，0表示可以进行电机初始化，1表示电机初始化完毕，运行电机主循环
/*----------------------------------------------* /*----------------------------------------------*
* 常量定义 * * 常量定义 *
@ -72,6 +73,11 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]); g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]);
break; break;
} }
case MOTOR_POWER_ENABLE:
{
g_isMotorInit = 0;
break;
}
case MOTOR_POWER_DISABLE: case MOTOR_POWER_DISABLE:
{ {
HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1); HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1);
@ -125,83 +131,88 @@ static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize)
void MotorTask(void *argument) void MotorTask(void *argument)
{ {
// 此时消息中心还未就绪，直接调用HAL库接口给电机上电 g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler);
HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 0);
// 初始化FDCAN1，使用雷赛电机的回调
TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0);
g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);
CAN_IT_init(g_ptCAN1);
// 初始化电机管理模块
MotorMgr_Init();
// 配置左轮电机
Motor_Config_t stLeftMotorConfig = {
.m_ucMotorID = 1,
.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 = 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);
g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler);
MotorMgr_ResetAll(g_apstMotors[0]);
Rd_Delay(500);
MotorMgr_ResetAll(g_apstMotors[1]);
Rd_Delay(500);
int iCount = 6;
while(iCount--)
{
MotorMgr_ActivateAll(g_apstMotors[0]);
Rd_Delay(500);
MotorMgr_ActivateAll(g_apstMotors[1]);
Rd_Delay(500);
}
MotorMgr_SetHome(g_apstMotors[0]);
MotorMgr_SetHome(g_apstMotors[1]);
MotorMgr_SpeedModeInit(g_apstMotors[0]);
MotorMgr_SpeedModeInit(g_apstMotors[1]);
while(1) while(1)
{ {
static int LS_Motor_Config_Count = 0; // 处理消息
if (LS_Motor_Config_Count <= 200) MsgCenter_ProcessWait(g_uiMotorModuleID, 2);
{
LS_Motor_Config_Count++; if (0 == g_isMotorInit)
MotorMgr_SetWatchdog(g_apstMotors[0], 1000); {
HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 0);
// 初始化FDCAN1，使用雷赛电机的回调
TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0);
g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);
CAN_IT_init(g_ptCAN1);
// 初始化电机管理模块
MotorMgr_Init();
// 配置左轮电机
Motor_Config_t stLeftMotorConfig = {
.m_ucMotorID = 1,
.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 = 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);
MotorMgr_ResetAll(g_apstMotors[0]);
Rd_Delay(500);
MotorMgr_ResetAll(g_apstMotors[1]);
Rd_Delay(500);
int iCount = 6;
while(iCount--)
{
MotorMgr_ActivateAll(g_apstMotors[0]);
Rd_Delay(500);
MotorMgr_ActivateAll(g_apstMotors[1]);
Rd_Delay(500);
}
MotorMgr_SetHome(g_apstMotors[0]);
MotorMgr_SetHome(g_apstMotors[1]);
MotorMgr_SpeedModeInit(g_apstMotors[0]);
MotorMgr_SpeedModeInit(g_apstMotors[1]);
g_isMotorInit = 1;
}
else if (g_isMotorInit > 0)
{
static int LS_Motor_Config_Count = 0;
if (LS_Motor_Config_Count <= 200)
{
LS_Motor_Config_Count++;
MotorMgr_SetWatchdog(g_apstMotors[0], 1000);
Rd_Delay(4);
MotorMgr_SetWatchdog(g_apstMotors[1], 1000);
Rd_Delay(4);
}
MotorMgr_RequestPosition(g_apstMotors[0]);
Rd_Delay(4); Rd_Delay(4);
MotorMgr_SetWatchdog(g_apstMotors[1], 1000); MotorMgr_RequestPosition(g_apstMotors[1]);
Rd_Delay(4); Rd_Delay(4);
} MotorMgr_RequestFault(g_apstMotors[0]);
// 处理消息 Rd_Delay(4);
MsgCenter_ProcessWait(g_uiMotorModuleID, 2); MotorMgr_RequestFault(g_apstMotors[1]);
MotorMgr_RequestPosition(g_apstMotors[0]); Rd_Delay(4);
Rd_Delay(4); MotorMgr_RequestVelocity(g_apstMotors[0]);
MotorMgr_RequestPosition(g_apstMotors[1]); Rd_Delay(4);
Rd_Delay(4); MotorMgr_RequestVelocity(g_apstMotors[1]);
MotorMgr_RequestFault(g_apstMotors[0]); Rd_Delay(4);
Rd_Delay(4); MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]);
MotorMgr_RequestFault(g_apstMotors[1]); Rd_Delay(4);
Rd_Delay(4); MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]);
MotorMgr_RequestVelocity(g_apstMotors[0]); Rd_Delay(4);
Rd_Delay(4); }
MotorMgr_RequestVelocity(g_apstMotors[1]);
Rd_Delay(4);
MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]);
Rd_Delay(4);
MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]);
Rd_Delay(4);
} }
} }

Write Preview
Loading…
Cancel
Save