diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 47c0d8d..61aa1cc 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -7,16 +7,17 @@ using namespace iflytop; using namespace std; #define TAG "XYRobotCtrlModule" -void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // - IStepperMotor* stepM2, // - ZGPIO* gpiotable, // +void XYRobotCtrlModule::initialize(int32_t id, IStepperMotor* stepM1, // + IStepperMotor* stepM2, // + ZGPIO* Xgpio, // int ngpio, const char* flashmark) { m_stepM1 = stepM1; m_stepM2 = stepM2; - m_Xgpio = &gpiotable[0]; - m_Ygpio = &gpiotable[1]; - m_gpiotable = gpiotable; + m_Xgpio = &Xgpio[0]; + m_Ygpio = &Xgpio[1]; + m_gpiotable = Xgpio; m_ngpio = ngpio; + m_moduleId = id; ZASSERT(m_stepM1); ZASSERT(m_stepM2); ZASSERT(m_Xgpio); @@ -35,7 +36,7 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); } -int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) { +int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int32_t speed, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_to x:%d y:%d", x, y); int runspeed = speed; @@ -75,7 +76,7 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st return 0; } -int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) { +int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_by_no_limit dx:%d dy:%d", dx, dy); @@ -107,7 +108,7 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, a return 0; }; -int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) { +int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_by dx:%d dy:%d %d", dx, dy, speed); @@ -526,7 +527,7 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t } x += m_basecfg.shift_x; y += m_basecfg.shift_y; - + if (m_basecfg.x_shaft) x = -x; if (m_basecfg.y_shaft) y = -y; } @@ -594,4 +595,102 @@ int32_t XYRobotCtrlModule::factory_reset() { // } active_cfg(); return 0; -}; \ No newline at end of file +}; + +int32_t XYRobotCtrlModule::getid(int32_t* id) { + *id = m_moduleId; + return 0; +} +int32_t XYRobotCtrlModule::module_stop() { + ZLOGI(TAG, "module_stop"); + m_thread.stop(); + _motor_stop(); + return 0; +} +int32_t XYRobotCtrlModule::module_break() { + ZLOGI(TAG, "module_break"); + m_thread.stop(); + _motor_stop(m_basecfg.breakdec); + return 0; +} +int32_t XYRobotCtrlModule::module_get_last_exec_status(int32_t* status) { + *status = 0; + return 0; +} +int32_t XYRobotCtrlModule::module_get_status(int32_t* status) { + *status = m_thread.isworking() ? 0x01 : 0x00; + return 0; +} +int32_t XYRobotCtrlModule::module_get_error(int32_t* iserror) { + *iserror = 0; + return 0; +} +int32_t XYRobotCtrlModule::module_clear_error() { return 0; } +int32_t XYRobotCtrlModule::module_set_param(int32_t param_id, int32_t param_value) { return _module_set_or_get_param(true, param_id, param_value); } +int32_t XYRobotCtrlModule::module_get_param(int32_t param_id, int32_t* param_value) { return _module_set_or_get_param(false, param_id, *param_value); } +#define SET_CONFIG(param_id, configval, param_value) \ + case param_id: \ + if (set) { \ + configval = param_value; \ + } else { \ + param_value = configval; \ + } \ + break; +int32_t XYRobotCtrlModule::_module_set_or_get_param(bool set, int32_t param_id, int32_t& param_value) { + switch (param_id) { + SET_CONFIG(kcfg_motor_x_shift, m_basecfg.shift_x, param_value); + SET_CONFIG(kcfg_motor_y_shift, m_basecfg.shift_y, param_value); + SET_CONFIG(kcfg_motor_x_shaft, m_basecfg.x_shaft, param_value); + SET_CONFIG(kcfg_motor_y_shaft, m_basecfg.y_shaft, param_value); + SET_CONFIG(kcfg_motor_x_one_circle_pulse, m_basecfg.distance_scale, param_value); + SET_CONFIG(kcfg_motor_y_one_circle_pulse, m_basecfg.distance_scale, param_value); + SET_CONFIG(kcfg_motor_default_velocity, m_basecfg.maxspeed, param_value); + SET_CONFIG(kcfg_motor_default_acc, m_basecfg.acc, param_value); + SET_CONFIG(kcfg_motor_default_dec, m_basecfg.dec, param_value); + SET_CONFIG(kcfg_motor_default_break_dec, m_basecfg.breakdec, param_value); + SET_CONFIG(kcfg_motor_run_to_zero_max_x_d, m_basecfg.run_to_zero_max_d, param_value); + SET_CONFIG(kcfg_motor_run_to_zero_max_y_d, m_basecfg.run_to_zero_max_d, param_value); + SET_CONFIG(kcfg_motor_look_zero_edge_max_x_d, m_basecfg.look_zero_edge_max_d, param_value); + SET_CONFIG(kcfg_motor_look_zero_edge_max_y_d, m_basecfg.look_zero_edge_max_d, param_value); + SET_CONFIG(kcfg_motor_run_to_zero_speed, m_basecfg.run_to_zero_speed, param_value); + SET_CONFIG(kcfg_motor_run_to_zero_dec, m_basecfg.run_to_zero_dec, param_value); + SET_CONFIG(kcfg_motor_look_zero_edge_speed, m_basecfg.look_zero_edge_speed, param_value); + SET_CONFIG(kcfg_motor_look_zero_edge_dec, m_basecfg.look_zero_edge_dec, param_value); + SET_CONFIG(k_cfg_stepmotor_ihold, m_basecfg.ihold, param_value); + SET_CONFIG(k_cfg_stepmotor_irun, m_basecfg.irun, param_value); + SET_CONFIG(k_cfg_stepmotor_iholddelay, m_basecfg.iholddelay, param_value); + SET_CONFIG(k_cfg_xyrobot_robot_type, m_basecfg.robot_type, param_value); + + default: + return err::kmodule_not_find_config_index; + } + return 0; +} + +int32_t XYRobotCtrlModule::module_readio(int32_t* io) { + *io = 0; + for (int i = 0; i < m_ngpio; i++) { + if (i > 32) break; + if (m_gpiotable[i].getState()) *io |= (1 << i); + } + return 0; +} +int32_t XYRobotCtrlModule::module_writeio(int32_t io) { return 0; } +int32_t XYRobotCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) { + *adc = 0; + return 0; +} +int32_t XYRobotCtrlModule::module_factory_reset() { return factory_reset(); } +int32_t XYRobotCtrlModule::module_flush_cfg() { return flush(); } +int32_t XYRobotCtrlModule::module_active_cfg() { + active_cfg(); + return 0; +} + +int32_t XYRobotCtrlModule::xymotor_enable(int32_t varenable) { + enable(varenable); + return 0; +} +int32_t XYRobotCtrlModule::xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity) { return move_by(dx, dy, motor_velocity, nullptr); } +int32_t XYRobotCtrlModule::xymotor_move_to(int32_t x, int32_t y, int32_t motor_velocity) { return move_to(x, y, motor_velocity, nullptr); } +int32_t XYRobotCtrlModule::xymotor_move_to_zero() { return move_to_zero(nullptr); } 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 4974451..1e8a503 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -4,9 +4,10 @@ #include "sdk\components\tmc\basic\tmc_ic_interface.hpp" #include "sdk\components\zcancmder\zcanreceiver.hpp" #include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp" namespace iflytop { -class XYRobotCtrlModule : public I_XYRobotCtrlModule { +class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public ZIModule { public: private: IStepperMotor* m_stepM1; @@ -20,6 +21,8 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { ZThread m_thread; + int32_t m_moduleId; + int m_x = 0; int m_y = 0; @@ -38,14 +41,14 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { // zmutex m_statu_lock; public: - void initialize(IStepperMotor* stepM1, // - IStepperMotor* stepM2, // - ZGPIO* Xgpio, // + void initialize(int32_t id, IStepperMotor* stepM1, // + IStepperMotor* stepM2, // + ZGPIO* Xgpio, // int ngpio, const char* flashmark); - virtual int32_t move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) override; - virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override; - virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override; + virtual int32_t move_to(int32_t x, int32_t y, int32_t speed, action_cb_status_t status_cb) override; + virtual int32_t move_by(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) override; + virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) override; virtual int32_t move_to_zero(action_cb_status_t status_cb) override; virtual int32_t move_to_zero_with_calibrate(int32_t x, int32_t y, action_cb_status_t status_cb) override; @@ -67,7 +70,30 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { void loop(); void dumpcfg(); + virtual int32_t getid(int32_t* id); + virtual int32_t module_stop(); + virtual int32_t module_break(); + virtual int32_t module_get_last_exec_status(int32_t* status); + virtual int32_t module_get_status(int32_t* status); + virtual int32_t module_get_error(int32_t* iserror); + virtual int32_t module_clear_error(); + virtual int32_t module_set_param(int32_t param_id, int32_t param_value); + virtual int32_t module_get_param(int32_t param_id, int32_t* param_value); + virtual int32_t module_readio(int32_t* io); + virtual int32_t module_writeio(int32_t io); + virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc); + virtual int32_t module_factory_reset(); + virtual int32_t module_flush_cfg(); + virtual int32_t module_active_cfg(); + + virtual int32_t xymotor_enable(int32_t enable); + virtual int32_t xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity); + virtual int32_t xymotor_move_to(int32_t x, int32_t y, int32_t motor_velocity); + virtual int32_t xymotor_move_to_zero(); + private: + virtual int32_t _module_set_or_get_param(bool set, int32_t param_id, int32_t& param_value); + void active_cfg(); void computeTargetMotorPos(); void getnowpos(int32_t& x, int32_t& y); diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 676b588..7a3d980 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 676b588a68b809d8b7c7db9ea377f52cef377885 +Subproject commit 7a3d9800ce4e710385e513f2c430c267dabefc7b diff --git a/components/zprotocols/zcancmder_v2 b/components/zprotocols/zcancmder_v2 index 87d1a54..850402c 160000 --- a/components/zprotocols/zcancmder_v2 +++ b/components/zprotocols/zcancmder_v2 @@ -1 +1 @@ -Subproject commit 87d1a547ef95f5739c1177063dfb8b3db6b6df81 +Subproject commit 850402ceaf70cdcabc1773af1ef4bc7019cada36