|
|
@ -32,6 +32,7 @@ void Device::init() { |
|
|
|
init_dm(); |
|
|
|
init_bus(); |
|
|
|
sub_module_init(); |
|
|
|
ZLOGI(TAG, "init end..."); |
|
|
|
|
|
|
|
while (true) { |
|
|
|
OSDefaultSchduler::getInstance()->loop(); |
|
|
@ -78,7 +79,6 @@ void Device::init_bus() { |
|
|
|
ZLOGI(TAG, "init feite bus"); |
|
|
|
ZASSERT(huart3.Init.BaudRate == 115200); |
|
|
|
m_feitebus.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx); |
|
|
|
|
|
|
|
/*******************************************************************************
|
|
|
|
* 初始化MODBUS总线 * |
|
|
|
*******************************************************************************/ |
|
|
@ -101,7 +101,7 @@ void Device::sub_module_init() { |
|
|
|
*/ |
|
|
|
static MiniRobotCtrlModule::flash_config_t servo_cfg = {.default_torque = 330}; |
|
|
|
static MiniRobotCtrlModule mini_servo[10]; |
|
|
|
for (size_t i = 0; i < 6; i++) { |
|
|
|
for (size_t i = 1; i <= 6; i++) { |
|
|
|
int idnum = 10 + i; |
|
|
|
ZLOGI(TAG, "init servo:%d", idnum); |
|
|
|
mini_servo[i].initialize(idnum, &m_feitebus, idnum, &servo_cfg); |
|
|
@ -128,23 +128,27 @@ void Device::sub_module_init() { |
|
|
|
static ZIProtocolProxy z_axis_board_can_module_proxy[4]; |
|
|
|
z_axis_board_can_module_proxy[0].initialize(30, &m_zcanbus); // BOARD3 Z轴板子
|
|
|
|
z_axis_board_can_module_proxy[1].initialize(31, &m_zcanbus); |
|
|
|
z_axis_board_can_module_proxy[1].initialize(32, &m_zcanbus); |
|
|
|
z_axis_board_can_module_proxy[2].initialize(33, &m_zcanbus); |
|
|
|
z_axis_board_can_module_proxy[2].initialize(32, &m_zcanbus); |
|
|
|
z_axis_board_can_module_proxy[3].initialize(33, &m_zcanbus); |
|
|
|
REG_SUB_MODULE(z_axis_board_can_module_proxy); |
|
|
|
|
|
|
|
/**
|
|
|
|
* @brief 初始化 |
|
|
|
* CAN步进电机模块 |
|
|
|
*/ |
|
|
|
static ZIProtocolProxy can_step_motor_proxy[6]; |
|
|
|
can_step_motor_proxy[0].initialize(40, &m_zcanbus); // BOARD4
|
|
|
|
can_step_motor_proxy[1].initialize(41, &m_zcanbus); |
|
|
|
|
|
|
|
can_step_motor_proxy[2].initialize(50, &m_zcanbus); // BOARD5
|
|
|
|
can_step_motor_proxy[3].initialize(51, &m_zcanbus); |
|
|
|
|
|
|
|
can_step_motor_proxy[4].initialize(60, &m_zcanbus); // BOARD6
|
|
|
|
can_step_motor_proxy[5].initialize(61, &m_zcanbus); |
|
|
|
static ZIProtocolProxy can_step_motor_proxy[9]; |
|
|
|
int i = 0; |
|
|
|
can_step_motor_proxy[i++].initialize(40, &m_zcanbus); // BOARD4
|
|
|
|
can_step_motor_proxy[i++].initialize(41, &m_zcanbus); |
|
|
|
can_step_motor_proxy[i++].initialize(42, &m_zcanbus); |
|
|
|
|
|
|
|
can_step_motor_proxy[i++].initialize(50, &m_zcanbus); // BOARD5
|
|
|
|
can_step_motor_proxy[i++].initialize(51, &m_zcanbus); |
|
|
|
can_step_motor_proxy[i++].initialize(52, &m_zcanbus); |
|
|
|
|
|
|
|
can_step_motor_proxy[i++].initialize(60, &m_zcanbus); // BOARD6
|
|
|
|
can_step_motor_proxy[i++].initialize(61, &m_zcanbus); |
|
|
|
can_step_motor_proxy[i++].initialize(62, &m_zcanbus); |
|
|
|
REG_SUB_MODULE(can_step_motor_proxy); |
|
|
|
} |
|
|
|
|
|
|
|