diff --git a/components/sensors/i2ceeprom/m24lr64e_i2c_eeprom.cpp b/components/sensors/i2ceeprom/m24lr64e_i2c_eeprom.cpp index a9900b9..742a146 100644 --- a/components/sensors/i2ceeprom/m24lr64e_i2c_eeprom.cpp +++ b/components/sensors/i2ceeprom/m24lr64e_i2c_eeprom.cpp @@ -38,7 +38,7 @@ int32_t M24LR64E_I2CEEPROM::start_monitor_status(functionenable(venable); m_stepM2->enable(venable); + m_enable = venable; return 0; } int32_t XYRobotCtrlModule::stop(uint8_t stopType) { @@ -336,8 +339,8 @@ int32_t XYRobotCtrlModule::set_base_param(const base_param_t& inparam) { }; void XYRobotCtrlModule::active_cfg() { - m_stepM1->setScale(m_basecfg.distance_scale); - m_stepM2->setScale(m_basecfg.distance_scale); + m_stepM1->setScale(m_basecfg.distance_scale / 2); + m_stepM2->setScale(m_basecfg.distance_scale / 2); m_stepM1->setIHOLD_IRUN(m_basecfg.ihold, m_basecfg.irun, m_basecfg.iholddelay); m_stepM2->setIHOLD_IRUN(m_basecfg.ihold, m_basecfg.irun, m_basecfg.iholddelay); } @@ -525,19 +528,20 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t x = m1pos + m2pos; y = m1pos - m2pos; } - x += m_basecfg.shift_x; - y += m_basecfg.shift_y; if (m_basecfg.x_shaft) x = -x; if (m_basecfg.y_shaft) y = -y; + + x += m_basecfg.shift_x; + y += m_basecfg.shift_y; } void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { - if (m_basecfg.x_shaft) x = -x; - if (m_basecfg.y_shaft) y = -y; - x -= m_basecfg.shift_x; y -= m_basecfg.shift_y; + if (m_basecfg.x_shaft) x = -x; + if (m_basecfg.y_shaft) y = -y; + if (m_basecfg.robot_type == corexy) { m1pos = ((x + y) / 2); m2pos = ((y - x) / 2); @@ -667,6 +671,42 @@ int32_t XYRobotCtrlModule::_module_set_or_get_param(bool set, int32_t param_id, return 0; } +int32_t XYRobotCtrlModule::module_set_state(int32_t state_id, int32_t state_value) { return err::kmodule_not_find_state_index; } +int32_t XYRobotCtrlModule::module_get_state(int32_t state_id, int32_t* state_value) { +#if 0 +kstate_module_status +kstate_module_errorcode +// kstate_xyrobot_move +kstate_xyrobot_enable +kstate_xyrobot_x_pos +kstate_xyrobot_y_pos +kstate_xyrobot_x_dpos +kstate_xyrobot_y_dpos +#endif + // state_value +#define GET_STATE(param_id, configval) \ + case param_id: \ + *state_value = configval; \ + return 0; \ + break; + + int32_t x, y; + getnowpos(x, y); + + switch (state_id) { + GET_STATE(kstate_module_status, m_thread.isworking() ? 0x01 : 0x00); + GET_STATE(kstate_module_errorcode, 0); + GET_STATE(kstate_xyrobot_enable, m_enable); + GET_STATE(kstate_xyrobot_x_pos, x); + GET_STATE(kstate_xyrobot_y_pos, y); + GET_STATE(kstate_xyrobot_x_dpos, m_dx); + GET_STATE(kstate_xyrobot_y_dpos, m_dy); + + default: + return err::kmodule_not_find_state_index; + } +} + int32_t XYRobotCtrlModule::module_readio(int32_t* io) { *io = 0; for (int i = 0; i < m_ngpio; i++) { @@ -699,4 +739,27 @@ int32_t XYRobotCtrlModule::xymotor_move_to_zero_and_calculated_shift() { return int32_t XYRobotCtrlModule::xymotor_read_pos(int32_t* x, int32_t* y) { getnowpos(*x, *y); return 0; -} \ No newline at end of file +} + +int32_t XYRobotCtrlModule::xymotor_calculated_pos_by_move_to_zero() { + zlock_guard lock(m_lock); + ZLOGI(TAG, "xymotor_calculated_pos_by_move_to_zero "); + m_thread.stop(); + + m_thread.start([this]() { + int32_t dx, dy; + int32_t erro = exec_move_to_zero_task(dx, dy); + if (erro != 0) { + m_dx = 0; + m_dy = 0; + _motor_stop(); + return; + } + m_dx = dx; + m_dy = dy; + m_stepM1->setXACTUAL(0); + m_stepM2->setXACTUAL(0); + return; + }); + return 0; +} diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index 47bf014..a993fe9 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -26,6 +26,9 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z int m_x = 0; int m_y = 0; + int32_t m_dx; + int32_t m_dy; + // uint8_t m_status = 0; // base_param_t m_cfg; @@ -36,6 +39,7 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z const char* m_flashmark = nullptr; flash_config_t m_cfg; base_param_t& m_basecfg = m_cfg.basecfg; + bool m_enable = false; zmutex m_lock; // zmutex m_statu_lock; @@ -85,6 +89,8 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z virtual int32_t module_factory_reset(); virtual int32_t module_flush_cfg(); virtual int32_t module_active_cfg(); + virtual int32_t module_set_state(int32_t state_id, int32_t state_value); + virtual int32_t module_get_state(int32_t state_id, int32_t* state_value); virtual int32_t xymotor_enable(int32_t enable); virtual int32_t xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity); @@ -92,6 +98,7 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z virtual int32_t xymotor_move_to_zero(); virtual int32_t xymotor_move_to_zero_and_calculated_shift(); virtual int32_t xymotor_read_pos(int32_t* x, int32_t* y); + virtual int32_t xymotor_calculated_pos_by_move_to_zero(); private: virtual int32_t _module_set_or_get_param(bool set, int32_t param_id, int32_t& param_value); diff --git a/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp b/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp index dd945c0..495f483 100644 --- a/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp +++ b/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp @@ -15,10 +15,7 @@ void MicroComputerModuleDeviceScriptCmderPaser::do_dumpconfig(int32_t moduleId) ICmdParserACK ack; const char paraV_cache[1][10] = {{0}}; sprintf((char*)paraV_cache[0], "%ld", moduleId); - // sprintf((char*)¶V[0][0], "%d", moduleId); - const char* paraV[1] = {paraV_cache[0]}; - do_dumpconfig(1, (const char**)paraV, &ack); } @@ -70,24 +67,81 @@ void MicroComputerModuleDeviceScriptCmderPaser::do_dumpstate(int32_t paramN, con #if 0 typedef enum { - kstate_module_status = STATE_INDEX(0, 0), // + kstate_module_status = STATE_INDEX(0, 0), // 0idle,1busy,2error kstate_module_errorcode = STATE_INDEX(0, 1), // - kstate_motor_x_pos = STATE_INDEX(100, 0), // x偏移 - kstate_motor_y_pos = STATE_INDEX(100, 1), // y偏移 - kstate_motor_z_pos = STATE_INDEX(100, 2), // z偏移 - - kstate_motor_move = STATE_INDEX(100, 3), // 电机是否移动 - kstate_motor_enable = STATE_INDEX(100, 4), // 电机是否使能 + kstate_motor1_move = STATE_INDEX(100, 0), // + kstate_motor1_enable = STATE_INDEX(100, 1), // + kstate_motor1_current = STATE_INDEX(100, 2), // + kstate_motor1_vel = STATE_INDEX(100, 3), // + kstate_motor1_pos = STATE_INDEX(100, 4), // + kstate_motor1_temperature = STATE_INDEX(100, 5), // + kstate_motor1_dpos = STATE_INDEX(100, 6), // + + kstate_motor2_move = STATE_INDEX(110, 0), // + kstate_motor2_enable = STATE_INDEX(110, 1), // + kstate_motor2_current = STATE_INDEX(110, 2), // + kstate_motor2_vel = STATE_INDEX(110, 3), // + kstate_motor2_pos = STATE_INDEX(110, 4), // + kstate_motor2_temperature = STATE_INDEX(110, 5), // + kstate_motor2_dpos = STATE_INDEX(110, 6), // + + kstate_xyrobot_move = STATE_INDEX(1000, 0), // + kstate_xyrobot_enable = STATE_INDEX(1000, 1), // + kstate_xyrobot_x_pos = STATE_INDEX(1000, 2), // + kstate_xyrobot_y_pos = STATE_INDEX(1000, 3), // + kstate_xyrobot_z_pos = STATE_INDEX(1000, 4), // + kstate_xyrobot_x_dpos = STATE_INDEX(1000, 5), // + kstate_xyrobot_y_dpos = STATE_INDEX(1000, 6), // + kstate_xyrobot_z_dpos = STATE_INDEX(1000, 7), // } config_index_t; #endif + + ack->ecode = 0; + + uint16_t moduleId = atoi(paraV[0]); + int32_t configval = 0; + int32_t ecode = 0; + ZLOGI(TAG, "dumpstate %s", paraV[0]); + +#define DUMP_STATE(tag, configid) \ + ecode = m_deviceManager->module_get_state(moduleId, configid, &configval); \ + if (ecode == 0) { \ + ZLOGI(TAG, "%-30s :%d", tag, configval); \ + } + DUMP_STATE("kstate_module_status", kstate_module_status); + DUMP_STATE("kstate_module_errorcode", kstate_module_errorcode); + DUMP_STATE("kstate_motor1_move", kstate_motor1_move); + DUMP_STATE("kstate_motor1_enable", kstate_motor1_enable); + DUMP_STATE("kstate_motor1_current", kstate_motor1_current); + DUMP_STATE("kstate_motor1_vel", kstate_motor1_vel); + DUMP_STATE("kstate_motor1_pos", kstate_motor1_pos); + DUMP_STATE("kstate_motor1_temperature", kstate_motor1_temperature); + DUMP_STATE("kstate_motor1_dpos", kstate_motor1_dpos); + DUMP_STATE("kstate_motor2_move", kstate_motor2_move); + DUMP_STATE("kstate_motor2_enable", kstate_motor2_enable); + DUMP_STATE("kstate_motor2_current", kstate_motor2_current); + DUMP_STATE("kstate_motor2_vel", kstate_motor2_vel); + DUMP_STATE("kstate_motor2_pos", kstate_motor2_pos); + DUMP_STATE("kstate_motor2_temperature", kstate_motor2_temperature); + DUMP_STATE("kstate_motor2_dpos", kstate_motor2_dpos); + DUMP_STATE("kstate_xyrobot_move", kstate_xyrobot_move); + DUMP_STATE("kstate_xyrobot_enable", kstate_xyrobot_enable); + DUMP_STATE("kstate_xyrobot_x_pos", kstate_xyrobot_x_pos); + DUMP_STATE("kstate_xyrobot_y_pos", kstate_xyrobot_y_pos); + DUMP_STATE("kstate_xyrobot_z_pos", kstate_xyrobot_z_pos); + DUMP_STATE("kstate_xyrobot_x_dpos", kstate_xyrobot_x_dpos); + DUMP_STATE("kstate_xyrobot_y_dpos", kstate_xyrobot_y_dpos); + DUMP_STATE("kstate_xyrobot_z_dpos", kstate_xyrobot_z_dpos); + + // module_get_state } void MicroComputerModuleDeviceScriptCmderPaser::do_dumpstate(int32_t moduleId) { ICmdParserACK ack; - const char paraV[1][10] = {{0}}; - sprintf((char*)paraV[0], "%d", moduleId); - + const char paraV_cache[1][10] = {{0}}; + sprintf((char*)paraV_cache[0], "%ld", moduleId); + const char* paraV[1] = {paraV_cache[0]}; do_dumpstate(1, (const char**)paraV, &ack); } diff --git a/components/zprotocols/zcancmder_v2 b/components/zprotocols/zcancmder_v2 index 363a87e..e37e4ff 160000 --- a/components/zprotocols/zcancmder_v2 +++ b/components/zprotocols/zcancmder_v2 @@ -1 +1 @@ -Subproject commit 363a87ebdc1d83cc41ae8a2b5f8d598d6baa7dfa +Subproject commit e37e4ff1ba87b827d26168d25955be8219ed058e